t3dmath.h

Go to the documentation of this file.

 
#ifndef TINY3D_T3DMATH_H
#define TINY3D_T3DMATH_H
 
#include <libdragon.h>
 
#ifdef __cplusplus
extern "C"
{
#endif
 
#define T3D_DEG_TO_RAD(deg) (deg * 0.01745329252f)
#define T3D_F32_TO_FIXED(val) (int32_t)((val) * (float)(1<<16))
#define T3D_PI 3.14159265358979f
 
// 3D float vector
typedef fm_vec3_t T3DVec3;
typedef fm_vec4_t T3DVec4;
typedef fm_quat_t T3DQuat;
typedef fm_mat4_t T3DMat4;
 
// 3D s16.16 fixed-point vector, used as-is by the RSP.
typedef struct {
  int16_t i[4];
  uint16_t f[4];
} T3DVec4FP;
 
// 4x4 Matrix of 16.16 fixed-point numbers, used as-is by the RSP.
typedef struct {
  T3DVec4FP m[4];
} __attribute__((aligned(16))) T3DMat4FP;
 
// View frustum, used in culling functions
typedef struct {
  T3DVec4 planes[6];
} T3DFrustum;
 
inline static float s1616_to_float(int16_t partI, uint16_t partF)
{
  float res = (float)partI;
  res += (float)partF / 65536.f;
  return res;
}
 
inline static float t3d_lerp(float a, float b, float t) {
  return a + (b - a) * t;
}
 
inline static float t3d_lerp_angle(float a, float b, float t) {
  float angleDiff = fmodf((b - a), T3D_PI*2);
  float shortDist = fmodf(angleDiff*2, T3D_PI*2) - angleDiff;
  return a + shortDist * t;
}
 
inline static void t3d_vec3_add(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b) {
  res->v[0] = a->v[0] + b->v[0];
  res->v[1] = a->v[1] + b->v[1];
  res->v[2] = a->v[2] + b->v[2];
}
 
inline static void t3d_vec3_mul(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b) {
  res->v[0] = a->v[0] * b->v[0];
  res->v[1] = a->v[1] * b->v[1];
  res->v[2] = a->v[2] * b->v[2];
}
 
inline static void t3d_vec3_scale(T3DVec3 *res, const T3DVec3 *a, float s) {
  res->v[0] = a->v[0] * s;
  res->v[1] = a->v[1] * s;
  res->v[2] = a->v[2] * s;
}
 
inline static void t3d_vec3_diff(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b) {
  res->v[0] = a->v[0] - b->v[0];
  res->v[1] = a->v[1] - b->v[1];
  res->v[2] = a->v[2] - b->v[2];
}
 
inline static float t3d_vec3_len2(const T3DVec3 *vec) {
  return vec->v[0] * vec->v[0]
       + vec->v[1] * vec->v[1]
       + vec->v[2] * vec->v[2];
}
 
inline static float t3d_vec3_len(const T3DVec3 *vec) {
  return sqrtf(t3d_vec3_len2(vec));
}
 
inline static float t3d_vec3_distance2(const T3DVec3 *vecA, const T3DVec3 *vecB) {
  T3DVec3 diff;
  t3d_vec3_diff(&diff, vecA, vecB);
  return t3d_vec3_len2(&diff);
}
 
inline static float t3d_vec3_distance(const T3DVec3 *vecA, const T3DVec3 *vecB) {
  return sqrtf(t3d_vec3_distance2(vecA, vecB));
}
 
inline static void t3d_vec3_norm(T3DVec3 *res) {
  float len = sqrtf(t3d_vec3_len2(res));
  if(len < 0.0001f)len = 0.0001f;
  res->v[0] /= len;
  res->v[1] /= len;
  res->v[2] /= len;
}
 
inline static void t3d_vec3_cross(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b) {
  res->v[0] = a->v[1]*b->v[2] - b->v[1]*a->v[2];
  res->v[1] = a->v[2]*b->v[0] - b->v[2]*a->v[0];
  res->v[2] = a->v[0]*b->v[1] - b->v[0]*a->v[1];
}
 
inline static float t3d_vec3_dot(const T3DVec3 *a, const T3DVec3 *b) {
  return a->v[0] * b->v[0] +
         a->v[1] * b->v[1] +
         a->v[2] * b->v[2];
}
 
inline static void t3d_vec3_lerp(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b, float t)
{
  res->v[0] = a->v[0] + (b->v[0] - a->v[0]) * t;
  res->v[1] = a->v[1] + (b->v[1] - a->v[1]) * t;
  res->v[2] = a->v[2] + (b->v[2] - a->v[2]) * t;
}
 
// @brief Resets a quaternion to the identity quaternion
inline static void t3d_quat_identity(T3DQuat *quat) {
  *quat = (T3DQuat){{0, 0, 0, 1}};
}
 
inline static void t3d_quat_from_euler(T3DQuat *quat, const float rotEuler[3])
{
  float c1 = fm_cosf(rotEuler[0] / 2.0f);
  float s1 = fm_sinf(rotEuler[0] / 2.0f);
  float c2 = fm_cosf(rotEuler[1] / 2.0f);
  float s2 = fm_sinf(rotEuler[1] / 2.0f);
  float c3 = fm_cosf(rotEuler[2] / 2.0f);
  float s3 = fm_sinf(rotEuler[2] / 2.0f);
 
  quat->v[0] = c1 * c2 * s3 - s1 * s2 * c3;
  quat->v[1] = s1 * c2 * c3 - c1 * s2 * s3;
  quat->v[2] = c1 * s2 * c3 + s1 * c2 * s3;
  quat->v[3] = c1 * c2 * c3 + s1 * s2 * s3;
}
 
inline static void t3d_quat_from_rotation(T3DQuat *quat, float axis[3], float angleRad)
{
  float s = fm_sinf(angleRad / 2.0f);
  float c = fm_cosf(angleRad / 2.0f);
  *quat = (T3DQuat){{axis[0] * s, axis[1] * s, axis[2] * s, c}};
}
 
inline static void t3d_quat_mul(T3DQuat *res, T3DQuat *a, T3DQuat *b)
{
  res->v[0] = a->v[3] * b->v[0] + a->v[0] * b->v[3] + a->v[1] * b->v[2] - a->v[2] * b->v[1];
  res->v[1] = a->v[3] * b->v[1] - a->v[0] * b->v[2] + a->v[1] * b->v[3] + a->v[2] * b->v[0];
  res->v[2] = a->v[3] * b->v[2] + a->v[0] * b->v[1] - a->v[1] * b->v[0] + a->v[2] * b->v[3];
  res->v[3] = a->v[3] * b->v[3] - a->v[0] * b->v[0] - a->v[1] * b->v[1] - a->v[2] * b->v[2];
}
 
inline static void t3d_quat_rotate_euler(T3DQuat *quat, float axis[3], float angleRad)
{
  T3DQuat tmp, quatRot;
  t3d_quat_from_rotation(&quatRot, axis, angleRad);
  t3d_quat_mul(&tmp, quat, &quatRot);
  *quat = tmp;
}
 
inline static float t3d_quat_dot(const T3DQuat *a, const T3DQuat *b)
{
  return a->v[0] * b->v[0] + a->v[1] * b->v[1] + a->v[2] * b->v[2] + a->v[3] * b->v[3];
}
 
inline static void t3d_quat_normalize(T3DQuat *quat)
{
  float scale = 1.0f / sqrtf(quat->v[0]*quat->v[0] + quat->v[1]*quat->v[1] + quat->v[2]*quat->v[2] + quat->v[3]*quat->v[3]);
  quat->v[0] *= scale;
  quat->v[1] *= scale;
  quat->v[2] *= scale;
  quat->v[3] *= scale;
}
 
void t3d_quat_nlerp(T3DQuat *res, const T3DQuat *a, const T3DQuat *b, float t);
 
void t3d_quat_slerp(T3DQuat *res, const T3DQuat *a, const T3DQuat *b, float t);
 
inline static void t3d_mat4_identity(T3DMat4 *mat)
{
  *mat = (T3DMat4){{
    {1, 0, 0, 0},
    {0, 1, 0, 0},
    {0, 0, 1, 0},
    {0, 0, 0, 1},
  }};
}
 
inline static void t3d_mat4_scale(T3DMat4 *mat, float scaleX, float scaleY, float scaleZ)
{
  mat->m[0][0] *= scaleX;
  mat->m[0][1] *= scaleX;
  mat->m[0][2] *= scaleX;
  mat->m[0][3] *= scaleX;
 
  mat->m[1][0] *= scaleY;
  mat->m[1][1] *= scaleY;
  mat->m[1][2] *= scaleY;
  mat->m[1][3] *= scaleY;
 
  mat->m[2][0] *= scaleZ;
  mat->m[2][1] *= scaleZ;
  mat->m[2][2] *= scaleZ;
  mat->m[2][3] *= scaleZ;
}
 
inline static void t3d_mat4_translate(T3DMat4 *mat, float offsetX, float offsetY, float offsetZ)
{
  mat->m[3][0] += offsetX;
  mat->m[3][1] += offsetY;
  mat->m[3][2] += offsetZ;
}
 
void t3d_mat4_rotate(T3DMat4 *mat, const T3DVec3* axis, float angleRad);
 
void t3d_mat4_from_srt(T3DMat4 *mat, const float scale[3], const float quat[4], const  float translate[3]);
 
void t3d_mat4_from_srt_euler(T3DMat4 *mat, const float scale[3], const float rot[3], const float translate[3]);
 
void t3d_mat4_rot_from_dir(T3DMat4 *mat, const T3DVec3 *dir, const T3DVec3 *up);
 
void t3d_mat4fp_from_srt_euler(T3DMat4FP *mat, const float scale[3], const float rot[3], const float translate[3]);
 
void t3d_mat4fp_from_srt(T3DMat4FP *mat, const float scale[3], const float rotQuat[4], const float translate[3]);
 
inline static void t3d_mat4fp_set_float(T3DMat4FP *mat, uint32_t column, uint32_t row, float val)
{
  int32_t fixed = T3D_F32_TO_FIXED(val);
  mat->m[column].i[row] = (int16_t)(fixed >> 16);
  mat->m[column].f[row] = fixed & 0xFFFF;
}
 
inline static void t3d_mat4fp_set_pos(T3DMat4FP *mat, const float pos[3]) {
  t3d_mat4fp_set_float(mat, 3, 0, pos[0]);
  t3d_mat4fp_set_float(mat, 3, 1, pos[1]);
  t3d_mat4fp_set_float(mat, 3, 2, pos[2]);
}
 
inline static float t3d_mat4fp_get_float(const T3DMat4FP *mat, uint32_t y, uint32_t x)
{
  return s1616_to_float(mat->m[y].i[x], mat->m[y].f[x]);
}
 
inline static void t3d_mat4fp_identity(T3DMat4FP *mat)
{
  *mat = (T3DMat4FP){{
    {{1, 0, 0, 0}, {0, 0, 0, 0}},
    {{0, 1, 0, 0}, {0, 0, 0, 0}},
    {{0, 0, 1, 0}, {0, 0, 0, 0}},
    {{0, 0, 0, 1}, {0, 0, 0, 0}},
  }};
}
void t3d_mat4_to_fixed(T3DMat4FP *matOut, const T3DMat4 *matIn);
 
void t3d_mat4_to_fixed_3x4(T3DMat4FP *matOut, const T3DMat4 *matIn);
 
void t3d_mat4_perspective(T3DMat4 *mat, float fov, float aspect, float near, float far);
 
void t3d_mat4_ortho(T3DMat4 *mat, float left, float right, float bottom, float top, float near, float far);
 
void t3d_mat4_look_at(T3DMat4 *mat, const T3DVec3 *eye, const T3DVec3 *target, const T3DVec3 *up);
 
void t3d_mat4_to_frustum(T3DFrustum *frustum, const T3DMat4 *mat);
 
void t3d_frustum_scale(T3DFrustum *frustum, float scale);
 
bool t3d_frustum_vs_aabb(const T3DFrustum *frustum, const T3DVec3 *min, const T3DVec3 *max);
 
bool t3d_frustum_vs_aabb_s16(const T3DFrustum *frustum, const int16_t min[3], const int16_t max[3]);
 
bool t3d_frustum_vs_sphere(const T3DFrustum *frustum, const T3DVec3 *center, const float radius);
 
inline static void t3d_mat4_mul(T3DMat4 *matRes, const T3DMat4 *matA, const T3DMat4 *matB)
{
  for(uint32_t i=0; i<4; i++) {
    for(uint32_t  j=0; j<4; j++) {
      matRes->m[j][i] = matA->m[0][i] * matB->m[j][0] +
                        matA->m[1][i] * matB->m[j][1] +
                        matA->m[2][i] * matB->m[j][2] +
                        matA->m[3][i] * matB->m[j][3];
    }
  }
}
 
inline static void t3d_mat3_mul_vec3(T3DVec3* vecOut, const T3DMat4 *mat, const T3DVec3* vec)
{
  for(uint32_t i=0; i<3; i++) {
    vecOut->v[i] = mat->m[0][i] * vec->v[0] +
                   mat->m[1][i] * vec->v[1] +
                   mat->m[2][i] * vec->v[2];
  }
}
 
inline static void t3d_mat4_mul_vec3(T3DVec4* vecOut, const T3DMat4 *mat, const T3DVec3* vec)
{
  for(uint32_t i=0; i<4; i++) {
    vecOut->v[i] = mat->m[0][i] * vec->v[0] +
                   mat->m[1][i] * vec->v[1] +
                   mat->m[2][i] * vec->v[2] +
                   mat->m[3][i];
  }
}
 
#ifdef __cplusplus
}
#endif
 
