1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/mathutil.c	Fri Aug 29 03:45:25 2014 +0300
     1.3 @@ -0,0 +1,78 @@
     1.4 +#include <stdio.h>
     1.5 +#include <string.h>
     1.6 +#include "mathutil.h"
     1.7 +
     1.8 +#define M(i, j)	((i) * 4 + (j))
     1.9 +
    1.10 +static const float idmat[] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
    1.11 +
    1.12 +void rotation_matrix(const float *quat, float *matrix)
    1.13 +{
    1.14 +	matrix[0] = 1.0 - 2.0 * quat[1] * quat[1] - 2.0 * quat[2] * quat[2];
    1.15 +	matrix[1] = 2.0 * quat[0] * quat[1] + 2.0 * quat[3] * quat[2];
    1.16 +	matrix[2] = 2.0 * quat[2] * quat[0] - 2.0 * quat[3] * quat[1];
    1.17 +	matrix[3] = 0.0f;
    1.18 +
    1.19 +	matrix[4] = 2.0 * quat[0] * quat[1] - 2.0 * quat[3] * quat[2];
    1.20 +	matrix[5] = 1.0 - 2.0 * quat[0]*quat[0] - 2.0 * quat[2]*quat[2];
    1.21 +	matrix[6] = 2.0 * quat[1] * quat[2] + 2.0 * quat[3] * quat[0];
    1.22 +	matrix[7] = 0.0f;
    1.23 +
    1.24 +	matrix[8] = 2.0 * quat[2] * quat[0] + 2.0 * quat[3] * quat[1];
    1.25 +	matrix[9] = 2.0 * quat[1] * quat[2] - 2.0 * quat[3] * quat[0];
    1.26 +	matrix[10] = 1.0 - 2.0 * quat[0]*quat[0] - 2.0 * quat[1]*quat[1];
    1.27 +	matrix[11] = 0.0f;
    1.28 +
    1.29 +	matrix[12] = matrix[13] = matrix[14] = 0.0f;
    1.30 +	matrix[15] = 1.0f;
    1.31 +
    1.32 +	transpose_matrix(matrix);
    1.33 +}
    1.34 +
    1.35 +void translation_matrix(const float *vec, float *matrix)
    1.36 +{
    1.37 +	memcpy(matrix, idmat, sizeof idmat);
    1.38 +	matrix[12] = vec[0];
    1.39 +	matrix[13] = vec[1];
    1.40 +	matrix[14] = vec[2];
    1.41 +}
    1.42 +
    1.43 +void mult_matrix(float *dest, const float *m1, const float *m2)
    1.44 +{
    1.45 +	int i, j;
    1.46 +	float tmp[16];
    1.47 +
    1.48 +	for(i=0; i<4; i++) {
    1.49 +		for(j=0; j<4; j++) {
    1.50 +			tmp[M(i, j)] = m1[M(i, 0)] * m2[M(0, j)] + m1[M(i, 1)] * m2[M(1, j)] +
    1.51 +				m1[M(i, 2)] * m2[M(2, j)] + m1[M(i, 3)] * m2[M(3, j)];
    1.52 +		}
    1.53 +	}
    1.54 +	memcpy(dest, tmp, sizeof tmp);
    1.55 +}
    1.56 +
    1.57 +void transpose_matrix(float *matrix)
    1.58 +{
    1.59 +	int i, j;
    1.60 +	float tmp[16];
    1.61 +
    1.62 +	for(i=0; i<4; i++) {
    1.63 +		for(j=0; j<4; j++) {
    1.64 +			tmp[M(i, j)] = matrix[M(j, i)];
    1.65 +		}
    1.66 +	}
    1.67 +	memcpy(matrix, tmp, sizeof tmp);
    1.68 +}
    1.69 +
    1.70 +void print_matrix(const float *matrix)
    1.71 +{
    1.72 +	int i, j;
    1.73 +
    1.74 +	for(i=0; i<4; i++) {
    1.75 +		putchar('[');
    1.76 +		for(j=0; j<4; j++) {
    1.77 +			printf("%6.3f ", matrix[M(i, j)]);
    1.78 +		}
    1.79 +		printf("]\n");
    1.80 +	}
    1.81 +}