1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vmath/matrix_c.c	Sat Sep 19 05:51:51 2015 +0300
     1.3 @@ -0,0 +1,265 @@
     1.4 +/*
     1.5 +libvmath - a vector math library
     1.6 +Copyright (C) 2004-2011 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_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
    1.61 +{
    1.62 +	mat4_t tm;
    1.63 +	m4_identity(tm);
    1.64 +	tm[0][3] = x;
    1.65 +	tm[1][3] = y;
    1.66 +	tm[2][3] = z;
    1.67 +	m4_mult(m, m, tm);
    1.68 +}
    1.69 +
    1.70 +void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
    1.71 +{
    1.72 +	m4_rotate_x(m, x);
    1.73 +	m4_rotate_y(m, y);
    1.74 +	m4_rotate_z(m, z);
    1.75 +}
    1.76 +
    1.77 +void m4_rotate_x(mat4_t m, scalar_t angle)
    1.78 +{
    1.79 +	mat4_t rm;
    1.80 +	m4_identity(rm);
    1.81 +	rm[1][1] = cos(angle); rm[1][2] = -sin(angle);
    1.82 +	rm[2][1] = sin(angle); rm[2][2] = cos(angle);
    1.83 +	m4_mult(m, m, rm);
    1.84 +}
    1.85 +
    1.86 +void m4_rotate_y(mat4_t m, scalar_t angle)
    1.87 +{
    1.88 +	mat4_t rm;
    1.89 +	m4_identity(rm);
    1.90 +	rm[0][0] = cos(angle); rm[0][2] = sin(angle);
    1.91 +	rm[2][0] = -sin(angle); rm[2][2] = cos(angle);
    1.92 +	m4_mult(m, m, rm);
    1.93 +}
    1.94 +
    1.95 +void m4_rotate_z(mat4_t m, scalar_t angle)
    1.96 +{
    1.97 +	mat4_t rm;
    1.98 +	m4_identity(rm);
    1.99 +	rm[0][0] = cos(angle); rm[0][1] = -sin(angle);
   1.100 +	rm[1][0] = sin(angle); rm[1][1] = cos(angle);
   1.101 +	m4_mult(m, m, rm);
   1.102 +}
   1.103 +
   1.104 +void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
   1.105 +{
   1.106 +	mat4_t xform;
   1.107 +	scalar_t sina = sin(angle);
   1.108 +	scalar_t cosa = cos(angle);
   1.109 +	scalar_t one_minus_cosa = 1.0 - cosa;
   1.110 +	scalar_t nxsq = x * x;
   1.111 +	scalar_t nysq = y * y;
   1.112 +	scalar_t nzsq = z * z;
   1.113 +
   1.114 +	m4_identity(xform);
   1.115 +	xform[0][0] = nxsq + (1.0 - nxsq) * cosa;
   1.116 +	xform[0][1] = x * y * one_minus_cosa - z * sina;
   1.117 +	xform[0][2] = x * z * one_minus_cosa + y * sina;
   1.118 +	xform[1][0] = x * y * one_minus_cosa + z * sina;
   1.119 +	xform[1][1] = nysq + (1.0 - nysq) * cosa;
   1.120 +	xform[1][2] = y * z * one_minus_cosa - x * sina;
   1.121 +	xform[2][0] = x * z * one_minus_cosa - y * sina;
   1.122 +	xform[2][1] = y * z * one_minus_cosa + x * sina;
   1.123 +	xform[2][2] = nzsq + (1.0 - nzsq) * cosa;
   1.124 +
   1.125 +	m4_mult(m, m, xform);
   1.126 +}
   1.127 +
   1.128 +void m4_rotate_quat(mat4_t m, quat_t q)
   1.129 +{
   1.130 +	mat4_t rm;
   1.131 +	quat_to_mat4(rm, q);
   1.132 +	m4_mult(m, m, rm);
   1.133 +}
   1.134 +
   1.135 +void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
   1.136 +{
   1.137 +	mat4_t sm;
   1.138 +	m4_identity(sm);
   1.139 +	sm[0][0] = x;
   1.140 +	sm[1][1] = y;
   1.141 +	sm[2][2] = z;
   1.142 +	m4_mult(m, m, sm);
   1.143 +}
   1.144 +
   1.145 +void m4_transpose(mat4_t res, mat4_t m)
   1.146 +{
   1.147 +	int i, j;
   1.148 +	mat4_t tmp;
   1.149 +	m4_copy(tmp, m);
   1.150 +
   1.151 +	for(i=0; i<4; i++) {
   1.152 +		for(j=0; j<4; j++) {
   1.153 +			res[i][j] = tmp[j][i];
   1.154 +		}
   1.155 +	}
   1.156 +}
   1.157 +
   1.158 +scalar_t m4_determinant(mat4_t m)
   1.159 +{
   1.160 +	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.161 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.162 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.163 +
   1.164 +	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.165 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.166 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.167 +
   1.168 +	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.169 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.170 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.171 +
   1.172 +	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.173 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.174 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.175 +
   1.176 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
   1.177 +}
   1.178 +
   1.179 +void m4_adjoint(mat4_t res, mat4_t m)
   1.180 +{
   1.181 +	int i, j;
   1.182 +	mat4_t coef;
   1.183 +
   1.184 +	coef[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.185 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.186 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.187 +	coef[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.188 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.189 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.190 +	coef[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.191 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.192 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.193 +	coef[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.194 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.195 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.196 +
   1.197 +	coef[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.198 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.199 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.200 +	coef[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.201 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.202 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.203 +	coef[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.204 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.205 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.206 +	coef[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.207 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.208 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.209 +
   1.210 +	coef[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.211 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
   1.212 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
   1.213 +	coef[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.214 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.215 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
   1.216 +	coef[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
   1.217 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.218 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.219 +	coef[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
   1.220 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
   1.221 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.222 +
   1.223 +	coef[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.224 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
   1.225 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
   1.226 +	coef[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.227 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.228 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
   1.229 +	coef[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
   1.230 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.231 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.232 +	coef[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
   1.233 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
   1.234 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.235 +
   1.236 +	m4_transpose(res, coef);
   1.237 +
   1.238 +	for(i=0; i<4; i++) {
   1.239 +		for(j=0; j<4; j++) {
   1.240 +			res[i][j] = j % 2 ? -res[i][j] : res[i][j];
   1.241 +			if(i % 2) res[i][j] = -res[i][j];
   1.242 +		}
   1.243 +	}
   1.244 +}
   1.245 +
   1.246 +void m4_inverse(mat4_t res, mat4_t m)
   1.247 +{
   1.248 +	int i, j;
   1.249 +	mat4_t adj;
   1.250 +	scalar_t det;
   1.251 +
   1.252 +	m4_adjoint(adj, m);
   1.253 +	det = m4_determinant(m);
   1.254 +	
   1.255 +	for(i=0; i<4; i++) {
   1.256 +		for(j=0; j<4; j++) {
   1.257 +			res[i][j] = adj[i][j] / det;
   1.258 +		}
   1.259 +	}
   1.260 +}
   1.261 +
   1.262 +void m4_print(FILE *fp, mat4_t m)
   1.263 +{
   1.264 +	int i;
   1.265 +	for(i=0; i<4; i++) {
   1.266 +		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.267 +	}
   1.268 +}