Skip to content

Instantly share code, notes, and snippets.

@nsf

nsf/linear_math.c

Created January 26, 2011 10:40
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save nsf/796537 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save nsf/796537 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
linear_math.c
#include <xmmintrin.h>
#include <pmmintrin.h>
#include <float.h>
#include "common.h"
#include "linear_math.h"
/**************************************************************************
vec2: two-dimensional vector
**************************************************************************/
void vec2_rotate(vec2_t out, float deg, const vec2_t origin)
{
deg *= MATH_DEG_TO_RAD;
float cosdeg = cosf(deg);
float sindeg = sinf(deg);
vec2_suba(out, origin);
vec2_set(out,
VEC_X(out) * cosdeg - VEC_Y(out) * sindeg,
VEC_X(out) * sindeg + VEC_Y(out) * cosdeg);
vec2_adda(out, origin);
}
/**************************************************************************
vec3: three-dimensional vector
**************************************************************************/
void vec3_rotate_x(vec3_t out, float deg, const vec3_t origin)
{
deg *= MATH_DEG_TO_RAD;
float cosdeg = cosf(deg);
float sindeg = sinf(deg);
VEC_Y(out) -= VEC_Y(origin);
VEC_Z(out) -= VEC_Z(origin);
vec3_set(out,
VEC_X(out),
VEC_Y(out) * cosdeg - VEC_Z(out) * sindeg,
VEC_Y(out) * sindeg + VEC_Z(out) * cosdeg);
VEC_Y(out) += VEC_Y(origin);
VEC_Z(out) += VEC_Z(origin);
}
void vec3_rotate_y(vec3_t out, float deg, const vec3_t origin)
{
deg *= MATH_DEG_TO_RAD;
float cosdeg = cosf(deg);
float sindeg = sinf(deg);
VEC_X(out) -= VEC_X(origin);
VEC_Z(out) -= VEC_Z(origin);
vec3_set(out,
VEC_X(out) * cosdeg - VEC_Z(out) * sindeg,
VEC_Y(out),
VEC_X(out) * sindeg + VEC_Z(out) * cosdeg);
VEC_X(out) += VEC_X(origin);
VEC_Z(out) += VEC_Z(origin);
}
void vec3_rotate_z(vec3_t out, float deg, const vec3_t origin)
{
vec2_rotate(out, deg, origin);
}
/**************************************************************************
mat4: 4x4 OpenGL matrix
**************************************************************************/
void mat4_copy(mat4_t out, const mat4_t m)
{
memcpy(out, m, sizeof(float) * 16);
}
void mat4_transform_vec3(vec3_t v, const mat4_t m)
{
vec3_t r;
r[0] = m[0] * v[0] + m[4] * v[1] + m[8] * v[2] + m[12];
r[1] = m[1] * v[0] + m[5] * v[1] + m[9] * v[2] + m[13];
r[2] = m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14];
vec3_copy(v, r);
}
void mat4_mul(mat4_t out, const mat4_t m1, const mat4_t m2)
{
out[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3];
out[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3];
out[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3];
out[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3];
out[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7];
out[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7];
out[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7];
out[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7];
out[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11];
out[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11];
out[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11];
out[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11];
out[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15];
out[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15];
out[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
out[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
}
bool mat4_equals(const mat4_t m1, const mat4_t m2)
{
return (fabs(m1[0] - m2[0]) < MATH_EPSILON &&
fabs(m1[1] - m2[1]) < MATH_EPSILON &&
fabs(m1[2] - m2[2]) < MATH_EPSILON &&
fabs(m1[3] - m2[3]) < MATH_EPSILON &&
fabs(m1[4] - m2[4]) < MATH_EPSILON &&
fabs(m1[5] - m2[5]) < MATH_EPSILON &&
fabs(m1[6] - m2[6]) < MATH_EPSILON &&
fabs(m1[7] - m2[7]) < MATH_EPSILON &&
fabs(m1[8] - m2[8]) < MATH_EPSILON &&
fabs(m1[9] - m2[9]) < MATH_EPSILON &&
fabs(m1[10] - m2[10]) < MATH_EPSILON &&
fabs(m1[11] - m2[11]) < MATH_EPSILON &&
fabs(m1[12] - m2[12]) < MATH_EPSILON &&
fabs(m1[13] - m2[13]) < MATH_EPSILON &&
fabs(m1[14] - m2[14]) < MATH_EPSILON &&
fabs(m1[15] - m2[15]) < MATH_EPSILON);
}
void mat4_set_identity(mat4_t out)
{
out[0] = 1.0f; out[4] = 0.0f; out[8] = 0.0f; out[12] = 0.0f;
out[1] = 0.0f; out[5] = 1.0f; out[9] = 0.0f; out[13] = 0.0f;
out[2] = 0.0f; out[6] = 0.0f; out[10] = 1.0f; out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void mat4_set_zero(mat4_t out)
{
out[0] = 0.0f; out[4] = 0.0f; out[8] = 0.0f; out[12] = 0.0f;
out[1] = 0.0f; out[5] = 0.0f; out[9] = 0.0f; out[13] = 0.0f;
out[2] = 0.0f; out[6] = 0.0f; out[10] = 0.0f; out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 0.0f;
}
bool mat4_is_identity(const mat4_t m)
{
return (fabs(m[0] - 1.0f) < MATH_EPSILON &&
fabs(m[1]) < MATH_EPSILON &&
fabs(m[2]) < MATH_EPSILON &&
fabs(m[3]) < MATH_EPSILON &&
fabs(m[4]) < MATH_EPSILON &&
fabs(m[5] - 1.0f) < MATH_EPSILON &&
fabs(m[6]) < MATH_EPSILON &&
fabs(m[7]) < MATH_EPSILON &&
fabs(m[8]) < MATH_EPSILON &&
fabs(m[9]) < MATH_EPSILON &&
fabs(m[10] - 1.0f) < MATH_EPSILON &&
fabs(m[11]) < MATH_EPSILON &&
fabs(m[12]) < MATH_EPSILON &&
fabs(m[13]) < MATH_EPSILON &&
fabs(m[14]) < MATH_EPSILON &&
fabs(m[15] - 1.0f) < MATH_EPSILON);
}
float mat4_determinant(const mat4_t m)
{
float d;
d = m[0] * (m[5] * (m[10] * m[15] - m[14] * m[11]) - m[9] * (m[6] * m[15] - m[14] * m[7]) + m[13] * (m[6] * m[11] - m[10] * m[7]));
d -= m[4] * (m[1] * (m[10] * m[15] - m[14] * m[11]) - m[9] * (m[2] * m[15] - m[14] * m[3]) + m[13] * (m[2] * m[11] - m[10] * m[3]));
d += m[8] * (m[1] * (m[6] * m[15] - m[14] * m[7]) - m[5] * (m[2] * m[15] - m[14] * m[3]) + m[13] * (m[2] * m[7] - m[6] * m[3]));
d -= m[12] * (m[1] * (m[6] * m[11] - m[10] * m[7]) - m[5] * (m[2] * m[11] - m[10] * m[3]) + m[9] * (m[2] * m[7] - m[6] * m[3]));
return d;
}
void mat4_inverse(mat4_t out, const mat4_t m)
{
float d, id;
d = mat4_determinant(m);
if (d < MATH_EPSILON)
return;
id = 1.0f / d;
out[0] = (m[5] * (m[10] * m[15] - m[14] * m[11]) - m[9] * (m[6] * m[15] - m[14] * m[7]) + m[13] * (m[6] * m[11] - m[10] * m[7])) * id;
out[1] = -(m[1] * (m[10] * m[15] - m[14] * m[11]) - m[9] * (m[2] * m[15] - m[14] * m[3]) + m[13] * (m[2] * m[11] - m[10] * m[3])) * id;
out[2] = (m[1] * (m[6] * m[15] - m[14] * m[7]) - m[5] * (m[2] * m[15] - m[14] * m[3]) + m[13] * (m[2] * m[7] - m[6] * m[3])) * id;
out[3] = -(m[1] * (m[6] * m[11] - m[10] * m[7]) - m[5] * (m[2] * m[11] - m[10] * m[3]) + m[9] * (m[2] * m[7] - m[6] * m[3])) * id;
out[4] = -(m[4] * (m[10] * m[15] - m[14] * m[11]) - m[8] * (m[6] * m[15] - m[14] * m[7]) + m[12] * (m[6] * m[11] - m[10] * m[7])) * id;
out[5] = (m[0] * (m[10] * m[15] - m[14] * m[11]) - m[8] * (m[2] * m[15] - m[14] * m[3]) + m[12] * (m[2] * m[11] - m[10] * m[3])) * id;
out[6] = -(m[0] * (m[6] * m[15] - m[14] * m[7]) - m[4] * (m[2] * m[15] - m[14] * m[3]) + m[12] * (m[2] * m[7] - m[6] * m[3])) * id;
out[7] = (m[0] * (m[6] * m[11] - m[10] * m[7]) - m[4] * (m[2] * m[11] - m[10] * m[3]) + m[8] * (m[2] * m[7] - m[6] * m[3])) * id;
out[8] = (m[4] * (m[9] * m[15] - m[13] * m[11]) - m[8] * (m[5] * m[15] - m[13] * m[7]) + m[12] * (m[5] * m[11] - m[9] * m[7])) * id;
out[9] = -(m[0] * (m[9] * m[15] - m[13] * m[11]) - m[8] * (m[1] * m[15] - m[13] * m[3]) + m[12] * (m[1] * m[11] - m[9] * m[3])) * id;
out[10] = (m[0] * (m[5] * m[15] - m[13] * m[7]) - m[4] * (m[1] * m[15] - m[13] * m[3]) + m[12] * (m[1] * m[7] - m[5] * m[3])) * id;
out[11] = -(m[0] * (m[5] * m[11] - m[9] * m[7]) - m[4] * (m[1] * m[11] - m[9] * m[3]) + m[8] * (m[1] * m[7] - m[5] * m[3])) * id;
out[12] = -(m[4] * (m[9] * m[14] - m[13] * m[10]) - m[8] * (m[5] * m[14] - m[13] * m[6]) + m[12] * (m[5] * m[10] - m[9] * m[6])) * id;
out[13] = (m[0] * (m[9] * m[14] - m[13] * m[10]) - m[8] * (m[1] * m[14] - m[13] * m[2]) + m[12] * (m[1] * m[10] - m[9] * m[2])) * id;
out[14] = -(m[0] * (m[5] * m[14] - m[13] * m[6]) - m[4] * (m[1] * m[14] - m[13] * m[2]) + m[12] * (m[1] * m[6] - m[5] * m[2])) * id;
out[15] = (m[0] * (m[5] * m[10] - m[9] * m[6]) - m[4] * (m[1] * m[10] - m[9] * m[2]) + m[8] * (m[1] * m[6] - m[5] * m[2])) * id;
}
void mat4_set_rotate(mat4_t out, const vec3_t axis, float angle)
{
vec3_t v;
float rad = angle * MATH_DEG_TO_RAD;
float c = cosf(rad);
float s = sinf(rad);
vec3_copy(v, axis);
vec3_normalize(v);
float xx = v[0] * v[0];
float yy = v[1] * v[1];
float zz = v[2] * v[2];
float xy = v[0] * v[1];
float yz = v[1] * v[2];
float zx = v[2] * v[0];
float xs = v[0] * s;
float ys = v[1] * s;
float zs = v[2] * s;
out[0] = (1.0f - c) * xx + c; out[4] = (1.0f - c) * xy - zs; out[8] = (1.0f - c) * zx + ys; out[12] = 0.0f;
out[1] = (1.0f - c) * xy + zs; out[5] = (1.0f - c) * yy + c; out[9] = (1.0f - c) * yz - xs; out[13] = 0.0f;
out[2] = (1.0f - c) * zx - ys; out[6] = (1.0f - c) * yz + xs; out[10] = (1.0f - c) * zz + c; out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void mat4_set_rotate_x(mat4_t out, float angle)
{
float rad = angle * MATH_DEG_TO_RAD;
float c = cosf(rad);
float s = sinf(rad);
out[0] = 1.0f; out[4] = 0.0f; out[8] = 0.0f; out[12] = 0.0f;
out[1] = 0.0f; out[5] = c; out[9] = -s; out[13] = 0.0f;
out[2] = 0.0f; out[6] = s; out[10] = c; out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void mat4_set_rotate_y(mat4_t out, float angle)
{
float rad = angle * MATH_DEG_TO_RAD;
float c = cosf(rad);
float s = sinf(rad);
out[0] = c; out[4] = 0.0f; out[8] = s; out[12] = 0.0f;
out[1] = 0.0f; out[5] = 1.0f; out[9] = 0.0f; out[13] = 0.0f;
out[2] = -s; out[6] = 0.0f; out[10] = c; out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void mat4_set_rotate_z(mat4_t out, float angle)
{
float rad = angle * MATH_DEG_TO_RAD;
float c = cosf(rad);
float s = sinf(rad);
out[0] = c; out[4] = -s; out[8] = 0.0f; out[12] = 0.0f;
out[1] = s; out[5] = c; out[9] = 0.0f; out[13] = 0.0f;
out[2] = 0.0f; out[6] = 0.0f; out[10] = 1.0f; out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void mat4_set_scale(mat4_t out, const vec3_t scale)
{
out[0] = scale[0]; out[4] = 0.0f; out[8] = 0.0f; out[12] = 0.0f;
out[1] = 0.0f; out[5] = scale[1]; out[9] = 0.0f; out[13] = 0.0f;
out[2] = 0.0f; out[6] = 0.0f; out[10] = scale[2]; out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void mat4_set_translate(mat4_t out, const vec3_t translate)
{
out[0] = 1.0f; out[4] = 0.0f; out[8] = 0.0f; out[12] = translate[0];
out[1] = 0.0f; out[5] = 1.0f; out[9] = 0.0f; out[13] = translate[1];
out[2] = 0.0f; out[6] = 0.0f; out[10] = 1.0f; out[14] = translate[2];
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void mat4_set_perspective(mat4_t out, float fov, float aspect,
float znear, float zfar)
{
if (fabs(fov - 90.0f) < MATH_EPSILON)
fov = 89.9f;
float y = tanf(fov * MATH_PI / 360.0f);
float x = y * aspect;
out[0] = 1.0f / x;
out[1] = 0.0f;
out[2] = 0.0f;
out[3] = 0.0f;
out[4] = 0.0f;
out[5] = 1.0f / y;
out[6] = 0.0f;
out[7] = 0.0f;
out[8] = 0.0f;
out[9] = 0.0f;
out[10] = -(zfar + znear) / (zfar - znear);
out[11] = -1.0f;
out[12] = 0.0f;
out[13] = 0.0f;
out[14] = -(2.0f * zfar * znear) / (zfar - znear);
out[15] = 0.0f;
}
void mat4_set_ortho(mat4_t out, float left, float right,
float bottom, float top, float znear, float zfar)
{
float x = 2.0f / (right - left);
float y = 2.0f / (top - bottom);
float z = -2.0f / (zfar - znear);
float tx = - ((right + left) / (right - left));
float ty = - ((top + bottom) / (top - bottom));
float tz = - ((zfar + znear) / (zfar - znear));
out[0] = x;
out[1] = 0.0f;
out[2] = 0.0f;
out[3] = 0.0f;
out[4] = 0.0f;
out[5] = y;
out[6] = 0.0f;
out[7] = 0.0f;
out[8] = 0.0f;
out[9] = 0.0f;
out[10] = z;
out[11] = 0.0f;
out[12] = tx;
out[13] = ty;
out[14] = tz;
out[15] = 1.0f;
}
void mat4_set_look_at(mat4_t out, const vec3_t eye,
const vec3_t center, const vec3_t up)
{
vec3_t n,u,s,neye;
mat4_t translate, tmp;
vec3_sub(n, eye, center);
vec3_normalize(n);
vec3_cross(s, up, n);
vec3_normalize(s);
vec3_cross(u, n, s);
vec3_normalize(u);
tmp[0] = s[0]; tmp[4] = s[1]; tmp[8] = s[2]; tmp[12] = 0.0f;
tmp[1] = u[0]; tmp[5] = u[1]; tmp[9] = u[2]; tmp[13] = 0.0f;
tmp[2] = n[0]; tmp[6] = n[1]; tmp[10] = n[2]; tmp[14] = 0.0f;
tmp[3] = 0.0f; tmp[7] = 0.0f; tmp[11] = 0.0f; tmp[15] = 1.0f;
vec3_neg(neye, eye);
mat4_set_translate(translate, neye);
mat4_mul(out, tmp, translate);
}
/**************************************************************************
quat: quaterion representing rotation
**************************************************************************/
void quat_from_mat4(quat_t out, const mat4_t m)
{
float trace = m[0] + m[5] + m[10];
if (trace > 0.0f) {
float s = sqrtf(trace + 1.0f);
out[3] = 0.5f * s;
s = 0.5f / s;
out[0] = (m[6] - m[9]) * s;
out[1] = (m[8] - m[2]) * s;
out[2] = (m[1] - m[4]) * s;
} else {
static int next[3] = { 1, 2, 0 };
int i = 0;
if (m[5] > m[0]) i = 1;
if (m[10] > m[4 * i + i]) i = 2;
int j = next[i];
int k = next[j];
float s = sqrtf(m[4 * i + i] -
m[4 * j + j] -
m[4 * k + k] + 1.0f);
out[i] = 0.5f * s;
if (s != 0) s = 0.5f / s;
out[3] = (m[4 * j + k] - m[4 * k + j]) * s;
out[j] = (m[4 * i + j] + m[4 * j + i]) * s;
out[k] = (m[4 * i + k] + m[4 * k + i]) * s;
}
}
void quat_mul(quat_t out, const quat_t q1, const quat_t q2)
{
out[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
out[1] = q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2];
out[2] = q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0];
out[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
}
void quat_to_mat4(mat4_t out, const quat_t q)
{
float x2 = q[0] + q[0];
float y2 = q[1] + q[1];
float z2 = q[2] + q[2];
float xx = q[0] * x2;
float yy = q[1] * y2;
float zz = q[2] * z2;
float xy = q[0] * y2;
float yz = q[1] * z2;
float xz = q[2] * x2;
float wx = q[3] * x2;
float wy = q[3] * y2;
float wz = q[3] * z2;
out[0] = 1.0f - (yy + zz); out[4] = xy - wz; out[8] = xz + wy; out[12] = 0.0f;
out[1] = xy + wz; out[5] = 1.0f - (xx + zz); out[9] = yz - wx; out[13] = 0.0f;
out[2] = xz - wy; out[6] = yz + wx; out[10] = 1.0f - (xx + yy); out[14] = 0.0f;
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void quat_slerp(quat_t out, const quat_t q1, const quat_t q2, float t)
{
float k0,k1,cosomega = q1[0] * q2[0] + q1[1] * q2[1] +
q1[2] * q2[2] + q1[3] * q2[3];
quat_t q;
if (cosomega < 0.0f) {
cosomega = -cosomega;
q[0] = -q1[0];
q[1] = -q1[1];
q[2] = -q1[2];
q[3] = -q1[3];
} else {
q[0] = q1[0];
q[1] = q1[1];
q[2] = q1[2];
q[3] = q1[3];
}
if (1.0f - cosomega > MATH_EPSILON) {
float omega = acosf(cosomega);
float sinomega = sinf(omega);
k0 = sinf((1.0f - t) * omega) / sinomega;
k1 = sinf(t * omega) / sinomega;
} else {
k0 = 1.0f - t;
k1 = t;
}
out[0] = q1[0] * k0 + q[0] * k1;
out[1] = q1[1] * k0 + q[1] * k1;
out[2] = q1[2] * k0 + q[2] * k1;
out[3] = q1[3] * k0 + q[3] * k1;
}
void quat_from_angle(quat_t out, const vec3_t dir, float angle)
{
out[0] = out[1] = out[2] = 0.0f;
out[3] = 1.0f;
float length = vec3_length(dir);
if (length != 0.0f)
{
float halfangle = angle * MATH_DEG_TO_RAD / 2.0f;
length = 1.0f / length;
float sinangle = sinf(halfangle);
out[0] = dir[0] * length * sinangle;
out[1] = dir[1] * length * sinangle;
out[2] = dir[2] * length * sinangle;
out[3] = cosf(halfangle);
}
}
/**************************************************************************
aabb: axis-aligned bounding box
**************************************************************************/
void aabb_add_point(aabb_t aabb, const vec3_t point)
{
if (AABB_MIN_X(aabb) > VEC_X(point))
AABB_MIN_X(aabb) = VEC_X(point);
if (AABB_MIN_Y(aabb) > VEC_Y(point))
AABB_MIN_Y(aabb) = VEC_Y(point);
if (AABB_MIN_Z(aabb) > VEC_Z(point))
AABB_MIN_Z(aabb) = VEC_Z(point);
if (AABB_MAX_X(aabb) < VEC_X(point))
AABB_MAX_X(aabb) = VEC_X(point);
if (AABB_MAX_Y(aabb) < VEC_Y(point))
AABB_MAX_Y(aabb) = VEC_Y(point);
if (AABB_MAX_Z(aabb) < VEC_Z(point))
AABB_MAX_Z(aabb) = VEC_Z(point);
}
void aabb_add_aabb(aabb_t aabb1, const aabb_t aabb2)
{
aabb_add_point(aabb1, AABB_MIN(aabb2));
aabb_add_point(aabb1, AABB_MAX(aabb2));
}
/**************************************************************************
plane: three-dimensional plane
**************************************************************************/
enum plane_side plane_side_point(const plane_t plane, const vec3_t point)
{
if (vec3_dot(plane, point) + PLANE_D(plane) >= 0.0f)
return PLANE_FRONT;
return PLANE_BACK;
}
enum plane_side plane_side_aabb(const plane_t plane, const aabb_t aabb)
{
vec3_t near, far;
vec3_copy(near, AABB_MAX(aabb));
vec3_copy(far, AABB_MIN(aabb));
if (PLANE_NX(plane) > 0) {
VEC_X(near) = AABB_MIN_X(aabb);
VEC_X(far) = AABB_MAX_X(aabb);
}
if (PLANE_NY(plane) > 0) {
VEC_Y(near) = AABB_MIN_Y(aabb);
VEC_Y(far) = AABB_MAX_Y(aabb);
}
if (PLANE_NZ(plane) > 0) {
VEC_Z(near) = AABB_MIN_Z(aabb);
VEC_Z(far) = AABB_MAX_Z(aabb);
}
if (vec3_dot(plane, near) + PLANE_D(plane) > 0)
return PLANE_FRONT;
if (vec3_dot(plane, far) + PLANE_D(plane) > 0)
return PLANE_BOTH;
return PLANE_BACK;
}
/**************************************************************************
frustum
**************************************************************************/
void frustum_set(frustum_t out, const mat4_t m)
{
int i;
/* left clipping plane */
PLANE_NX(FRUSTUM_PLANE(out, FRUSTUM_PLANE_LEFT)) = m[3] + m[0];
PLANE_NY(FRUSTUM_PLANE(out, FRUSTUM_PLANE_LEFT)) = m[7] + m[4];
PLANE_NZ(FRUSTUM_PLANE(out, FRUSTUM_PLANE_LEFT)) = m[11] + m[8];
PLANE_D(FRUSTUM_PLANE(out, FRUSTUM_PLANE_LEFT)) = m[15] + m[12];
/* right clipping plane */
PLANE_NX(FRUSTUM_PLANE(out, FRUSTUM_PLANE_RIGHT)) = m[3] - m[0];
PLANE_NY(FRUSTUM_PLANE(out, FRUSTUM_PLANE_RIGHT)) = m[7] - m[4];
PLANE_NZ(FRUSTUM_PLANE(out, FRUSTUM_PLANE_RIGHT)) = m[11] - m[8];
PLANE_D(FRUSTUM_PLANE(out, FRUSTUM_PLANE_RIGHT)) = m[15] - m[12];
/* top clipping plane */
PLANE_NX(FRUSTUM_PLANE(out, FRUSTUM_PLANE_TOP)) = m[3] - m[1];
PLANE_NY(FRUSTUM_PLANE(out, FRUSTUM_PLANE_TOP)) = m[7] - m[5];
PLANE_NZ(FRUSTUM_PLANE(out, FRUSTUM_PLANE_TOP)) = m[11] - m[9];
PLANE_D(FRUSTUM_PLANE(out, FRUSTUM_PLANE_TOP)) = m[15] - m[13];
/* bottom clipping plane */
PLANE_NX(FRUSTUM_PLANE(out, FRUSTUM_PLANE_BOTTOM)) = m[3] + m[1];
PLANE_NY(FRUSTUM_PLANE(out, FRUSTUM_PLANE_BOTTOM)) = m[7] + m[5];
PLANE_NZ(FRUSTUM_PLANE(out, FRUSTUM_PLANE_BOTTOM)) = m[11] + m[9];
PLANE_D(FRUSTUM_PLANE(out, FRUSTUM_PLANE_BOTTOM)) = m[15] + m[13];
/* far clipping plane */
PLANE_NX(FRUSTUM_PLANE(out, FRUSTUM_PLANE_FAR)) = m[3] - m[2];
PLANE_NY(FRUSTUM_PLANE(out, FRUSTUM_PLANE_FAR)) = m[7] - m[6];
PLANE_NZ(FRUSTUM_PLANE(out, FRUSTUM_PLANE_FAR)) = m[11] - m[10];
PLANE_D(FRUSTUM_PLANE(out, FRUSTUM_PLANE_FAR)) = m[15] - m[14];
/* near clipping plane */
PLANE_NX(FRUSTUM_PLANE(out, FRUSTUM_PLANE_NEAR)) = m[3] + m[2];
PLANE_NY(FRUSTUM_PLANE(out, FRUSTUM_PLANE_NEAR)) = m[7] + m[6];
PLANE_NZ(FRUSTUM_PLANE(out, FRUSTUM_PLANE_NEAR)) = m[11] + m[10];
PLANE_D(FRUSTUM_PLANE(out, FRUSTUM_PLANE_NEAR)) = m[15] + m[14];
for (i = 0; i < 6; ++i) {
const float ilen = 1.0f / vec3_length(FRUSTUM_PLANE(out, i));
vec3_smula(FRUSTUM_PLANE(out, i), ilen);
PLANE_D(FRUSTUM_PLANE(out, i)) *= ilen;
}
}
enum frustum_side frustum_side_aabb(const frustum_t frustum, const aabb_t aabb)
{
int i;
enum frustum_side result = FRUSTUM_OUTSIDE;
for (i = 0; i < 6; ++i) {
enum plane_side ps = plane_side_aabb(FRUSTUM_PLANE(frustum, i), aabb);
switch (ps) {
case PLANE_FRONT:
if (result != FRUSTUM_BOTH)
result = FRUSTUM_INSIDE;
break;
case PLANE_BOTH:
result = FRUSTUM_BOTH;
break;
case PLANE_BACK:
return FRUSTUM_OUTSIDE;
}
}
return result;
}
/**************************************************************************
intersections
**************************************************************************/
static bool aabb_is_point_inside(const aabb_t aabb, const vec3_t point)
{
return (AABB_MIN_X(aabb) <= VEC_X(point) &&
AABB_MIN_Y(aabb) <= VEC_Y(point) &&
AABB_MIN_Z(aabb) <= VEC_Z(point) &&
AABB_MAX_X(aabb) >= VEC_X(point) &&
AABB_MAX_Y(aabb) >= VEC_Y(point) &&
AABB_MAX_Z(aabb) >= VEC_Z(point));
}
bool aabb_clip_ray(const aabb_t aabb, const vec3_t orig, const vec3_t dir,
float *t_min, float *t_max)
{
vec3_t inv_dir;
vec3_inv(inv_dir, dir);
unsigned int a;
if (aabb_is_point_inside(aabb, orig)) {
*t_min = 0.0f;
float t_max_min = FLT_MAX;
for (a = 0; a < 3; ++a) {
float tmin = (AABB_MIN(aabb)[a] - orig[a]) * inv_dir[a];
float tmax = (AABB_MAX(aabb)[a] - orig[a]) * inv_dir[a];
if (tmin > 0.0f)
t_max_min = MIN(t_max_min, tmin);
if (tmax > 0.0f)
t_max_min = MIN(t_max_min, tmax);
}
*t_max = t_max_min;
return true;
}
float t_min_max = 0.0f;
float t_max_min = FLT_MAX;
for (a = 0; a < 3; ++a) {
float tmin = (AABB_MIN(aabb)[a] - orig[a]) * inv_dir[a];
float tmax = (AABB_MAX(aabb)[a] - orig[a]) * inv_dir[a];
if (AABB_MIN(aabb)[a] > orig[a]) {
t_min_max = MAX(t_min_max, tmin);
t_max_min = MIN(t_max_min, tmax);
} else if (AABB_MAX(aabb)[a] < orig[a]) {
t_min_max = MAX(t_min_max, tmax);
t_max_min = MIN(t_max_min, tmin);
}
}
if (t_min_max > t_max_min) {
return false;
}
*t_min = t_min_max;
*t_max = t_max_min;
return true;
}
void basis_to_mat4(mat4_t out, const vec3_t origin, const vec3_t d)
{
vec3_t u, v;
if (fabs(d[0]) >= fabs(d[1])) {
float f = 1.0f / sqrtf(d[0]*d[0] + d[2]*d[2]);
u[0] = -d[2]*f;
u[1] = 0;
u[2] = d[0]*f;
} else {
float f = 1.0f / sqrtf(d[1]*d[1] + d[2]*d[2]);
u[0] = 0;
u[1] = d[2]*f;
u[2] = -d[1]*f;
}
vec3_cross(v, u, d);
out[0] = v[0]; out[4] = d[0]; out[8] = u[0]; out[12] = origin[0];
out[1] = v[1]; out[5] = d[1]; out[9] = u[1]; out[13] = origin[1];
out[2] = v[2]; out[6] = d[2]; out[10] = u[2]; out[14] = origin[2];
out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
}
void cylinder_to_obb(mat4_t basis_out, aabb_t extents_out,
const vec3_t origin, const vec3_t dir,
float height, float radius)
{
float halfh = height * 0.5f;
basis_to_mat4(basis_out, origin, dir);
AABB_MIN_X(extents_out) = -radius;
AABB_MIN_Y(extents_out) = -halfh;
AABB_MIN_Z(extents_out) = -radius;
AABB_MAX_X(extents_out) = radius;
AABB_MAX_Y(extents_out) = halfh;
AABB_MAX_Z(extents_out) = radius;
}
bool aabb_obb_intersection(const aabb_t aabb, const mat4_t basis,
const aabb_t obb)
{
vec3_t a, b, aabb_center, T;
float ra, rb, t;
vec3_sub(a, AABB_MAX(aabb), AABB_MIN(aabb));
vec3_smula(a, 0.5f);
vec3_sub(b, AABB_MAX(obb), AABB_MIN(obb));
vec3_smula(b, 0.5f);
vec3_add(aabb_center, AABB_MIN(aabb), AABB_MAX(aabb));
vec3_smula(aabb_center, 0.5f);
T[0] = basis[12] - aabb_center[0];
T[1] = basis[13] - aabb_center[1];
T[2] = basis[14] - aabb_center[2];
for (unsigned int i = 0; i < 3; ++i) {
ra = a[i];
rb = b[0] * fabs(MAT4_RC(basis, 0, i)) +
b[1] * fabs(MAT4_RC(basis, 1, i)) +
b[2] * fabs(MAT4_RC(basis, 2, i));
t = fabs(T[i]);
if (t > ra + rb)
return false;
}
for (unsigned int i = 0; i < 3; ++i) {
ra = a[0] * fabs(MAT4_RC(basis, i, 0)) +
a[1] * fabs(MAT4_RC(basis, i, 1)) +
a[2] * fabs(MAT4_RC(basis, i, 2));
rb = b[i];
t = fabs(T[0] * MAT4_RC(basis, i, 0) +
T[1] * MAT4_RC(basis, i, 1) +
T[2] * MAT4_RC(basis, i, 2));
if (t > ra + rb)
return false;
}
/* 9 cross products */
/* L = A0 x B0 */
ra = a[1] * fabs(MAT4_RC(basis, 0, 2)) + a[2] * fabs(MAT4_RC(basis, 0, 1));
rb = b[1] * fabs(MAT4_RC(basis, 2, 0)) + b[2] * fabs(MAT4_RC(basis, 1, 0));
t = fabs(T[2] * MAT4_RC(basis, 0, 1) - T[1] * MAT4_RC(basis, 0, 2));
if (t > ra + rb)
return false;
/* L = A0 x B1 */
ra = a[1] * fabs(MAT4_RC(basis, 1, 2)) + a[2] * fabs(MAT4_RC(basis, 1, 1));
rb = b[0] * fabs(MAT4_RC(basis, 2, 0)) + b[2] * fabs(MAT4_RC(basis, 0, 0));
t = fabs(T[2] * MAT4_RC(basis, 1, 1) - T[1] * MAT4_RC(basis, 1, 2));
if (t > ra + rb)
return false;
/* L = A0 x B2 */
ra = a[1] * fabs(MAT4_RC(basis, 2, 2)) + a[2] * fabs(MAT4_RC(basis, 2, 1));
rb = b[0] * fabs(MAT4_RC(basis, 1, 0)) + b[1] * fabs(MAT4_RC(basis, 0, 0));
t = fabs(T[2] * MAT4_RC(basis, 2, 1) - T[1] * MAT4_RC(basis, 2, 2));
if (t > ra + rb)
return false;
/* L = A1 x B0 */
ra = a[0] * fabs(MAT4_RC(basis, 0, 2)) + a[2] * fabs(MAT4_RC(basis, 0, 0));
rb = b[1] * fabs(MAT4_RC(basis, 2, 1)) + b[2] * fabs(MAT4_RC(basis, 1, 1));
t = fabs(T[0] * MAT4_RC(basis, 0, 2) - T[2] * MAT4_RC(basis, 0, 0));
if (t > ra + rb)
return false;
/* L = A1 x B1 */
ra = a[0] * fabs(MAT4_RC(basis, 1, 2)) + a[2] * fabs(MAT4_RC(basis, 1, 0));
rb = b[0] * fabs(MAT4_RC(basis, 2, 1)) + b[2] * fabs(MAT4_RC(basis, 0, 1));
t = fabs(T[0] * MAT4_RC(basis, 1, 2) - T[2] * MAT4_RC(basis, 1, 0));
if (t > ra + rb)
return false;
/* L = A1 x B2 */
ra = a[0] * fabs(MAT4_RC(basis, 2, 2)) + a[2] * fabs(MAT4_RC(basis, 2, 0));
rb = b[0] * fabs(MAT4_RC(basis, 1, 1)) + b[1] * fabs(MAT4_RC(basis, 0, 1));
t = fabs(T[0] * MAT4_RC(basis, 2, 2) - T[2] * MAT4_RC(basis, 2, 0));
if (t > ra + rb)
return false;
/* L = A2 x B0 */
ra = a[0] * fabs(MAT4_RC(basis, 0, 1)) + a[1] * fabs(MAT4_RC(basis, 0, 0));
rb = b[1] * fabs(MAT4_RC(basis, 2, 2)) + b[2] * fabs(MAT4_RC(basis, 1, 2));
t = fabs(T[1] * MAT4_RC(basis, 0, 0) - T[0] * MAT4_RC(basis, 0, 1));
if (t > ra + rb)
return false;
/* L = A2 x B1 */
ra = a[0] * fabs(MAT4_RC(basis, 1, 1)) + a[1] * fabs(MAT4_RC(basis, 1, 0));
rb = b[0] * fabs(MAT4_RC(basis, 2, 2)) + b[2] * fabs(MAT4_RC(basis, 0, 2));
t = fabs(T[1] * MAT4_RC(basis, 1, 0) - T[0] * MAT4_RC(basis, 1, 1));
if (t > ra + rb)
return false;
/* L = A2 x B2 */
ra = a[0] * fabs(MAT4_RC(basis, 2, 1)) + a[1] * fabs(MAT4_RC(basis, 2, 0));
rb = b[0] * fabs(MAT4_RC(basis, 1, 2)) + b[1] * fabs(MAT4_RC(basis, 0, 2));
t = fabs(T[1] * MAT4_RC(basis, 2, 0) - T[0] * MAT4_RC(basis, 2, 1));
if (t > ra + rb)
return false;
return true;
}
/**************************************************************************
SSSS SSSS EEEEEE
S S S S E
S S E
SSSS SSSS EEEEEE
S S E
S S S S E
SSSS SSSS EEEEEE
**************************************************************************/
void mat4_sse_mul(mat4_sse_t out, const mat4_sse_t m1, const mat4_sse_t m2)
{
register __m128 x0, x1, x2, x3, x4, x5, x6, x7;
x0 = _mm_load_ps(&m1[0]);
x1 = _mm_load_ps(&m1[4]);
x2 = _mm_load_ps(&m1[8]);
x3 = _mm_load_ps(&m1[12]);
x4 = _mm_load_ps1(&m2[0]);
x5 = _mm_load_ps1(&m2[1]);
x6 = _mm_load_ps1(&m2[2]);
x7 = _mm_load_ps1(&m2[3]);
x4 = _mm_mul_ps(x4, x0);
x5 = _mm_mul_ps(x5, x1);
x6 = _mm_mul_ps(x6, x2);
x7 = _mm_mul_ps(x7, x3);
x4 = _mm_add_ps(x4, x5);
x6 = _mm_add_ps(x6, x7);
x4 = _mm_add_ps(x4, x6);
x5 = _mm_load_ps1(&m2[4]);
x6 = _mm_load_ps1(&m2[5]);
x7 = _mm_load_ps1(&m2[6]);
x5 = _mm_mul_ps(x5, x0);
x6 = _mm_mul_ps(x6, x1);
x7 = _mm_mul_ps(x7, x2);
x5 = _mm_add_ps(x5, x6);
x5 = _mm_add_ps(x5, x7);
x6 = _mm_load_ps1(&m2[7]);
x6 = _mm_mul_ps(x6, x3);
x5 = _mm_add_ps(x5, x6);
x6 = _mm_load_ps1(&m2[8]);
x7 = _mm_load_ps1(&m2[9]);
x6 = _mm_mul_ps(x6, x0);
x7 = _mm_mul_ps(x7, x1);
x6 = _mm_add_ps(x6, x7);
x7 = _mm_load_ps1(&m2[10]);
x7 = _mm_mul_ps(x7, x2);
x6 = _mm_add_ps(x6, x7);
x7 = _mm_load_ps1(&m2[11]);
x7 = _mm_mul_ps(x7, x3);
x6 = _mm_add_ps(x6, x7);
x7 = _mm_load_ps1(&m2[12]);
x0 = _mm_mul_ps(x0, x7);
x7 = _mm_load_ps1(&m2[13]);
x1 = _mm_mul_ps(x1, x7);
x7 = _mm_load_ps1(&m2[14]);
x2 = _mm_mul_ps(x2, x7);
x7 = _mm_load_ps1(&m2[15]);
x3 = _mm_mul_ps(x3, x7);
x0 = _mm_add_ps(x0, x1);
x2 = _mm_add_ps(x2, x3);
x0 = _mm_add_ps(x0, x2);
/* store result */
_mm_store_ps(&out[0], x4);
_mm_store_ps(&out[4], x5);
_mm_store_ps(&out[8], x6);
_mm_store_ps(&out[12], x0);
}
#define vec4_swizzle(v, x, y, z, w) \
_mm_shuffle_ps((v),(v), _MM_SHUFFLE(w, z, y, x))
static const uint32_t change_w_sign_mask_i[4] __attribute__((aligned(16))) = {
0, 0, 0, 0x80000000
};
void quat_sse_mul(quat_sse_t out, const quat_sse_t q1, const quat_sse_t q2)
{
/*
mul0 mul1 mul2 mul3
out[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
out[1] = q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2];
out[2] = q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0];
out[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
*/
__m128 a = _mm_load_ps(q1);
__m128 b = _mm_load_ps(q2);
__m128 s0 = vec4_swizzle(a, 3, 3, 3, 3);
__m128 s1 = vec4_swizzle(a, 0, 1, 2, 0);
__m128 s2 = vec4_swizzle(b, 3, 3, 3, 0);
__m128 s3 = vec4_swizzle(a, 1, 2, 0, 1);
__m128 s4 = vec4_swizzle(b, 2, 0, 1, 1);
__m128 s5 = vec4_swizzle(a, 2, 0, 1, 2);
__m128 s6 = vec4_swizzle(b, 1, 2, 0, 2);
__m128 mul0 = _mm_mul_ps(s0, b);
__m128 mul1 = _mm_mul_ps(s1, s2);
__m128 mul2 = _mm_mul_ps(s3, s4);
__m128 mul3 = _mm_mul_ps(s5, s6);
__m128 change_w_sign_mask = _mm_load_ps((float*)change_w_sign_mask_i);
/* flip w sign for mul1 and mul2 */
mul1 = _mm_xor_ps(mul1, change_w_sign_mask);
mul2 = _mm_xor_ps(mul2, change_w_sign_mask);
__m128 mul2_m_mul3 = _mm_sub_ps(mul2, mul3);
__m128 mul1_p_mul2m3 = _mm_add_ps(mul1, mul2_m_mul3);
__m128 result = _mm_add_ps(mul0, mul1_p_mul2m3);
_mm_store_ps(out, result);
}
Raw
linear_math.h
#pragma once
/*
* Linear math.
*/
#include <string.h>
#include <math.h>
#include "common.h"
/* Math constants. */
#define MATH_PI (3.14159265359f)
#define MATH_DEG_TO_RAD (MATH_PI / 180.0f)
#define MATH_RAD_TO_DEG (180.0f / MATH_PI)
#define MATH_EPSILON (1e-6f)
/*
* Basic math types.
* But don't forget that these are just arrays of numbers. And you can use
* vec3_t functions on vec4_t for example.
*/
typedef int rect_t[4]; /* x, y, w, h */
typedef float vec2_t[2]; /* x. y */
typedef float vec3_t[3]; /* x, y, z */
typedef float vec4_t[4]; /* x, y, z, w */
typedef float quat_t[4]; /* x, y, z, w */
typedef float aabb_t[6]; /* vec3_t min, vec3_t max */
typedef float mat4_t[16]; /* matrix 4x4 */
typedef float plane_t[4]; /* nx, ny, nz, d */
typedef float frustum_t[24]; /* far, near, left, right, bottom, top planes */
/* Fast 16-bytes aligned types (SSE) */
typedef float vec4_sse_t[4] __attribute__((aligned(16)));
typedef float quat_sse_t[4] __attribute__((aligned(16)));
typedef float mat4_sse_t[16] __attribute__((aligned(16)));
enum plane_side {
PLANE_FRONT,
PLANE_BACK,
PLANE_BOTH
};
enum frustum_side {
FRUSTUM_INSIDE,
FRUSTUM_OUTSIDE,
FRUSTUM_BOTH
};
enum frustum_plane {
FRUSTUM_PLANE_NEAR,
FRUSTUM_PLANE_FAR,
FRUSTUM_PLANE_LEFT,
FRUSTUM_PLANE_RIGHT,
FRUSTUM_PLANE_BOTTOM,
FRUSTUM_PLANE_TOP
};
/* Accessors. */
#define RECT_X(r) ((r)[0])
#define RECT_Y(r) ((r)[1])
#define RECT_W(r) ((r)[2])
#define RECT_H(r) ((r)[3])
#define VEC_X(v) ((v)[0])
#define VEC_Y(v) ((v)[1])
#define VEC_Z(v) ((v)[2])
#define VEC_W(v) ((v)[3])
#define MAT4_RC(m, row, column) ((m)[(row)*4+(column)])
#define PLANE_NX(p) ((p)[0])
#define PLANE_NY(p) ((p)[1])
#define PLANE_NZ(p) ((p)[2])
#define PLANE_D(p) ((p)[3])
#define AABB_MIN(s) (s)
#define AABB_MAX(s) ((s)+3)
#define AABB_MIN_X(s) ((s)[0])
#define AABB_MIN_Y(s) ((s)[1])
#define AABB_MIN_Z(s) ((s)[2])
#define AABB_MAX_X(s) ((s)[3])
#define AABB_MAX_Y(s) ((s)[4])
#define AABB_MAX_Z(s) ((s)[5])
#define FRUSTUM_PLANE(f, p) ((f)+((p)*4))
/* Constants */
#define VEC2_ZERO (vec2_t){0.0f, 0.0f}
#define VEC2_UNIT_X (vec2_t){1.0f, 0.0f}
#define VEC2_UNIT_Y (vec2_t){0.0f, 1.0f}
#define VEC3_ZERO (vec3_t){0.0f, 0.0f, 0.0f}
#define VEC3_UNIT_X (vec3_t){1.0f, 0.0f, 0.0f}
#define VEC3_UNIT_Y (vec3_t){0.0f, 1.0f, 0.0f}
#define VEC3_UNIT_Z (vec3_t){0.0f, 0.0f, 1.0f}
/* Other useful macros */
#undef MAX
#undef MIN
#undef CLAMP
#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MAX3(a,b,c) MAX((a), MAX((b), (c)))
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#define MIN3(a,b,c) MIN((a), MIN((b), (c)))
#define CLAMP(v, l, h) \
do { \
if ((v) > (h)) \
(v) = (h); \
if ((v) < (l)) \
(v) = (l); \
} while (0)
/*
/------------------------------------------------------------------\
/ Vector math operations table. \
+-------+-----------------------------------+--------+---------------+
| name | meaning | args | example |
+-------+-----------------------------------+--------+---------------+
| adda | addition assignment | 2 | out += v |
| suba | subtraction assignment | 2 | out -= v |
| mula | multiplication assignment | 2 | out *= v |
| diva | division assignment | 2 | out /= v |
| add | addition | 3 | out = v1 + v2 |
| sub | subtraction | 3 | out = v1 - v2 |
| mul | multiplication | 3 | out = v1 * v2 |
| div | division | 3 | out = v1 / v2 |
| sadda | scalar addition assignment | 2 | out += s |
| ssuba | scalar subtraction assignment | 2 | out -= s |
| smula | scalar multiplication assignment | 2 | out *= s |
| sdiva | scalar division assignment | 2 | out /= s |
| sadd | scalar addition | 3 | out = v1 + s |
| ssub | scalar subtraction | 3 | out = v1 - s |
| smul | scalar multiplication | 3 | out = v1 * s |
| sdiv | scalar division | 3 | out = v1 / s |
+-------+-----------------------------------+--------+---------------+
*/
/**************************************************************************
vec2: two-dimensional vector
**************************************************************************/
/************************************************************
* Macro generated vector operation functions for vec2 *
* (select in vim with visual mode and type !m4 to process) *
************************************************************
define(`ASSIGNMENT_VEC2_FUNCTION',
`static inline void vec2_$2(vec2_t out, const vec2_t v)
{
VEC_X(out) $1 VEC_X(v);
VEC_Y(out) $1 VEC_Y(v);
}
')dnl
define(`GENERAL_VEC2_FUNCTION',
`static inline void vec2_$2(vec2_t out, const vec2_t v1, const vec2_t v2)
{
VEC_X(out) = VEC_X(v1) $1 VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) $1 VEC_Y(v2);
}
')dnl
ASSIGNMENT_VEC2_FUNCTION(+=, adda)
ASSIGNMENT_VEC2_FUNCTION(-=, suba)
ASSIGNMENT_VEC2_FUNCTION(*=, mula)
ASSIGNMENT_VEC2_FUNCTION(/=, diva)
GENERAL_VEC2_FUNCTION(+, add)
GENERAL_VEC2_FUNCTION(-, sub)
GENERAL_VEC2_FUNCTION(*, mul)
GENERAL_VEC2_FUNCTION(/, div)
*/
static inline void vec2_adda(vec2_t out, const vec2_t v)
{
VEC_X(out) += VEC_X(v);
VEC_Y(out) += VEC_Y(v);
}
static inline void vec2_suba(vec2_t out, const vec2_t v)
{
VEC_X(out) -= VEC_X(v);
VEC_Y(out) -= VEC_Y(v);
}
static inline void vec2_mula(vec2_t out, const vec2_t v)
{
VEC_X(out) *= VEC_X(v);
VEC_Y(out) *= VEC_Y(v);
}
static inline void vec2_diva(vec2_t out, const vec2_t v)
{
VEC_X(out) /= VEC_X(v);
VEC_Y(out) /= VEC_Y(v);
}
static inline void vec2_add(vec2_t out, const vec2_t v1, const vec2_t v2)
{
VEC_X(out) = VEC_X(v1) + VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) + VEC_Y(v2);
}
static inline void vec2_sub(vec2_t out, const vec2_t v1, const vec2_t v2)
{
VEC_X(out) = VEC_X(v1) - VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) - VEC_Y(v2);
}
static inline void vec2_mul(vec2_t out, const vec2_t v1, const vec2_t v2)
{
VEC_X(out) = VEC_X(v1) * VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) * VEC_Y(v2);
}
static inline void vec2_div(vec2_t out, const vec2_t v1, const vec2_t v2)
{
VEC_X(out) = VEC_X(v1) / VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) / VEC_Y(v2);
}
/* END
************************************************************************/
/************************************************************
* Macro generated scalar operation functions for vec2 *
* (select in vim with visual mode and type !m4 to process) *
************************************************************
define(`SCALAR_ASSIGNMENT_VEC2_FUNCTION',
`static inline void vec2_$2(vec2_t out, float s)
{
VEC_X(out) $1 s;
VEC_Y(out) $1 s;
}
')dnl
define(`SCALAR_GENERAL_VEC2_FUNCTION',
`static inline void vec2_$2(vec2_t out, const vec2_t v1, float s)
{
VEC_X(out) = VEC_X(v1) $1 s;
VEC_Y(out) = VEC_Y(v1) $1 s;
}
')dnl
SCALAR_ASSIGNMENT_VEC2_FUNCTION(+=, sadda)
SCALAR_ASSIGNMENT_VEC2_FUNCTION(-=, ssuba)
SCALAR_ASSIGNMENT_VEC2_FUNCTION(*=, smula)
SCALAR_ASSIGNMENT_VEC2_FUNCTION(/=, sdiva)
SCALAR_GENERAL_VEC2_FUNCTION(+, sadd)
SCALAR_GENERAL_VEC2_FUNCTION(-, ssub)
SCALAR_GENERAL_VEC2_FUNCTION(*, smul)
SCALAR_GENERAL_VEC2_FUNCTION(/, sdiv)
*/
static inline void vec2_sadda(vec2_t out, float s)
{
VEC_X(out) += s;
VEC_Y(out) += s;
}
static inline void vec2_ssuba(vec2_t out, float s)
{
VEC_X(out) -= s;
VEC_Y(out) -= s;
}
static inline void vec2_smula(vec2_t out, float s)
{
VEC_X(out) *= s;
VEC_Y(out) *= s;
}
static inline void vec2_sdiva(vec2_t out, float s)
{
VEC_X(out) /= s;
VEC_Y(out) /= s;
}
static inline void vec2_sadd(vec2_t out, const vec2_t v1, float s)
{
VEC_X(out) = VEC_X(v1) + s;
VEC_Y(out) = VEC_Y(v1) + s;
}
static inline void vec2_ssub(vec2_t out, const vec2_t v1, float s)
{
VEC_X(out) = VEC_X(v1) - s;
VEC_Y(out) = VEC_Y(v1) - s;
}
static inline void vec2_smul(vec2_t out, const vec2_t v1, float s)
{
VEC_X(out) = VEC_X(v1) * s;
VEC_Y(out) = VEC_Y(v1) * s;
}
static inline void vec2_sdiv(vec2_t out, const vec2_t v1, float s)
{
VEC_X(out) = VEC_X(v1) / s;
VEC_Y(out) = VEC_Y(v1) / s;
}
/* END
************************************************************************/
/* utility functions for vec2 */
static inline void vec2_set(vec2_t out, float x, float y)
{
VEC_X(out) = x;
VEC_Y(out) = y;
}
static inline void vec2_neg(vec2_t out, const vec2_t v)
{
VEC_X(out) = -VEC_X(v);
VEC_Y(out) = -VEC_Y(v);
}
static inline void vec2_copy(vec2_t out, const vec2_t v)
{
memcpy(out, v, sizeof(float) * 2);
}
static inline float vec2_length2(const vec2_t v)
{
return (VEC_X(v)*VEC_X(v) + VEC_Y(v)*VEC_Y(v));
}
static inline float vec2_length(const vec2_t v)
{
return sqrtf(vec2_length2(v));
}
static inline void vec2_normalize(vec2_t out)
{
float ilength = 1.0f / vec2_length(out);
vec2_smula(out, ilength);
}
static inline float vec2_distance(const vec2_t v1, const vec2_t v2)
{
vec2_t v;
vec2_sub(v, v1, v2);
return vec2_length(v);
}
static inline float vec2_distance2(const vec2_t v1, const vec2_t v2)
{
vec2_t v;
vec2_sub(v, v1, v2);
return vec2_length2(v);
}
static inline bool vec2_equals(const vec2_t v1, const vec2_t v2)
{
return (VEC_X(v1) == VEC_X(v2) && VEC_Y(v1) == VEC_Y(v2));
}
static inline float vec2_dot(const vec2_t v1, const vec2_t v2)
{
return (VEC_X(v1)*VEC_X(v2) + VEC_Y(v1)*VEC_Y(v2));
}
void vec2_rotate(vec2_t out, float deg, const vec2_t origin);
/**************************************************************************
vec3: three-dimensional vector
**************************************************************************/
/************************************************************
* Macro generated vector operation functions for vec3 *
* (select in vim with visual mode and type !m4 to process) *
************************************************************
define(`ASSIGNMENT_VEC3_FUNCTION',
`static inline void vec3_$2(vec3_t out, const vec3_t v)
{
VEC_X(out) $1 VEC_X(v);
VEC_Y(out) $1 VEC_Y(v);
VEC_Z(out) $1 VEC_Z(v);
}
')dnl
define(`GENERAL_VEC3_FUNCTION',
`static inline void vec3_$2(vec3_t out, const vec3_t v1, const vec3_t v2)
{
VEC_X(out) = VEC_X(v1) $1 VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) $1 VEC_Y(v2);
VEC_Z(out) = VEC_Z(v1) $1 VEC_Z(v2);
}
')dnl
ASSIGNMENT_VEC3_FUNCTION(+=, adda)
ASSIGNMENT_VEC3_FUNCTION(-=, suba)
ASSIGNMENT_VEC3_FUNCTION(*=, mula)
ASSIGNMENT_VEC3_FUNCTION(/=, diva)
GENERAL_VEC3_FUNCTION(+, add)
GENERAL_VEC3_FUNCTION(-, sub)
GENERAL_VEC3_FUNCTION(*, mul)
GENERAL_VEC3_FUNCTION(/, div)
*/
static inline void vec3_adda(vec3_t out, const vec3_t v)
{
VEC_X(out) += VEC_X(v);
VEC_Y(out) += VEC_Y(v);
VEC_Z(out) += VEC_Z(v);
}
static inline void vec3_suba(vec3_t out, const vec3_t v)
{
VEC_X(out) -= VEC_X(v);
VEC_Y(out) -= VEC_Y(v);
VEC_Z(out) -= VEC_Z(v);
}
static inline void vec3_mula(vec3_t out, const vec3_t v)
{
VEC_X(out) *= VEC_X(v);
VEC_Y(out) *= VEC_Y(v);
VEC_Z(out) *= VEC_Z(v);
}
static inline void vec3_diva(vec3_t out, const vec3_t v)
{
VEC_X(out) /= VEC_X(v);
VEC_Y(out) /= VEC_Y(v);
VEC_Z(out) /= VEC_Z(v);
}
static inline void vec3_add(vec3_t out, const vec3_t v1, const vec3_t v2)
{
VEC_X(out) = VEC_X(v1) + VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) + VEC_Y(v2);
VEC_Z(out) = VEC_Z(v1) + VEC_Z(v2);
}
static inline void vec3_sub(vec3_t out, const vec3_t v1, const vec3_t v2)
{
VEC_X(out) = VEC_X(v1) - VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) - VEC_Y(v2);
VEC_Z(out) = VEC_Z(v1) - VEC_Z(v2);
}
static inline void vec3_mul(vec3_t out, const vec3_t v1, const vec3_t v2)
{
VEC_X(out) = VEC_X(v1) * VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) * VEC_Y(v2);
VEC_Z(out) = VEC_Z(v1) * VEC_Z(v2);
}
static inline void vec3_div(vec3_t out, const vec3_t v1, const vec3_t v2)
{
VEC_X(out) = VEC_X(v1) / VEC_X(v2);
VEC_Y(out) = VEC_Y(v1) / VEC_Y(v2);
VEC_Z(out) = VEC_Z(v1) / VEC_Z(v2);
}
/* END
************************************************************************/
/************************************************************
* Macro generated scalar operation functions for vec3 *
* (select in vim with visual mode and type !m4 to process) *
************************************************************
define(`SCALAR_ASSIGNMENT_VEC3_FUNCTION',
`static inline void vec3_$2(vec3_t out, float s)
{
VEC_X(out) $1 s;
VEC_Y(out) $1 s;
VEC_Z(out) $1 s;
}
')dnl
define(`SCALAR_GENERAL_VEC3_FUNCTION',
`static inline void vec3_$2(vec3_t out, const vec3_t v1, float s)
{
VEC_X(out) = VEC_X(v1) $1 s;
VEC_Y(out) = VEC_Y(v1) $1 s;
VEC_Z(out) = VEC_Z(v1) $1 s;
}
')dnl
SCALAR_ASSIGNMENT_VEC3_FUNCTION(+=, sadda)
SCALAR_ASSIGNMENT_VEC3_FUNCTION(-=, ssuba)
SCALAR_ASSIGNMENT_VEC3_FUNCTION(*=, smula)
SCALAR_ASSIGNMENT_VEC3_FUNCTION(/=, sdiva)
SCALAR_GENERAL_VEC3_FUNCTION(+, sadd)
SCALAR_GENERAL_VEC3_FUNCTION(-, ssub)
SCALAR_GENERAL_VEC3_FUNCTION(*, smul)
SCALAR_GENERAL_VEC3_FUNCTION(/, sdiv)
*/
static inline void vec3_sadda(vec3_t out, float s)
{
VEC_X(out) += s;
VEC_Y(out) += s;
VEC_Z(out) += s;
}
static inline void vec3_ssuba(vec3_t out, float s)
{
VEC_X(out) -= s;
VEC_Y(out) -= s;
VEC_Z(out) -= s;
}
static inline void vec3_smula(vec3_t out, float s)
{
VEC_X(out) *= s;
VEC_Y(out) *= s;
VEC_Z(out) *= s;
}
static inline void vec3_sdiva(vec3_t out, float s)
{
VEC_X(out) /= s;
VEC_Y(out) /= s;
VEC_Z(out) /= s;
}
static inline void vec3_sadd(vec3_t out, const vec3_t v1, float s)
{
VEC_X(out) = VEC_X(v1) + s;
VEC_Y(out) = VEC_Y(v1) + s;
VEC_Z(out) = VEC_Z(v1) + s;
}
static inline void vec3_ssub(vec3_t out, const vec3_t v1, float s)
{
VEC_X(out) = VEC_X(v1) - s;
VEC_Y(out) = VEC_Y(v1) - s;
VEC_Z(out) = VEC_Z(v1) - s;
}
static inline void vec3_smul(vec3_t out, const vec3_t v1, float s)
{
VEC_X(out) = VEC_X(v1) * s;
VEC_Y(out) = VEC_Y(v1) * s;
VEC_Z(out) = VEC_Z(v1) * s;
}
static inline void vec3_sdiv(vec3_t out, const vec3_t v1, float s)
{
VEC_X(out) = VEC_X(v1) / s;
VEC_Y(out) = VEC_Y(v1) / s;
VEC_Z(out) = VEC_Z(v1) / s;
}
/* END
************************************************************************/
/* utility functions for vec3 */
static inline void vec3_set(vec3_t out, float x, float y, float z)
{
VEC_X(out) = x;
VEC_Y(out) = y;
VEC_Z(out) = z;
}
static inline void vec3_neg(vec3_t out, const vec3_t v)
{
VEC_X(out) = -VEC_X(v);
VEC_Y(out) = -VEC_Y(v);
VEC_Z(out) = -VEC_Z(v);
}
static inline void vec3_copy(vec3_t out, const vec3_t v)
{
memcpy(out, v, sizeof(float) * 3);
}
static inline float vec3_length2(const vec3_t v)
{
return (VEC_X(v)*VEC_X(v) + VEC_Y(v)*VEC_Y(v) + VEC_Z(v)*VEC_Z(v));
}
static inline float vec3_length(const vec3_t v)
{
return sqrtf(vec3_length2(v));
}
static inline void vec3_normalize(vec3_t out)
{
float ilength = 1.0f / vec3_length(out);
vec3_smula(out, ilength);
}
static inline void vec3_inv(vec3_t out, const vec3_t v)
{
VEC_X(out) = 1.0f / VEC_X(v);
VEC_Y(out) = 1.0f / VEC_Y(v);
VEC_Z(out) = 1.0f / VEC_Z(v);
}
static inline float vec3_distance(const vec3_t v1, const vec3_t v2)
{
vec3_t v;
vec3_sub(v, v1, v2);
return vec3_length(v);
}
static inline float vec3_distance2(const vec3_t v1, const vec3_t v2)
{
vec3_t v;
vec3_sub(v, v1, v2);
return vec3_length2(v);
}
static inline float vec3_dot(const vec3_t v1, const vec3_t v2)
{
return (VEC_X(v1)*VEC_X(v2) + VEC_Y(v1)*VEC_Y(v2) + VEC_Z(v1)*VEC_Z(v2));
}
static inline void vec3_cross(vec3_t out, const vec3_t v1, const vec3_t v2)
{
VEC_X(out) = VEC_Y(v1) * VEC_Z(v2) - VEC_Z(v1) * VEC_Y(v2);
VEC_Y(out) = VEC_Z(v1) * VEC_X(v2) - VEC_X(v1) * VEC_Z(v2);
VEC_Z(out) = VEC_X(v1) * VEC_Y(v2) - VEC_Y(v1) * VEC_X(v2);
}
static inline void vec3_advance(vec3_t out, const vec3_t origin, const vec3_t dir,
float distance)
{
VEC_X(out) = VEC_X(origin) + VEC_X(dir) * distance;
VEC_Y(out) = VEC_Y(origin) + VEC_Y(dir) * distance;
VEC_Z(out) = VEC_Z(origin) + VEC_Z(dir) * distance;
}
static inline bool vec3_equals(const vec3_t v1, const vec3_t v2)
{
return (VEC_X(v1) == VEC_X(v2) && VEC_Y(v1) == VEC_Y(v2) && VEC_Z(v1) == VEC_Z(v2));
}
void vec3_rotate_x(vec3_t out, float deg, const vec3_t origin);
void vec3_rotate_y(vec3_t out, float deg, const vec3_t origin);
void vec3_rotate_z(vec3_t out, float deg, const vec3_t origin);
/**************************************************************************
vec4: four-dimensional vector
**************************************************************************/
static inline void vec4_set(vec4_t out, float x, float y, float z, float w)
{
VEC_X(out) = x;
VEC_Y(out) = y;
VEC_Z(out) = z;
VEC_W(out) = w;
}
static inline void vec4_neg(vec4_t out, const vec4_t v)
{
VEC_X(out) = -VEC_X(v);
VEC_Y(out) = -VEC_Y(v);
VEC_Z(out) = -VEC_Z(v);
VEC_W(out) = -VEC_W(v);
}
static inline void vec4_copy(vec4_t out, const vec4_t v)
{
memcpy(out, v, sizeof(float) * 4);
}
static inline bool vec4_equals(const vec4_t v1, const vec4_t v2)
{
return (VEC_X(v1) == VEC_X(v2) &&
VEC_Y(v1) == VEC_Y(v2) &&
VEC_Z(v1) == VEC_Z(v2) &&
VEC_W(v1) == VEC_W(v2));
}
/**************************************************************************
mat4: 4x4 OpenGL (column-major) matrix
**************************************************************************/
void mat4_copy(mat4_t out, const mat4_t m);
void mat4_transform_vec3(vec3_t v, const mat4_t m);
void mat4_mul(mat4_t out, const mat4_t m1, const mat4_t m2);
bool mat4_equals(const mat4_t m1, const mat4_t m2);
bool mat4_is_identity(const mat4_t m);
float mat4_determinant(const mat4_t m);
void mat4_inverse(mat4_t out, const mat4_t m);
void mat4_set_identity(mat4_t out);
void mat4_set_zero(mat4_t out);
void mat4_set_rotate(mat4_t out, const vec3_t axis, float angle);
void mat4_set_rotate_x(mat4_t out, float angle);
void mat4_set_rotate_y(mat4_t out, float angle);
void mat4_set_rotate_z(mat4_t out, float angle);
void mat4_set_scale(mat4_t out, const vec3_t scale);
void mat4_set_translate(mat4_t out, const vec3_t translate);
void mat4_set_perspective(mat4_t out, float fov, float aspect, float znear, float zfar);
void mat4_set_ortho(mat4_t out, float left, float right, float bottom, float top, float znear, float zfar);
void mat4_set_look_at(mat4_t out, const vec3_t eye, const vec3_t center, const vec3_t up);
/**************************************************************************
quat: quaterion representing rotation
**************************************************************************/
static inline void quat_conjugate(quat_t out, const quat_t q)
{
out[0] = -q[0];
out[1] = -q[1];
out[2] = -q[2];
out[3] = q[3];
}
static inline void quat_copy(quat_t out, const quat_t q)
{
memcpy(out, q, sizeof(float) * 4);
}
void quat_from_mat4(quat_t out, const mat4_t m);
void quat_from_angle(quat_t out, const vec3_t dir, float angle);
void quat_to_mat4(mat4_t out, const quat_t q);
void quat_slerp(quat_t out, const quat_t q1, const quat_t q2, float t);
void quat_mul(quat_t out, const quat_t q1, const quat_t q2);
/**************************************************************************
rect: rectangle with integers
**************************************************************************/
static inline void rect_copy(rect_t out, const rect_t r)
{
memcpy(out, r, sizeof(int) * 4);
}
static inline bool rect_equals(const rect_t r1, const rect_t r2)
{
return (r1[0] == r2[0] &&
r1[1] == r2[1] &&
r1[2] == r2[2] &&
r1[3] == r2[3]);
}
/**************************************************************************
aabb: axis-aligned bounding box
**************************************************************************/
static inline void aabb_copy(aabb_t out, const aabb_t aabb)
{
memcpy(out, aabb, sizeof(float) * 6);
}
static inline void aabb_set(aabb_t out, const vec3_t min, const vec3_t max)
{
AABB_MIN_X(out) = VEC_X(min);
AABB_MIN_Y(out) = VEC_Y(min);
AABB_MIN_Z(out) = VEC_Z(min);
AABB_MAX_X(out) = VEC_X(max);
AABB_MAX_Y(out) = VEC_Y(max);
AABB_MAX_Z(out) = VEC_Z(max);
}
static inline void aabb_expand(aabb_t out, float f)
{
vec3_ssuba(AABB_MIN(out), f);
vec3_sadda(AABB_MAX(out), f);
}
static inline void aabb_reset(aabb_t out, const vec3_t point)
{
AABB_MIN_X(out) = VEC_X(point);
AABB_MIN_Y(out) = VEC_Y(point);
AABB_MIN_Z(out) = VEC_Z(point);
AABB_MAX_X(out) = VEC_X(point);
AABB_MAX_Y(out) = VEC_Y(point);
AABB_MAX_Z(out) = VEC_Z(point);
}
static inline void aabb_center(vec3_t out, const aabb_t aabb)
{
vec3_t tmp;
vec3_add(tmp, AABB_MIN(aabb), AABB_MAX(aabb));
vec3_sdiv(out, tmp, 2.0f);
}
void aabb_add_point(aabb_t aabb, const vec3_t point);
void aabb_add_aabb(aabb_t aabb1, const aabb_t aabb2);
/**************************************************************************
plane: three-dimensional plane
**************************************************************************/
static inline void plane_set(plane_t out, const vec3_t origin, const vec3_t normal)
{
vec3_copy(out, normal); /* it's assumed that normal is normalized */
PLANE_D(out) = -vec3_dot(out, origin);
}
static inline void plane_from_points(plane_t out, const vec3_t v1, const vec3_t v2, const vec3_t v3)
{
vec3_t v21, v31;
vec3_sub(v21, v2, v1);
vec3_sub(v31, v3, v1);
vec3_cross(out, v21, v31);
vec3_normalize(out);
PLANE_D(out) = -vec3_dot(out, v1);
}
enum plane_side plane_side_point(const plane_t plane, const vec3_t point);
enum plane_side plane_side_aabb(const plane_t plane, const aabb_t aabb);
/**************************************************************************
frustum
**************************************************************************/
void frustum_set(frustum_t out, const mat4_t matrix);
enum frustum_side frustum_side_aabb(const frustum_t frustum, const aabb_t aabb);
/**************************************************************************
intersections
**************************************************************************/
bool aabb_clip_ray(const aabb_t aabb, const vec3_t orig, const vec3_t dir,
float *t_min, float *t_max);
static inline float plane_ray_intersect(const plane_t plane, const vec3_t origin,
const vec3_t dir)
{
return -(vec3_dot(origin, plane) + PLANE_D(plane)) / vec3_dot(dir, plane);
}
static inline bool ray_triangle_intersection(const vec3_t orig, const vec3_t dir,
const vec3_t a,
const vec3_t b,
const vec3_t c,
float *t_out,
float *u_out, float *v_out)
{
vec3_t edge1;
vec3_t edge2;
vec3_t tvec, pvec, qvec;
float det, invdet;
float u, v, t;
vec3_sub(edge1, c, a);
vec3_sub(edge2, b, a);
vec3_cross(pvec, dir, edge2);
det = vec3_dot(edge1, pvec);
if (det > -MATH_EPSILON && det < MATH_EPSILON)
return false;
invdet = 1.0f / det;
vec3_sub(tvec, orig, a);
u = vec3_dot(tvec, pvec) * invdet;
if (u < 0.0f || u > 1.0f)
return false;
vec3_cross(qvec, tvec, edge1);
v = vec3_dot(dir, qvec) * invdet;
if (v < 0.0f || u + v > 1.0f)
return false;
t = vec3_dot(edge2, qvec) * invdet;
if (t < 0.0f)
return false;
*t_out = t;
*u_out = u;
*v_out = v;
return true;
}
static inline bool aabb_aabb_intersection(const aabb_t aabb1,
const aabb_t aabb2)
{
return (AABB_MIN_X(aabb1) < AABB_MAX_X(aabb2) &&
AABB_MIN_Y(aabb1) < AABB_MAX_Y(aabb2) &&
AABB_MIN_Z(aabb1) < AABB_MAX_Z(aabb2) &&
AABB_MAX_X(aabb1) > AABB_MIN_X(aabb2) &&
AABB_MAX_Y(aabb1) > AABB_MIN_Y(aabb2) &&
AABB_MAX_Z(aabb1) > AABB_MIN_Z(aabb2));
}
static inline bool point_in_2d_triangle(const vec2_t p,
const vec2_t a,
const vec2_t b,
const vec2_t c,
float *uout,
float *vout)
{
vec2_t v0, v1, v2;
vec2_sub(v0, c, a);
vec2_sub(v1, b, a);
vec2_sub(v2, p, a);
float dot00 = vec2_dot(v0, v0);
float dot01 = vec2_dot(v0, v1);
float dot02 = vec2_dot(v0, v2);
float dot11 = vec2_dot(v1, v1);
float dot12 = vec2_dot(v1, v2);
float inv_det = 1.0f / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * inv_det;
float v = (dot00 * dot12 - dot01 * dot02) * inv_det;
*uout = u;
*vout = v;
return (u > 0.0f) && (v > 0.0f) && (u + v < 1.0f);
}
static inline bool point_in_cylinder(const vec3_t point,
const vec3_t origin, const vec3_t dir,
float height, float radius)
{
/* TODO: It's possible to speed up this thing by using one
* to many pattern. Precalculate things like 'hsq' and 'rsq' for a
* cylinder and use them directly. Also there is '1.0f/hsq', also can
* be precomputed.
*/
vec3_t vechh, vech, o, pvec;
float halfh = height * 0.5f;
float hsq = height * height;
float rsq = radius * radius;
vec3_smul(vechh, dir, halfh);
vec3_smul(vech, dir, height);
vec3_sub(o, origin, vechh);
vec3_sub(pvec, point, o);
float d = vec3_dot(pvec, vech);
if (d < 0.0f || d > hsq)
return false;
float dist = vec3_dot(pvec, pvec) - d*d / hsq;
if (dist > rsq)
return false;
return true;
}
/* extents_out center is always == (0,0,0) */
void cylinder_to_obb(mat4_t basis_out, aabb_t extents_out,
const vec3_t origin, const vec3_t dir,
float height, float radius);
void basis_to_mat4(mat4_t out, const vec3_t origin, const vec3_t dir);
bool aabb_obb_intersection(const aabb_t aabb1, const mat4_t obb_basis,
const aabb_t obb);
/**************************************************************************
SSSS SSSS EEEEEE
S S S S E
S S E
SSSS SSSS EEEEEE
S S E
S S S S E
SSSS SSSS EEEEEE
**************************************************************************/
void mat4_sse_mul(mat4_sse_t out, const mat4_sse_t m1, const mat4_sse_t m2);
void quat_sse_mul(quat_sse_t out, const quat_sse_t q1, const quat_sse_t q2);
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment