1033 lines
26 KiB
C
1033 lines
26 KiB
C
#include "vector3d.h"
|
|
#include <math.h>
|
|
#include <fixmath.h>
|
|
|
|
void vec2_set(Vector2 * vd, float x, float y)
|
|
{
|
|
vd->v[0] = x;
|
|
vd->v[1] = y;
|
|
}
|
|
|
|
void vec2_sum(Vector2 * vd, const Vector2 * v1, const Vector2 * v2)
|
|
{
|
|
vd->v[0] = v1->v[0] + v2->v[0];
|
|
vd->v[1] = v1->v[1] + v2->v[1];
|
|
}
|
|
|
|
void vec2_diff(Vector2 * vd, const Vector2 * v1, const Vector2 * v2)
|
|
{
|
|
vd->v[0] = v1->v[0] - v2->v[0];
|
|
vd->v[1] = v1->v[1] - v2->v[1];
|
|
}
|
|
|
|
void vec2_add(Vector2 * vd, const Vector2 * vs)
|
|
{
|
|
vd->v[0] += vs->v[0];
|
|
vd->v[1] += vs->v[1];
|
|
}
|
|
|
|
void vec2_madd(Vector2 * vd, float s, const Vector2 * vs)
|
|
{
|
|
vd->v[0] += s * vs->v[0];
|
|
vd->v[1] += s * vs->v[1];
|
|
}
|
|
|
|
void vec2_sub(Vector2 * vd, const Vector2 * vs)
|
|
{
|
|
vd->v[0] -= vs->v[0];
|
|
vd->v[1] -= vs->v[1];
|
|
}
|
|
|
|
void vec2_scale(Vector2 * vd, float s)
|
|
{
|
|
vd->v[0] *= s;
|
|
vd->v[1] *= s;
|
|
}
|
|
|
|
void vec2_lincomb(Vector2 * vd, const Vector2 * vb, float x, const Vector2 * vx)
|
|
{
|
|
vd->v[0] = vb->v[0] + x * vx->v[0];
|
|
vd->v[1] = vb->v[1] + x * vx->v[1];
|
|
}
|
|
|
|
void vec2_lincomb2(Vector2 * vd, const Vector2 * vb, float x, const Vector2 * vx, float y, const Vector2 * vy)
|
|
{
|
|
vd->v[0] = vb->v[0] + x * vx->v[0] + y * vy->v[0];
|
|
vd->v[1] = vb->v[1] + x * vx->v[1] + y * vy->v[1];
|
|
}
|
|
|
|
void vec2_lerp(Vector2 * vd, const Vector2 * v1, const Vector2 * v2, float t)
|
|
{
|
|
vd->v[0] = (1 - t) * v1->v[0] + t * v2->v[0];
|
|
vd->v[1] = (1 - t) * v1->v[1] + t * v2->v[1];
|
|
}
|
|
|
|
bool vec2_is_zero(const Vector2 * v)
|
|
{
|
|
return v->v[0] == 0 && v->v[1] == 0;
|
|
}
|
|
|
|
float vec2_vmul(const Vector2 * v1, const Vector2 * v2)
|
|
{
|
|
return v1->v[0] * v2->v[0] + v1->v[1] * v2->v[1];
|
|
}
|
|
|
|
void vec2_norm(Vector2 * v)
|
|
{
|
|
vec2_scale(v, 1.0 / vec2_length(v));
|
|
}
|
|
|
|
float vec2_length(const Vector2 * v)
|
|
{
|
|
return sqrt(vec2_qlength(v));
|
|
}
|
|
|
|
float vec2_qlength(const Vector2 * v)
|
|
{
|
|
return vec2_vmul(v, v);
|
|
}
|
|
|
|
float vec2_distance(const Vector2 * v1, const Vector2 * v2)
|
|
{
|
|
return sqrt(vec2_qdistance(v1, v2));
|
|
}
|
|
|
|
float vec2_qdistance(const Vector2 * v1, const Vector2 * v2)
|
|
{
|
|
float dx = v1->v[0] - v2->v[0], dy = v1->v[1] - v2->v[1];
|
|
return dx * dx + dy * dy;
|
|
}
|
|
|
|
|
|
|
|
|
|
void mat2_ident(Matrix2 * md)
|
|
{
|
|
md[0] = 1.0;
|
|
md[1] = 0.0;
|
|
md[2] = 0.0;
|
|
md[3] = 1.0;
|
|
}
|
|
|
|
void mat2_add(Matrix2 * md, const Matrix2 * ms)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
md->m[i] += ms->m[i];
|
|
}
|
|
|
|
void mat2_scale(Matrix2 * md, float s)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
md->m[i] *= s;
|
|
}
|
|
|
|
void mat2_mmul(Matrix2 * md, const Matrix2 * ms)
|
|
{
|
|
float m0 = md->m[0] * ms->m[0] + md->m[2] * ms->m[1];
|
|
float m1 = md->m[1] * ms->m[0] + md->m[3] * ms->m[1];
|
|
|
|
float m2 = md->m[0] * ms->m[2] + md->m[2] * ms->m[3];
|
|
float m3 = md->m[1] * ms->m[2] + md->m[3] * ms->m[3];
|
|
|
|
md->m[0] = m0; md->m[1] = m1;
|
|
md->m[2] = m2; md->m[3] = m3;
|
|
}
|
|
|
|
void mat2_rmmul(Matrix2 * md, const Matrix2 * ms)
|
|
{
|
|
float m0 = ms->m[0] * md->m[0] + ms->m[2] * md->m[1];
|
|
float m1 = ms->m[1] * md->m[0] + ms->m[3] * md->m[1];
|
|
|
|
float m2 = ms->m[0] * md->m[2] + ms->m[2] * md->m[3];
|
|
float m3 = ms->m[1] * md->m[2] + ms->m[3] * md->m[3];
|
|
|
|
md->m[0] = m0; md->m[1] = m1;
|
|
md->m[2] = m2; md->m[3] = m3;
|
|
}
|
|
|
|
void mat2_tranpose(Matrix2 * md)
|
|
{
|
|
float t = md->m[1]; md->m[1] = md->m[2]; md->m[2] = t;
|
|
}
|
|
|
|
void vec2_mmul(Vector2 * vd, const Matrix2 * m, const Vector2 * vs)
|
|
{
|
|
float x = m->m[0] * vs->v[0] + m->m[2] * vs->v[1];
|
|
float y = m->m[1] * vs->v[0] + m->m[3] * vs->v[1];
|
|
|
|
vd->v[0] = x;
|
|
vd->v[1] = y;
|
|
}
|
|
|
|
void vec2_mimul(Vector2 * vd, const Matrix2 * m, const Vector2 * vs)
|
|
{
|
|
float det = 1 / mat2_det(m);
|
|
|
|
float x = det * ( m->m[3] * vs->v[0] - m->m[2] * vs->v[1]);
|
|
float y = det * (-m->m[1] * vs->v[0] + m->m[0] * vs->v[1]);
|
|
|
|
vd->v[0] = x;
|
|
vd->v[1] = y;
|
|
}
|
|
|
|
void mat2_set_rotate(Matrix2 * md, float a)
|
|
{
|
|
float c = cos(a);
|
|
float s = sin(a);
|
|
md->m[0] = c; md->m[2] = s;
|
|
md->m[1] =-s; md->m[3] = c;
|
|
|
|
}
|
|
|
|
void mat2_rotate(Matrix2 * md, float a)
|
|
{
|
|
Matrix2 mr;
|
|
mat2_set_rotate(mr, a);
|
|
mat2_rmmul(md, mr);
|
|
}
|
|
|
|
float mat2_det(const Matrix2 * ms)
|
|
{
|
|
return ms->m[0] * ms->m[3] - ms->m[1] * ms->m[2];
|
|
}
|
|
|
|
void mat2_invert(Matrix2 * md, const Matrix2 * ms)
|
|
{
|
|
float det = 1 / mat2_det(ms);
|
|
|
|
float m0 = det * ms->m[3];
|
|
float m1 = -det * ms->m[1];
|
|
float m2 = -det * ms->m[2];
|
|
float m3 = det * ms->m[0];
|
|
|
|
md->m[0] = m0; md->m[1] = m1;
|
|
md->m[2] = m2; md->m[3] = m3;
|
|
}
|
|
|
|
|
|
inline void vec3_set(Vector3 * vd, float x, float y, float z)
|
|
{
|
|
vd->v[0] = x;
|
|
vd->v[1] = y;
|
|
vd->v[2] = z;
|
|
}
|
|
|
|
void vec3_sum(Vector3 * vd, const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] = v1->v[i] + v2->v[i];
|
|
}
|
|
|
|
void vec3_diff(Vector3 * vd, const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] = v1->v[i] - v2->v[i];
|
|
}
|
|
|
|
void vec3_add(Vector3 * vd, const Vector3 * vs)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] += vs->v[i];
|
|
}
|
|
|
|
void vec3_madd(Vector3 * vd, float s, const Vector3 * vs)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] += s * vs->v[i];
|
|
}
|
|
|
|
void vec3_sub(Vector3 * vd, const Vector3 * vs)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] -= vs->v[i];
|
|
}
|
|
|
|
void vec3_scale(Vector3 * vd, float s)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] *= s;
|
|
}
|
|
|
|
void vec3_lincomb(Vector3 * vd, const Vector3 * vb, float x, const Vector3 * vx)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] = vb->v[i] + x * vx->v[i];
|
|
}
|
|
|
|
void vec3_lincomb2(Vector3 * vd, const Vector3 * vb, float x, const Vector3 * vx, float y, const Vector3 * vy)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] = vb->v[i] + x * vx->v[i] + y * vy->v[i];
|
|
}
|
|
|
|
void vec3_lerp(Vector3 * vd, const Vector3 * v1, const Vector3 * v2, float t)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] = (1 - t) * v1->v[i] + t * v2->v[i];
|
|
}
|
|
|
|
bool vec3_is_zero(const Vector3 * v)
|
|
{
|
|
return v->v[0] == 0 && v->v[1] == 0 && v->v[2] == 0;
|
|
}
|
|
|
|
float vec3_vmul(const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
return v1->v[0] * v2->v[0] + v1->v[1] * v2->v[1] + v1->v[2] * v2->v[2];
|
|
}
|
|
|
|
void vec3_norm(Vector3 * v)
|
|
{
|
|
vec3_scale(v, 1.0 / vec3_length(v));
|
|
}
|
|
|
|
float vec3_length(const Vector3 * v)
|
|
{
|
|
return sqrt(vec3_qlength(v));
|
|
}
|
|
|
|
float vec3_qlength(const Vector3 * v)
|
|
{
|
|
return vec3_vmul(v, v);
|
|
}
|
|
|
|
float vec3_distance(const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
return sqrt(vec3_qdistance(v1, v2));
|
|
}
|
|
|
|
float vec3_qdistance(const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
float dx = v1->v[0] - v2->v[0], dy = v1->v[1] - v2->v[1], dz = v1->v[2] - v2->v[2];
|
|
return dx * dx + dy * dy + dz * dz;
|
|
}
|
|
|
|
void vec3_mcadd(Vector3 * vd, const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] += v1->v[i] * v2->v[i];
|
|
}
|
|
|
|
void vec3_mscadd(Vector3 * vd, float s, const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] += s * v1->v[i] * v2->v[i];
|
|
}
|
|
|
|
void vec3_cmul(Vector3 * vd, const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
vd->v[i] = v1->v[i] * v2->v[i];
|
|
}
|
|
|
|
void vec3_xmul(Vector3 * vd, const Vector3 * v1, const Vector3 * v2)
|
|
{
|
|
float tx = v1->v[1] * v2->v[2] - v1->v[2] * v2->v[1];
|
|
float ty = v1->v[2] * v2->v[0] - v1->v[0] * v2->v[2];
|
|
float tz = v1->v[0] * v2->v[1] - v1->v[1] * v2->v[0];
|
|
|
|
vd->v[0] = tx;
|
|
vd->v[1] = ty;
|
|
vd->v[2] = tz;
|
|
}
|
|
|
|
void vec3_mbase(Vector3 * v1, Vector3 * v2, Vector3 * v3)
|
|
{
|
|
if (fabs(v1->v[2]) >= fabs(v1->v[0]) && fabs(v1->v[2]) >= fabs(v1->v[1]))
|
|
{
|
|
v2->v[0] = v1->v[2];
|
|
v2->v[1] = 0;
|
|
v2->v[2] = - v1->v[0];
|
|
}
|
|
else if (fabs(v1->v[1]) >= fabs(v1->v[0]) && fabs(v1->v[1]) >= fabs(v1->v[2]))
|
|
{
|
|
v2->v[0] = v1->v[1];
|
|
v2->v[1] = -v1->v[0];
|
|
v2->v[2] = 0;
|
|
}
|
|
else
|
|
{
|
|
v2->v[0] = -v1->v[1];
|
|
v2->v[1] = v1->v[0];
|
|
v2->v[2] = 0;
|
|
}
|
|
vec3_norm(v2);
|
|
vec3_xmul(v3, v1, v2);
|
|
}
|
|
|
|
void vec3_bend(Vector3 * vd, const Vector3 * vs, float chi1, float chi2)
|
|
{
|
|
float m0 = sqrt(1 - chi2);
|
|
|
|
float hn1x, hn1y, hn1z;
|
|
float hn2x, hn2y, hn2z;
|
|
float hnl;
|
|
|
|
if (fabs(vs->v[2]) >= fabs(vs->v[0]) && fabs(vs->v[2]) >= fabs(vs->v[1]))
|
|
{
|
|
hn1x = vs->v[1]; hn1y = 0; hn1z = - vs->v[0];
|
|
chi2 /= (hn1x * hn1x + hn1z * hn1z);
|
|
hn2x = vs->v[1] * hn1z;
|
|
hn2y = vs->v[2] * hn1x - vs->v[0] * hn1z;
|
|
hn2z = - vs->v[1] * hn1x;
|
|
}
|
|
else
|
|
{
|
|
hn1x = - vs->v[1]; hn1y = vs->v[0]; hn1z = 0;
|
|
chi2 /= (hn1x * hn1x + hn1y * hn1y );
|
|
hn2x = - vs->v[2] * hn1y;
|
|
hn2y = vs->v[2] * hn1x;
|
|
hn2z = vs->v[0] * hn1y - vs->v[1] * hn1x;
|
|
}
|
|
|
|
float m1 = cos(2 * PI * chi1) * sqrt(chi2);
|
|
float m2 = sin(2 * PI * chi1) * sqrt(chi2);
|
|
|
|
Vector3 vt;
|
|
vt.v[0] = vs->v[0] * m0 + hn1x * m1 + hn2x * m2;
|
|
vt.v[1] = vs->v[1] * m0 + hn1y * m1 + hn2y * m2;
|
|
vt.v[2] = vs->v[2] * m0 + hn1z * m1 + hn2z * m2;
|
|
*vd = vt;
|
|
}
|
|
|
|
|
|
|
|
void mat3_ident(Matrix3 * m)
|
|
{
|
|
m->m[0] = 1.0;
|
|
m->m[1] = 0.0;
|
|
m->m[2] = 0.0;
|
|
|
|
m->m[3] = 0.0;
|
|
m->m[4] = 1.0;
|
|
m->m[5] = 0.0;
|
|
|
|
m->m[6] = 0.0;
|
|
m->m[7] = 0.0;
|
|
m->m[8] = 1.0;
|
|
}
|
|
|
|
void mat3_scale(Matrix3 * md, float s)
|
|
{
|
|
for(char i=0; i<9; i++)
|
|
md->m[i] *= s;
|
|
}
|
|
|
|
void mat3_add(Matrix3 * md, const Matrix3 * ms)
|
|
{
|
|
for(char i=0; i<9; i++)
|
|
md->m[i] += md->m[i];
|
|
}
|
|
|
|
void mat3_mmul(Matrix3 * md, const Matrix3 * ms)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
{
|
|
char j = 3 * i;
|
|
|
|
float m0 = md->m[i + 0] * ms->m[0] + md->m[i + 3] * ms->m[1] + md->m[i + 6] * ms->m[2];
|
|
float m3 = md->m[i + 0] * ms->m[3] + md->m[i + 3] * ms->m[4] + md->m[i + 6] * ms->m[5];
|
|
float m6 = md->m[i + 0] * ms->m[6] + md->m[i + 3] * ms->m[7] + md->m[i + 6] * ms->m[8];
|
|
|
|
md->m[i + 0] = m0; md->m[i + 3] = m3; md->m[i + 6] = m6;
|
|
}
|
|
}
|
|
|
|
void mat3_rmmul(Matrix3 * md, const Matrix3 * ms)
|
|
{
|
|
for(char i=0; i<9; i+=3)
|
|
{
|
|
float m0 = md->m[i + 0] * ms->m[0] + md->m[i + 1] * ms->m[3] + md->m[i + 2] * ms->m[6];
|
|
float m1 = md->m[i + 0] * ms->m[1] + md->m[i + 1] * ms->m[4] + md->m[i + 2] * ms->m[7];
|
|
float m2 = md->m[i + 0] * ms->m[2] + md->m[i + 1] * ms->m[5] + md->m[i + 2] * ms->m[8];
|
|
|
|
md->m[i + 0] = m0; md->m[i + 1] = m1; md->m[i + 2] = m2;
|
|
}
|
|
}
|
|
|
|
void mat3_transpose(Matrix3 * md, const Matrix3 * ms)
|
|
{
|
|
float t
|
|
t = ms->m[1]; md->m[1] = ms->m[3]; md->m[3] = t; md->m[0] = ms->m[0];
|
|
t = ms->m[2]; md->m[2] = ms->m[6]; md->m[6] = t; md->m[4] = ms->m[4];
|
|
t = ms->m[5]; md->m[5] = ms->m[7]; md->m[7] = t; md->m[8] = ms->m[8];
|
|
}
|
|
|
|
float mat3_det(const Matrix3 * ms)
|
|
{
|
|
return ms->m[0] * ms->m[4] * ms->m[8] +
|
|
ms->m[1] * ms->m[5] * ms->m[6] +
|
|
ms->m[2] * ms->m[3] * ms->m[7] -
|
|
ms->m[0] * ms->m[5] * ms->m[7] -
|
|
ms->m[2] * ms->m[4] * ms->m[6] -
|
|
ms->m[1] * ms->m[3] * ms->m[8];
|
|
}
|
|
|
|
void mat3_invert(Matrix3 * md, const Matrix3 * ms)
|
|
{
|
|
float det = 1 / mat3_det(ms);
|
|
|
|
Matrix3 mt;
|
|
mt.m[0] = det * (ms->m[4] * ms->m[8] - ms->m[5] * ms->m[7]);
|
|
mt.m[1] = det * (ms->m[7] * ms->m[2] - ms->m[1] * ms->m[8]);
|
|
mt.m[2] = det * (ms->m[1] * ms->m[5] - ms->m[4] * ms->m[2]);
|
|
|
|
mt.m[3] = det * (ms->m[6] * ms->m[5] - ms->m[3] * ms->m[8]);
|
|
mt.m[4] = det * (ms->m[0] * ms->m[8] - ms->m[6] * ms->m[2]);
|
|
mt.m[5] = det * (ms->m[3] * ms->m[2] - ms->m[0] * ms->m[5]);
|
|
|
|
mt.m[6] = det * (ms->m[3] * ms->m[7] - ms->m[6] * ms->m[4]);
|
|
mt.m[7] = det * (ms->m[6] * ms->m[1] - ms->m[0] * ms->m[7]);
|
|
mt.m[8] = det * (ms->m[0] * ms->m[4] - ms->m[3] * ms->m[1]);
|
|
|
|
*md = mt;
|
|
}
|
|
|
|
void vec3_mmul(Vector3 * vd, const Matrix3 * m, const Vector3 * vs)
|
|
{
|
|
Vector3 vt;
|
|
for(char i=0; i<3; i++)
|
|
vt.v[i] = m->m[i] * vs->v[0] + m->m[3 + i] * vs->v[1] + m->m[6 + i] * vs->v[2];
|
|
*vd = vt;
|
|
}
|
|
|
|
void mat3_set_rotate_x(Matrix3 * m, float a)
|
|
{
|
|
float c = cos(a);
|
|
float s = sin(a);
|
|
m->m[0] = 1; m->m[3] = 0; m->m[6] = 0;
|
|
m->m[1] = 0; m->m[4] = c; m->m[7] = s;
|
|
m->m[2] = 0; m->m[5] =-s; m->m[8] = c;
|
|
}
|
|
|
|
void mat3_set_rotate_y(Matrix3 * m, float a)
|
|
{
|
|
float c = cos(a);
|
|
float s = sin(a);
|
|
m->m[0] = c; m->m[3] = 0; m->m[6] = s;
|
|
m->m[1] = 0; m->m[4] = 1; m->m[7] = 0;
|
|
m->m[2] =-s; m->m[5] = 0; m->m[8] = c;
|
|
}
|
|
|
|
|
|
void mat3_set_rotate_z(Matrix3 * m, float a)
|
|
{
|
|
float c = cos(a);
|
|
float s = sin(a);
|
|
m->m[0] = c; m->m[3] =-s; m->m[6] = 0;
|
|
m->m[1] = s; m->m[4] = c; m->m[7] = 0;
|
|
m->m[2] = 0; m->m[5] = 0; m->m[8] = 1;
|
|
}
|
|
|
|
void mat3_set_rotate(Matrix3 * m, const Vector3 * v, float a)
|
|
{
|
|
float co = cos(a);
|
|
float si = sin(a);
|
|
float ico = (1- co);
|
|
|
|
m->m[0] = co + v->v[0]*v->v[0]*ico;
|
|
m->m[1] = v->v[1]*v->v[0]*ico+v->v[2]*si;
|
|
m->m[2] = v->v[2]*v->v[0]*ico-v->v[1]*si;
|
|
|
|
m->m[3] = v->v[0]*v->v[1]*ico - v->v[2]*si;
|
|
m->m[4] = co + v->v[1]*v->v[1]*ico;
|
|
m->m[5] = v->v[2]*v->v[1]*ico+v->v[0]*si;
|
|
|
|
m->m[6] = v->v[0]*v->v[2]*ico + v->v[1]*si;
|
|
m->m[7] = v->v[1]*v->v[2]*ico - v->v[0]*si;
|
|
m->m[8] = co + v->v[2]*v->v[2]*ico;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
inline void vec4_set(Vector4 * vd, float x, float y, float z, float w)
|
|
{
|
|
vd->v[0] = x;
|
|
vd->v[1] = y;
|
|
vd->v[2] = z;
|
|
vd->v[3] = w;
|
|
}
|
|
|
|
|
|
void vec4_sum(Vector4 * vd, const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] = v1->v[i] + v2->v[i];
|
|
}
|
|
|
|
void vec4_diff(Vector4 * vd, const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] = v1->v[i] - v2->v[i];
|
|
}
|
|
|
|
void vec4_add(Vector4 * vd, const Vector4 * vs)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] += vs->v[i];
|
|
}
|
|
|
|
void vec4_madd(Vector4 * vd, float s, const Vector4 * vs)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] += s * vs->v[i];
|
|
}
|
|
|
|
void vec4_sub(Vector4 * vd, const Vector4 * vs)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] -= vs->v[i];
|
|
}
|
|
|
|
void vec4_scale(Vector4 * vd, float s)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] *= s;
|
|
}
|
|
|
|
void vec4_lincomb(Vector4 * vd, const Vector4 * vb, float x, const Vector4 * vx)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] = vb->v[i] + x * vx->v[i];
|
|
}
|
|
|
|
void vec4_lincomb2(Vector4 * vd, const Vector4 * vb, float x, const Vector4 * vx, float y, const Vector4 * vy)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] = vb->v[i] + x * vx->v[i] + y * vy->v[i];
|
|
}
|
|
|
|
void vec4_lerp(Vector4 * vd, const Vector4 * v1, const Vector4 * v2, float t)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] = (1 - t) * v1->v[i] + t * v2->v[i];
|
|
}
|
|
|
|
bool vec4_is_zero(const Vector4 * v)
|
|
{
|
|
return v->v[0] == 0 && v->v[1] == 0 && v->v[2] == 0 && v->v[3] == 0;
|
|
}
|
|
|
|
float vec4_vmul(const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
return v1->v[0] * v2->v[0] + v1->v[1] * v2->v[1] + v1->v[2] * v2->v[2] + v1->v[3] * v2->v[3];
|
|
}
|
|
|
|
void vec4_norm(Vector4 * v)
|
|
{
|
|
vec4_scale(v, 1.0 / vec4_length(v));
|
|
}
|
|
|
|
float vec4_length(const Vector4 * v)
|
|
{
|
|
return sqrt(vec4_qlength(v));
|
|
}
|
|
|
|
float vec4_qlength(const Vector4 * v)
|
|
{
|
|
return vec4_vmul(v, v);
|
|
}
|
|
|
|
float vec4_distance(const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
return sqrt(vec4_qdistance(v1, v2));
|
|
}
|
|
|
|
float vec4_qdistance(const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
float dx = v1->v[0] - v2->v[0], dy = v1->v[1] - v2->v[1], dz = v1->v[2] - v2->v[2], dw = v1->v[3] - v2->v[3];
|
|
return dx * dx + dy * dy + dz * dz + dw * dw;
|
|
}
|
|
|
|
void vec4_mcadd(Vector4 * vd, const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] += v1->v[i] * v2->v[i];
|
|
}
|
|
|
|
void vec4_mscadd(Vector4 * vd, float s, const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] += s * v1->v[i] * v2->v[i];
|
|
}
|
|
|
|
void vec4_cmul(Vector4 * vd, const Vector4 * v1, const Vector4 * v2)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
vd->v[i] = v1->v[i] * v2->v[i];
|
|
}
|
|
|
|
|
|
|
|
void mat4_ident(Matrix4 * m)
|
|
{
|
|
for(char i=0; i<4; i++)
|
|
for(char j=0; j<4; j++)
|
|
m->m[4 * i + j] = (i == j) ? 1 : 0;
|
|
}
|
|
|
|
void mat4_from_vec4(Matrix4 * m, const Vector4 * vx, const Vector4 * vy, const Vector4 * vz, const Vector4 * vw)
|
|
{
|
|
m->m[ 0] = vx->v[0];
|
|
m->m[ 1] = vx->v[1];
|
|
m->m[ 2] = vx->v[2];
|
|
m->m[ 3] = vx->v[3];
|
|
|
|
m->m[ 4] = vy->v[0];
|
|
m->m[ 5] = vy->v[1];
|
|
m->m[ 6] = vy->v[2];
|
|
m->m[ 7] = vy->v[3];
|
|
|
|
m->m[ 8] = vz->v[0];
|
|
m->m[ 9] = vz->v[1];
|
|
m->m[10] = vz->v[2];
|
|
m->m[11] = vz->v[3];
|
|
|
|
m->m[12] = vw->v[0];
|
|
m->m[13] = vw->v[1];
|
|
m->m[14] = vw->v[2];
|
|
m->m[15] = vw->v[3];
|
|
}
|
|
|
|
void mat4_from_vec3(Matrix4 * m, const Vector3 * vx, const Vector3 * vy, const Vector3 * vz, const Vector3 * vw)
|
|
{
|
|
m->m[ 0] = vx->v[0];
|
|
m->m[ 1] = vx->v[1];
|
|
m->m[ 2] = vx->v[2];
|
|
m->m[ 3] = 0;
|
|
|
|
m->m[ 4] = vy->v[0];
|
|
m->m[ 5] = vy->v[1];
|
|
m->m[ 6] = vy->v[2];
|
|
m->m[ 7] = 0;
|
|
|
|
m->m[ 8] = vz->v[0];
|
|
m->m[ 9] = vz->v[1];
|
|
m->m[10] = vz->v[2];
|
|
m->m[11] = 0;
|
|
|
|
if (vw)
|
|
{
|
|
m->m[12] = vw->v[0];
|
|
m->m[13] = vw->v[1];
|
|
m->m[14] = vw->v[2];
|
|
m->m[15] = 1;
|
|
}
|
|
else
|
|
{
|
|
m->m[12] = 0;
|
|
m->m[13] = 0;
|
|
m->m[14] = 0;
|
|
m->m[15] = 0;
|
|
}
|
|
}
|
|
|
|
void mat4_make_perspective(Matrix4 * m, float fieldOfViewInRadians, float aspect, float near, float far)
|
|
{
|
|
float f = tan(PI * 0.5 - 0.5 * fieldOfViewInRadians);
|
|
float rangeInv = 1.0 / (far - near);
|
|
|
|
m->m[ 0] = f / aspect;
|
|
m->m[ 1] = 0;
|
|
m->m[ 2] = 0;
|
|
m->m[ 3] = 0;
|
|
|
|
m->m[ 4] = 0;
|
|
m->m[ 5] = f;
|
|
m->m[ 6] = 0;
|
|
m->m[ 7] = 0;
|
|
|
|
m->m[ 8] = 0;
|
|
m->m[ 9] = 0;
|
|
m->m[10] = (near + far) * rangeInv;
|
|
m->m[11] = 1;
|
|
|
|
m->m[12] = 0;
|
|
m->m[13] = 0;
|
|
m->m[14] = near * far * rangeInv * -2;
|
|
m->m[15] = 0;
|
|
}
|
|
|
|
void mat4_make_view(Matrix4 * m, const Vector3 * pos, const Vector3 * target, const Vector3 * up)
|
|
{
|
|
Vector3 vx, vy, vz;
|
|
|
|
vec3_diff(&vz, target, pos); vec3_norm(&vz);
|
|
vec3_xmul(&vx, up, &vz);
|
|
vec3_xmul(&vy, &vz, &vx);
|
|
|
|
mat4_from_vec3(m, &vx, &vy, &vz, pos);
|
|
mat4_invert(m);
|
|
}
|
|
|
|
|
|
void mat4_scale(Matrix4 * md, float s)
|
|
{
|
|
for(char i=0; i<16; i++)
|
|
md->m[i] *= s;
|
|
}
|
|
|
|
void mat4_add(Matrix4 * md, const Matrix4 * ms)
|
|
{
|
|
for(char i=0; i<16; i++)
|
|
md->m[i] += md->m[i];
|
|
}
|
|
|
|
void mat4_mmul(Matrix4 * md, const Matrix4 * ms)
|
|
{
|
|
float ma[4];
|
|
for(char i=0; i<4; i++)
|
|
{
|
|
for(char j=0; j<4; j++)
|
|
ma[j] = md->m[i + 0] * ms->m[4 * j] + md->m[i + 4] * ms->m[4 * j + 1] + md->m[i + 8] * ms->m[4 * j + 2] + md->m[i + 12] * ms->m[4 * j + 3];
|
|
for(char j=0; j<4; j++)
|
|
md->m[i + 4 * j] = ma[j];
|
|
}
|
|
}
|
|
|
|
void mat4_rmmul(Matrix4 * md, const Matrix4 * ms)
|
|
{
|
|
float ma[4];
|
|
for(char i=0; i<16; i+=4)
|
|
{
|
|
for(char j=0; j<4; j++)
|
|
ma[j] = md->m[i + 0] * ms->m[j] + md->m[i + 1] * ms->m[j + 4] + md->m[i + 2] * ms->m[j + 8] + md->m[i + 3] * ms->m[j + 12];
|
|
|
|
for(char j=0; j<4; j++)
|
|
md->m[i + j] = ma[j];
|
|
}
|
|
}
|
|
|
|
float mat4_det(const Matrix4 * m)
|
|
{
|
|
return m->m[ 0] * m->m[ 5] * m->m[10] * m->m[15] + m->m[ 0] * m->m[ 9] * m->m[14] * m->m[ 7] + m->m[ 0] * m->m[13] * m->m[ 6] * m->m[11] +
|
|
m->m[ 4] * m->m[ 1] * m->m[14] * m->m[11] + m->m[ 4] * m->m[ 9] * m->m[ 2] * m->m[15] + m->m[ 4] * m->m[13] * m->m[10] * m->m[ 3] +
|
|
m->m[ 8] * m->m[ 1] * m->m[ 6] * m->m[15] + m->m[ 8] * m->m[ 5] * m->m[14] * m->m[ 3] + m->m[ 8] * m->m[13] * m->m[ 2] * m->m[ 7] +
|
|
m->m[12] * m->m[ 1] * m->m[10] * m->m[ 7] + m->m[12] * m->m[ 5] * m->m[ 2] * m->m[11] + m->m[12] * m->m[ 9] * m->m[ 6] * m->m[ 3] -
|
|
m->m[ 0] * m->m[ 5] * m->m[14] * m->m[11] - m->m[ 0] * m->m[ 9] * m->m[ 6] * m->m[15] - m->m[ 0] * m->m[13] * m->m[10] * m->m[ 7] -
|
|
m->m[ 4] * m->m[ 1] * m->m[10] * m->m[15] - m->m[ 4] * m->m[ 9] * m->m[14] * m->m[ 3] - m->m[ 4] * m->m[13] * m->m[ 2] * m->m[11] -
|
|
m->m[ 8] * m->m[ 1] * m->m[14] * m->m[ 7] - m->m[ 8] * m->m[ 5] * m->m[ 2] * m->m[15] - m->m[ 8] * m->m[13] * m->m[ 6] * m->m[ 3] -
|
|
m->m[12] * m->m[ 1] * m->m[ 6] * m->m[11] - m->m[12] * m->m[ 5] * m->m[10] * m->m[ 3] - m->m[12] * m->m[ 9] * m->m[ 2] * m->m[ 7];
|
|
}
|
|
|
|
void mat4_invert(Matrix4 * md, const Matrix4 * ms)
|
|
{
|
|
float det = 1 / mat4_det(ms);
|
|
|
|
Matrix4 mt;
|
|
|
|
mt.m[ 0] = det * (ms->m[ 5] * ms->m[10] * ms->m[15]+ms->m[ 9] * ms->m[14] * ms->m[ 7]+ms->m[13] * ms->m[ 6] * ms->m[11]-ms->m[ 5] * ms->m[14] * ms->m[11]-ms->m[ 9] * ms->m[ 6] * ms->m[15]-ms->m[13] * ms->m[10] * ms->m[ 7]);
|
|
mt.m[ 4] = det * (ms->m[ 4] * ms->m[14] * ms->m[11]+ms->m[ 8] * ms->m[ 6] * ms->m[15]+ms->m[12] * ms->m[10] * ms->m[ 7]-ms->m[ 4] * ms->m[10] * ms->m[15]-ms->m[ 8] * ms->m[14] * ms->m[ 7]-ms->m[12] * ms->m[ 6] * ms->m[11]);
|
|
mt.m[ 8] = det * (ms->m[ 4] * ms->m[ 9] * ms->m[15]+ms->m[ 8] * ms->m[13] * ms->m[ 7]+ms->m[12] * ms->m[ 5] * ms->m[11]-ms->m[ 4] * ms->m[13] * ms->m[11]-ms->m[ 8] * ms->m[ 5] * ms->m[15]-ms->m[12] * ms->m[ 9] * ms->m[ 7]);
|
|
mt.m[12] = det * (ms->m[ 4] * ms->m[13] * ms->m[10]+ms->m[ 8] * ms->m[ 5] * ms->m[14]+ms->m[12] * ms->m[ 9] * ms->m[ 6]-ms->m[ 4] * ms->m[ 9] * ms->m[14]-ms->m[ 8] * ms->m[13] * ms->m[ 6]-ms->m[12] * ms->m[ 5] * ms->m[10]);
|
|
|
|
mt.m[ 1] = det * (ms->m[ 1] * ms->m[14] * ms->m[11]+ms->m[ 9] * ms->m[ 2] * ms->m[15]+ms->m[13] * ms->m[10] * ms->m[ 3]-ms->m[ 1] * ms->m[10] * ms->m[15]-ms->m[ 9] * ms->m[14] * ms->m[ 3]-ms->m[13] * ms->m[ 2] * ms->m[11]);
|
|
mt.m[ 5] = det * (ms->m[ 0] * ms->m[10] * ms->m[15]+ms->m[ 8] * ms->m[14] * ms->m[ 3]+ms->m[12] * ms->m[ 2] * ms->m[11]-ms->m[ 0] * ms->m[14] * ms->m[11]-ms->m[ 8] * ms->m[ 2] * ms->m[15]-ms->m[12] * ms->m[10] * ms->m[ 3]);
|
|
mt.m[ 9] = det * (ms->m[ 0] * ms->m[13] * ms->m[11]+ms->m[ 8] * ms->m[ 1] * ms->m[15]+ms->m[12] * ms->m[ 9] * ms->m[ 3]-ms->m[ 0] * ms->m[ 9] * ms->m[15]-ms->m[ 8] * ms->m[13] * ms->m[ 3]-ms->m[12] * ms->m[ 1] * ms->m[11]);
|
|
mt.m[13] = det * (ms->m[ 0] * ms->m[ 9] * ms->m[14]+ms->m[ 8] * ms->m[13] * ms->m[ 2]+ms->m[12] * ms->m[ 1] * ms->m[10]-ms->m[ 0] * ms->m[13] * ms->m[10]-ms->m[ 8] * ms->m[ 1] * ms->m[14]-ms->m[12] * ms->m[ 9] * ms->m[ 2]);
|
|
|
|
mt.m[ 2] = det * (ms->m[ 1] * ms->m[ 6] * ms->m[15]+ms->m[ 5] * ms->m[14] * ms->m[ 3]+ms->m[13] * ms->m[ 2] * ms->m[ 7]-ms->m[ 1] * ms->m[14] * ms->m[ 7]-ms->m[ 5] * ms->m[ 2] * ms->m[15]-ms->m[13] * ms->m[ 6] * ms->m[ 3]);
|
|
mt.m[ 6] = det * (ms->m[ 0] * ms->m[14] * ms->m[ 7]+ms->m[ 4] * ms->m[ 2] * ms->m[15]+ms->m[12] * ms->m[ 6] * ms->m[ 3]-ms->m[ 0] * ms->m[ 6] * ms->m[15]-ms->m[ 4] * ms->m[14] * ms->m[ 3]-ms->m[12] * ms->m[ 2] * ms->m[ 7]);
|
|
mt.m[10] = det * (ms->m[ 0] * ms->m[ 5] * ms->m[15]+ms->m[ 4] * ms->m[13] * ms->m[ 3]+ms->m[12] * ms->m[ 1] * ms->m[ 7]-ms->m[ 0] * ms->m[13] * ms->m[ 7]-ms->m[ 4] * ms->m[ 1] * ms->m[15]-ms->m[12] * ms->m[ 5] * ms->m[ 3]);
|
|
mt.m[14] = det * (ms->m[ 0] * ms->m[13] * ms->m[ 6]+ms->m[ 4] * ms->m[ 1] * ms->m[14]+ms->m[12] * ms->m[ 5] * ms->m[ 2]-ms->m[ 0] * ms->m[ 5] * ms->m[14]-ms->m[ 4] * ms->m[13] * ms->m[ 2]-ms->m[12] * ms->m[ 1] * ms->m[ 6]);
|
|
|
|
mt.m[ 3] = det * (ms->m[ 1] * ms->m[10] * ms->m[ 7]+ms->m[ 5] * ms->m[ 2] * ms->m[11]+ms->m[ 9] * ms->m[ 6] * ms->m[ 3]-ms->m[ 1] * ms->m[ 6] * ms->m[11]-ms->m[ 5] * ms->m[10] * ms->m[ 3]-ms->m[ 9] * ms->m[ 2] * ms->m[ 7]);
|
|
mt.m[ 7] = det * (ms->m[ 0] * ms->m[ 6] * ms->m[11]+ms->m[ 4] * ms->m[10] * ms->m[ 3]+ms->m[ 8] * ms->m[ 2] * ms->m[ 7]-ms->m[ 0] * ms->m[10] * ms->m[ 7]-ms->m[ 4] * ms->m[ 2] * ms->m[11]-ms->m[ 8] * ms->m[ 6] * ms->m[ 3]);
|
|
mt.m[11] = det * (ms->m[ 0] * ms->m[ 9] * ms->m[ 7]+ms->m[ 4] * ms->m[ 1] * ms->m[11]+ms->m[ 8] * ms->m[ 5] * ms->m[ 3]-ms->m[ 0] * ms->m[ 5] * ms->m[11]-ms->m[ 4] * ms->m[ 9] * ms->m[ 3]-ms->m[ 8] * ms->m[ 1] * ms->m[ 7]);
|
|
mt.m[15] = det * (ms->m[ 0] * ms->m[ 5] * ms->m[10]+ms->m[ 4] * ms->m[ 9] * ms->m[ 2]+ms->m[ 8] * ms->m[ 1] * ms->m[ 6]-ms->m[ 0] * ms->m[ 9] * ms->m[ 6]-ms->m[ 4] * ms->m[ 1] * ms->m[10]-ms->m[ 8] * ms->m[ 5] * ms->m[ 2]);
|
|
|
|
*md = mt;
|
|
}
|
|
|
|
|
|
void vec4_mmul(Vector4 * vd, const Matrix4 * m, const Vector4 * vs)
|
|
{
|
|
Vector4 vt;
|
|
for(char i=0; i<4; i++)
|
|
vt.v[i] = m->m[0 + i] * vs->v[0] + m->m[4 + i] * vs->v[1] + m->m[8 + i] * vs->v[2] + m->m[12 + i] * vs->v[3];
|
|
*vd = vt;
|
|
}
|
|
|
|
void vec3_mmulp(Vector3 * vd, const Matrix4 * m, const Vector3 * vs)
|
|
{
|
|
Vector3 vt;
|
|
for(char i=0; i<3; i++)
|
|
vt.v[i] = m->m[0 + i] * vs->v[0] + m->m[4 + i] * vs->v[1] + m->m[8 + i] * vs->v[2] + m->m[12 + i];
|
|
*vd = vt;
|
|
}
|
|
|
|
void vec3_mmuld(Vector3 * vd, const Matrix4 * m, const Vector3 * vs)
|
|
{
|
|
Vector3 vt;
|
|
for(char i=0; i<3; i++)
|
|
vt.v[i] = m->m[0 + i] * vs->v[0] + m->m[4 + i] * vs->v[1] + m->m[8 + i] * vs->v[2];
|
|
*vd = vt;
|
|
}
|
|
|
|
|
|
void mat4_set_rotate_x(Matrix4 * m, float a)
|
|
{
|
|
float c = cos(a);
|
|
float s = sin(a);
|
|
m->m[ 0] = 1; m->m[ 4] = 0; m->m[ 8] = 0; m->m[12] = 0;
|
|
m->m[ 1] = 0; m->m[ 5] = c; m->m[ 9] = s; m->m[13] = 0;
|
|
m->m[ 2] = 0; m->m[ 6] =-s; m->m[10] = c; m->m[14] = 0;
|
|
m->m[ 3] = 0; m->m[ 7] = 0; m->m[11] = 0; m->m[15] = 1;
|
|
}
|
|
|
|
void mat4_set_rotate_y(Matrix4 * m, float a)
|
|
{
|
|
float c = cos(a);
|
|
float s = sin(a);
|
|
m->m[ 0] = c; m->m[ 4] = 0; m->m[ 8] = s; m->m[12] = 0;
|
|
m->m[ 1] = 0; m->m[ 5] = 1; m->m[ 9] = 0; m->m[13] = 0;
|
|
m->m[ 2] =-s; m->m[ 6] = 0; m->m[10] = c; m->m[14] = 0;
|
|
m->m[ 3] = 0; m->m[ 7] = 0; m->m[11] = 0; m->m[15] = 1;
|
|
}
|
|
|
|
|
|
void mat4_set_rotate_z(Matrix4 * m, float a)
|
|
{
|
|
float c = cos(a);
|
|
float s = sin(a);
|
|
m->m[ 0] = c; m->m[ 4] =-s; m->m[ 8] = 0; m->m[12] = 0;
|
|
m->m[ 1] = s; m->m[ 5] = c; m->m[ 9] = 0; m->m[13] = 0;
|
|
m->m[ 2] = 0; m->m[ 6] = 0; m->m[10] = 1; m->m[14] = 0;
|
|
m->m[ 3] = 0; m->m[ 7] = 0; m->m[11] = 0; m->m[15] = 1;
|
|
}
|
|
|
|
void mat4_set_rotate(Matrix4 * m, const Vector3 * v, float a)
|
|
{
|
|
float co = cos(a);
|
|
float si = sin(a);
|
|
float ico = (1- co);
|
|
|
|
m->m[ 0] = co + v->v[0]*v->v[0]*ico;
|
|
m->m[ 1] = v->v[1]*v->v[0]*ico+v->v[2]*si;
|
|
m->m[ 2] = v->v[2]*v->v[0]*ico-v->v[1]*si;
|
|
m->m[ 3] = 0;
|
|
|
|
m->m[ 4] = v->v[0]*v->v[1]*ico - v->v[2]*si;
|
|
m->m[ 5] = co + v->v[1]*v->v[1]*ico;
|
|
m->m[ 6] = v->v[2]*v->v[1]*ico+v->v[0]*si;
|
|
m->m[ 7] = 0;
|
|
|
|
m->m[ 8] = v->v[0]*v->v[2]*ico + v->v[1]*si;
|
|
m->m[ 9] = v->v[1]*v->v[2]*ico - v->v[0]*si;
|
|
m->m[10] = co + v->v[2]*v->v[2]*ico;
|
|
m->m[11] = 0;
|
|
|
|
m->m[12] = 0;
|
|
m->m[13] = 0;
|
|
m->m[14] = 0;
|
|
m->m[15] = 1;
|
|
}
|
|
|
|
|
|
void mat4_set_translate(Matrix4 * m, const Vector3 * v)
|
|
{
|
|
mat4_ident(m);
|
|
m->m[12] = v->v[0];
|
|
m->m[13] = v->v[1];
|
|
m->m[14] = v->v[2];
|
|
}
|
|
|
|
void mat4_set_scale(Matrix4 * m, float s)
|
|
{
|
|
mat4_ident(m);
|
|
m->m[ 0] = s;
|
|
m->m[ 5] = s;
|
|
m->m[10] = s;
|
|
}
|
|
|
|
|
|
void vec3_project(Vector3 * vd, const Matrix4 * m, const Vector3 * vs)
|
|
{
|
|
vec3_mmulp(vd, m, vs);
|
|
vd->v[0] /= vd->v[2];
|
|
vd->v[1] /= vd->v[2];
|
|
}
|
|
|
|
|
|
|
|
void f12mat3_ident(F12Matrix3 * m)
|
|
{
|
|
m->m[0] = FIX12_ONE;
|
|
m->m[1] = 0;
|
|
m->m[2] = 0;
|
|
|
|
m->m[3] = 0;
|
|
m->m[4] = FIX12_ONE;
|
|
m->m[5] = 0;
|
|
|
|
m->m[6] = 0;
|
|
m->m[7] = 0;
|
|
m->m[8] = FIX12_ONE;
|
|
}
|
|
|
|
void f12mat3_mmul(F12Matrix3 * md, const F12Matrix3 * ms)
|
|
{
|
|
for(char i=0; i<3; i++)
|
|
{
|
|
char j = 3 * i;
|
|
|
|
int m0 = lmul4f12s(md->m[i + 0], ms->m[0]) + lmul4f12s(md->m[i + 3], ms->m[1]) + lmul4f12s(md->m[i + 6], ms->m[2]);
|
|
int m3 = lmul4f12s(md->m[i + 0], ms->m[3]) + lmul4f12s(md->m[i + 3], ms->m[4]) + lmul4f12s(md->m[i + 6], ms->m[5]);
|
|
int m6 = lmul4f12s(md->m[i + 0], ms->m[6]) + lmul4f12s(md->m[i + 3], ms->m[7]) + lmul4f12s(md->m[i + 6], ms->m[8]);
|
|
|
|
md->m[i + 0] = m0; md->m[i + 3] = m3; md->m[i + 6] = m6;
|
|
}
|
|
}
|
|
|
|
void f12mat3_rmmul(F12Matrix3 * md, const F12Matrix3 * ms)
|
|
{
|
|
for(char i=0; i<9; i+=3)
|
|
{
|
|
int m0 = lmul4f12s(md->m[i + 0], ms->m[0]) + lmul4f12s(md->m[i + 1], ms->m[3]) + lmul4f12s(md->m[i + 2], ms->m[6]);
|
|
int m1 = lmul4f12s(md->m[i + 0], ms->m[1]) + lmul4f12s(md->m[i + 1], ms->m[4]) + lmul4f12s(md->m[i + 2], ms->m[7]);
|
|
int m2 = lmul4f12s(md->m[i + 0], ms->m[2]) + lmul4f12s(md->m[i + 1], ms->m[5]) + lmul4f12s(md->m[i + 2], ms->m[8]);
|
|
|
|
md->m[i + 0] = m0; md->m[i + 1] = m1; md->m[i + 2] = m2;
|
|
}
|
|
}
|
|
|
|
void f12mat3_set_rotate_x(F12Matrix3 * m, float a)
|
|
{
|
|
int c = (int)(FIX12_ONE * cos(a) + 0.5);
|
|
int s = (int)(FIX12_ONE * sin(a) + 0.5);
|
|
m->m[0] = FIX12_ONE; m->m[3] = 0; m->m[6] = 0;
|
|
m->m[1] = 0; m->m[4] = c; m->m[7] = s;
|
|
m->m[2] = 0; m->m[5] =-s; m->m[8] = c;
|
|
}
|
|
|
|
void f12mat3_set_rotate_y(F12Matrix3 * m, float a)
|
|
{
|
|
int c = (int)(FIX12_ONE * cos(a) + 0.5);
|
|
int s = (int)(FIX12_ONE * sin(a) + 0.5);
|
|
m->m[0] = c; m->m[3] = 0; m->m[6] = s;
|
|
m->m[1] = 0; m->m[4] = FIX12_ONE; m->m[7] = 0;
|
|
m->m[2] =-s; m->m[5] = 0; m->m[8] = c;
|
|
}
|
|
|
|
|
|
void f12mat3_set_rotate_z(F12Matrix3 * m, float a)
|
|
{
|
|
int c = (int)(FIX12_ONE * cos(a) + 0.5);
|
|
int s = (int)(FIX12_ONE * sin(a) + 0.5);
|
|
m->m[0] = c; m->m[3] =-s; m->m[6] = 0;
|
|
m->m[1] = s; m->m[4] = c; m->m[7] = 0;
|
|
m->m[2] = 0; m->m[5] = 0; m->m[8] = FIX12_ONE;
|
|
}
|
|
|
|
void f12vec3_mmul(F12Vector3 * vd, const F12Matrix3 * m, const F12Vector3 * vs)
|
|
{
|
|
F12Vector3 vt;
|
|
for(char i=0; i<3; i++)
|
|
vt.v[i] = lmul4f12s(m->m[i], vs->v[0]) + lmul4f12s(m->m[3 + i], vs->v[1]) + lmul4f12s(m->m[6 + i], vs->v[2]);
|
|
*vd = vt;
|
|
}
|
|
|
|
|