// C++ wrappers that allow (const-)references to be passed instead of pointers
#ifdef __cplusplus
  inline void t3d_vec3_add(T3DVec3 &res, const T3DVec3 &a, const T3DVec3 &b) { t3d_vec3_add(&res, &a, &b); }
  inline void t3d_vec3_mul(T3DVec3 &res, const T3DVec3 &a, const T3DVec3 &b) { t3d_vec3_mul(&res, &a, &b); }
  inline void t3d_vec3_scale(T3DVec3 &res, const T3DVec3 &a, float s) { t3d_vec3_scale(&res, &a, s); }
  inline void t3d_vec3_diff(T3DVec3 &res, const T3DVec3 &a, const T3DVec3 &b) { t3d_vec3_diff(&res, &a, &b); }
  inline float t3d_vec3_len2(const T3DVec3 &vec) { return t3d_vec3_len2(&vec); }
  inline float t3d_vec3_len(const T3DVec3 &vec) { return t3d_vec3_len(&vec); }
  inline float t3d_vec3_distance2(const T3DVec3 &vecA, const T3DVec3 &vecB) { return t3d_vec3_distance2(&vecA, &vecB); }
  inline float t3d_vec3_distance(const T3DVec3 &vecA, const T3DVec3 &vecB) { return t3d_vec3_distance(&vecA, &vecB); }
  inline void t3d_vec3_norm(T3DVec3 &res) { t3d_vec3_norm(&res); }
  inline void t3d_vec3_cross(T3DVec3 &res, const T3DVec3 &a, const T3DVec3 &b) { t3d_vec3_cross(&res, &a, &b); }
  inline float t3d_vec3_dot(const T3DVec3 &a, const T3DVec3 &b) { return t3d_vec3_dot(&a, &b); }
  inline void t3d_vec3_lerp(T3DVec3 &res, const T3DVec3 &a, const T3DVec3 &b, float t) { t3d_vec3_lerp(&res, &a, &b, t); }
  inline void t3d_quat_identity(T3DQuat &quat) { t3d_quat_identity(&quat); }
  inline void t3d_quat_from_euler(T3DQuat &quat, const float rotEuler[3]) { t3d_quat_from_euler(&quat, rotEuler); }
  inline void t3d_quat_from_rotation(T3DQuat &quat, float axis[3], float angleRad) { t3d_quat_from_rotation(&quat, axis, angleRad); }
  inline void t3d_quat_mul(T3DQuat &res, T3DQuat &a, T3DQuat &b) { t3d_quat_mul(&res, &a, &b); }
  inline void t3d_quat_rotate_euler(T3DQuat &quat, float axis[3], float angleRad) { t3d_quat_rotate_euler(&quat, axis, angleRad); }
  inline float t3d_quat_dot(const T3DQuat &a, const T3DQuat &b) { return t3d_quat_dot(&a, &b); }
  inline void t3d_quat_normalize(T3DQuat &quat) { t3d_quat_normalize(&quat); }
  inline void t3d_quat_nlerp(T3DQuat &res, const T3DQuat &a, const T3DQuat &b, float t) { t3d_quat_nlerp(&res, &a, &b, t); }
  inline void t3d_quat_slerp(T3DQuat &res, const T3DQuat &a, const T3DQuat &b, float t) { t3d_quat_slerp(&res, &a, &b, t); }
  inline void t3d_mat4_identity(T3DMat4 &mat) { t3d_mat4_identity(&mat); }
  inline void t3d_mat4_scale(T3DMat4 &mat, float scaleX, float scaleY, float scaleZ) { t3d_mat4_scale(&mat, scaleX, scaleY, scaleZ); }
  inline void t3d_mat4_translate(T3DMat4 &mat, float offsetX, float offsetY, float offsetZ) { t3d_mat4_translate(&mat, offsetX, offsetY, offsetZ); }
  inline void t3d_mat4_rotate(T3DMat4 &mat, const T3DVec3 &axis, float angleRad) { t3d_mat4_rotate(&mat, &axis, angleRad); }
  inline void t3d_mat4_from_srt(T3DMat4 &mat, const float scale[3], const float quat[4], const float translate[3]) { t3d_mat4_from_srt(&mat, scale, quat, translate); }
  inline void t3d_mat4_from_srt_euler(T3DMat4 &mat, const float scale[3], const float rot[3], const float translate[3]) { t3d_mat4_from_srt_euler(&mat, scale, rot, translate); }
  inline void t3d_mat4_rot_from_dir(T3DMat4 &mat, const T3DVec3 &dir, const T3DVec3 &up) { t3d_mat4_rot_from_dir(&mat, &dir, &up); }
 
  inline void t3d_mat4_from_srt(T3DMat4 &mat, const T3DVec3 &scale, const T3DQuat &rot, const T3DVec3 &translate) { t3d_mat4_from_srt(&mat, scale.v, rot.v, translate.v); }
  inline void t3d_mat4_from_srt_euler(T3DMat4 &mat, const T3DVec3 &scale, const T3DVec3 &rot, const T3DVec3 &translate) { t3d_mat4_from_srt_euler(&mat, scale.v, rot.v, translate.v); }
 
  inline void t3d_mat4fp_from_srt_euler(T3DMat4FP *mat, const T3DVec3 &scale, const T3DVec3 &rot, const T3DVec3 &translate) { t3d_mat4fp_from_srt_euler(mat, scale.v, rot.v, translate.v); }
  inline void t3d_mat4fp_from_srt(T3DMat4FP *mat, const T3DVec3 &scale, const T3DQuat &rot, const T3DVec3 &translate) { t3d_mat4fp_from_srt(mat, scale.v, rot.v, translate.v); }
  inline void t3d_mat4fp_set_pos(T3DMat4FP *mat, const T3DVec3 &pos) { t3d_mat4fp_set_pos(mat, pos.v); }
  inline float t3d_mat4fp_get_float(const T3DMat4FP &mat, uint32_t y, uint32_t x) { return t3d_mat4fp_get_float(&mat, y, x); }
 
  inline void t3d_mat4_perspective(T3DMat4 &mat, float fov, float aspect, float near, float far) { t3d_mat4_perspective(&mat, fov, aspect, near, far); }
  inline void t3d_mat4_ortho(T3DMat4 &mat, float left, float right, float bottom, float top, float near, float far) { t3d_mat4_ortho(&mat, left, right, bottom, top, near, far); }
  inline void t3d_mat4_look_at(T3DMat4 &mat, const T3DVec3 &eye, const T3DVec3 &target, const T3DVec3 &up) { t3d_mat4_look_at(&mat, &eye, &target, &up); }
  inline void t3d_mat4_to_frustum(T3DFrustum &frustum, const T3DMat4 &mat) { t3d_mat4_to_frustum(&frustum, &mat); }
  inline void t3d_frustum_scale(T3DFrustum &frustum, float scale) { t3d_frustum_scale(&frustum, scale); }
  inline bool t3d_frustum_vs_aabb(const T3DFrustum &frustum, const T3DVec3 &min, const T3DVec3 &max) { return t3d_frustum_vs_aabb(&frustum, &min, &max); }
  inline bool t3d_frustum_vs_aabb_s16(const T3DFrustum &frustum, const int16_t min[3], const int16_t max[3]) { return t3d_frustum_vs_aabb_s16(&frustum, min, max); }
  inline void t3d_mat4_mul(T3DMat4 &matRes, const T3DMat4 &matA, const T3DMat4 &matB) { t3d_mat4_mul(&matRes, &matA, &matB); }
  inline void t3d_mat3_mul_vec3(T3DVec3 &vecOut, const T3DMat4 &mat, const T3DVec3 &vec) { t3d_mat3_mul_vec3(&vecOut, &mat, &vec); }
  inline void t3d_mat4_mul_vec3(T3DVec4 &vecOut, const T3DMat4 &mat, const T3DVec3 &vec) { t3d_mat4_mul_vec3(&vecOut, &mat, &vec); }
 
#endif
 
#endif //TINY3D_T3DMATH_H