You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
836 lines
23 KiB
C
836 lines
23 KiB
C
// This is a rewrite version (in C) from Horde3D (utMath.h) , http://www.horde3d.org
|
|
// Math library
|
|
//
|
|
// Coordinate system is right-handed with positive y as up axis
|
|
//
|
|
|
|
#ifndef ejoy3d_math_h
|
|
#define ejoy3d_math_h
|
|
|
|
#include <math.h>
|
|
#include <float.h>
|
|
#include <stddef.h>
|
|
|
|
struct vector3
|
|
{
|
|
float x, y, z;
|
|
};
|
|
|
|
struct vector4
|
|
{
|
|
float x, y, z, w;
|
|
};
|
|
|
|
struct quaternion
|
|
{
|
|
float x, y, z, w;
|
|
};
|
|
|
|
union matrix44
|
|
{
|
|
float c[4][4];
|
|
float x[16];
|
|
};
|
|
|
|
struct plane
|
|
{
|
|
struct vector3 normal;
|
|
float dist;
|
|
};
|
|
|
|
// vector
|
|
|
|
static inline float *
|
|
vector3_array(struct vector3 *v)
|
|
{
|
|
return (float *)v;
|
|
}
|
|
|
|
static inline float *
|
|
vector4_array(struct vector4 *v)
|
|
{
|
|
return (float *)v;
|
|
}
|
|
|
|
static inline float
|
|
vector3_dot(const struct vector3 *a, const struct vector3 *b)
|
|
{
|
|
return a->x * b->x + a->y * b->y + a->z * b->z;
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
vector3_cross(struct vector3 *v, const struct vector3 *a, const struct vector3 *b)
|
|
{
|
|
float x = a->y * b->z - a->z * b->y;
|
|
float y = a->z * b->x - a->x * b->z;
|
|
float z = a->x * b->y - a->y * b->x;
|
|
|
|
v->x = x;
|
|
v->y = y;
|
|
v->z = z;
|
|
|
|
return v;
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
vector3_vector(struct vector3 *v, const struct vector3 *p1, const struct vector3 *p2)
|
|
{
|
|
v->x = p1->x - p2->x;
|
|
v->y = p1->y - p2->y;
|
|
v->z = p1->z - p2->z;
|
|
|
|
return v;
|
|
}
|
|
|
|
static inline float
|
|
vector3_length(const struct vector3 *v)
|
|
{
|
|
return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z);
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
vector3_normalize(struct vector3 *v)
|
|
{
|
|
float invLen = 1.0f / vector3_length(v);
|
|
v->x *= invLen;
|
|
v->y *= invLen;
|
|
v->z *= invLen;
|
|
|
|
return v;
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
vector3_to_rotation(struct vector3 *v, const struct vector3 *r)
|
|
{
|
|
// Assumes that the unrotated view vector is (0, 0, -1)
|
|
v->x = v->y = v->z = 0;
|
|
if (r->y != 0)
|
|
{
|
|
v->x = atan2f(r->y, sqrtf(r->x * r->x + r->z * r->z));
|
|
}
|
|
if (r->x != 0 || r->z != 0)
|
|
{
|
|
v->y = atan2f(-r->x, -r->z);
|
|
}
|
|
|
|
return v;
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
vector3_lerp(struct vector3 *v, const struct vector3 *a, const struct vector3 *b, float f)
|
|
{
|
|
float x = a->x + (b->x - a->x) * f;
|
|
float y = a->y + (b->y - a->y) * f;
|
|
float z = a->z + (b->z - a->z) * f;
|
|
|
|
v->x = x;
|
|
v->y = y;
|
|
v->z = z;
|
|
|
|
return v;
|
|
}
|
|
|
|
// quaternion
|
|
|
|
static inline struct quaternion *
|
|
quaternion_mul(struct quaternion *q, const struct quaternion *a, const struct quaternion *b)
|
|
{
|
|
float x = a->y * b->z - a->z * b->y + b->x * a->w + a->x * b->w;
|
|
float y = a->z * b->x - a->x * b->z + b->y * a->w + a->y * b->w;
|
|
float z = a->x * b->y - a->y * b->x + b->z * a->w + a->z * b->w;
|
|
float w = a->w * b->w - (a->x * b->x + a->y * b->y + a->z * b->z);
|
|
|
|
q->x = x;
|
|
q->y = y;
|
|
q->z = z;
|
|
q->w = w;
|
|
|
|
return q;
|
|
}
|
|
|
|
static inline struct quaternion *
|
|
quaternion_init(struct quaternion *q, float x, float y, float z)
|
|
{
|
|
struct quaternion roll = {sinf(x * 0.5f), 0, 0, cosf(x * 0.5f)};
|
|
struct quaternion pitch = {0, sinf(y * 0.5f), 0, cosf(y * 0.5f)};
|
|
struct quaternion yaw = {0, 0, sinf(z * 0.5f), cosf(z * 0.5f)};
|
|
|
|
// Order: y * x * z
|
|
quaternion_mul(q, &pitch, &roll);
|
|
quaternion_mul(q, q, &yaw);
|
|
|
|
return q;
|
|
}
|
|
|
|
static inline struct quaternion *
|
|
quaternion_slerp(struct quaternion *q, const struct quaternion *a, const struct quaternion *b, float t)
|
|
{
|
|
float cosTheta = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w;
|
|
if (cosTheta < 0)
|
|
{
|
|
cosTheta = -cosTheta;
|
|
q->x = -b->x;
|
|
q->y = -b->y;
|
|
q->z = -b->z;
|
|
q->w = -b->w;
|
|
}
|
|
else
|
|
{
|
|
*q = *b;
|
|
}
|
|
float scale0 = 1 - t, scale1 = t;
|
|
if ((1 - cosTheta) > 0.001f)
|
|
{
|
|
// use spherical interpolation
|
|
float theta = acosf(cosTheta);
|
|
float sinTheta = sinf(theta);
|
|
scale0 = sinf((1 - t) * theta) / sinTheta;
|
|
scale1 = sinf(t * theta) / sinTheta;
|
|
}
|
|
|
|
q->x = a->x * scale0 + q->x * scale1;
|
|
q->y = a->y * scale0 + q->y * scale1;
|
|
q->z = a->z * scale0 + q->z * scale1;
|
|
q->w = a->w * scale0 + q->w * scale1;
|
|
|
|
return q;
|
|
}
|
|
|
|
static inline struct quaternion *
|
|
quaternion_nslerp(struct quaternion *q, const struct quaternion *a, const struct quaternion *b, float t)
|
|
{
|
|
// Normalized linear quaternion interpolation
|
|
// Note: NLERP is faster than SLERP and commutative but does not yield constant velocity
|
|
|
|
float cosTheta = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w;
|
|
|
|
if (cosTheta < 0)
|
|
{
|
|
q->x = a->x + (-b->x - a->x) * t;
|
|
q->y = a->y + (-b->y - a->y) * t;
|
|
q->z = a->z + (-b->z - a->z) * t;
|
|
q->w = a->w + (-b->w - a->w) * t;
|
|
}
|
|
else
|
|
{
|
|
q->x = a->x + (b->x - a->x) * t;
|
|
q->y = a->y + (b->y - a->y) * t;
|
|
q->z = a->z + (b->z - a->z) * t;
|
|
q->w = a->w + (b->w - a->w) * t;
|
|
}
|
|
|
|
float invLen = 1.0f / sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
|
|
|
|
q->x *= invLen;
|
|
q->y *= invLen;
|
|
q->z *= invLen;
|
|
q->w *= invLen;
|
|
|
|
return q;
|
|
}
|
|
|
|
static inline struct quaternion *
|
|
quaternion_inverted(struct quaternion *q)
|
|
{
|
|
float len = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
|
|
if (len > 0)
|
|
{
|
|
float invLen = -1.0f / len;
|
|
q->x *= invLen;
|
|
q->y *= invLen;
|
|
q->z *= invLen;
|
|
q->w *= invLen;
|
|
q->w = -q->w;
|
|
}
|
|
else
|
|
{
|
|
q->x = q->y = q->z = q->w = 0;
|
|
}
|
|
return q;
|
|
}
|
|
|
|
// matrix 4*4
|
|
|
|
#define C m->c
|
|
|
|
static inline union matrix44 *
|
|
matrix44_identity(union matrix44 *m)
|
|
{
|
|
C[0][0] = 1;
|
|
C[1][0] = 0;
|
|
C[2][0] = 0;
|
|
C[3][0] = 0;
|
|
C[0][1] = 0;
|
|
C[1][1] = 1;
|
|
C[2][1] = 0;
|
|
C[3][1] = 0;
|
|
C[0][2] = 0;
|
|
C[1][2] = 0;
|
|
C[2][2] = 1;
|
|
C[3][2] = 0;
|
|
C[0][3] = 0;
|
|
C[1][3] = 0;
|
|
C[2][3] = 0;
|
|
C[3][3] = 1;
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_from_quaternion(union matrix44 *m, const struct quaternion *q)
|
|
{
|
|
// Calculate coefficients
|
|
float x2 = q->x + q->x, y2 = q->y + q->y, z2 = q->z + q->z;
|
|
float xx = q->x * x2, xy = q->x * y2, xz = q->x * z2;
|
|
float yy = q->y * y2, yz = q->y * z2, zz = q->z * z2;
|
|
float wx = q->w * x2, wy = q->w * y2, wz = q->w * z2;
|
|
|
|
C[0][0] = 1 - (yy + zz);
|
|
C[1][0] = xy - wz;
|
|
C[2][0] = xz + wy;
|
|
C[3][0] = 0;
|
|
C[0][1] = xy + wz;
|
|
C[1][1] = 1 - (xx + zz);
|
|
C[2][1] = yz - wx;
|
|
C[3][1] = 0;
|
|
C[0][2] = xz - wy;
|
|
C[1][2] = yz + wx;
|
|
C[2][2] = 1 - (xx + yy);
|
|
C[3][2] = 0;
|
|
C[0][3] = 0;
|
|
C[1][3] = 0;
|
|
C[2][3] = 0;
|
|
C[3][3] = 1;
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_trans(union matrix44 *m, float x, float y, float z)
|
|
{
|
|
matrix44_identity(m);
|
|
C[3][0] = x;
|
|
C[3][1] = y;
|
|
C[3][2] = z;
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_scale(union matrix44 *m, float x, float y, float z)
|
|
{
|
|
matrix44_identity(m);
|
|
C[0][0] = x;
|
|
C[1][1] = y;
|
|
C[2][2] = z;
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_rot(union matrix44 *m, float x, float y, float z)
|
|
{
|
|
// Rotation order: YXZ [* Vector]
|
|
struct quaternion q;
|
|
quaternion_init(&q, x, y, z);
|
|
|
|
return matrix44_from_quaternion(m, &q);
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_rot_axis(union matrix44 *m, const struct vector3 *axis, float angle)
|
|
{
|
|
float t = sinf(angle * 0.5f);
|
|
float x = axis->x * t;
|
|
float y = axis->y * t;
|
|
float z = axis->z * t;
|
|
struct quaternion q = {x, y, z, cosf(angle * 0.5f)};
|
|
|
|
return matrix44_from_quaternion(m, &q);
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_perspective(union matrix44 *m, float l, float r, float b, float t, float n, float f)
|
|
{
|
|
matrix44_identity(m);
|
|
float *mx = m->x;
|
|
|
|
mx[0] = 2 * n / (r - l);
|
|
mx[5] = 2 * n / (t - b);
|
|
mx[8] = (r + l) / (r - l);
|
|
mx[9] = (t + b) / (t - b);
|
|
mx[10] = -(f + n) / (f - n);
|
|
mx[11] = -1;
|
|
mx[14] = -2 * f * n / (f - n);
|
|
mx[15] = 0;
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_ortho(union matrix44 *m, float l, float r, float b, float t, float n, float f)
|
|
{
|
|
matrix44_identity(m);
|
|
float *mx = m->x;
|
|
|
|
mx[0] = 2 / (r - l);
|
|
mx[5] = 2 / (t - b);
|
|
mx[10] = -2 / (f - n);
|
|
mx[12] = -(r + l) / (r - l);
|
|
mx[13] = -(t + b) / (t - b);
|
|
mx[14] = -(f + n) / (f - n);
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_fastmul43(union matrix44 *m, const union matrix44 *m1, const union matrix44 *m2)
|
|
{
|
|
// Note: m may not be the same as m1 or m2
|
|
|
|
const float *m1x = m1->x;
|
|
const float *m2x = m2->x;
|
|
float *mx = m->x;
|
|
|
|
mx[0] = m1x[0] * m2x[0] + m1x[4] * m2x[1] + m1x[8] * m2x[2];
|
|
mx[1] = m1x[1] * m2x[0] + m1x[5] * m2x[1] + m1x[9] * m2x[2];
|
|
mx[2] = m1x[2] * m2x[0] + m1x[6] * m2x[1] + m1x[10] * m2x[2];
|
|
mx[3] = 0.0f;
|
|
|
|
mx[4] = m1x[0] * m2x[4] + m1x[4] * m2x[5] + m1x[8] * m2x[6];
|
|
mx[5] = m1x[1] * m2x[4] + m1x[5] * m2x[5] + m1x[9] * m2x[6];
|
|
mx[6] = m1x[2] * m2x[4] + m1x[6] * m2x[5] + m1x[10] * m2x[6];
|
|
mx[7] = 0.0f;
|
|
|
|
mx[8] = m1x[0] * m2x[8] + m1x[4] * m2x[9] + m1x[8] * m2x[10];
|
|
mx[9] = m1x[1] * m2x[8] + m1x[5] * m2x[9] + m1x[9] * m2x[10];
|
|
mx[10] = m1x[2] * m2x[8] + m1x[6] * m2x[9] + m1x[10] * m2x[10];
|
|
mx[11] = 0.0f;
|
|
|
|
mx[12] = m1x[0] * m2x[12] + m1x[4] * m2x[13] + m1x[8] * m2x[14] + m1x[12] * m2x[15];
|
|
mx[13] = m1x[1] * m2x[12] + m1x[5] * m2x[13] + m1x[9] * m2x[14] + m1x[13] * m2x[15];
|
|
mx[14] = m1x[2] * m2x[12] + m1x[6] * m2x[13] + m1x[10] * m2x[14] + m1x[14] * m2x[15];
|
|
mx[15] = 1.0f;
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_mul(union matrix44 *m, const union matrix44 *m1, const union matrix44 *m2)
|
|
{
|
|
union matrix44 mf;
|
|
const float *m1x = m1->x;
|
|
const float *m2x = m2->x;
|
|
|
|
mf.x[0] = m1x[0] * m2x[0] + m1x[4] * m2x[1] + m1x[8] * m2x[2] + m1x[12] * m2x[3];
|
|
mf.x[1] = m1x[1] * m2x[0] + m1x[5] * m2x[1] + m1x[9] * m2x[2] + m1x[13] * m2x[3];
|
|
mf.x[2] = m1x[2] * m2x[0] + m1x[6] * m2x[1] + m1x[10] * m2x[2] + m1x[14] * m2x[3];
|
|
mf.x[3] = m1x[3] * m2x[0] + m1x[7] * m2x[1] + m1x[11] * m2x[2] + m1x[15] * m2x[3];
|
|
|
|
mf.x[4] = m1x[0] * m2x[4] + m1x[4] * m2x[5] + m1x[8] * m2x[6] + m1x[12] * m2x[7];
|
|
mf.x[5] = m1x[1] * m2x[4] + m1x[5] * m2x[5] + m1x[9] * m2x[6] + m1x[13] * m2x[7];
|
|
mf.x[6] = m1x[2] * m2x[4] + m1x[6] * m2x[5] + m1x[10] * m2x[6] + m1x[14] * m2x[7];
|
|
mf.x[7] = m1x[3] * m2x[4] + m1x[7] * m2x[5] + m1x[11] * m2x[6] + m1x[15] * m2x[7];
|
|
|
|
mf.x[8] = m1x[0] * m2x[8] + m1x[4] * m2x[9] + m1x[8] * m2x[10] + m1x[12] * m2x[11];
|
|
mf.x[9] = m1x[1] * m2x[8] + m1x[5] * m2x[9] + m1x[9] * m2x[10] + m1x[13] * m2x[11];
|
|
mf.x[10] = m1x[2] * m2x[8] + m1x[6] * m2x[9] + m1x[10] * m2x[10] + m1x[14] * m2x[11];
|
|
mf.x[11] = m1x[3] * m2x[8] + m1x[7] * m2x[9] + m1x[11] * m2x[10] + m1x[15] * m2x[11];
|
|
|
|
mf.x[12] = m1x[0] * m2x[12] + m1x[4] * m2x[13] + m1x[8] * m2x[14] + m1x[12] * m2x[15];
|
|
mf.x[13] = m1x[1] * m2x[12] + m1x[5] * m2x[13] + m1x[9] * m2x[14] + m1x[13] * m2x[15];
|
|
mf.x[14] = m1x[2] * m2x[12] + m1x[6] * m2x[13] + m1x[10] * m2x[14] + m1x[14] * m2x[15];
|
|
mf.x[15] = m1x[3] * m2x[12] + m1x[7] * m2x[13] + m1x[11] * m2x[14] + m1x[15] * m2x[15];
|
|
|
|
*m = mf;
|
|
|
|
return m;
|
|
}
|
|
|
|
// vector * matrix
|
|
|
|
static inline struct vector3 *
|
|
vector3_mul(struct vector3 *v, const union matrix44 *m)
|
|
{
|
|
float x = v->x * C[0][0] + v->y * C[1][0] + v->z * C[2][0] + C[3][0];
|
|
float y = v->x * C[0][1] + v->y * C[1][1] + v->z * C[2][1] + C[3][1];
|
|
float z = v->x * C[0][2] + v->y * C[1][2] + v->z * C[2][2] + C[3][2];
|
|
|
|
v->x = x;
|
|
v->y = y;
|
|
v->z = z;
|
|
|
|
return v;
|
|
}
|
|
|
|
static inline struct vector4 *
|
|
vector4_mul(struct vector4 *v, const union matrix44 *m)
|
|
{
|
|
float x = v->x * C[0][0] + v->y * C[1][0] + v->z * C[2][0] + v->w * C[3][0];
|
|
float y = v->x * C[0][1] + v->y * C[1][1] + v->z * C[2][1] + v->w * C[3][1];
|
|
float z = v->x * C[0][2] + v->y * C[1][2] + v->z * C[2][2] + v->w * C[3][2];
|
|
float w = v->x * C[0][3] + v->y * C[1][3] + v->z * C[2][3] + v->w * C[3][3];
|
|
|
|
v->x = x;
|
|
v->y = y;
|
|
v->z = z;
|
|
v->w = w;
|
|
return v;
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
vector3_mul33(struct vector3 *v, const union matrix44 *m)
|
|
{
|
|
float x = v->x * C[0][0] + v->y * C[1][0] + v->z * C[2][0];
|
|
float y = v->x * C[0][1] + v->y * C[1][1] + v->z * C[2][1];
|
|
float z = v->x * C[0][2] + v->y * C[1][2] + v->z * C[2][2];
|
|
|
|
v->x = x;
|
|
v->y = y;
|
|
v->z = z;
|
|
|
|
return v;
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_transposed(union matrix44 *m)
|
|
{
|
|
int x, y;
|
|
for (y = 0; y < 4; ++y)
|
|
{
|
|
for (x = y + 1; x < 4; ++x)
|
|
{
|
|
float tmp = C[x][y];
|
|
C[x][y] = C[y][x];
|
|
C[y][x] = tmp;
|
|
}
|
|
}
|
|
|
|
return m;
|
|
}
|
|
|
|
static inline float
|
|
matrix44_determinant(const union matrix44 *m)
|
|
{
|
|
return C[0][3] * C[1][2] * C[2][1] * C[3][0] - C[0][2] * C[1][3] * C[2][1] * C[3][0] - C[0][3] * C[1][1] * C[2][2] * C[3][0] + C[0][1] * C[1][3] * C[2][2] * C[3][0] +
|
|
C[0][2] * C[1][1] * C[2][3] * C[3][0] - C[0][1] * C[1][2] * C[2][3] * C[3][0] - C[0][3] * C[1][2] * C[2][0] * C[3][1] + C[0][2] * C[1][3] * C[2][0] * C[3][1] +
|
|
C[0][3] * C[1][0] * C[2][2] * C[3][1] - C[0][0] * C[1][3] * C[2][2] * C[3][1] - C[0][2] * C[1][0] * C[2][3] * C[3][1] + C[0][0] * C[1][2] * C[2][3] * C[3][1] +
|
|
C[0][3] * C[1][1] * C[2][0] * C[3][2] - C[0][1] * C[1][3] * C[2][0] * C[3][2] - C[0][3] * C[1][0] * C[2][1] * C[3][2] + C[0][0] * C[1][3] * C[2][1] * C[3][2] +
|
|
C[0][1] * C[1][0] * C[2][3] * C[3][2] - C[0][0] * C[1][1] * C[2][3] * C[3][2] - C[0][2] * C[1][1] * C[2][0] * C[3][3] + C[0][1] * C[1][2] * C[2][0] * C[3][3] +
|
|
C[0][2] * C[1][0] * C[2][1] * C[3][3] - C[0][0] * C[1][2] * C[2][1] * C[3][3] - C[0][1] * C[1][0] * C[2][2] * C[3][3] + C[0][0] * C[1][1] * C[2][2] * C[3][3];
|
|
}
|
|
|
|
static inline union matrix44 *
|
|
matrix44_inverted(union matrix44 *dst, const union matrix44 *m)
|
|
{
|
|
float d = matrix44_determinant(m);
|
|
if (d == 0)
|
|
{
|
|
*dst = *m;
|
|
return dst;
|
|
}
|
|
d = 1.0f / d;
|
|
|
|
dst->c[0][0] = d * (C[1][2] * C[2][3] * C[3][1] - C[1][3] * C[2][2] * C[3][1] + C[1][3] * C[2][1] * C[3][2] - C[1][1] * C[2][3] * C[3][2] - C[1][2] * C[2][1] * C[3][3] + C[1][1] * C[2][2] * C[3][3]);
|
|
dst->c[0][1] = d * (C[0][3] * C[2][2] * C[3][1] - C[0][2] * C[2][3] * C[3][1] - C[0][3] * C[2][1] * C[3][2] + C[0][1] * C[2][3] * C[3][2] + C[0][2] * C[2][1] * C[3][3] - C[0][1] * C[2][2] * C[3][3]);
|
|
dst->c[0][2] = d * (C[0][2] * C[1][3] * C[3][1] - C[0][3] * C[1][2] * C[3][1] + C[0][3] * C[1][1] * C[3][2] - C[0][1] * C[1][3] * C[3][2] - C[0][2] * C[1][1] * C[3][3] + C[0][1] * C[1][2] * C[3][3]);
|
|
dst->c[0][3] = d * (C[0][3] * C[1][2] * C[2][1] - C[0][2] * C[1][3] * C[2][1] - C[0][3] * C[1][1] * C[2][2] + C[0][1] * C[1][3] * C[2][2] + C[0][2] * C[1][1] * C[2][3] - C[0][1] * C[1][2] * C[2][3]);
|
|
dst->c[1][0] = d * (C[1][3] * C[2][2] * C[3][0] - C[1][2] * C[2][3] * C[3][0] - C[1][3] * C[2][0] * C[3][2] + C[1][0] * C[2][3] * C[3][2] + C[1][2] * C[2][0] * C[3][3] - C[1][0] * C[2][2] * C[3][3]);
|
|
dst->c[1][1] = d * (C[0][2] * C[2][3] * C[3][0] - C[0][3] * C[2][2] * C[3][0] + C[0][3] * C[2][0] * C[3][2] - C[0][0] * C[2][3] * C[3][2] - C[0][2] * C[2][0] * C[3][3] + C[0][0] * C[2][2] * C[3][3]);
|
|
dst->c[1][2] = d * (C[0][3] * C[1][2] * C[3][0] - C[0][2] * C[1][3] * C[3][0] - C[0][3] * C[1][0] * C[3][2] + C[0][0] * C[1][3] * C[3][2] + C[0][2] * C[1][0] * C[3][3] - C[0][0] * C[1][2] * C[3][3]);
|
|
dst->c[1][3] = d * (C[0][2] * C[1][3] * C[2][0] - C[0][3] * C[1][2] * C[2][0] + C[0][3] * C[1][0] * C[2][2] - C[0][0] * C[1][3] * C[2][2] - C[0][2] * C[1][0] * C[2][3] + C[0][0] * C[1][2] * C[2][3]);
|
|
dst->c[2][0] = d * (C[1][1] * C[2][3] * C[3][0] - C[1][3] * C[2][1] * C[3][0] + C[1][3] * C[2][0] * C[3][1] - C[1][0] * C[2][3] * C[3][1] - C[1][1] * C[2][0] * C[3][3] + C[1][0] * C[2][1] * C[3][3]);
|
|
dst->c[2][1] = d * (C[0][3] * C[2][1] * C[3][0] - C[0][1] * C[2][3] * C[3][0] - C[0][3] * C[2][0] * C[3][1] + C[0][0] * C[2][3] * C[3][1] + C[0][1] * C[2][0] * C[3][3] - C[0][0] * C[2][1] * C[3][3]);
|
|
dst->c[2][2] = d * (C[0][1] * C[1][3] * C[3][0] - C[0][3] * C[1][1] * C[3][0] + C[0][3] * C[1][0] * C[3][1] - C[0][0] * C[1][3] * C[3][1] - C[0][1] * C[1][0] * C[3][3] + C[0][0] * C[1][1] * C[3][3]);
|
|
dst->c[2][3] = d * (C[0][3] * C[1][1] * C[2][0] - C[0][1] * C[1][3] * C[2][0] - C[0][3] * C[1][0] * C[2][1] + C[0][0] * C[1][3] * C[2][1] + C[0][1] * C[1][0] * C[2][3] - C[0][0] * C[1][1] * C[2][3]);
|
|
dst->c[3][0] = d * (C[1][2] * C[2][1] * C[3][0] - C[1][1] * C[2][2] * C[3][0] - C[1][2] * C[2][0] * C[3][1] + C[1][0] * C[2][2] * C[3][1] + C[1][1] * C[2][0] * C[3][2] - C[1][0] * C[2][1] * C[3][2]);
|
|
dst->c[3][1] = d * (C[0][1] * C[2][2] * C[3][0] - C[0][2] * C[2][1] * C[3][0] + C[0][2] * C[2][0] * C[3][1] - C[0][0] * C[2][2] * C[3][1] - C[0][1] * C[2][0] * C[3][2] + C[0][0] * C[2][1] * C[3][2]);
|
|
dst->c[3][2] = d * (C[0][2] * C[1][1] * C[3][0] - C[0][1] * C[1][2] * C[3][0] - C[0][2] * C[1][0] * C[3][1] + C[0][0] * C[1][2] * C[3][1] + C[0][1] * C[1][0] * C[3][2] - C[0][0] * C[1][1] * C[3][2]);
|
|
dst->c[3][3] = d * (C[0][1] * C[1][2] * C[2][0] - C[0][2] * C[1][1] * C[2][0] + C[0][2] * C[1][0] * C[2][1] - C[0][0] * C[1][2] * C[2][1] - C[0][1] * C[1][0] * C[2][2] + C[0][0] * C[1][1] * C[2][2]);
|
|
|
|
return dst;
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
matrix44_gettrans(const union matrix44 *m, struct vector3 *trans)
|
|
{
|
|
// Getting translation is trivial
|
|
trans->x = C[3][0];
|
|
trans->y = C[3][1];
|
|
trans->z = C[3][2];
|
|
|
|
return trans;
|
|
}
|
|
|
|
static inline struct vector3 *
|
|
matrix44_getscale(const union matrix44 *m, struct vector3 *scale)
|
|
{
|
|
// Scale is length of columns
|
|
scale->x = sqrtf(C[0][0] * C[0][0] + C[0][1] * C[0][1] + C[0][2] * C[0][2]);
|
|
scale->y = sqrtf(C[1][0] * C[1][0] + C[1][1] * C[1][1] + C[1][2] * C[1][2]);
|
|
scale->z = sqrtf(C[2][0] * C[2][0] + C[2][1] * C[2][1] + C[2][2] * C[2][2]);
|
|
|
|
return scale;
|
|
}
|
|
|
|
static inline void
|
|
matrix44_decompose(const union matrix44 *m, struct vector3 *trans, struct vector3 *rot, struct vector3 *scale)
|
|
{
|
|
matrix44_gettrans(m, trans);
|
|
matrix44_getscale(m, scale);
|
|
|
|
if (scale->x == 0 || scale->y == 0 || scale->z == 0)
|
|
{
|
|
rot->x = 0;
|
|
rot->y = 0;
|
|
rot->z = 0;
|
|
return;
|
|
}
|
|
|
|
// Detect negative scale with determinant and flip one arbitrary axis
|
|
if (matrix44_determinant(m) < 0)
|
|
scale->x = -scale->x;
|
|
|
|
// Combined rotation matrix YXZ
|
|
//
|
|
// Cos[y]*Cos[z]+Sin[x]*Sin[y]*Sin[z] Cos[z]*Sin[x]*Sin[y]-Cos[y]*Sin[z] Cos[x]*Sin[y]
|
|
// Cos[x]*Sin[z] Cos[x]*Cos[z] -Sin[x]
|
|
// -Cos[z]*Sin[y]+Cos[y]*Sin[x]*Sin[z] Cos[y]*Cos[z]*Sin[x]+Sin[y]*Sin[z] Cos[x]*Cos[y]
|
|
|
|
rot->x = asinf(-C[2][1] / scale->z);
|
|
|
|
// Special case: Cos[x] == 0 (when Sin[x] is +/-1)
|
|
float f = fabsf(C[2][1] / scale->z);
|
|
|
|
if (f > 0.999f && f < 1.001f)
|
|
{
|
|
// Pin arbitrarily one of y or z to zero
|
|
// Mathematical equivalent of gimbal lock
|
|
rot->y = 0;
|
|
|
|
// Now: Cos[x] = 0, Sin[x] = +/-1, Cos[y] = 1, Sin[y] = 0
|
|
// => m[0][0] = Cos[z] and m[1][0] = Sin[z]
|
|
rot->z = atan2f(-C[1][0] / scale->y, C[0][0] / scale->x);
|
|
}
|
|
else
|
|
{
|
|
// Standard case
|
|
rot->y = atan2f(C[2][0] / scale->z, C[2][2] / scale->z);
|
|
rot->z = atan2f(C[0][1] / scale->x, C[1][1] / scale->y);
|
|
}
|
|
}
|
|
|
|
static inline float *
|
|
matrix44_to33(const union matrix44 *m, float m33[9])
|
|
{
|
|
m33[0] = C[0][0];
|
|
m33[1] = C[0][1];
|
|
m33[2] = C[0][2];
|
|
m33[3] = C[1][0];
|
|
m33[4] = C[1][1];
|
|
m33[5] = C[1][2];
|
|
m33[6] = C[2][0];
|
|
m33[7] = C[2][1];
|
|
m33[8] = C[2][2];
|
|
|
|
return m33;
|
|
}
|
|
|
|
#undef C
|
|
|
|
// plane
|
|
|
|
static inline struct plane *
|
|
plane_init(struct plane *p, const struct vector3 *normal, float d)
|
|
{
|
|
p->normal = *normal;
|
|
// normalize
|
|
float invLen = 1.0f / vector3_length(normal);
|
|
p->normal.x *= invLen;
|
|
p->normal.y *= invLen;
|
|
p->normal.z *= invLen;
|
|
p->dist = d * invLen;
|
|
|
|
return p;
|
|
}
|
|
|
|
static inline struct plane *
|
|
plane_init_dot3(struct plane *p, const struct vector3 *v0, const struct vector3 *v1, const struct vector3 *v2)
|
|
{
|
|
struct vector3 a, b;
|
|
vector3_vector(&a, v1, v0);
|
|
vector3_vector(&b, v2, v0);
|
|
|
|
vector3_cross(&p->normal, &a, &b);
|
|
vector3_normalize(&p->normal);
|
|
p->dist = -vector3_dot(&p->normal, v0);
|
|
|
|
return p;
|
|
}
|
|
|
|
static inline float
|
|
plane_dist(const struct plane *p, const struct vector3 *v)
|
|
{
|
|
float d = vector3_dot(&p->normal, v);
|
|
return d + p->dist;
|
|
}
|
|
|
|
// Intersection
|
|
|
|
static inline struct vector3 *
|
|
intersection_raytriangle(const struct vector3 *rayOrig, const struct vector3 *rayDir,
|
|
const struct vector3 *vert0, const struct vector3 *vert1, const struct vector3 *vert2,
|
|
struct vector3 *intsPoint)
|
|
{
|
|
// Idea: Tomas Moeller and Ben Trumbore
|
|
// in Fast, Minimum Storage Ray/Triangle Intersection
|
|
|
|
// Find vectors for two edges sharing vert0
|
|
struct vector3 edge1, edge2;
|
|
vector3_vector(&edge1, vert1, vert0);
|
|
vector3_vector(&edge2, vert2, vert0);
|
|
|
|
// Begin calculating determinant - also used to calculate U parameter
|
|
struct vector3 pvec;
|
|
vector3_cross(&pvec, rayDir, &edge2);
|
|
|
|
// If determinant is near zero, ray lies in plane of triangle
|
|
float det = vector3_dot(&edge1, &pvec);
|
|
|
|
// *** Culling branch ***
|
|
/*if( det < FLT_EPSILON )
|
|
return NULL;
|
|
|
|
// Calculate distance from vert0 to ray origin
|
|
struct vector3 tvec;
|
|
vector3_vector(&tvec, rayOrig, &vert0);
|
|
|
|
// Calculate U parameter and test bounds
|
|
float u = vector3_dot(&tvec, &pvec);
|
|
if (u < 0 || u > det )
|
|
return NULL;
|
|
|
|
// Prepare to test V parameter
|
|
struct vector3 qvec;
|
|
vector3_cross(&qvec, &tvec, &edge1);
|
|
|
|
// Calculate V parameter and test bounds
|
|
float v = vector3_dot(rayDir, &qvec);
|
|
if (v < 0 || u + v > det )
|
|
return NULL;
|
|
|
|
// Calculate t, scale parameters, ray intersects triangle
|
|
float t = vector3_dot(&edge2, &qvec ) / det;*/
|
|
|
|
// *** Non-culling branch ***
|
|
if (det > -FLT_EPSILON && det < FLT_EPSILON)
|
|
return 0;
|
|
float inv_det = 1.0f / det;
|
|
|
|
// Calculate distance from vert0 to ray origin
|
|
struct vector3 tvec;
|
|
vector3_vector(&tvec, rayOrig, vert0);
|
|
|
|
// Calculate U parameter and test bounds
|
|
float u = vector3_dot(&tvec, &pvec) * inv_det;
|
|
if (u < 0.0f || u > 1.0f)
|
|
return 0;
|
|
|
|
// Prepare to test V parameter
|
|
struct vector3 qvec;
|
|
vector3_cross(&qvec, &tvec, &edge1);
|
|
|
|
// Calculate V parameter and test bounds
|
|
float v = vector3_dot(rayDir, &qvec) * inv_det;
|
|
if (v < 0.0f || u + v > 1.0f)
|
|
return 0;
|
|
|
|
// Calculate t, ray intersects triangle
|
|
float t = vector3_dot(&edge2, &qvec) * inv_det;
|
|
|
|
// Calculate intersection point and test ray length and direction
|
|
intsPoint->x = rayOrig->x + rayDir->x * t;
|
|
intsPoint->y = rayOrig->y + rayDir->y * t;
|
|
intsPoint->z = rayOrig->z + rayDir->z * t;
|
|
|
|
struct vector3 vec;
|
|
vector3_vector(&vec, intsPoint, rayOrig);
|
|
if (vector3_dot(&vec, rayDir) < 0 || vector3_length(&vec) > vector3_length(rayDir))
|
|
return NULL;
|
|
|
|
return intsPoint;
|
|
}
|
|
|
|
static inline float
|
|
minf(float a, float b)
|
|
{
|
|
return a < b ? a : b;
|
|
}
|
|
|
|
static inline float
|
|
maxf(float a, float b)
|
|
{
|
|
return a > b ? a : b;
|
|
}
|
|
|
|
static inline int
|
|
intersection_rayAABB(const struct vector3 *rayOrig, const struct vector3 *rayDir,
|
|
const struct vector3 *mins, const struct vector3 *maxs)
|
|
{
|
|
// SLAB based optimized ray/AABB intersection routine
|
|
// Idea taken from http://ompf.org/ray/
|
|
|
|
float l1 = (mins->x - rayOrig->x) / rayDir->x;
|
|
float l2 = (maxs->x - rayOrig->x) / rayDir->x;
|
|
float lmin = minf(l1, l2);
|
|
float lmax = maxf(l1, l2);
|
|
|
|
l1 = (mins->y - rayOrig->y) / rayDir->y;
|
|
l2 = (maxs->y - rayOrig->y) / rayDir->y;
|
|
lmin = maxf(minf(l1, l2), lmin);
|
|
lmax = minf(maxf(l1, l2), lmax);
|
|
|
|
l1 = (mins->z - rayOrig->z) / rayDir->z;
|
|
l2 = (maxs->z - rayOrig->z) / rayDir->z;
|
|
lmin = maxf(minf(l1, l2), lmin);
|
|
lmax = minf(maxf(l1, l2), lmax);
|
|
|
|
if ((lmax >= 0.0f) & (lmax >= lmin))
|
|
{
|
|
// Consider length
|
|
const struct vector3 rayDest = {rayOrig->x + rayDir->x, rayOrig->y + rayDir->y, rayOrig->z + rayDir->z};
|
|
const struct vector3 rayMins = {minf(rayDest.x, rayOrig->x), minf(rayDest.y, rayOrig->y), minf(rayDest.z, rayOrig->z)};
|
|
const struct vector3 rayMaxs = {maxf(rayDest.x, rayOrig->x), maxf(rayDest.y, rayOrig->y), maxf(rayDest.z, rayOrig->z)};
|
|
return (rayMins.x < maxs->x) && (rayMaxs.x > mins->x) &&
|
|
(rayMins.y < maxs->y) && (rayMaxs.y > mins->y) &&
|
|
(rayMins.z < maxs->z) && (rayMaxs.z > mins->z);
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
static inline float
|
|
vector3_distAABB(const struct vector3 *pos, const struct vector3 *mins, const struct vector3 *maxs)
|
|
{
|
|
struct vector3 center;
|
|
struct vector3 extent;
|
|
center.x = (mins->x + maxs->x) * 0.5f;
|
|
center.y = (mins->y + maxs->y) * 0.5f;
|
|
center.z = (mins->z + maxs->z) * 0.5f;
|
|
|
|
extent.x = (maxs->x - mins->x) * 0.5f;
|
|
extent.y = (maxs->y - mins->y) * 0.5f;
|
|
extent.z = (maxs->z - mins->z) * 0.5f;
|
|
|
|
struct vector3 nearestVec;
|
|
nearestVec.x = maxf(0, fabsf(pos->x - center.x) - extent.x);
|
|
nearestVec.y = maxf(0, fabsf(pos->y - center.y) - extent.y);
|
|
nearestVec.z = maxf(0, fabsf(pos->z - center.z) - extent.z);
|
|
|
|
return vector3_length(&nearestVec);
|
|
}
|
|
|
|
#endif
|