nuclear@0: #include <stdio.h>
nuclear@0: #include <string.h>
nuclear@0: #include "mathutil.h"
nuclear@0: 
nuclear@0: #define M(i, j)	((i) * 4 + (j))
nuclear@0: 
nuclear@0: static const float idmat[] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
nuclear@0: 
nuclear@8: void vrimp_rotation_matrix(const float *quat, float *matrix)
nuclear@0: {
nuclear@0: 	matrix[0] = 1.0 - 2.0 * quat[1] * quat[1] - 2.0 * quat[2] * quat[2];
nuclear@0: 	matrix[1] = 2.0 * quat[0] * quat[1] + 2.0 * quat[3] * quat[2];
nuclear@0: 	matrix[2] = 2.0 * quat[2] * quat[0] - 2.0 * quat[3] * quat[1];
nuclear@0: 	matrix[3] = 0.0f;
nuclear@0: 
nuclear@0: 	matrix[4] = 2.0 * quat[0] * quat[1] - 2.0 * quat[3] * quat[2];
nuclear@0: 	matrix[5] = 1.0 - 2.0 * quat[0]*quat[0] - 2.0 * quat[2]*quat[2];
nuclear@0: 	matrix[6] = 2.0 * quat[1] * quat[2] + 2.0 * quat[3] * quat[0];
nuclear@0: 	matrix[7] = 0.0f;
nuclear@0: 
nuclear@0: 	matrix[8] = 2.0 * quat[2] * quat[0] + 2.0 * quat[3] * quat[1];
nuclear@0: 	matrix[9] = 2.0 * quat[1] * quat[2] - 2.0 * quat[3] * quat[0];
nuclear@0: 	matrix[10] = 1.0 - 2.0 * quat[0]*quat[0] - 2.0 * quat[1]*quat[1];
nuclear@0: 	matrix[11] = 0.0f;
nuclear@0: 
nuclear@0: 	matrix[12] = matrix[13] = matrix[14] = 0.0f;
nuclear@0: 	matrix[15] = 1.0f;
nuclear@0: 
nuclear@8: 	vrimp_transpose_matrix(matrix);
nuclear@0: }
nuclear@0: 
nuclear@8: void vrimp_translation_matrix(const float *vec, float *matrix)
nuclear@0: {
nuclear@0: 	memcpy(matrix, idmat, sizeof idmat);
nuclear@0: 	matrix[12] = vec[0];
nuclear@0: 	matrix[13] = vec[1];
nuclear@0: 	matrix[14] = vec[2];
nuclear@0: }
nuclear@0: 
nuclear@8: void vrimp_mult_matrix(float *dest, const float *m1, const float *m2)
nuclear@0: {
nuclear@0: 	int i, j;
nuclear@0: 	float tmp[16];
nuclear@0: 
nuclear@0: 	for(i=0; i<4; i++) {
nuclear@0: 		for(j=0; j<4; j++) {
nuclear@0: 			tmp[M(i, j)] = m1[M(i, 0)] * m2[M(0, j)] + m1[M(i, 1)] * m2[M(1, j)] +
nuclear@0: 				m1[M(i, 2)] * m2[M(2, j)] + m1[M(i, 3)] * m2[M(3, j)];
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: 	memcpy(dest, tmp, sizeof tmp);
nuclear@0: }
nuclear@0: 
nuclear@8: void vrimp_transpose_matrix(float *matrix)
nuclear@0: {
nuclear@0: 	int i, j;
nuclear@0: 	float tmp[16];
nuclear@0: 
nuclear@0: 	for(i=0; i<4; i++) {
nuclear@0: 		for(j=0; j<4; j++) {
nuclear@0: 			tmp[M(i, j)] = matrix[M(j, i)];
nuclear@0: 		}
nuclear@0: 	}
nuclear@0: 	memcpy(matrix, tmp, sizeof tmp);
nuclear@0: }
nuclear@0: 
nuclear@8: void vrimp_print_matrix(const float *matrix)
nuclear@0: {
nuclear@0: 	int i, j;
nuclear@0: 
nuclear@0: 	for(i=0; i<4; i++) {
nuclear@0: 		putchar('[');
nuclear@0: 		for(j=0; j<4; j++) {
nuclear@0: 			printf("%6.3f ", matrix[M(i, j)]);
nuclear@0: 		}
nuclear@0: 		printf("]\n");
nuclear@0: 	}
nuclear@0: }