ref: 0590ba5de9b6b9c07a6de2f2b1f7246e6cefa22c
dir: /src/zmath.c/
/* Some simple mathematical functions. Don't look for some logic in
the function names :-) */
#include "zmath.h"
#include <stdlib.h>
#include <string.h>
/* ******* Gestion des matrices 4x4 ****** */
void gl_M4_Id(M4* a) {
/*
GLint i, j;
#ifdef _OPENMP
#pragma omp simd collapse(2)
#endif
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
if (i == j)
a->m[i][j] = 1.0;
else
a->m[i][j] = 0.0;
*/
const GLfloat m[4][4] = {
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1,
};
memmove(a->m, m, sizeof m);
}
GLint gl_M4_IsId(M4* a) {
const GLfloat m[4][4] = {
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1,
};
M4 c;
memmove(c.m, m, sizeof m);
return (memcmp(a->m, c.m, 16 * sizeof(GLfloat)) == 0);
/*
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++) {
if (i == j) {
if (a->m[i][j] != 1.0)
return 0;
} else if (a->m[i][j] != 0.0)
return 0;
}
return 1;
*/
}
void gl_M4_Mul(M4* c, M4* a, M4* b) {
GLint i, j, k;
GLfloat s;
#ifdef _OPENMP
#pragma omp simd
#endif
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++) {
s = 0.0;
for (k = 0; k < 4; k++)
s += a->m[i][k] * b->m[k][j];
c->m[i][j] = s;
}
}
/* c=c*a */
void gl_M4_MulLeft(M4* c, M4* b) {
GLint i, j, k;
GLfloat s;
M4 a;
/*memcpy(&a, c, 16*sizeof(GLfloat));
*/
a = *c;
#ifdef _OPENMP
#pragma omp simd
#endif
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++) {
s = 0.0;
for (k = 0; k < 4; k++)
s += a.m[i][k] * b->m[k][j];
c->m[i][j] = s;
}
}
void gl_M4_Move(M4* a, M4* b) { memcpy(a, b, sizeof(M4)); }
void gl_MoveV3(V3* a, V3* b) { memcpy(a, b, sizeof(V3)); }
void gl_MulM4V3(V3* a, M4* b, V3* c) {
a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3];
a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3];
a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3];
}
void gl_MulM3V3(V3* a, M4* b, V3* c) {
a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z;
a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z;
a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z;
}
void gl_M4_MulV4(V4* a, M4* b, V4* c) {
{
a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3] * c->W;
a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3] * c->W;
a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3] * c->W;
a->W = b->m[3][0] * c->X + b->m[3][1] * c->Y + b->m[3][2] * c->Z + b->m[3][3] * c->W;
}
}
/* transposition of a 4x4 matrix */
void gl_M4_Transpose(M4* a, M4* b) {
{
a->m[0][0] = b->m[0][0];
a->m[0][1] = b->m[1][0];
a->m[0][2] = b->m[2][0];
a->m[0][3] = b->m[3][0];
a->m[1][0] = b->m[0][1];
a->m[1][1] = b->m[1][1];
a->m[1][2] = b->m[2][1];
a->m[1][3] = b->m[3][1];
a->m[2][0] = b->m[0][2];
a->m[2][1] = b->m[1][2];
a->m[2][2] = b->m[2][2];
a->m[2][3] = b->m[3][2];
a->m[3][0] = b->m[0][3];
a->m[3][1] = b->m[1][3];
a->m[3][2] = b->m[2][3];
a->m[3][3] = b->m[3][3];
}
}
/* inversion of an orthogonal matrix of type Y=M.X+P */
void gl_M4_InvOrtho(M4* a, M4 b) {
GLint i, j;
GLfloat s;
#ifdef _OPENMP
#pragma omp simd
#endif
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
a->m[i][j] = b.m[j][i];
a->m[3][0] = 0.0;
a->m[3][1] = 0.0;
a->m[3][2] = 0.0;
a->m[3][3] = 1.0;
for (i = 0; i < 3; i++) {
s = 0;
#ifdef _OPENMP
#pragma omp simd
#endif
for (j = 0; j < 3; j++)
s -= b.m[j][i] * b.m[j][3];
a->m[i][3] = s;
}
}
/* Inversion of a general nxn matrix.
Note : m is destroyed */
GLint Matrix_Inv(GLfloat* r, GLfloat* m, GLint n) {
GLint i, j, k, l;
GLfloat max, tmp, t;
/* */
#ifdef _OPENMP
#pragma omp simd
#endif
for (i = 0; i < n * n; i++)
r[i] = 0;
for (i = 0; i < n; i++)
r[i * n + i] = 1;
for (j = 0; j < n; j++) {
/* recherche du nombre de plus grand module sur la colonne j */
max = m[j * n + j];
k = j;
for (i = j + 1; i < n; i++)
if (fabs(m[i * n + j]) > fabs(max)) {
k = i;
max = m[i * n + j];
}
/* non GLintersible matrix */
if (max == 0)
return 1;
/* permutation des lignes j et k */
if (k != j) {
#ifdef _OPENMP
#pragma omp simd
#endif
for (i = 0; i < n; i++) {
tmp = m[j * n + i];
m[j * n + i] = m[k * n + i];
m[k * n + i] = tmp;
tmp = r[j * n + i];
r[j * n + i] = r[k * n + i];
r[k * n + i] = tmp;
}
}
/* multiplication de la ligne j par 1/max */
max = 1 / max;
#ifdef _OPENMP
#pragma omp simd
#endif
for (i = 0; i < n; i++) {
m[j * n + i] *= max;
r[j * n + i] *= max;
}
for (l = 0; l < n; l++)
if (l != j) {
t = m[l * n + j];
for (i = 0; i < n; i++) {
m[l * n + i] -= m[j * n + i] * t;
r[l * n + i] -= r[j * n + i] * t;
}
}
}
return 0;
}
/* inversion of a 4x4 matrix */
void gl_M4_Inv(M4* a, M4* b) {
M4 tmp;
memcpy(&tmp, b, sizeof(M4));
/*tmp=*b;*/
Matrix_Inv(&a->m[0][0], &tmp.m[0][0], 4);
}
void gl_M4_Rotate(M4* a, GLfloat t, GLint u) {
GLfloat s, c;
GLint v, w;
if ((v = u + 1) > 2)
v = 0;
if ((w = v + 1) > 2)
w = 0;
s = sin(t);
c = cos(t);
gl_M4_Id(a);
a->m[v][v] = c;
a->m[v][w] = -s;
a->m[w][v] = s;
a->m[w][w] = c;
}
/* inverse of a 3x3 matrix */
void gl_M3_Inv(M3* a, M3* m) {
GLfloat det;
det = m->m[0][0] * m->m[1][1] * m->m[2][2] - m->m[0][0] * m->m[1][2] * m->m[2][1] - m->m[1][0] * m->m[0][1] * m->m[2][2] +
m->m[1][0] * m->m[0][2] * m->m[2][1] + m->m[2][0] * m->m[0][1] * m->m[1][2] - m->m[2][0] * m->m[0][2] * m->m[1][1];
a->m[0][0] = (m->m[1][1] * m->m[2][2] - m->m[1][2] * m->m[2][1]) / det;
a->m[0][1] = -(m->m[0][1] * m->m[2][2] - m->m[0][2] * m->m[2][1]) / det;
a->m[0][2] = -(-m->m[0][1] * m->m[1][2] + m->m[0][2] * m->m[1][1]) / det;
a->m[1][0] = -(m->m[1][0] * m->m[2][2] - m->m[1][2] * m->m[2][0]) / det;
a->m[1][1] = (m->m[0][0] * m->m[2][2] - m->m[0][2] * m->m[2][0]) / det;
a->m[1][2] = -(m->m[0][0] * m->m[1][2] - m->m[0][2] * m->m[1][0]) / det;
a->m[2][0] = (m->m[1][0] * m->m[2][1] - m->m[1][1] * m->m[2][0]) / det;
a->m[2][1] = -(m->m[0][0] * m->m[2][1] - m->m[0][1] * m->m[2][0]) / det;
a->m[2][2] = (m->m[0][0] * m->m[1][1] - m->m[0][1] * m->m[1][0]) / det;
}
/* vector arithmetic */
/*
int gl_V3_Norm(V3* a) {
GLfloat n;
n = sqrt(a->X * a->X + a->Y * a->Y + a->Z * a->Z);
if (n == 0)
return 1;
a->X /= n;
a->Y /= n;
a->Z /= n;
return 0;
}
*/
V3 gl_V3_New(GLfloat x, GLfloat y, GLfloat z) {
V3 a;
a.X = x;
a.Y = y;
a.Z = z;
return a;
}
V4 gl_V4_New(GLfloat x, GLfloat y, GLfloat z, GLfloat w) {
V4 a;
a.X = x;
a.Y = y;
a.Z = z;
a.W = w;
return a;
}