1353 lines
31 KiB
C
1353 lines
31 KiB
C
/*
|
|
* 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_matrix.h"
|
|
#include "sweet_math.h"
|
|
#include "sweet_types.h"
|
|
|
|
mat2
|
|
sweet_matrix_null2 (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
|
|
sweet_matrix_null3 (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
|
|
sweet_matrix_null4 (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
|
|
sweet_matrix_identity2 (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
|
|
sweet_matrix_identity3 (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
|
|
sweet_matrix_identity4 (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
|
|
sweet_matrix_ortho_transform2 (vec2 u, vec2 v)
|
|
{
|
|
mat2 m;
|
|
m.v[0] = u.x;
|
|
m.v[3] = v.x;
|
|
|
|
m.v[2] = u.y;
|
|
m.v[3] = v.y;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat3
|
|
sweet_matrix_ortho_transform3 (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
|
|
sweet_matrix_ortho_transform4 (vec4 u, vec4 v, vec4 w, vec4 t)
|
|
{
|
|
mat4 m;
|
|
m.v[0] = u.x;
|
|
m.v[1] = v.y;
|
|
m.v[2] = w.z;
|
|
m.v[3] = t.w;
|
|
|
|
m.v[4] = u.x;
|
|
m.v[5] = v.y;
|
|
m.v[6] = w.z;
|
|
m.v[7] = t.w;
|
|
|
|
m.v[8] = u.x;
|
|
m.v[9] = v.y;
|
|
m.v[10] = w.z;
|
|
m.v[11] = t.w;
|
|
|
|
m.v[12] = u.x;
|
|
m.v[13] = v.y;
|
|
m.v[14] = w.z;
|
|
m.v[15] = t.w;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat4
|
|
sweet_matrix_perspective (double fovy, double aspect, double z_near, double z_far)
|
|
{
|
|
mat4 m;
|
|
float f;
|
|
|
|
fovy = sweet_math_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
|
|
sweet_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
|
|
sweet_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
|
|
sweet_matrix_look_at (vec3 pos, vec3 dir, vec3 up)
|
|
{
|
|
mat4 m;
|
|
vec3 side;
|
|
side = sweet_vector_cross (dir, up);
|
|
side = sweet_vector_normalize3 (side);
|
|
up = sweet_vector_cross (side, dir);
|
|
up = sweet_vector_normalize3 (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;
|
|
}
|
|
|
|
mat3
|
|
sweet_matrix_quat_rotation3 (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
|
|
sweet_matrix_quat_rotation3h (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;
|
|
}
|
|
|
|
mat4
|
|
sweet_matrix_rotation3h (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
|
|
sweet_matrix_rotation3 (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;
|
|
}
|
|
|
|
mat2
|
|
sweet_matrix_rotation2 (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
|
|
sweet_matrix_rotation2h (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;
|
|
}
|
|
|
|
mat4
|
|
sweet_matrix_translation3h (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;
|
|
}
|
|
|
|
mat3
|
|
sweet_matrix_translation2h (vec2 v)
|
|
{
|
|
mat3 m;
|
|
|
|
m.v[0] = 1.0;
|
|
m.v[1] = 0.0;
|
|
m.v[2] = 0.0;
|
|
|
|
m.v[4] = 0.0;
|
|
m.v[5] = 1.0;
|
|
m.v[6] = 0.0;
|
|
|
|
m.v[8] = v.x;
|
|
m.v[9] = v.y;
|
|
m.v[10] = 1.0;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat4
|
|
sweet_matrix_scale4 (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;
|
|
}
|
|
|
|
mat3
|
|
sweet_matrix_scale3 (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;
|
|
}
|
|
|
|
mat2
|
|
sweet_matrix_scale2 (float x, float y)
|
|
{
|
|
mat2 m;
|
|
|
|
m.v[0] = x;
|
|
m.v[1] = 0;
|
|
|
|
m.v[2] = 0;
|
|
m.v[3] = y;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat4
|
|
sweet_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
|
|
sweet_matrix_transpose2 (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
|
|
sweet_matrix_transpose3 (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
|
|
sweet_matrix_transpose4 (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
|
|
sweet_matrix_det2 (mat2 * m)
|
|
{
|
|
return m->v[0] * m->v[3] - m->v[1] * m->v[2];
|
|
}
|
|
|
|
float
|
|
sweet_matrix_det3 (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
|
|
sweet_matrix_det4 (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
|
|
sweet_matrix_inverse2 (mat2 * inv, mat2 * m)
|
|
{
|
|
float invdet = 1.0 / sweet_matrix_det2 (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
|
|
sweet_matrix_inverse3 (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 / sweet_matrix_det3 (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 / sweet_matrix_det4 (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
|
|
sweet_matrix_inverse4 (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 (sweet_math_approx_equals (detp, 0, 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
|
|
sweet_matrix_product2 (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
|
|
sweet_matrix_product3 (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
|
|
sweet_matrix_product4 (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
|
|
sweet_matrix_sub3 (mat2 * sub, mat3 * m)
|
|
{
|
|
sub->v[0] = m->v[0];
|
|
sub->v[1] = m->v[1];
|
|
sub->v[2] = m->v[4];
|
|
sub->v[3] = m->v[5];
|
|
}
|
|
|
|
void
|
|
sweet_matrix_sub4 (mat3 * sub, mat4 * m)
|
|
{
|
|
sub->v[0] = m->v[0];
|
|
sub->v[1] = m->v[1];
|
|
sub->v[2] = m->v[2];
|
|
sub->v[3] = m->v[4];
|
|
sub->v[4] = m->v[5];
|
|
sub->v[5] = m->v[6];
|
|
sub->v[6] = m->v[8];
|
|
sub->v[7] = m->v[9];
|
|
sub->v[8] = m->v[10];
|
|
}
|
|
|
|
vec2
|
|
sweet_matrix_column2 (mat2 * m, int idx)
|
|
{
|
|
vec2 v;
|
|
idx = idx * 2;
|
|
v.x = m->v[idx];
|
|
v.y = m->v[idx+1];
|
|
|
|
return v;
|
|
}
|
|
|
|
vec3
|
|
sweet_matrix_column3 (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
|
|
sweet_matrix_column4 (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
|
|
sweet_matrix_mult2 (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
|
|
sweet_matrix_mult3 (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
|
|
sweet_matrix_mult4 (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
|
|
sweet_matrix_eigen_value3 (float * h1, float * h2, float * h3, mat3 * m)
|
|
{
|
|
float a;
|
|
float b;
|
|
float c;
|
|
int n;
|
|
vec3 v;
|
|
|
|
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[0] * m->v[7] * m->v[5] -
|
|
m->v[1] * m->v[3] * m->v[8] + m->v[1] * m->v[6] * m->v[5] +
|
|
m->v[2] * m->v[3] * m->v[7] - m->v[2] * m->v[6] * m->v[4];
|
|
|
|
n = sweet_math_cubic_polynomial (&v, -1, a, b, c);
|
|
*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;
|
|
|
|
}
|
|
|
|
#define EPSILON 0.00005
|
|
|
|
int
|
|
sweet_matrix_gaussian_elimination3 (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 (sweet_math_approx_equals (max, 0, EPSILON)) { 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 (sweet_math_approx_equals (max, 0, EPSILON)) { 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 (sweet_math_approx_equals (a->v[8], 0, EPSILON)) { return -1; }
|
|
normalize_row (a, s, 2, 2);
|
|
|
|
v->x = s[0];
|
|
v->y = s[1];
|
|
v->z = s[2];
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
sweet_matrix_solve3 (mat3 * m, vec3 * v)
|
|
{
|
|
mat3 a = *m;
|
|
if (sweet_matrix_gaussian_elimination3 (&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 int is_identity (mat3 * m)
|
|
{
|
|
if (!sweet_math_approx_equals (m->v[0], 1.0, EPSILON)) { return 0; }
|
|
if (!sweet_math_approx_equals (m->v[1], 0.0, EPSILON)) { return 0; }
|
|
if (!sweet_math_approx_equals (m->v[2], 0.0, EPSILON)) { return 0; }
|
|
|
|
if (!sweet_math_approx_equals (m->v[3], 0.0, EPSILON)) { return 0; }
|
|
if (!sweet_math_approx_equals (m->v[4], 1.0, EPSILON)) { return 0; }
|
|
if (!sweet_math_approx_equals (m->v[5], 0.0, EPSILON)) { return 0; }
|
|
|
|
if (!sweet_math_approx_equals (m->v[6], 0.0, EPSILON)) { return 0; }
|
|
if (!sweet_math_approx_equals (m->v[7], 0.0, EPSILON)) { return 0; }
|
|
if (!sweet_math_approx_equals (m->v[8], 1.0, EPSILON)) { return 0; }
|
|
return 1;
|
|
}
|
|
|
|
static void get_eigen_vector (vec3 * v, mat3 * a, float h)
|
|
{
|
|
char free_value;
|
|
a->v[0] = a->v[0] - h;
|
|
a->v[4] = a->v[4] - h;
|
|
a->v[8] = a->v[8] - h;
|
|
|
|
v->x = 0;
|
|
v->y = 0;
|
|
v->z = 0;
|
|
free_value = 0;
|
|
|
|
sweet_matrix_gaussian_elimination3 (a, v);
|
|
|
|
if (sweet_math_approx_equals (a->v[0], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[1], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[2], 0, EPSILON))
|
|
{
|
|
free_value = 1;
|
|
v->x = 1;
|
|
}
|
|
if (sweet_math_approx_equals (a->v[3], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[4], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[5], 0, EPSILON))
|
|
{
|
|
free_value = 1;
|
|
v->y = 1;
|
|
}
|
|
if (sweet_math_approx_equals (a->v[6], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[7], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[8], 0, EPSILON))
|
|
{
|
|
free_value = 1;
|
|
v->z = 1;
|
|
}
|
|
|
|
if (!free_value &&
|
|
sweet_math_approx_equals (a->v[2], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[5], 0, EPSILON) &&
|
|
sweet_math_approx_equals (a->v[8], 0, EPSILON))
|
|
{
|
|
v->z = 1;
|
|
v->y = - a->v[7];
|
|
v->x = - a->v[3] * v->y - a->v[6];
|
|
}
|
|
*v = sweet_vector_normalize3 (*v);
|
|
}
|
|
|
|
int
|
|
sweet_matrix_eigen_vector3 (vec3 * vectors, mat3 * m)
|
|
{
|
|
/* (A - hI)V = 0 */
|
|
mat3 a;
|
|
int n;
|
|
float h1, h2, h3;
|
|
|
|
if (is_identity (m))
|
|
{
|
|
vectors[0] = sweet_vector_new3 (1,0,0);
|
|
vectors[1] = sweet_vector_new3 (0,1,0);
|
|
vectors[2] = sweet_vector_new3 (0,0,1);
|
|
return 3;
|
|
}
|
|
|
|
n = sweet_matrix_eigen_value3 (&h1, &h2, &h3, m);
|
|
|
|
/* vector associated with h1 */
|
|
a = *m;
|
|
get_eigen_vector (&vectors[0], &a, h1);
|
|
|
|
/* vector associated with h2 */
|
|
a = *m;
|
|
get_eigen_vector (&vectors[1], &a, h2);
|
|
|
|
/* vector associated with h3 */
|
|
a = *m;
|
|
get_eigen_vector (&vectors[2], &a, h3);
|
|
return n;
|
|
}
|
|
|