You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
815 lines
19 KiB
815 lines
19 KiB
#include "linmath.h"
|
|
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
|
|
const vec3 UNIT_X = {.x = 1, .y = 0, .z = 0};
|
|
const vec3 UNIT_Y = {.x = 0, .y = 1, .z = 0};
|
|
const vec3 UNIT_Z = {.x = 0, .y = 0, .z = 1};
|
|
|
|
const vec3 UNIT_X_NEG = {.x = -1, .y = 0, .z = 0};
|
|
const vec3 UNIT_Y_NEG = {.x = 0, .y = -1, .z = 0};
|
|
const vec3 UNIT_Z_NEG = {.x = 0, .y = 0, .z = -1};
|
|
|
|
|
|
void vec2_fill(vec2* res, float x, float y)
|
|
{
|
|
res->x = x;
|
|
res->y = y;
|
|
}
|
|
|
|
void vec2_add(vec2* res, vec2* v1, vec2* v2)
|
|
{
|
|
res->x = v1->x + v2->x;
|
|
res->y = v1->y + v2->y;
|
|
}
|
|
|
|
void vec2_sub(vec2* res, vec2* v1, vec2* v2)
|
|
{
|
|
res->x = v1->x - v2->x;
|
|
res->y = v1->y - v2->y;
|
|
}
|
|
|
|
void vec2_assign(vec2* res, vec2* val)
|
|
{
|
|
res->x = val->x;
|
|
res->y = val->y;
|
|
}
|
|
|
|
float vec2_len(vec2* val)
|
|
{
|
|
return sqrtf((val->x * val->x) + (val->y * val->y));
|
|
}
|
|
|
|
void vec2_norm(vec2* res, vec2* val)
|
|
{
|
|
if (!val->x && !val->y)
|
|
{
|
|
vec2_assign(res, val);
|
|
return;
|
|
}
|
|
|
|
float l = 1.0f / vec2_len(val);
|
|
vec2 v;
|
|
v.x = val->x * l;
|
|
v.y = val->y * l;
|
|
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
}
|
|
|
|
void vec2_mul(vec2* res, vec2* v1, vec2* v2)
|
|
{
|
|
res->x = v1->x * v2->x;
|
|
res->y = v1->y * v2->y;
|
|
}
|
|
|
|
void vec3_fill(vec3* res, float x, float y, float z)
|
|
{
|
|
res->x = x;
|
|
res->y = y;
|
|
res->z = z;
|
|
}
|
|
|
|
void vec3_add(vec3* res, const vec3* v1, const vec3* v3)
|
|
{
|
|
res->x = v1->x + v3->x;
|
|
res->y = v1->y + v3->y;
|
|
res->z = v1->z + v3->z;
|
|
}
|
|
|
|
void vec3_sub(vec3* res, const vec3* v1, const vec3* v3)
|
|
{
|
|
res->x = v1->x - v3->x;
|
|
res->y = v1->y - v3->y;
|
|
res->z = v1->z - v3->z;
|
|
}
|
|
|
|
void vec3_assign(vec3* res, const vec3* val)
|
|
{
|
|
res->x = val->x;
|
|
res->y = val->y;
|
|
res->z = val->z;
|
|
}
|
|
|
|
void vec3_cross(vec3* res, const vec3* v1, const vec3* v2)
|
|
{
|
|
vec3 v;
|
|
v.x = (v1->y * v2->z) - (v1->z * v2->y);
|
|
v.y = (v1->z * v2->x) - (v1->x * v2->z);
|
|
v.z = (v1->x * v2->y) - (v1->y * v2->x);
|
|
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
res->z = v.z;
|
|
}
|
|
|
|
float vec3_len(vec3* val)
|
|
{
|
|
return sqrtf((val->x * val->x) + (val->y * val->y) + (val->z * val->z));
|
|
}
|
|
|
|
void vec3_norm(vec3* res, vec3* val)
|
|
{
|
|
if (!val->x && !val->y && !val->z)
|
|
{
|
|
vec3_assign(res, val);
|
|
return;
|
|
}
|
|
|
|
float l = 1.0f / vec3_len(val);
|
|
vec3 v;
|
|
v.x = val->x * l;
|
|
v.y = val->y * l;
|
|
v.z = val->z * l;
|
|
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
res->z = v.z;
|
|
}
|
|
|
|
void vec3_mul(vec3* res, vec3* v1, vec3* v3)
|
|
{
|
|
res->x = v1->x * v3->x;
|
|
res->y = v1->y * v3->y;
|
|
res->z = v1->z * v3->z;
|
|
}
|
|
|
|
void vec3_mul_mat4(vec3* res, vec3* val, mat4* mat)
|
|
{
|
|
vec3 v;
|
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8] + mat->mat[12];
|
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9] + mat->mat[13];
|
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10] + mat->mat[14];
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
res->z = v.z;
|
|
}
|
|
|
|
void vec3_scale(vec3* res, const vec3* val, float s)
|
|
{
|
|
res->x = val->x * s;
|
|
res->y = val->y * s;
|
|
res->z = val->z * s;
|
|
}
|
|
|
|
int vec3_equals(vec3* v1, vec3* v2)
|
|
{
|
|
if ((v1->x < (v2->x + EPSILON) && v1->x > (v2->x - EPSILON)) &&
|
|
(v1->y < (v2->y + EPSILON) && v1->y > (v2->y - EPSILON)) &&
|
|
(v1->z < (v2->z + EPSILON) && v1->z > (v2->z - EPSILON)))
|
|
return 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void vec3_transform_norm(vec3* res, const vec3* val, const mat4* mat)
|
|
{
|
|
/*
|
|
a = (Vx, Vy, Vz, 0)
|
|
b = (a×M)T
|
|
Out = (bx, by, bz)
|
|
|
|
Omits the translation, only scaling + rotating*/
|
|
vec3 v;
|
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8];
|
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9];
|
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10];
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
res->z = v.z;
|
|
}
|
|
|
|
float vec3_dot(vec3* v1, vec3* v2)
|
|
{
|
|
return (v1->x * v2->x +
|
|
v1->y * v2->y +
|
|
v1->z * v2->z);
|
|
}
|
|
|
|
|
|
void vec4_fill(vec4* res, float x, float y, float z, float w)
|
|
{
|
|
res->x = x;
|
|
res->y = y;
|
|
res->z = z;
|
|
res->w = w;
|
|
}
|
|
|
|
void vec4_fill_vec3(vec4* res, const vec3* v, float w)
|
|
{
|
|
res->x = v->x;
|
|
res->y = v->y;
|
|
res->z = v->z;
|
|
res->w = w;
|
|
}
|
|
|
|
void vec4_add(vec4* res, vec4* v1, vec4* v4)
|
|
{
|
|
res->x = v1->x + v4->x;
|
|
res->y = v1->y + v4->y;
|
|
res->z = v1->z + v4->z;
|
|
res->w = v1->w + v4->w;
|
|
}
|
|
|
|
void vec4_sub(vec4* res, vec4* v1, vec4* v4)
|
|
{
|
|
res->x = v1->x - v4->x;
|
|
res->y = v1->y - v4->y;
|
|
res->z = v1->z - v4->z;
|
|
res->w = v1->w - v4->w;
|
|
}
|
|
|
|
void vec4_assign(vec4* res, vec4* val)
|
|
{
|
|
res->x = val->x;
|
|
res->y = val->y;
|
|
res->z = val->z;
|
|
res->w = val->w;
|
|
}
|
|
|
|
float vec4_len(vec4* val)
|
|
{
|
|
return sqrtf((val->x * val->x) +
|
|
(val->y * val->y) +
|
|
(val->z * val->z) +
|
|
(val->w * val->w));
|
|
}
|
|
|
|
void vec4_norm(vec4* res, vec4* val)
|
|
{
|
|
if (!val->x && !val->y && !val->z && !val->w)
|
|
{
|
|
vec4_assign(res, val);
|
|
return;
|
|
}
|
|
|
|
float l = 1.0f / vec4_len(val);
|
|
vec4 v;
|
|
v.x = val->x * l;
|
|
v.y = val->y * l;
|
|
v.z = val->z * l;
|
|
v.w = val->w * l;
|
|
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
res->z = v.z;
|
|
res->w = v.w;
|
|
}
|
|
|
|
void vec4_mul(vec4* res, vec4* v1, vec4* v4)
|
|
{
|
|
res->x = v1->x * v4->x;
|
|
res->y = v1->y * v4->y;
|
|
res->z = v1->z * v4->z;
|
|
res->w = v1->w * v4->w;
|
|
}
|
|
|
|
void vec4_mul_mat4(vec4* res, vec4* val, mat4* mat)
|
|
{
|
|
vec4 v;
|
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8] + mat->mat[12];
|
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9] + mat->mat[14];
|
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10] + mat->mat[14];
|
|
v.w = val->x * mat->mat[2] + val->y * mat->mat[6] + val->w * mat->mat[10] + mat->mat[14];
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
res->z = v.z;
|
|
res->w = v.w;
|
|
}
|
|
|
|
void vec4_scale(vec4* res, const vec4* val, float s)
|
|
{
|
|
res->x = val->x * s;
|
|
res->y = val->y * s;
|
|
res->z = val->z * s;
|
|
res->w = val->w * s;
|
|
}
|
|
|
|
int vec4_equals(vec4* v1, vec4* v2)
|
|
{
|
|
if ((v1->x < (v2->x + EPSILON) && v1->x > (v2->x - EPSILON)) &&
|
|
(v1->y < (v2->y + EPSILON) && v1->y > (v2->y - EPSILON)) &&
|
|
(v1->z < (v2->z + EPSILON) && v1->z > (v2->z - EPSILON)) &&
|
|
(v1->w < (v2->w + EPSILON) && v1->w > (v2->w - EPSILON)))
|
|
return 1;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void vec4_transform_norm(vec4* res, const vec4* val, const mat4* mat)
|
|
{
|
|
/*
|
|
a = (Vx, Vy, Vz, 0)
|
|
b = (a×M)T
|
|
Out = (bx, by, bz)
|
|
|
|
Omits the translation, only scaling + rotating*/
|
|
vec4 v;
|
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8];
|
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9];
|
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10];
|
|
res->x = v.x;
|
|
res->y = v.y;
|
|
res->z = v.z;
|
|
}
|
|
|
|
void mat4_identity(mat4* res)
|
|
{
|
|
memset(res->mat, 0, sizeof(float) * 16);
|
|
res->mat[0] = res->mat[5] = res->mat[10] = res->mat[15] = 1.0f;
|
|
}
|
|
|
|
void mat4_mul(mat4* res, const mat4* mat1, const mat4* mat2)
|
|
{
|
|
float mat[16];
|
|
const float *m1 = mat1->mat, *m2 = mat2->mat;
|
|
|
|
mat[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3];
|
|
mat[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3];
|
|
mat[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3];
|
|
mat[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3];
|
|
|
|
mat[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7];
|
|
mat[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7];
|
|
mat[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7];
|
|
mat[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7];
|
|
|
|
mat[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11];
|
|
mat[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11];
|
|
mat[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11];
|
|
mat[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11];
|
|
|
|
mat[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15];
|
|
mat[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15];
|
|
mat[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
|
|
mat[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
|
|
|
|
memcpy(res->mat, mat, sizeof(float)*16);
|
|
}
|
|
|
|
void mat4_translate(mat4* res, float x, float y, float z)
|
|
{
|
|
memset(res->mat, 0, sizeof(float) * 16);
|
|
res->mat[0] = 1.0f;
|
|
res->mat[5] = 1.0f;
|
|
res->mat[10] = 1.0f;
|
|
|
|
res->mat[12] = x;
|
|
res->mat[13] = y;
|
|
res->mat[14] = z;
|
|
res->mat[15] = 1.0f;
|
|
}
|
|
|
|
void mat4_lookat(mat4* res, const vec3* eye, const vec3* center, const vec3* up_vec)
|
|
{
|
|
vec3 f, up, s, u;
|
|
mat4 translate;
|
|
|
|
vec3_sub(&f, center, eye);
|
|
vec3_norm(&f, &f);
|
|
|
|
vec3_assign(&up, up_vec);
|
|
vec3_norm(&up, &up);
|
|
|
|
vec3_cross(&s, &f, &up);
|
|
vec3_norm(&s, &s);
|
|
|
|
vec3_cross(&u, &s, &f);
|
|
vec3_norm(&s, &s);
|
|
|
|
mat4_identity(res);
|
|
|
|
res->mat[0] = s.x;
|
|
res->mat[4] = s.y;
|
|
res->mat[8] = s.z;
|
|
|
|
res->mat[1] = u.x;
|
|
res->mat[5] = u.y;
|
|
res->mat[9] = u.z;
|
|
|
|
res->mat[2] = -f.x;
|
|
res->mat[6] = -f.y;
|
|
res->mat[10] = -f.z;
|
|
|
|
mat4_translate(&translate, -eye->x, -eye->y, -eye->z);
|
|
mat4_mul(res, res, &translate);
|
|
}
|
|
|
|
void mat4_perspective(mat4* res, float fov, float aspect, float nearz, float farz )
|
|
{
|
|
float r = TO_RADIANS(fov / 2);
|
|
float deltaz = farz - nearz;
|
|
float s = sin(r);
|
|
float cotangent = 0;
|
|
|
|
if (deltaz == 0 || s == 0 || aspect == 0)
|
|
return;
|
|
|
|
cotangent = cos(r) / s;
|
|
mat4_identity(res);
|
|
res->mat[0] = cotangent / aspect;
|
|
res->mat[5] = cotangent;
|
|
res->mat[10] = -(farz + nearz) / deltaz;
|
|
res->mat[11] = -1;
|
|
res->mat[14] = -2 * nearz * farz / deltaz;
|
|
res->mat[15] = 0;
|
|
}
|
|
|
|
void mat4_ortho(mat4* res,
|
|
float left, float right,
|
|
float bottom, float top,
|
|
float nearz, float farz)
|
|
{
|
|
float tx = -((right + left) / (right - left));
|
|
float ty = -((top + bottom) / (top - bottom));
|
|
float tz = -((farz + nearz) / (farz - nearz));
|
|
mat4_identity(res);
|
|
res->mat[0] = 2 / (right - left);
|
|
res->mat[5] = 2 / (top - bottom);
|
|
res->mat[10] = -2 / (farz - nearz);
|
|
res->mat[12] = tx;
|
|
res->mat[13] = ty;
|
|
res->mat[14] = tz;
|
|
}
|
|
|
|
void mat4_scale(mat4* res, float x, float y, float z)
|
|
{
|
|
memset(res->mat, 0, sizeof(float) * 16);
|
|
res->mat[0] = x;
|
|
res->mat[5] = y;
|
|
res->mat[10] = z;
|
|
res->mat[15] = 1.0f;
|
|
}
|
|
|
|
void mat4_from_quat(mat4* res, const quat* q)
|
|
{
|
|
double xx = q->x * q->x;
|
|
double xy = q->x * q->y;
|
|
double xz = q->x * q->z;
|
|
double xw = q->x * q->w;
|
|
|
|
double yy = q->y * q->y;
|
|
double yz = q->y * q->z;
|
|
double yw = q->y * q->w;
|
|
|
|
double zz = q->z * q->z;
|
|
double zw = q->z * q->w;
|
|
|
|
res->mat[0] = 1 - 2 * (yy + zz);
|
|
res->mat[1] = 2 * (xy + zw);
|
|
res->mat[2] = 2 * (xz - yw);
|
|
res->mat[3] = 0;
|
|
|
|
res->mat[4] = 2 * (xy - zw);
|
|
res->mat[5] = 1 - 2 * (xx + zz);
|
|
res->mat[6] = 2 * (yz + xw);
|
|
res->mat[7] = 0.0;
|
|
|
|
res->mat[8] = 2 * (xz + yw);
|
|
res->mat[9] = 2 * (yz - xw);
|
|
res->mat[10] = 1 - 2 * (xx + yy);
|
|
res->mat[11] = 0.0;
|
|
|
|
res->mat[12] = 0.0;
|
|
res->mat[13] = 0.0;
|
|
res->mat[14] = 0.0;
|
|
res->mat[15] = 1.0;
|
|
}
|
|
|
|
void mat4_rot_x(mat4* res, const float angle)
|
|
{
|
|
res->mat[0] = 1.0f;
|
|
res->mat[1] = 0.0f;
|
|
res->mat[2] = 0.0f;
|
|
res->mat[3] = 0.0f;
|
|
|
|
res->mat[4] = 0.0f;
|
|
res->mat[5] = cosf(angle);
|
|
res->mat[6] = sinf(angle);
|
|
res->mat[7] = 0.0f;
|
|
|
|
res->mat[8] = 0.0f;
|
|
res->mat[9] = -sinf(angle);
|
|
res->mat[10] = cosf(angle);
|
|
res->mat[11] = 0.0f;
|
|
|
|
res->mat[12] = 0.0f;
|
|
res->mat[13] = 0.0f;
|
|
res->mat[14] = 0.0f;
|
|
res->mat[15] = 1.0f;
|
|
}
|
|
|
|
void mat4_rot_y(mat4* res, float angle)
|
|
{
|
|
res->mat[0] = cosf(angle);
|
|
res->mat[1] = 0.0f;
|
|
res->mat[2] = -sinf(angle);
|
|
res->mat[3] = 0.0f;
|
|
|
|
res->mat[4] = 0.0f;
|
|
res->mat[5] = 1.0f;
|
|
res->mat[6] = 0.0f;
|
|
res->mat[7] = 0.0f;
|
|
|
|
res->mat[8] = sinf(angle);
|
|
res->mat[9] = 0.0f;
|
|
res->mat[10] = cosf(angle);
|
|
res->mat[11] = 0.0f;
|
|
|
|
res->mat[12] = 0.0f;
|
|
res->mat[13] = 0.0f;
|
|
res->mat[14] = 0.0f;
|
|
res->mat[15] = 1.0f;
|
|
}
|
|
|
|
void mat4_rot_z(mat4* res, float angle)
|
|
{
|
|
res->mat[0] = cosf(angle);
|
|
res->mat[1] = sinf(angle);
|
|
res->mat[2] = 0.0f;
|
|
res->mat[3] = 0.0f;
|
|
|
|
res->mat[4] = -sinf(angle);
|
|
res->mat[5] = cosf(angle);
|
|
res->mat[6] = 0.0f;
|
|
res->mat[7] = 0.0f;
|
|
|
|
res->mat[8] = 0.0f;
|
|
res->mat[9] = 0.0f;
|
|
res->mat[10] = 1.0f;
|
|
res->mat[11] = 0.0f;
|
|
|
|
res->mat[12] = 0.0f;
|
|
res->mat[13] = 0.0f;
|
|
res->mat[14] = 0.0f;
|
|
res->mat[15] = 1.0f;
|
|
}
|
|
|
|
void mat4_assign(mat4* res, const mat4* m)
|
|
{
|
|
if(res == m)
|
|
return;
|
|
memcpy(res->mat, m->mat, sizeof(float) * 16);
|
|
}
|
|
|
|
|
|
void mat4_inverse(mat4* res, mat4* mat)
|
|
{
|
|
mat4 tmp;
|
|
float det;
|
|
int i;
|
|
|
|
tmp.mat[0] = mat->mat[5] * mat->mat[10] * mat->mat[15] -
|
|
mat->mat[5] * mat->mat[11] * mat->mat[14] -
|
|
mat->mat[9] * mat->mat[6] * mat->mat[15] +
|
|
mat->mat[9] * mat->mat[7] * mat->mat[14] +
|
|
mat->mat[13] * mat->mat[6] * mat->mat[11] -
|
|
mat->mat[13] * mat->mat[7] * mat->mat[10];
|
|
|
|
tmp.mat[4] = -mat->mat[4] * mat->mat[10] * mat->mat[15] +
|
|
mat->mat[4] * mat->mat[11] * mat->mat[14] +
|
|
mat->mat[8] * mat->mat[6] * mat->mat[15] -
|
|
mat->mat[8] * mat->mat[7] * mat->mat[14] -
|
|
mat->mat[12] * mat->mat[6] * mat->mat[11] +
|
|
mat->mat[12] * mat->mat[7] * mat->mat[10];
|
|
|
|
tmp.mat[8] = mat->mat[4] * mat->mat[9] * mat->mat[15] -
|
|
mat->mat[4] * mat->mat[11] * mat->mat[13] -
|
|
mat->mat[8] * mat->mat[5] * mat->mat[15] +
|
|
mat->mat[8] * mat->mat[7] * mat->mat[13] +
|
|
mat->mat[12] * mat->mat[5] * mat->mat[11] -
|
|
mat->mat[12] * mat->mat[7] * mat->mat[9];
|
|
|
|
tmp.mat[12] = -mat->mat[4] * mat->mat[9] * mat->mat[14] +
|
|
mat->mat[4] * mat->mat[10] * mat->mat[13] +
|
|
mat->mat[8] * mat->mat[5] * mat->mat[14] -
|
|
mat->mat[8] * mat->mat[6] * mat->mat[13] -
|
|
mat->mat[12] * mat->mat[5] * mat->mat[10] +
|
|
mat->mat[12] * mat->mat[6] * mat->mat[9];
|
|
|
|
tmp.mat[1] = -mat->mat[1] * mat->mat[10] * mat->mat[15] +
|
|
mat->mat[1] * mat->mat[11] * mat->mat[14] +
|
|
mat->mat[9] * mat->mat[2] * mat->mat[15] -
|
|
mat->mat[9] * mat->mat[3] * mat->mat[14] -
|
|
mat->mat[13] * mat->mat[2] * mat->mat[11] +
|
|
mat->mat[13] * mat->mat[3] * mat->mat[10];
|
|
|
|
tmp.mat[5] = mat->mat[0] * mat->mat[10] * mat->mat[15] -
|
|
mat->mat[0] * mat->mat[11] * mat->mat[14] -
|
|
mat->mat[8] * mat->mat[2] * mat->mat[15] +
|
|
mat->mat[8] * mat->mat[3] * mat->mat[14] +
|
|
mat->mat[12] * mat->mat[2] * mat->mat[11] -
|
|
mat->mat[12] * mat->mat[3] * mat->mat[10];
|
|
|
|
tmp.mat[9] = -mat->mat[0] * mat->mat[9] * mat->mat[15] +
|
|
mat->mat[0] * mat->mat[11] * mat->mat[13] +
|
|
mat->mat[8] * mat->mat[1] * mat->mat[15] -
|
|
mat->mat[8] * mat->mat[3] * mat->mat[13] -
|
|
mat->mat[12] * mat->mat[1] * mat->mat[11] +
|
|
mat->mat[12] * mat->mat[3] * mat->mat[9];
|
|
|
|
tmp.mat[13] = mat->mat[0] * mat->mat[9] * mat->mat[14] -
|
|
mat->mat[0] * mat->mat[10] * mat->mat[13] -
|
|
mat->mat[8] * mat->mat[1] * mat->mat[14] +
|
|
mat->mat[8] * mat->mat[2] * mat->mat[13] +
|
|
mat->mat[12] * mat->mat[1] * mat->mat[10] -
|
|
mat->mat[12] * mat->mat[2] * mat->mat[9];
|
|
|
|
tmp.mat[2] = mat->mat[1] * mat->mat[6] * mat->mat[15] -
|
|
mat->mat[1] * mat->mat[7] * mat->mat[14] -
|
|
mat->mat[5] * mat->mat[2] * mat->mat[15] +
|
|
mat->mat[5] * mat->mat[3] * mat->mat[14] +
|
|
mat->mat[13] * mat->mat[2] * mat->mat[7] -
|
|
mat->mat[13] * mat->mat[3] * mat->mat[6];
|
|
|
|
tmp.mat[6] = -mat->mat[0] * mat->mat[6] * mat->mat[15] +
|
|
mat->mat[0] * mat->mat[7] * mat->mat[14] +
|
|
mat->mat[4] * mat->mat[2] * mat->mat[15] -
|
|
mat->mat[4] * mat->mat[3] * mat->mat[14] -
|
|
mat->mat[12] * mat->mat[2] * mat->mat[7] +
|
|
mat->mat[12] * mat->mat[3] * mat->mat[6];
|
|
|
|
tmp.mat[10] = mat->mat[0] * mat->mat[5] * mat->mat[15] -
|
|
mat->mat[0] * mat->mat[7] * mat->mat[13] -
|
|
mat->mat[4] * mat->mat[1] * mat->mat[15] +
|
|
mat->mat[4] * mat->mat[3] * mat->mat[13] +
|
|
mat->mat[12] * mat->mat[1] * mat->mat[7] -
|
|
mat->mat[12] * mat->mat[3] * mat->mat[5];
|
|
|
|
tmp.mat[14] = -mat->mat[0] * mat->mat[5] * mat->mat[14] +
|
|
mat->mat[0] * mat->mat[6] * mat->mat[13] +
|
|
mat->mat[4] * mat->mat[1] * mat->mat[14] -
|
|
mat->mat[4] * mat->mat[2] * mat->mat[13] -
|
|
mat->mat[12] * mat->mat[1] * mat->mat[6] +
|
|
mat->mat[12] * mat->mat[2] * mat->mat[5];
|
|
|
|
tmp.mat[3] = -mat->mat[1] * mat->mat[6] * mat->mat[11] +
|
|
mat->mat[1] * mat->mat[7] * mat->mat[10] +
|
|
mat->mat[5] * mat->mat[2] * mat->mat[11] -
|
|
mat->mat[5] * mat->mat[3] * mat->mat[10] -
|
|
mat->mat[9] * mat->mat[2] * mat->mat[7] +
|
|
mat->mat[9] * mat->mat[3] * mat->mat[6];
|
|
|
|
tmp.mat[7] = mat->mat[0] * mat->mat[6] * mat->mat[11] -
|
|
mat->mat[0] * mat->mat[7] * mat->mat[10] -
|
|
mat->mat[4] * mat->mat[2] * mat->mat[11] +
|
|
mat->mat[4] * mat->mat[3] * mat->mat[10] +
|
|
mat->mat[8] * mat->mat[2] * mat->mat[7] -
|
|
mat->mat[8] * mat->mat[3] * mat->mat[6];
|
|
|
|
tmp.mat[11] = -mat->mat[0] * mat->mat[5] * mat->mat[11] +
|
|
mat->mat[0] * mat->mat[7] * mat->mat[9] +
|
|
mat->mat[4] * mat->mat[1] * mat->mat[11] -
|
|
mat->mat[4] * mat->mat[3] * mat->mat[9] -
|
|
mat->mat[8] * mat->mat[1] * mat->mat[7] +
|
|
mat->mat[8] * mat->mat[3] * mat->mat[5];
|
|
|
|
tmp.mat[15] = mat->mat[0] * mat->mat[5] * mat->mat[10] -
|
|
mat->mat[0] * mat->mat[6] * mat->mat[9] -
|
|
mat->mat[4] * mat->mat[1] * mat->mat[10] +
|
|
mat->mat[4] * mat->mat[2] * mat->mat[9] +
|
|
mat->mat[8] * mat->mat[1] * mat->mat[6] -
|
|
mat->mat[8] * mat->mat[2] * mat->mat[5];
|
|
|
|
det = mat->mat[0] * tmp.mat[0] + mat->mat[1] * tmp.mat[4] + mat->mat[2] * tmp.mat[8] + mat->mat[3] * tmp.mat[12];
|
|
|
|
if (det == 0) {
|
|
return;
|
|
}
|
|
|
|
det = 1.0 / det;
|
|
|
|
for (i = 0; i < 16; i++) {
|
|
res->mat[i] = tmp.mat[i] * det;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void quat_fill(quat* res, float x, float y, float z, float w)
|
|
{
|
|
res->x = x;
|
|
res->y = y;
|
|
res->z = z;
|
|
res->w = w;
|
|
}
|
|
|
|
void quat_identity(quat* res)
|
|
{
|
|
res->x = 0.0;
|
|
res->y = 0.0;
|
|
res->z = 0.0;
|
|
res->w = 1.0;
|
|
}
|
|
|
|
void quat_mul_vec3(vec3* res, const quat* q, const vec3* v)
|
|
{
|
|
vec3 uv, uuv, qvec;
|
|
qvec.x = q->x;
|
|
qvec.y = q->y;
|
|
qvec.z = q->z;
|
|
|
|
vec3_cross(&uv, &qvec, v);
|
|
vec3_cross(&uuv, &qvec, &uv);
|
|
|
|
vec3_scale(&uv, &uv, (2.0f * q->w));
|
|
vec3_scale(&uuv, &uuv, 2.0f);
|
|
|
|
vec3_add(res, v, &uv);
|
|
vec3_add(res, res, &uuv);
|
|
}
|
|
|
|
void quat_assign(quat* res, const quat* val)
|
|
{
|
|
res->x = val->x;
|
|
res->y = val->y;
|
|
res->z = val->z;
|
|
res->w = val->w;
|
|
}
|
|
|
|
void quat_mul(quat* res, const quat* q1, const quat* q2)
|
|
{
|
|
quat tmp1, tmp2;
|
|
quat_assign(&tmp1, q1);
|
|
quat_assign(&tmp2, q2);
|
|
|
|
res->x = tmp1.w * tmp2.x + tmp1.x * tmp2.w + tmp1.y * tmp2.z - tmp1.z * tmp2.y;
|
|
res->y = tmp1.w * tmp2.y + tmp1.y * tmp2.w + tmp1.z * tmp2.x - tmp1.x * tmp2.z;
|
|
res->z = tmp1.w * tmp2.z + tmp1.z * tmp2.w + tmp1.x * tmp2.y - tmp1.y * tmp2.x;
|
|
res->w = tmp1.w * tmp2.w - tmp1.x * tmp2.x - tmp1.y * tmp2.y - tmp1.z * tmp2.z;
|
|
}
|
|
|
|
float quat_len_sq(const quat* q)
|
|
{
|
|
return (q->x * q->x) + (q->y * q->y) + (q->z * q->z) + (q->w * q->w);
|
|
}
|
|
|
|
float quat_len(const quat* q)
|
|
{
|
|
return sqrt(quat_len_sq(q));
|
|
}
|
|
|
|
void quat_norm(quat* res, const quat* val)
|
|
{
|
|
float length = quat_len(val);
|
|
if(fabs(length) < EPSILON)
|
|
{
|
|
quat_fill(res, 0.0f, 0.0f, 0.0f, 0.0f);
|
|
return;
|
|
}
|
|
|
|
quat_fill(res, res->x / length, res->y / length, res->z / length, res->w / length);
|
|
}
|
|
|
|
void quat_axis_angle(quat* res, const vec3* v, float angle)
|
|
{
|
|
float half_angle = angle * 0.5f;
|
|
float scale = sinf(half_angle);
|
|
|
|
res->x = v->x * scale;
|
|
res->y = v->y * scale;
|
|
res->z = v->z * scale;
|
|
res->w = cosf(half_angle);
|
|
quat_norm(res, res);
|
|
}
|
|
|
|
void quat_get_forward_rh(vec3* res, const quat* q)
|
|
{
|
|
quat_mul_vec3(res, q, &UNIT_Z_NEG);
|
|
}
|
|
|
|
void quat_get_forward_lh(vec3* res, const quat* q)
|
|
{
|
|
quat_mul_vec3(res, q, &UNIT_Z);
|
|
}
|
|
|
|
void quat_get_up(vec3* res, const quat* q)
|
|
{
|
|
quat_mul_vec3(res, q, &UNIT_Y);
|
|
}
|
|
|
|
void quat_get_right(vec3* res, const quat* q)
|
|
{
|
|
quat_mul_vec3(res, q, &UNIT_X);
|
|
}
|
|
|
|
float quat_get_pitch(const quat* q)
|
|
{
|
|
float result = atan2(2 * (q->y * q->z + q->w * q->x), q->w * q->w - q->x * q->x - q->y * q->y + q->z * q->z);
|
|
return result;
|
|
}
|
|
|
|
float quat_get_yaw(const quat* q)
|
|
{
|
|
float result = asin(-2 * (q->x * q->z - q->w * q->y));
|
|
return result;
|
|
}
|
|
|
|
float quat_get_roll(const quat* q)
|
|
{
|
|
float result = atan2(2 * (q->x * q->y + q->w * q->z), q->w * q->w + q->x * q->x - q->y * q->y - q->z * q->z);
|
|
return result;
|
|
}
|
|
|