1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vmath/matrix_c.c	Sun May 31 00:40:26 2015 +0300
     1.3 @@ -0,0 +1,292 @@
     1.4 +/*
     1.5 +libvmath - a vector math library
     1.6 +Copyright (C) 2004-2015 John Tsiombikas <nuclear@member.fsf.org>
     1.7 +
     1.8 +This program is free software: you can redistribute it and/or modify
     1.9 +it under the terms of the GNU Lesser General Public License as published
    1.10 +by the Free Software Foundation, either version 3 of the License, or
    1.11 +(at your option) any later version.
    1.12 +
    1.13 +This program is distributed in the hope that it will be useful,
    1.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
    1.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    1.16 +GNU Lesser General Public License for more details.
    1.17 +
    1.18 +You should have received a copy of the GNU Lesser General Public License
    1.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
    1.20 +*/
    1.21 +
    1.22 +
    1.23 +#include <stdio.h>
    1.24 +#include "matrix.h"
    1.25 +#include "vector.h"
    1.26 +#include "quat.h"
    1.27 +
    1.28 +void m3_to_m4(mat4_t dest, mat3_t src)
    1.29 +{
    1.30 +	int i, j;
    1.31 +
    1.32 +	memset(dest, 0, sizeof(mat4_t));
    1.33 +	for(i=0; i<3; i++) {
    1.34 +		for(j=0; j<3; j++) {
    1.35 +			dest[i][j] = src[i][j];
    1.36 +		}
    1.37 +	}
    1.38 +	dest[3][3] = 1.0;
    1.39 +}
    1.40 +
    1.41 +void m3_print(FILE *fp, mat3_t m)
    1.42 +{
    1.43 +	int i;
    1.44 +	for(i=0; i<3; i++) {
    1.45 +		fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]);
    1.46 +	}
    1.47 +}
    1.48 +
    1.49 +/* C matrix 4x4 functions */
    1.50 +void m4_to_m3(mat3_t dest, mat4_t src)
    1.51 +{
    1.52 +	int i, j;
    1.53 +	for(i=0; i<3; i++) {
    1.54 +		for(j=0; j<3; j++) {
    1.55 +			dest[i][j] = src[i][j];
    1.56 +		}
    1.57 +	}
    1.58 +}
    1.59 +
    1.60 +void m4_set_translation(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
    1.61 +{
    1.62 +	m4_identity(m);
    1.63 +	m[0][3] = x;
    1.64 +	m[1][3] = y;
    1.65 +	m[2][3] = z;
    1.66 +}
    1.67 +
    1.68 +void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
    1.69 +{
    1.70 +	mat4_t tm;
    1.71 +	m4_set_translation(tm, x, y, z);
    1.72 +	m4_mult(m, m, tm);
    1.73 +}
    1.74 +
    1.75 +void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
    1.76 +{
    1.77 +	m4_rotate_x(m, x);
    1.78 +	m4_rotate_y(m, y);
    1.79 +	m4_rotate_z(m, z);
    1.80 +}
    1.81 +
    1.82 +void m4_set_rotation_x(mat4_t m, scalar_t angle)
    1.83 +{
    1.84 +	m4_identity(m);
    1.85 +	m[1][1] = cos(angle); m[1][2] = -sin(angle);
    1.86 +	m[2][1] = sin(angle); m[2][2] = cos(angle);
    1.87 +}
    1.88 +
    1.89 +void m4_rotate_x(mat4_t m, scalar_t angle)
    1.90 +{
    1.91 +	mat4_t rm;
    1.92 +	m4_set_rotation_x(rm, angle);
    1.93 +	m4_mult(m, m, rm);
    1.94 +}
    1.95 +
    1.96 +void m4_set_rotation_y(mat4_t m, scalar_t angle)
    1.97 +{
    1.98 +	m4_identity(m);
    1.99 +	m[0][0] = cos(angle); m[0][2] = sin(angle);
   1.100 +	m[2][0] = -sin(angle); m[2][2] = cos(angle);
   1.101 +}
   1.102 +
   1.103 +void m4_rotate_y(mat4_t m, scalar_t angle)
   1.104 +{
   1.105 +	mat4_t rm;
   1.106 +	m4_set_rotation_y(rm, angle);
   1.107 +	m4_mult(m, m, rm);
   1.108 +}
   1.109 +
   1.110 +void m4_set_rotation_z(mat4_t m, scalar_t angle)
   1.111 +{
   1.112 +	m4_identity(m);
   1.113 +	m[0][0] = cos(angle); m[0][1] = -sin(angle);
   1.114 +	m[1][0] = sin(angle); m[1][1] = cos(angle);
   1.115 +}
   1.116 +
   1.117 +void m4_rotate_z(mat4_t m, scalar_t angle)
   1.118 +{
   1.119 +	mat4_t rm;
   1.120 +	m4_set_rotation_z(rm, angle);
   1.121 +	m4_mult(m, m, rm);
   1.122 +}
   1.123 +
   1.124 +void m4_set_rotation_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
   1.125 +{
   1.126 +	scalar_t sina = sin(angle);
   1.127 +	scalar_t cosa = cos(angle);
   1.128 +	scalar_t one_minus_cosa = 1.0 - cosa;
   1.129 +	scalar_t nxsq = x * x;
   1.130 +	scalar_t nysq = y * y;
   1.131 +	scalar_t nzsq = z * z;
   1.132 +
   1.133 +	m[0][0] = nxsq + (1.0 - nxsq) * cosa;
   1.134 +	m[0][1] = x * y * one_minus_cosa - z * sina;
   1.135 +	m[0][2] = x * z * one_minus_cosa + y * sina;
   1.136 +	m[1][0] = x * y * one_minus_cosa + z * sina;
   1.137 +	m[1][1] = nysq + (1.0 - nysq) * cosa;
   1.138 +	m[1][2] = y * z * one_minus_cosa - x * sina;
   1.139 +	m[2][0] = x * z * one_minus_cosa - y * sina;
   1.140 +	m[2][1] = y * z * one_minus_cosa + x * sina;
   1.141 +	m[2][2] = nzsq + (1.0 - nzsq) * cosa;
   1.142 +
   1.143 +	/* the rest are identity */
   1.144 +	m[3][0] = m[3][1] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0;
   1.145 +	m[3][3] = 1.0;
   1.146 +}
   1.147 +
   1.148 +void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
   1.149 +{
   1.150 +	mat4_t xform;
   1.151 +	m4_set_rotation_axis(xform, angle, x, y, z);
   1.152 +	m4_mult(m, m, xform);
   1.153 +}
   1.154 +
   1.155 +void m4_rotate_quat(mat4_t m, quat_t q)
   1.156 +{
   1.157 +	mat4_t rm;
   1.158 +	quat_to_mat4(rm, q);
   1.159 +	m4_mult(m, m, rm);
   1.160 +}
   1.161 +
   1.162 +void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
   1.163 +{
   1.164 +	mat4_t sm;
   1.165 +	m4_identity(sm);
   1.166 +	sm[0][0] = x;
   1.167 +	sm[1][1] = y;
   1.168 +	sm[2][2] = z;
   1.169 +	m4_mult(m, m, sm);
   1.170 +}
   1.171 +
   1.172 +void m4_transpose(mat4_t res, mat4_t m)
   1.173 +{
   1.174 +	int i, j;
   1.175 +	mat4_t tmp;
   1.176 +	m4_copy(tmp, m);
   1.177 +
   1.178 +	for(i=0; i<4; i++) {
   1.179 +		for(j=0; j<4; j++) {
   1.180 +			res[i][j] = tmp[j][i];
   1.181 +		}
   1.182 +	}
   1.183 +}
   1.184 +
   1.185 +scalar_t m4_determinant(mat4_t m)
   1.186 +{
   1.187 +	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.188 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.189 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.190 +
   1.191 +	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.192 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.193 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.194 +
   1.195 +	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.196 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.197 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.198 +
   1.199 +	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.200 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.201 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.202 +
   1.203 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
   1.204 +}
   1.205 +
   1.206 +void m4_adjoint(mat4_t res, mat4_t m)
   1.207 +{
   1.208 +	int i, j;
   1.209 +	mat4_t coef;
   1.210 +
   1.211 +	coef[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.212 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.213 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.214 +	coef[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.215 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.216 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.217 +	coef[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.218 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.219 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.220 +	coef[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.221 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.222 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.223 +
   1.224 +	coef[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.225 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.226 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.227 +	coef[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.228 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.229 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.230 +	coef[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.231 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.232 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.233 +	coef[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.234 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.235 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.236 +
   1.237 +	coef[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.238 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
   1.239 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
   1.240 +	coef[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.241 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.242 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
   1.243 +	coef[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
   1.244 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.245 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.246 +	coef[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
   1.247 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
   1.248 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.249 +
   1.250 +	coef[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.251 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
   1.252 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
   1.253 +	coef[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.254 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.255 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
   1.256 +	coef[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
   1.257 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.258 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.259 +	coef[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
   1.260 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
   1.261 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.262 +
   1.263 +	m4_transpose(res, coef);
   1.264 +
   1.265 +	for(i=0; i<4; i++) {
   1.266 +		for(j=0; j<4; j++) {
   1.267 +			res[i][j] = j % 2 ? -res[i][j] : res[i][j];
   1.268 +			if(i % 2) res[i][j] = -res[i][j];
   1.269 +		}
   1.270 +	}
   1.271 +}
   1.272 +
   1.273 +void m4_inverse(mat4_t res, mat4_t m)
   1.274 +{
   1.275 +	int i, j;
   1.276 +	mat4_t adj;
   1.277 +	scalar_t det;
   1.278 +
   1.279 +	m4_adjoint(adj, m);
   1.280 +	det = m4_determinant(m);
   1.281 +	
   1.282 +	for(i=0; i<4; i++) {
   1.283 +		for(j=0; j<4; j++) {
   1.284 +			res[i][j] = adj[i][j] / det;
   1.285 +		}
   1.286 +	}
   1.287 +}
   1.288 +
   1.289 +void m4_print(FILE *fp, mat4_t m)
   1.290 +{
   1.291 +	int i;
   1.292 +	for(i=0; i<4; i++) {
   1.293 +		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.294 +	}
   1.295 +}