luc/sweet
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
sweet/sweet_matrix.c
luc 84621fa281 refactor
2019-03-28 01:26:06 +01:00

1680 lines
36 KiB
C
Raw Blame History

/*
* Sweet is a small library for basic math and small matrix operations.
* Copyright 2014 Luc Girod.
*
* This library is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include <math.h>
#include "sweet_macro.h"
#include "sweet_matrix.h"
#include "sweet_math.h"
#include "sweet_types.h"
mat2 mat2_new_m3 (mat3 m)
{
mat2 n;
n.v[0] = m.v[0];
n.v[1] = m.v[1];
n.v[2] = m.v[3];
n.v[3] = m.v[4];
return n;
}
mat2 mat2_new_2v (vec2 e1, vec2 e2)
{
mat2 m;
m.v[0] = e1.x;
m.v[1] = e1.y;
m.v[2] = e2.x;
m.v[3] = e2.y;
return m;
}
mat2 mat2_new_3f (float a, float b, float c)
{
return mat2_new_4f (a, b, c, 0);
}
mat2 mat2_new_4f (float f0, float f1, float f2, float f3)
{
mat2 n;
n.v[0] = f0;
n.v[1] = f1;
n.v[2] = f2;
n.v[3] = f3;
return n;
}
mat3 mat3_new_m4 (mat4 m)
{
mat3 n;
n.v[0] = m.v[0];
n.v[1] = m.v[1];
n.v[2] = m.v[2];
n.v[3] = m.v[4];
n.v[4] = m.v[5];
n.v[5] = m.v[6];
n.v[6] = m.v[8];
n.v[7] = m.v[9];
n.v[8] = m.v[10];
return n;
}
mat3 mat3_new_2v (vec2 e1, vec2 e2)
{
mat3 m;
m.v[0] = e1.x;
m.v[1] = e1.y;
m.v[2] = 0;
m.v[3] = e2.x;
m.v[4] = e2.y;
m.v[5] = 0;
m.v[6] = 0;
m.v[7] = 0;
m.v[8] = 1;
return m;
}
mat3 mat3_new_3v (vec3 e1, vec3 e2, vec3 e3)
{
mat3 m;
m.v[0] = e1.x;
m.v[1] = e1.y;
m.v[2] = e1.z;
m.v[3] = e2.x;
m.v[4] = e2.y;
m.v[5] = e2.z;
m.v[6] = e3.x;
m.v[7] = e3.y;
m.v[8] = e3.z;
return m;
}
mat4 mat4_new_m4 (mat4 m)
{
return m;
}
mat4 mat4_new_2v (vec2 e1, vec2 e2)
{
mat4 m;
m.v[0] = e1.x;
m.v[1] = e1.y;
m.v[2] = 0;
m.v[3] = 0;
m.v[4] = e2.x;
m.v[5] = e2.y;
m.v[6] = 0;
m.v[7] = 0;
m.v[8] = 0;
m.v[9] = 0;
m.v[10] = 1;
m.v[11] = 0;
m.v[12] = 0;
m.v[13] = 0;
m.v[14] = 0;
m.v[15] = 1;
return m;
}
mat4 mat4_new_3v (vec3 e1, vec3 e2, vec3 e3)
{
mat4 m;
m.v[0] = e1.x;
m.v[1] = e1.y;
m.v[2] = e1.z;
m.v[3] = 0;
m.v[4] = e2.x;
m.v[5] = e2.y;
m.v[6] = e2.z;
m.v[7] = 0;
m.v[8] = e3.x;
m.v[9] = e3.y;
m.v[10] = e3.z;
m.v[11] = 0;
m.v[12] = 0;
m.v[13] = 0;
m.v[14] = 0;
m.v[15] = 1;
return m;
}
mat4 mat4_new_4v (vec4 e1, vec4 e2, vec4 e3, vec4 e4)
{
mat4 m;
m.v[0] = e1.x;
m.v[1] = e1.y;
m.v[2] = e1.z;
m.v[3] = e1.w;
m.v[4] = e2.x;
m.v[5] = e2.y;
m.v[6] = e2.z;
m.v[7] = e2.w;
m.v[8] = e3.x;
m.v[9] = e3.y;
m.v[10] = e3.z;
m.v[11] = e3.w;
m.v[12] = e4.x;
m.v[13] = e4.y;
m.v[14] = e4.z;
m.v[15] = e4.w;
return m;
}
mat2
mat2_null (void)
{
mat2 m;
m.v[0] = 0.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
return m;
}
mat3
mat3_null (void)
{
mat3 m;
m.v[0] = 0.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 0.0;
m.v[5] = 0.0;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 0.0;
return m;
}
mat4
mat4_null (void)
{
mat4 m;
m.v[0] = 0.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 0.0;
m.v[5] = 0.0;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 0.0;
m.v[9] = 0.0;
m.v[10] = 0.0;
m.v[11] = 0.0;
m.v[12] = 0.0;
m.v[13] = 0.0;
m.v[14] = 0.0;
m.v[15] = 0.0;
return m;
}
mat2
mat2_identity (void)
{
mat2 m;
m.v[0] = 1.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 1.0;
return m;
}
mat3
mat3_identity (void)
{
mat3 m;
m.v[0] = 1.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 1.0;
m.v[5] = 0.0;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 1.0;
return m;
}
mat4
mat4_identity (void)
{
mat4 m;
m.v[0] = 1.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 0.0;
m.v[5] = 1.0;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 0.0;
m.v[9] = 0.0;
m.v[10] = 1.0;
m.v[11] = 0.0;
m.v[12] = 0.0;
m.v[13] = 0.0;
m.v[14] = 0.0;
m.v[15] = 1.0;
return m;
}
mat2
mat2_ortho_basis (vec2 u, vec2 v)
{
mat2 m;
m.v[0] = u.x;
m.v[1] = v.x;
m.v[2] = u.y;
m.v[3] = v.y;
return m;
}
mat3
mat2h_ortho_basis (vec2 p, vec2 u, vec2 v)
{
mat3 m;
m.v[0] = u.x;
m.v[1] = v.x;
m.v[2] = 0;
m.v[3] = u.y;
m.v[4] = v.y;
m.v[5] = 0;
m.v[6] = - p.x * u.x - p.y * u.y;
m.v[7] = - p.x * v.x - p.y * v.y;
m.v[8] = 1;
return m;
}
mat3
mat3_ortho_basis (vec3 u, vec3 v, vec3 w)
{
mat3 m;
m.v[0] = u.x;
m.v[1] = v.x;
m.v[2] = w.x;
m.v[3] = u.y;
m.v[4] = v.y;
m.v[5] = w.y;
m.v[6] = u.z;
m.v[7] = v.z;
m.v[8] = w.z;
return m;
}
mat4
mat3h_ortho_basis (vec3 p, vec3 u, vec3 v, vec3 w)
{
mat4 m;
m.v[0] = u.x;
m.v[1] = v.x;
m.v[2] = w.x;
m.v[3] = 0;
m.v[4] = u.y;
m.v[5] = v.y;
m.v[6] = w.y;
m.v[7] = 0;
m.v[8] = u.z;
m.v[9] = v.z;
m.v[10] = w.z;
m.v[11] = 0;
m.v[12] = -p.x * u.x - p.y * u.y - p.z * u.z;
m.v[13] = -p.x * v.x - p.y * v.y - p.z * v.z;
m.v[14] = -p.x * w.x - p.y * w.y - p.z * w.z;
m.v[15] = 1;
return m;
}
mat4
mat4_ortho_basis (vec4 u, vec4 v, vec4 w, vec4 t)
{
mat4 m;
m.v[0] = u.x;
m.v[1] = v.x;
m.v[2] = w.x;
m.v[3] = t.x;
m.v[4] = u.y;
m.v[5] = v.y;
m.v[6] = w.y;
m.v[7] = t.y;
m.v[8] = u.z;
m.v[9] = v.z;
m.v[10] = w.z;
m.v[11] = t.z;
m.v[12] = u.w;
m.v[13] = v.w;
m.v[14] = w.w;
m.v[15] = t.w;
return m;
}
mat4
matrix_perspective (double fovy, double aspect, double z_near, double z_far)
{
mat4 m;
float f;
fovy = degree_2_radian (fovy);
f = 1.0 / tan (fovy / 2.0);
m.v[0] = f / aspect;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 0.0;
m.v[5] = f;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 0.0;
m.v[9] = 0.0;
m.v[10] = (z_far + z_near) / (z_near - z_far);
m.v[11] = -1.0;
m.v[12] = 0.0;
m.v[13] = 0.0;
m.v[14] = (2.0 * z_far * z_near) / (z_near - z_far);
m.v[15] = 0.0;
return m;
}
mat4
matrix_frustum (float left, float right,
float bottom, float top,
float z_near, float z_far)
{
mat4 m;
m.v[0] = 2.0 * z_near / (right - left);
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 0.0;
m.v[5] = 2.0 * z_near / (top - bottom);
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = (right + left) / (right - left);
m.v[9] = (top + bottom) / (top - bottom);
m.v[10] = (z_far + z_near) / (z_near - z_far);
m.v[11] = -1.0;
m.v[12] = 0.0;
m.v[13] = 0.0;
m.v[14] = (2 * z_far * z_near) / (z_near - z_far);
m.v[15] = 0.0;
return m;
}
mat4
matrix_ortho (float left, float right,
float bottom, float top,
float zNear, float zFar)
{
mat4 m;
m.v[0] = 2/(right - left);
m.v[1] = 0;
m.v[2] = 0;
m.v[3] = 0;
m.v[4] = 0;
m.v[5] = 2/(top - bottom);
m.v[6] = 0;
m.v[7] = 0;
m.v[8] = 0;
m.v[9] = 0;
m.v[10] = -2 / (zFar - zNear);
m.v[11] = 0;
m.v[12] = - (right + left) / (right - left);
m.v[13] = - (top + bottom) / (top - bottom);
m.v[14] = - (zFar + zNear) / (zFar - zNear);
m.v[15] = 1;
return m;
}
mat4
matrix_look_at (vec3 pos, vec3 dir, vec3 up)
{
mat4 m;
vec3 side;
side = cross (dir, up);
side = vec3_normalize (side);
up = cross (side, dir);
up = vec3_normalize (up);
m.v[0] = side.x;
m.v[1] = up.x;
m.v[2] = -dir.x;
m.v[3] = 0;
m.v[4] = side.y;
m.v[5] = up.y;
m.v[6] = -dir.y;
m.v[7] = 0;
m.v[8] = side.z;
m.v[9] = up.z;
m.v[10] = -dir.z;
m.v[11] = 0;
m.v[12] = -pos.x * side.x - pos.y * side.y - pos.z * side.z;
m.v[13] = -pos.x * up.x - pos.y * up.y - pos.z * up.z;
m.v[14] = pos.x * dir.x + pos.y * dir.y + pos.z * dir.z;
m.v[15] = 1;
return m;
}
mat4
matrix_look_at_ortho (vec3 pos, vec3 dir, vec3 up, vec3 right)
{
dir = vec3_inverse (dir);
return mat3h_ortho_basis (pos, right, up, dir);
}
mat3
mat3_quat_rotation (quaternion q)
{
mat3 m;
m.v[0] = - 2 * q.y * q.y - 2 * q.z * q.z + 1;
m.v[1] = 2 * q.x * q.y + 2 * q.w * q.z;
m.v[2] = 2 * q.x * q.z - 2 * q.w * q.y;
m.v[3] = 2 * q.x * q.y - 2 * q.w * q.z;
m.v[4] = - 2 * q.x * q.x - 2 * q.z * q.z + 1;
m.v[5] = 2 * q.y * q.z + 2 * q.w * q.x;
m.v[6] = 2 * q.x * q.z + 2 * q.w * q.y;
m.v[7] = 2 * q.y * q.z - 2 * q.w * q.x;
m.v[8] = - 2 * q.x * q.x - 2 * q.y * q.y;
return m;
}
mat4
mat3h_quat_rotation (quaternion q)
{
mat4 m;
m.v[0] = - 2 * q.y * q.y - 2 * q.z * q.z + 1;
m.v[1] = 2 * q.x * q.y + 2 * q.w * q.z;
m.v[2] = 2 * q.x * q.z - 2 * q.w * q.y;
m.v[3] = 0;
m.v[4] = 2 * q.x * q.y - 2 * q.w * q.z;
m.v[5] = - 2 * q.x * q.x - 2 * q.z * q.z + 1;
m.v[6] = 2 * q.y * q.z + 2 * q.w * q.x;
m.v[7] = 0;
m.v[8] = 2 * q.x * q.z + 2 * q.w * q.y;
m.v[9] = 2 * q.y * q.z - 2 * q.w * q.x;
m.v[10] = - 2 * q.x * q.x - 2 * q.y * q.y;
m.v[11] = 0;
m.v[12] = 0;
m.v[13] = 0;
m.v[14] = 0;
m.v[15] = 1;
return m;
}
mat2
mat2_rotation (float angle)
{
mat2 m;
m.v[0] = cos (angle);
m.v[1] = sin (angle);
m.v[2] = -sin (angle);
m.v[3] = cos (angle);
return m;
}
mat3
mat2h_rotation (float angle)
{
mat3 m;
m.v[0] = cos (angle);
m.v[1] = sin (angle);
m.v[2] = 0.0;
m.v[3] = -sin (angle);
m.v[4] = cos (angle);
m.v[5] = 0.0;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 1.0;
return m;
}
mat3
mat3_rotation (float angle, float x, float y, float z)
{
mat3 m;
m.v[0] = x * x * (1 - cos (angle)) + cos (angle);
m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle);
m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle);
m.v[3] = x * y * (1 - cos (angle)) - z * sin (angle);
m.v[4] = y * y * (1 - cos (angle)) + cos (angle);
m.v[5] = y * z * (1 - cos (angle)) + x * sin (angle);
m.v[6] = x * z * (1 - cos (angle)) + y * sin (angle);
m.v[7] = y * z * (1 - cos (angle)) - x * sin (angle);
m.v[8] = z * z * (1 - cos (angle)) + cos (angle);
return m;
}
mat4
mat3h_rotation (float angle, float x, float y, float z)
{
mat4 m;
m.v[0] = x * x * (1 - cos (angle)) + cos (angle);
m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle);
m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle);
m.v[3] = 0;
m.v[4] = x * y * (1 - cos (angle)) - z * sin (angle);
m.v[5] = y * y * (1 - cos (angle)) + cos (angle);
m.v[6] = y * z * (1 - cos (angle)) + x * sin (angle);
m.v[7] = 0;
m.v[8] = x * z * (1 - cos (angle)) + y * sin (angle);
m.v[9] = y * z * (1 - cos (angle)) - x * sin (angle);
m.v[10] = z * z * (1 - cos (angle)) + cos (angle);
m.v[11] = 0;
m.v[12] = 0;
m.v[13] = 0;
m.v[14] = 0;
m.v[15] = 1;
return m;
}
mat3
mat2h_translation (vec2 v)
{
mat3 m;
m.v[0] = 1.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 1.0;
m.v[5] = 0.0;
m.v[6] = v.x;
m.v[7] = v.y;
m.v[8] = 1.0;
return m;
}
mat4
mat3h_translation (vec3 v)
{
mat4 m;
m.v[0] = 1.0;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 0.0;
m.v[5] = 1.0;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 0.0;
m.v[9] = 0.0;
m.v[10] = 1.0;
m.v[11] = 0.0;
m.v[12] = v.x;
m.v[13] = v.y;
m.v[14] = v.z;
m.v[15] = 1.0;
return m;
}
mat2
mat2_scale (float x, float y)
{
mat2 m;
m.v[0] = x;
m.v[1] = 0;
m.v[2] = 0;
m.v[3] = y;
return m;
}
mat3
mat3_scale (float x, float y, float z)
{
mat3 m;
m.v[0] = x;
m.v[1] = 0;
m.v[2] = 0;
m.v[3] = 0;
m.v[4] = y;
m.v[5] = 0;
m.v[6] = 0;
m.v[7] = 0;
m.v[8] = z;
return m;
}
mat4
mat4_scale (float x, float y, float z, float w)
{
mat4 m;
m.v[0] = x;
m.v[1] = 0;
m.v[2] = 0;
m.v[3] = 0;
m.v[4] = 0;
m.v[5] = y;
m.v[6] = 0;
m.v[7] = 0;
m.v[8] = 0;
m.v[9] = 0;
m.v[10] = z;
m.v[11] = 0;
m.v[12] = 0;
m.v[13] = 0;
m.v[14] = 0;
m.v[15] = w;
return m;
}
mat4
matrix_texture_bias (void)
{
mat4 m;
m.v[0] = 0.5;
m.v[1] = 0.0;
m.v[2] = 0.0;
m.v[3] = 0.0;
m.v[4] = 0.0;
m.v[5] = 0.5;
m.v[6] = 0.0;
m.v[7] = 0.0;
m.v[8] = 0.0;
m.v[9] = 0.0;
m.v[10] = 0.5;
m.v[11] = 0.0;
m.v[12] = 0.5;
m.v[13] = 0.5;
m.v[14] = 0.5;
m.v[15] = 1.0;
return m;
}
void
mat2_transpose (mat2 * mt, mat2 * m)
{
float mv1 = m->v[1];
mt->v[0] = m->v[0];
mt->v[1] = m->v[2];
mt->v[2] = mv1;
mt->v[3] = m->v[3];
}
void
mat3_transpose (mat3 * mt, mat3 * m)
{
float mv1 = m->v[1];
float mv2 = m->v[2];
float mv5 = m->v[5];
mt->v[0] = m->v[0];
mt->v[1] = m->v[3];
mt->v[2] = m->v[6];
mt->v[3] = mv1;
mt->v[4] = m->v[4];
mt->v[5] = m->v[7];
mt->v[6] = mv2;
mt->v[7] = mv5;
mt->v[8] = m->v[8];
}
void
mat4_transpose (mat4 * mt, mat4 * m)
{
float mv2 = m->v[2];
float mv3 = m->v[3];
float mv6 = m->v[6];
float mv7 = m->v[7];
float mv11 = m->v[11];
mt->v[0] = m->v[0];
mt->v[1] = m->v[4];
mt->v[2] = m->v[8];
mt->v[3] = m->v[12];
mt->v[4] = mv2;
mt->v[5] = m->v[5];
mt->v[6] = m->v[9];
mt->v[7] = m->v[13];
mt->v[8] = mv2;
mt->v[9] = mv6;
mt->v[10] = m->v[10];
mt->v[11] = m->v[14];
mt->v[12] = mv3;
mt->v[13] = mv7;
mt->v[14] = mv11;
mt->v[15] = m->v[15];
}
float
mat2_det (mat2 * m)
{
return m->v[0] * m->v[3] - m->v[1] * m->v[2];
}
float
mat3_det (mat3 * m)
{
float det =
m->v[0] * (m->v[4] * m->v[8] - m->v[5] * m->v[7]) -
m->v[1] * (m->v[3] * m->v[8] - m->v[5] * m->v[6]) +
m->v[2] * (m->v[3] * m->v[7] - m->v[4] * m->v[6]);
return det;
}
float
mat4_det (mat4 * m)
{
float det =
m->v[0] * (m->v[ 5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
m->v[ 6] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) +
m->v[ 7] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13]))
-m->v[1] * (m->v[ 4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
m->v[ 6] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) +
m->v[ 7] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12]))
+m->v[2] * (m->v[ 4] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) -
m->v[ 5] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) +
m->v[ 7] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12]))
-m->v[3] * (m->v[ 4] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13]) -
m->v[ 5] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12]) +
m->v[ 6] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12]));
return det;
}
void
mat2_inverse (mat2 * inv, mat2 * m)
{
float invdet = 1.0 / mat2_det (m);
float mv0 = m->v[0];
inv->v[0] = m->v[3] * invdet;
inv->v[1] = m->v[1] * (-invdet);
inv->v[2] = m->v[2] * (-invdet);
inv->v[3] = mv0 * invdet;
}
void
mat3_inverse (mat3 * inv, mat3 * m)
{
float mv0 = m->v[0];
float mv1 = m->v[1];
float mv2 = m->v[2];
float mv3 = m->v[3];
float mv4 = m->v[4];
float mv6 = m->v[6];
float invdet;
invdet = 1.0 / mat3_det (m);
inv->v[0] = ((m->v[4] * m->v[8]) - (m->v[5] * m->v[7])) * invdet;
inv->v[1] = (-(m->v[1] * m->v[8]) + (m->v[2] * m->v[7])) * invdet;
inv->v[2] = ((mv1 * m->v[5]) - (m->v[2] * m->v[4])) * invdet;
inv->v[3] = (-(m->v[3] * m->v[8]) + (m->v[5] * m->v[6])) * invdet;
inv->v[4] = ((mv0 * m->v[8]) - (mv2 * m->v[6])) * invdet;
inv->v[5] = (-(mv0 * m->v[5]) + (mv2 * mv3)) * invdet;
inv->v[6] = ((mv3 * m->v[7]) - (mv4 * m->v[6])) * invdet;
inv->v[7] = (-(mv0 * m->v[7]) + (mv1 * mv6)) * invdet;
inv->v[8] = ((mv0 * mv4) - (mv1 * mv3)) * invdet;
}
static void
matrix_inverse4 (mat4 * inv, mat4 * m)
{
float mv0 = m->v[0];
float mv1 = m->v[1];
float mv2 = m->v[2];
float mv3 = m->v[3];
float mv4 = m->v[4];
float mv5 = m->v[5];
float mv6 = m->v[6];
float mv7 = m->v[7];
float mv8 = m->v[8];
float mv9 = m->v[9];
float mv10 = m->v[10];
float mv12 = m->v[12];
float mv13 = m->v[13];
float invdet = 1.0 / mat4_det (m);
inv->v[0] = m->v[5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
m->v[6] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) +
m->v[7] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]);
inv->v[1] = -m->v[1] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) +
m->v[2] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) -
m->v[3] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]);
inv->v[2] = mv1 * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) -
m->v[2] * (m->v[5] * m->v[15] - m->v[7] * m->v[13]) +
m->v[3] * (m->v[5] * m->v[14] - m->v[6] * m->v[13]);
inv->v[3] = -mv1 * (m->v[6] * m->v[11] - m->v[7] * m->v[10]) +
mv2 * (m->v[5] * m->v[11] - m->v[7] * m->v[9]) -
m->v[3] * (m->v[5] * m->v[10] - m->v[6] * m->v[9]);
inv->v[4] = -m->v[4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) +
m->v[6] * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) -
m->v[7] * (m->v[8] * m->v[14] - m->v[10] * m->v[12]);
inv->v[5] = mv0 * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
mv2 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) +
mv3 * (m->v[8] * m->v[14] - m->v[10] * m->v[12]);
inv->v[6] = -mv0 * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) +
mv2 * (mv4 * m->v[15] - m->v[7] * m->v[12]) -
mv3 * (mv4 * m->v[14] - m->v[6] * m->v[12]);
inv->v[7] = mv0 * (mv6 * m->v[11] - m->v[7] * m->v[10]) -
mv2 * (mv4 * m->v[11] - m->v[7] * m->v[8]) +
mv3 * (mv4 * m->v[10] - mv6 * m->v[8]);
inv->v[8] = mv4 * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) -
mv5 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) +
mv7 * (m->v[8] * m->v[13] - m->v[9] * m->v[12]);
inv->v[9] = -mv0 * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) +
mv1 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) -
mv3 * (m->v[8] * m->v[13] - m->v[9] * m->v[12]);
inv->v[10] = mv0 * (mv5 * m->v[15] - mv7 * m->v[13]) -
mv1 * (mv4 * m->v[15] - mv7 * m->v[12]) +
mv3 * (mv4 * m->v[13] - mv5 * m->v[12]);
inv->v[11] = -mv0 * (mv5 * m->v[11] - mv7 * mv9) +
mv1 * (mv4 * m->v[11] - mv7 * mv8) -
mv3 * (mv4 * mv9 - mv5 * mv8);
inv->v[12] = -mv4 * (mv9 * m->v[14] - mv10 * m->v[13]) +
mv5 * (mv8 * m->v[14] - mv10 * m->v[12]) -
mv6 * (mv8 * m->v[13] - mv9 * m->v[12]);
inv->v[13] = mv0 * (mv9 * m->v[14] - mv10 * m->v[13]) -
mv1 * (mv8 * m->v[14] - mv10 * mv12) +
mv2 * (mv8 * m->v[13] - mv9 * mv12);
inv->v[14] = -mv0 * (mv5 * m->v[14] - mv6 * mv13) +
mv1 * (mv4 * m->v[14] - mv6 * mv12) -
mv2 * (mv4 * mv13 - mv5 * mv12);
inv->v[15] = mv0 * (mv5 * mv10 - mv6 * mv9) -
mv1 * (mv4 * mv10 - mv6 * mv8) +
mv2 * (mv4 * mv9 - mv5 * mv8);
inv->v[ 0] *= invdet;
inv->v[ 1] *= invdet;
inv->v[ 2] *= invdet;
inv->v[ 3] *= invdet;
inv->v[ 4] *= invdet;
inv->v[ 5] *= invdet;
inv->v[ 6] *= invdet;
inv->v[ 7] *= invdet;
inv->v[ 8] *= invdet;
inv->v[ 9] *= invdet;
inv->v[10] *= invdet;
inv->v[11] *= invdet;
inv->v[12] *= invdet;
inv->v[13] *= invdet;
inv->v[14] *= invdet;
inv->v[15] *= invdet;
}
void
mat4_inverse (mat4 * inv, mat4 * m)
{
float detp;
float invdet;
mat2 rp;
mat2 pq;
mat2 rpq;
mat2 s;
mat2 p;
detp = m->v[0] * m->v[5] - m->v[1] * m->v[4];
if (float_approx_zero (detp, 0.0000001))
{
/* In this case the standard algorithm is used */
matrix_inverse4 (inv, m);
return;
}
/* In this case we can use a better approach */
invdet = 1.0 / detp;
p.v[0] = m->v[5] * invdet;
p.v[1] = -m->v[1] * invdet;
p.v[2] = -m->v[4] * invdet;
p.v[3] = m->v[0] * invdet;
rp.v[0] = (p.v[0] * m->v[2] + p.v[1] * m->v[6]);
rp.v[1] = (p.v[0] * m->v[3] + p.v[1] * m->v[7]);
rp.v[2] = (p.v[2] * m->v[2] + p.v[3] * m->v[6]);
rp.v[3] = (p.v[2] * m->v[3] + p.v[3] * m->v[7]);
pq.v[0] = (p.v[0] * m->v[8] + p.v[2] * m->v[9]);
pq.v[1] = (p.v[1] * m->v[8] + p.v[3] * m->v[9]);
pq.v[2] = (p.v[0] * m->v[12] + p.v[2] * m->v[13]);
pq.v[3] = (p.v[1] * m->v[12] + p.v[3] * m->v[13]);
rpq.v[0] = rp.v[0] * m->v[8] + rp.v[2] * m->v[9];
rpq.v[1] = rp.v[1] * m->v[8] + rp.v[3] * m->v[9];
rpq.v[2] = rp.v[0] * m->v[12] + rp.v[2] * m->v[13];
rpq.v[3] = rp.v[1] * m->v[12] + rp.v[3] * m->v[13];
s.v[0] = m->v[10] - rpq.v[0];
s.v[1] = m->v[11] - rpq.v[1];
s.v[2] = m->v[14] - rpq.v[2];
s.v[3] = m->v[15] - rpq.v[3];
invdet = 1.0 / (s.v[0] * s.v[3] - s.v[1] * s.v[2]);
inv->v[10] = s.v[3] * invdet;
inv->v[11] = - s.v[1] * invdet;
inv->v[14] = - s.v[2] * invdet;
inv->v[15] = s.v[0] * invdet;
inv->v[2] = -inv->v[10] * rp.v[0] - m->v[14] * rp.v[1];
inv->v[3] = -inv->v[11] * rp.v[0] - m->v[15] * rp.v[1];
inv->v[6] = -inv->v[10] * rp.v[2] - m->v[14] * rp.v[3];
inv->v[7] = -inv->v[11] * rp.v[2] - m->v[15] * rp.v[3];
inv->v[8] = -pq.v[0] * inv->v[10] - pq.v[2] * inv->v[11];
inv->v[9] = -pq.v[1] * inv->v[10] - pq.v[3] * inv->v[11];
inv->v[12] = -pq.v[0] * inv->v[14] - pq.v[2] * inv->v[15];
inv->v[13] = -pq.v[1] * inv->v[14] - pq.v[3] * inv->v[15];
inv->v[0] = p.v[0] - (pq.v[0] * inv->v[2] + pq.v[2] * inv->v[3]);
inv->v[1] = p.v[1] - (pq.v[1] * inv->v[2] + pq.v[3] * inv->v[3]);
inv->v[4] = p.v[2] - (pq.v[0] * inv->v[6] + pq.v[2] * inv->v[7]);
inv->v[5] = p.v[3] - (pq.v[1] * inv->v[6] + pq.v[3] * inv->v[7]);
}
void
mat2_product (mat2 * p, mat2 * a, mat2 * b)
{
float av0 = a->v[0];
float av1 = a->v[1];
float bv0 = b->v[0];
float bv2 = b->v[2];
p->v[0] = a->v[0] * b->v[0] + a->v[2] * b->v[1];
p->v[1] = a->v[1] * bv0 + a->v[3] * b->v[1];
p->v[2] = av0 * b->v[2] + a->v[2] * b->v[3];
p->v[3] = av1 * bv2 + a->v[3] * b->v[3];
}
void
mat3_product (mat3 * p, mat3 * a, mat3 * b)
{
float bv0 = b->v[0];
float bv1 = b->v[1];
float bv3 = b->v[3];
float bv4 = b->v[4];
float bv6 = b->v[6];
float bv7 = b->v[7];
float av0 = a->v[0];
float av1 = a->v[1];
float av2 = a->v[2];
float av3 = a->v[3];
float av4 = a->v[4];
float av5 = a->v[5];
p->v[0] = a->v[0] * b->v[0] + a->v[3] * b->v[1] + a->v[6] * b->v[2];
p->v[1] = a->v[1] * bv0 + a->v[4] * b->v[1] + a->v[7] * b->v[2];
p->v[2] = a->v[2] * bv0 + a->v[5] * bv1 + a->v[8] * b->v[2];
p->v[3] = av0 * b->v[3] + a->v[3] * b->v[4] + a->v[6] * b->v[5];
p->v[4] = av1 * bv3 + a->v[4] * b->v[4] + a->v[7] * b->v[5];
p->v[5] = av2 * bv3 + a->v[5] * bv4 + a->v[8] * b->v[5];
p->v[6] = av0 * b->v[6] + av3 * b->v[7] + a->v[6] * b->v[8];
p->v[7] = av1 * bv6 + av4 * b->v[7] + a->v[7] * b->v[8];
p->v[8] = av2 * bv6 + av5 * bv7 + a->v[8] * b->v[8];
}
void
mat4_product (mat4 * p, mat4 * a, mat4 * b)
{
float bv0 = b->v[0];
float bv1 = b->v[1];
float bv2 = b->v[2];
float bv4 = b->v[4];
float bv5 = b->v[5];
float bv6 = b->v[6];
float bv8 = b->v[8];
float bv9 = b->v[9];
float bv10 = b->v[10];
float bv12 = b->v[12];
float bv13 = b->v[13];
float bv14 = b->v[14];
float av0 = a->v[0];
float av1 = a->v[1];
float av2 = a->v[2];
float av3 = a->v[3];
float av4 = a->v[4];
float av5 = a->v[5];
float av6 = a->v[6];
float av7 = a->v[7];
float av8 = a->v[8];
float av9 = a->v[9];
float av10 = a->v[10];
float av11 = a->v[11];
p->v[0] = a->v[0] * b->v[0] + a->v[4] * b->v[1] + a->v[8] * b->v[2] + a->v[12] * b->v[3];
p->v[1] = a->v[1] * bv0 + a->v[5] * b->v[1] + a->v[9] * b->v[2] + a->v[13] * b->v[3];
p->v[2] = a->v[2] * bv0 + a->v[6] * bv1 + a->v[10] * b->v[2] + a->v[14] * b->v[3];
p->v[3] = a->v[3] * bv0 + a->v[7] * bv1 + a->v[11] * bv2 + a->v[15] * b->v[3];
p->v[4] = av0 * b->v[4] + a->v[4] * b->v[5] + a->v[8] * b->v[6] + a->v[12] * b->v[7];
p->v[5] = av1 * b->v[4] + a->v[5] * b->v[5] + a->v[9] * b->v[6] + a->v[13] * b->v[7];
p->v[6] = av2 * bv4 + a->v[6] * bv5 + a->v[10] * b->v[6] + a->v[14] * b->v[7];
p->v[7] = av3 * bv4 + a->v[7] * bv5 + a->v[11] * bv6 + a->v[15] * b->v[7];
p->v[8] = av0 * b->v[8] + av4 * b->v[9] + a->v[8] * b->v[10] + a->v[12] * b->v[11];
p->v[9] = av1 * bv8 + av5 * b->v[9] + a->v[9] * b->v[10] + a->v[13] * b->v[11];
p->v[10] = av2 * bv8 + av6 * bv9 + a->v[10] * b->v[10] + a->v[14] * b->v[11];
p->v[11] = av3 * bv8 + av7 * bv9 + a->v[11] * bv10 + a->v[15] * b->v[11];
p->v[12] = av0 * b->v[12] + av4 * b->v[13] + av8 * b->v[14] + a->v[12] * b->v[15];
p->v[13] = av1 * bv12 + av5 * b->v[13] + av9 * b->v[14] + a->v[13] * b->v[15];
p->v[14] = av2 * bv12 + av6 * bv13 + av10 * b->v[14] + a->v[14] * b->v[15];
p->v[15] = av3 * bv12 + av7 * bv13 + av11 * bv14 + a->v[15] * b->v[15];
}
void
mat3_add_diag (mat3 * out, mat3 * in, float x, float y, float z)
{
out->v[0] = in->v[0] + x;
out->v[1] = in->v[1];
out->v[2] = in->v[2];
out->v[3] = in->v[3];
out->v[4] = in->v[4] + y;
out->v[5] = in->v[5];
out->v[6] = in->v[6];
out->v[6] = in->v[7];
out->v[8] = in->v[8] + z;
}
void
mat3_substract_diag (mat3 * out, mat3 * in, float x, float y, float z)
{
out->v[0] = in->v[0] - x;
out->v[1] = in->v[1];
out->v[2] = in->v[2];
out->v[3] = in->v[3];
out->v[4] = in->v[4] - y;
out->v[5] = in->v[5];
out->v[6] = in->v[6];
out->v[6] = in->v[7];
out->v[8] = in->v[8] - z;
}
void
mat3_add (mat3 * out, mat3 * m1, mat3 * m2)
{
out->v[0] = m1->v[0] + m2->v[0];
out->v[1] = m1->v[1] + m2->v[1];
out->v[2] = m1->v[2] + m2->v[2];
out->v[3] = m1->v[3] + m2->v[3];
out->v[4] = m1->v[4] + m2->v[4];
out->v[5] = m1->v[5] + m2->v[5];
out->v[6] = m1->v[6] + m2->v[6];
out->v[7] = m1->v[7] + m2->v[7];
out->v[8] = m1->v[8] + m2->v[8];
}
void
mat3_substract (mat3 * out, mat3 * m1, mat3 * m2)
{
out->v[0] = m1->v[0] - m2->v[0];
out->v[1] = m1->v[1] - m2->v[1];
out->v[2] = m1->v[2] - m2->v[2];
out->v[3] = m1->v[3] - m2->v[3];
out->v[4] = m1->v[4] - m2->v[4];
out->v[5] = m1->v[5] - m2->v[5];
out->v[6] = m1->v[6] - m2->v[6];
out->v[7] = m1->v[7] - m2->v[7];
out->v[8] = m1->v[8] - m2->v[8];
}
vec2
mat2_column (mat2 * m, int idx)
{
vec2 v;
idx = idx * 2;
v.x = m->v[idx];
v.y = m->v[idx+1];
return v;
}
vec3
mat3_column (mat3 * m, int idx)
{
vec3 v;
idx = idx * 3;
v.x = m->v[idx];
v.y = m->v[idx+1];
v.z = m->v[idx+2];
return v;
}
vec4
mat4_column (mat4 * m, int idx)
{
vec4 v;
idx = idx * 4;
v.x = m->v[idx];
v.y = m->v[idx+1];
v.z = m->v[idx+2];
v.w = m->v[idx+3];
return v;
}
vec2
mat2_mult (mat2 * mat, vec2 vec)
{
vec2 result;
result.x = mat->v[0] * vec.x + mat->v[2] * vec.y;
result.y = mat->v[1] * vec.x + mat->v[3] * vec.y;
return result;
}
vec3
mat3_mult (mat3 * mat, vec3 vec)
{
vec3 r;
r.x = (mat->v[0] * vec.x) + (mat->v[3] * vec.y) + (mat->v[6] * vec.z);
r.y = (mat->v[1] * vec.x) + (mat->v[4] * vec.y) + (mat->v[7] * vec.z);
r.z = (mat->v[2] * vec.x) + (mat->v[5] * vec.y) + (mat->v[8] * vec.z);
return r;
}
vec4
mat4_mult (mat4 * mat, vec4 vec)
{
vec4 result;
result.x = (mat->v[0] * vec.x) + (mat->v[4] * vec.y) + (mat->v[8] * vec.z) + (mat->v[12] * vec.w);
result.y = (mat->v[1] * vec.x) + (mat->v[5] * vec.y) + (mat->v[9] * vec.z) + (mat->v[13] * vec.w);
result.z = (mat->v[2] * vec.x) + (mat->v[6] * vec.y) + (mat->v[10] * vec.z) + (mat->v[14] * vec.w);
result.w = (mat->v[3] * vec.x) + (mat->v[7] * vec.y) + (mat->v[11] * vec.z) + (mat->v[15] * vec.w);
return result;
}
int
mat3_eigen_values (float * h1, float * h2, float * h3, mat3 * m)
{
float a;
float b;
float c;
int n;
vec3 v = vec3_zero ();
a = m->v[0] + m->v[4] + m->v[8];
b = m->v[1] * m->v[3] + m->v[2] * m->v[6] + m->v[7] * m->v[5] -
m->v[0] * m->v[4] - m->v[0] * m->v[8] - m->v[4] * m->v[8];
c = m->v[0] * (m->v[4] * m->v[8] - m->v[5] * m->v[7]) -
m->v[1] * (m->v[3] * m->v[8] - m->v[5] * m->v[6]) +
m->v[2] * (m->v[3] * m->v[7] - m->v[4] * m->v[6]);
n = cubic_polynomial (&v, -1, a, b, c);
if (v.y > v.x)
{
float t = v.x; v.x = v.y; v.y = t;
}
if (v.z > v.y)
{
float t = v.y; v.y = v.z; v.z = t;
}
if (v.y > v.x)
{
float t = v.x; v.x = v.y; v.y = t;
}
*h1 = v.x;
*h2 = v.y;
*h3 = v.z;
return n;
}
/**
|a b c x|
|d e f y|
|g h i z|
*/
/**/
static int find_pivot (float * max, mat3 * a, int k)
{
int i_max;
if (k == 0)
{
*max = fabs (a->v[k]); i_max = 0;
if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 1;}
if (fabs (a->v[k+2]) > *max) { *max = fabs (a->v[k+2]); i_max = 2;}
}
else if (k == 1)
{
k = 4;
*max = fabs (a->v[k]); i_max = 1;
if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 2;}
}
else
{
k = 8;
*max = fabs (a->v[k]);
i_max = 2;
}
return i_max;
}
static void swap_rows (mat3 * a, float * s, int l1, int l2)
{
float tmp;
if (l1 == l2) { return; }
/* swap left part */
tmp = s[l1];
s[l1] = s[l2];
s[l2] = tmp;
/* swap first column */
tmp = a->v[l1];
a->v[l1] = a->v[l2];
a->v[l2] = tmp;
/* swap second column */
l1 += 3;
l2 += 3;
tmp = a->v[l1];
a->v[l1] = a->v[l2];
a->v[l2] = tmp;
/* swap third column */
l1 += 3;
l2 += 3;
tmp = a->v[l1];
a->v[l1] = a->v[l2];
a->v[l2] = tmp;
}
static void normalize_row (mat3 * a, float * s, int l, int i)
{
float d = a->v[l+i*3];
a->v[l+i*3] = 1.0;
s[l] = s[l] / d;
if (i <= 1) { a->v[l+6] = a->v[l+6] / d; }
if (i == 0) { a->v[l+3] = a->v[l+3] / d; }
}
static void substract_row (mat3 * a, float * s, int l1, int l2, int i)
{
float r;
if (i == 0)
{
r = - a->v[l2];
a->v[l2] = 0;
a->v[l2+3] = a->v[l2+3] + a->v[l1+3] * r;
a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r;
}
else if (i == 1)
{
r = - a->v[l2+3];
a->v[l2+3] = 0;
a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r;
}
else if (i == 2)
{
r = - a->v[l2+6];
a->v[l2+6] = 0;
}
s[l2] = s[l2] + s[l1] * r;
}
int
mat3_gaussian_elimination (mat3 * a, vec3 * v)
{
float s[3];
float max;
int i_max;
s[0] = v->x;
s[1] = v->y;
s[2] = v->z;
i_max = find_pivot (&max, a, 0);
/* If matrix is singular return error */
if (float_approx_zero (max, 1e-05f)) { return -1; }
swap_rows (a, s, 0, i_max);
normalize_row (a, s, 0, 0);
substract_row (a, s, 0, 1, 0);
substract_row (a, s, 0, 2, 0);
i_max = find_pivot (&max, a, 1);
/* If matrix is singular return error */
if (float_approx_zero (max, 1e-05f)) { return -1; }
swap_rows (a, s, 1, i_max);
normalize_row (a, s, 1, 1);
substract_row (a, s, 1, 2, 1);
/* If matrix is singular return error */
if (float_approx_zero (a->v[8], 1e-05f)) { return -1; }
normalize_row (a, s, 2, 2);
v->x = s[0];
v->y = s[1];
v->z = s[2];
return 0;
}
int
mat3_solve (mat3 * m, vec3 * v)
{
mat3 a = *m;
if (mat3_gaussian_elimination (&a, v) < 0) { return -1; }
v->y = v->y - v->z * a.v[7];
v->x = v->x - v->z * a.v[6] - v->y * a.v[3];
return 0;
}
static void build_Q0 (mat3 * q, mat3 * m)
{
float a = sqrt (m->v[0] * m->v[0] + m->v[1] * m->v[1] + m->v[2] * m->v[2]);
if (m->v[0] > 0) { a = - a; }
vec3 v = vec3_new (m->v[0] - a, m->v[1], m->v[2]);
float norm = vec3_norm (v);
if (norm > 0)
v = vec3_scale (v, 1.0 / norm);
q->v[0] = 1 - 2 * v.x * v.x;
q->v[1] = - 2 * v.y * v.x;
q->v[2] = - 2 * v.z * v.x;
q->v[3] = - 2 * v.x * v.y;
q->v[4] = 1 - 2 * v.y * v.y;
q->v[5] = - 2 * v.z * v.y;
q->v[6] = - 2 * v.x * v.z;
q->v[7] = - 2 * v.y * v.z;
q->v[8] = 1 - 2 * v.z * v.z;
}
static void build_Q1 (mat3 * q, mat3 * m)
{
float a = sqrt (m->v[4] * m->v[4] + m->v[5] * m->v[5]);
if (m->v[4] > 0) { a = -a; }
vec2 v = vec2_new (m->v[4] - a, m->v[5]);
float norm = vec2_norm (v);
if (norm > 0)
v = vec2_scale (v, 1.0 / norm);
q->v[0] = 1;
q->v[1] = 0;
q->v[2] = 0;
q->v[3] = 0;
q->v[4] = 1 - 2 * v.x * v.x;
q->v[5] = - 2 * v.y * v.x;
q->v[6] = 0;
q->v[7] = - 2 * v.x * v.y;
q->v[8] = 1 - 2 * v.y * v.y;
}
void mat3_QR (mat3 * Q, mat3 * R, mat3 * m)
{
mat3 tmp;
mat3 Q0;
mat3 Q1;
build_Q0 (&Q0, m);
mat3_product (&tmp, &Q0, m);
build_Q1 (&Q1, &tmp);
mat3_product (Q, &Q0, &Q1);
mat3_transpose (&tmp, Q);
mat3_product (R, &tmp, m);
}
void mat3_hess (mat3 * H, mat3 * Q, mat3 * m)
{
mat3 tmp;
float a = sqrt (m->v[1] * m->v[1] + m->v[2] * m->v[2]);
if (m->v[1] > 0)
a = - a;
vec2 v = vec2_new (m->v[1] - a, m->v[2]);
float norm = vec2_norm (v);
if (norm > 0)
v = vec2_scale (v, 1.0/norm);
Q->v[0] = 1;
Q->v[1] = 0;
Q->v[2] = 0;
Q->v[3] = 0;
Q->v[4] = 1 - 2 * v.x * v.x;
Q->v[5] = - 2 * v.x * v.y;
Q->v[6] = 0;
Q->v[7] = - 2 * v.x * v.y;
Q->v[8] = 1 - 2 * v.y * v.y;
mat3_product (&tmp, Q, m);
mat3_product (H, &tmp, Q);
}
static void shift (mat3 * m, float s)
{
m->v[0] = m->v[0] + s;
m->v[4] = m->v[4] + s;
m->v[8] = m->v[8] + s;
}
static void sort3 (float * val, vec3 * vec)
{
if (val[1] > val[0])
{
float t = val[0];
vec3 v = vec[0];
val[0] = val[1];
val[1] = t;
vec[0] = vec[1];
vec[1] = v;
}
if (val[2] > val[1])
{
float t = val[1];
vec3 v = vec[1];
val[1] = val[2];
val[2] = t;
vec[1] = vec[2];
vec[2] = v;
}
if (val[1] > val[0])
{
float t = val[0];
vec3 v = vec[0];
val[0] = val[1];
val[1] = t;
vec[0] = vec[1];
vec[1] = v;
}
}
void mat3_eigen_vectors (vec3 * eigen_vectors, float * eigen_values, mat3 * A, int it)
{
int i;
mat3 H, S, Q, W, R;
mat3_hess (&H, &W, A);
float s = H.v[8];
shift (&H, -H.v[8]);
mat3_QR (&Q, &R, &H);
mat3_product (&H, &R, &Q);
S = Q;
for (i = 0; i < it; i++)
{
mat3_QR (&Q, &R, &H);
mat3_product (&H, &R, &Q);
mat3_product (&S, &S, &Q);
}
shift (&H, s);
mat3_product (&S, &W, &S);
eigen_values[0] = H.v[0];
eigen_values[1] = H.v[4];
eigen_values[2] = H.v[8];
eigen_vectors[0] = vec3_new (S.v[0], S.v[1], S.v[2]);
eigen_vectors[1] = vec3_new (S.v[3], S.v[4], S.v[5]);
eigen_vectors[2] = vec3_new (S.v[6], S.v[7], S.v[8]);
sort3 (eigen_values, eigen_vectors);
}
Reference in New Issue View Git Blame Copy Permalink