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 +}