istereo
diff libs/vmath/matrix_c.c @ 28:c0ae8e668447
added vmath library
author | John Tsiombikas <nuclear@mutantstargoat.com> |
---|---|
date | Thu, 08 Sep 2011 08:30:42 +0300 |
parents | |
children | ff055bff6a15 |
line diff
1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000 1.2 +++ b/libs/vmath/matrix_c.c Thu Sep 08 08:30:42 2011 +0300 1.3 @@ -0,0 +1,246 @@ 1.4 +#include <stdio.h> 1.5 +#include "matrix.h" 1.6 +#include "vector.h" 1.7 +#include "quat.h" 1.8 + 1.9 +void m3_to_m4(mat4_t dest, mat3_t src) 1.10 +{ 1.11 + int i, j; 1.12 + 1.13 + memset(dest, 0, sizeof(mat4_t)); 1.14 + for(i=0; i<3; i++) { 1.15 + for(j=0; j<3; j++) { 1.16 + dest[i][j] = src[i][j]; 1.17 + } 1.18 + } 1.19 + dest[3][3] = 1.0; 1.20 +} 1.21 + 1.22 +void m3_print(FILE *fp, mat3_t m) 1.23 +{ 1.24 + int i; 1.25 + for(i=0; i<3; i++) { 1.26 + fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]); 1.27 + } 1.28 +} 1.29 + 1.30 +/* C matrix 4x4 functions */ 1.31 +void m4_to_m3(mat3_t dest, mat4_t src) 1.32 +{ 1.33 + int i, j; 1.34 + for(i=0; i<3; i++) { 1.35 + for(j=0; j<3; j++) { 1.36 + dest[i][j] = src[i][j]; 1.37 + } 1.38 + } 1.39 +} 1.40 + 1.41 +void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z) 1.42 +{ 1.43 + mat4_t tm; 1.44 + m4_identity(tm); 1.45 + tm[0][3] = x; 1.46 + tm[1][3] = y; 1.47 + tm[2][3] = z; 1.48 + m4_mult(m, m, tm); 1.49 +} 1.50 + 1.51 +void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z) 1.52 +{ 1.53 + m4_rotate_x(m, x); 1.54 + m4_rotate_y(m, y); 1.55 + m4_rotate_z(m, z); 1.56 +} 1.57 + 1.58 +void m4_rotate_x(mat4_t m, scalar_t angle) 1.59 +{ 1.60 + mat4_t rm; 1.61 + m4_identity(rm); 1.62 + rm[1][1] = cos(angle); rm[1][2] = -sin(angle); 1.63 + rm[2][1] = sin(angle); rm[2][2] = cos(angle); 1.64 + m4_mult(m, m, rm); 1.65 +} 1.66 + 1.67 +void m4_rotate_y(mat4_t m, scalar_t angle) 1.68 +{ 1.69 + mat4_t rm; 1.70 + m4_identity(rm); 1.71 + rm[0][0] = cos(angle); rm[0][2] = sin(angle); 1.72 + rm[2][0] = -sin(angle); rm[2][2] = cos(angle); 1.73 + m4_mult(m, m, rm); 1.74 +} 1.75 + 1.76 +void m4_rotate_z(mat4_t m, scalar_t angle) 1.77 +{ 1.78 + mat4_t rm; 1.79 + m4_identity(rm); 1.80 + rm[0][0] = cos(angle); rm[0][1] = -sin(angle); 1.81 + rm[1][0] = sin(angle); rm[1][1] = cos(angle); 1.82 + m4_mult(m, m, rm); 1.83 +} 1.84 + 1.85 +void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z) 1.86 +{ 1.87 + mat4_t xform; 1.88 + scalar_t sina = sin(angle); 1.89 + scalar_t cosa = cos(angle); 1.90 + scalar_t one_minus_cosa = 1.0 - cosa; 1.91 + scalar_t nxsq = x * x; 1.92 + scalar_t nysq = y * y; 1.93 + scalar_t nzsq = z * z; 1.94 + 1.95 + m4_identity(xform); 1.96 + xform[0][0] = nxsq + (1.0 - nxsq) * cosa; 1.97 + xform[0][1] = x * y * one_minus_cosa - z * sina; 1.98 + xform[0][2] = x * z * one_minus_cosa + y * sina; 1.99 + xform[1][0] = x * y * one_minus_cosa + z * sina; 1.100 + xform[1][1] = nysq + (1.0 - nysq) * cosa; 1.101 + xform[1][2] = y * z * one_minus_cosa - x * sina; 1.102 + xform[2][0] = x * z * one_minus_cosa - y * sina; 1.103 + xform[2][1] = y * z * one_minus_cosa + x * sina; 1.104 + xform[2][2] = nzsq + (1.0 - nzsq) * cosa; 1.105 + 1.106 + m4_mult(m, m, xform); 1.107 +} 1.108 + 1.109 +void m4_rotate_quat(mat4_t m, quat_t q) 1.110 +{ 1.111 + mat4_t rm; 1.112 + quat_to_mat4(rm, q); 1.113 + m4_mult(m, m, rm); 1.114 +} 1.115 + 1.116 +void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z) 1.117 +{ 1.118 + mat4_t sm; 1.119 + m4_identity(sm); 1.120 + sm[0][0] = x; 1.121 + sm[1][1] = y; 1.122 + sm[2][2] = z; 1.123 + m4_mult(m, m, sm); 1.124 +} 1.125 + 1.126 +void m4_transpose(mat4_t res, mat4_t m) 1.127 +{ 1.128 + int i, j; 1.129 + mat4_t tmp; 1.130 + m4_copy(tmp, m); 1.131 + 1.132 + for(i=0; i<4; i++) { 1.133 + for(j=0; j<4; j++) { 1.134 + res[i][j] = tmp[j][i]; 1.135 + } 1.136 + } 1.137 +} 1.138 + 1.139 +scalar_t m4_determinant(mat4_t m) 1.140 +{ 1.141 + scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - 1.142 + (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + 1.143 + (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); 1.144 + 1.145 + scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - 1.146 + (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + 1.147 + (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); 1.148 + 1.149 + scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - 1.150 + (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + 1.151 + (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); 1.152 + 1.153 + scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - 1.154 + (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + 1.155 + (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); 1.156 + 1.157 + return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14; 1.158 +} 1.159 + 1.160 +void m4_adjoint(mat4_t res, mat4_t m) 1.161 +{ 1.162 + int i, j; 1.163 + mat4_t coef; 1.164 + 1.165 + coef[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - 1.166 + (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + 1.167 + (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); 1.168 + coef[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - 1.169 + (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + 1.170 + (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); 1.171 + coef[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - 1.172 + (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + 1.173 + (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); 1.174 + coef[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - 1.175 + (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + 1.176 + (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); 1.177 + 1.178 + coef[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - 1.179 + (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + 1.180 + (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); 1.181 + coef[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - 1.182 + (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + 1.183 + (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); 1.184 + coef[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - 1.185 + (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + 1.186 + (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); 1.187 + coef[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - 1.188 + (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + 1.189 + (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); 1.190 + 1.191 + coef[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) - 1.192 + (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) + 1.193 + (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])); 1.194 + coef[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) - 1.195 + (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) + 1.196 + (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])); 1.197 + coef[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) - 1.198 + (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) + 1.199 + (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1])); 1.200 + coef[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) - 1.201 + (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) + 1.202 + (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1])); 1.203 + 1.204 + coef[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) - 1.205 + (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) + 1.206 + (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])); 1.207 + coef[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) - 1.208 + (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) + 1.209 + (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])); 1.210 + coef[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) - 1.211 + (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) + 1.212 + (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1])); 1.213 + coef[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) - 1.214 + (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) + 1.215 + (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1])); 1.216 + 1.217 + m4_transpose(res, coef); 1.218 + 1.219 + for(i=0; i<4; i++) { 1.220 + for(j=0; j<4; j++) { 1.221 + res[i][j] = j % 2 ? -res[i][j] : res[i][j]; 1.222 + if(i % 2) res[i][j] = -res[i][j]; 1.223 + } 1.224 + } 1.225 +} 1.226 + 1.227 +void m4_inverse(mat4_t res, mat4_t m) 1.228 +{ 1.229 + int i, j; 1.230 + mat4_t adj; 1.231 + scalar_t det; 1.232 + 1.233 + m4_adjoint(adj, m); 1.234 + det = m4_determinant(m); 1.235 + 1.236 + for(i=0; i<4; i++) { 1.237 + for(j=0; j<4; j++) { 1.238 + res[i][j] = adj[i][j] / det; 1.239 + } 1.240 + } 1.241 +} 1.242 + 1.243 +void m4_print(FILE *fp, mat4_t m) 1.244 +{ 1.245 + int i; 1.246 + for(i=0; i<4; i++) { 1.247 + fprintf(fp, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2], (float)m[i][3]); 1.248 + } 1.249 +}