6#ifndef TINY3D_T3DMATH_H
7#define TINY3D_T3DMATH_H
16#define T3D_DEG_TO_RAD(deg) (deg * 0.01745329252f)
17#define T3D_F32_TO_FIXED(val) (int32_t)((val) * (float)(1<<16))
18#define T3D_PI 3.14159265358979f
21typedef fm_vec3_t T3DVec3;
22typedef fm_vec4_t T3DVec4;
23typedef fm_quat_t T3DQuat;
24typedef fm_mat4_t T3DMat4;
50 float res = (float)partI;
51 res += (float)partF / 65536.f;
56inline static float t3d_lerp(
float a,
float b,
float t) {
57 return a + (b - a) * t;
62 float angleDiff = fmodf((b - a), T3D_PI*2);
63 float shortDist = fmodf(angleDiff*2, T3D_PI*2) - angleDiff;
64 return a + shortDist * t;
68inline static void t3d_vec3_add(T3DVec3 *res,
const T3DVec3 *a,
const T3DVec3 *b) {
69 res->v[0] = a->v[0] + b->v[0];
70 res->v[1] = a->v[1] + b->v[1];
71 res->v[2] = a->v[2] + b->v[2];
75inline static void t3d_vec3_mul(T3DVec3 *res,
const T3DVec3 *a,
const T3DVec3 *b) {
76 res->v[0] = a->v[0] * b->v[0];
77 res->v[1] = a->v[1] * b->v[1];
78 res->v[2] = a->v[2] * b->v[2];
83 res->v[0] = a->v[0] * s;
84 res->v[1] = a->v[1] * s;
85 res->v[2] = a->v[2] * s;
89inline static void t3d_vec3_diff(T3DVec3 *res,
const T3DVec3 *a,
const T3DVec3 *b) {
90 res->v[0] = a->v[0] - b->v[0];
91 res->v[1] = a->v[1] - b->v[1];
92 res->v[2] = a->v[2] - b->v[2];
97 return vec->v[0] * vec->v[0]
98 + vec->v[1] * vec->v[1]
99 + vec->v[2] * vec->v[2];
122 if(len < 0.0001f)len = 0.0001f;
129inline static void t3d_vec3_cross(T3DVec3 *res,
const T3DVec3 *a,
const T3DVec3 *b) {
130 res->v[0] = a->v[1]*b->v[2] - b->v[1]*a->v[2];
131 res->v[1] = a->v[2]*b->v[0] - b->v[2]*a->v[0];
132 res->v[2] = a->v[0]*b->v[1] - b->v[0]*a->v[1];
137 return a->v[0] * b->v[0] +
143inline static void t3d_vec3_lerp(T3DVec3 *res,
const T3DVec3 *a,
const T3DVec3 *b,
float t)
145 res->v[0] = a->v[0] + (b->v[0] - a->v[0]) * t;
146 res->v[1] = a->v[1] + (b->v[1] - a->v[1]) * t;
147 res->v[2] = a->v[2] + (b->v[2] - a->v[2]) * t;
151inline static void t3d_quat_identity(T3DQuat *quat) {
152 *quat = (T3DQuat){{0, 0, 0, 1}};
162 float c1 = fm_cosf(rotEuler[0] / 2.0f);
163 float s1 = fm_sinf(rotEuler[0] / 2.0f);
164 float c2 = fm_cosf(rotEuler[1] / 2.0f);
165 float s2 = fm_sinf(rotEuler[1] / 2.0f);
166 float c3 = fm_cosf(rotEuler[2] / 2.0f);
167 float s3 = fm_sinf(rotEuler[2] / 2.0f);
169 quat->v[0] = c1 * c2 * s3 - s1 * s2 * c3;
170 quat->v[1] = s1 * c2 * c3 - c1 * s2 * s3;
171 quat->v[2] = c1 * s2 * c3 + s1 * c2 * s3;
172 quat->v[3] = c1 * c2 * c3 + s1 * s2 * s3;
183 float s = fm_sinf(angleRad / 2.0f);
184 float c = fm_cosf(angleRad / 2.0f);
185 *quat = (T3DQuat){{axis[0] * s, axis[1] * s, axis[2] * s, c}};
198 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];
199 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];
200 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];
201 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];
212 T3DQuat tmp, quatRot;
226 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];
235 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]);
249void t3d_quat_nlerp(T3DQuat *res,
const T3DQuat *a,
const T3DQuat *b,
float t);
251void t3d_quat_slerp(T3DQuat *res,
const T3DQuat *a,
const T3DQuat *b,
float t);
274inline static void t3d_mat4_scale(T3DMat4 *mat,
float scaleX,
float scaleY,
float scaleZ)
276 mat->m[0][0] *= scaleX;
277 mat->m[0][1] *= scaleX;
278 mat->m[0][2] *= scaleX;
279 mat->m[0][3] *= scaleX;
281 mat->m[1][0] *= scaleY;
282 mat->m[1][1] *= scaleY;
283 mat->m[1][2] *= scaleY;
284 mat->m[1][3] *= scaleY;
286 mat->m[2][0] *= scaleZ;
287 mat->m[2][1] *= scaleZ;
288 mat->m[2][2] *= scaleZ;
289 mat->m[2][3] *= scaleZ;
301 mat->m[3][0] += offsetX;
302 mat->m[3][1] += offsetY;
303 mat->m[3][2] += offsetZ;
312void t3d_mat4_rotate(T3DMat4 *mat,
const T3DVec3* axis,
float angleRad);
323void t3d_mat4_from_srt(T3DMat4 *mat,
const float scale[3],
const float quat[4],
const float translate[3]);
363void t3d_mat4fp_from_srt(T3DMat4FP *mat,
const float scale[3],
const float rotQuat[4],
const float translate[3]);
374 int32_t fixed = T3D_F32_TO_FIXED(val);
375 mat->m[column].i[row] = (int16_t)(fixed >> 16);
376 mat->m[column].f[row] = fixed & 0xFFFF;
403inline static void t3d_mat4fp_identity(T3DMat4FP *mat)
406 {{1, 0, 0, 0}, {0, 0, 0, 0}},
407 {{0, 1, 0, 0}, {0, 0, 0, 0}},
408 {{0, 0, 1, 0}, {0, 0, 0, 0}},
409 {{0, 0, 0, 1}, {0, 0, 0, 0}},
448void t3d_mat4_ortho(T3DMat4 *mat,
float left,
float right,
float bottom,
float top,
float near,
float far);
457void t3d_mat4_look_at(T3DMat4 *mat,
const T3DVec3 *eye,
const T3DVec3 *target,
const T3DVec3 *up);
508inline static void t3d_mat4_mul(T3DMat4 *matRes,
const T3DMat4 *matA,
const T3DMat4 *matB)
510 for(uint32_t i=0; i<4; i++) {
511 for(uint32_t j=0; j<4; j++) {
512 matRes->m[j][i] = matA->m[0][i] * matB->m[j][0] +
513 matA->m[1][i] * matB->m[j][1] +
514 matA->m[2][i] * matB->m[j][2] +
515 matA->m[3][i] * matB->m[j][3];
528 for(uint32_t i=0; i<3; i++) {
529 vecOut->v[i] = mat->m[0][i] * vec->v[0] +
530 mat->m[1][i] * vec->v[1] +
531 mat->m[2][i] * vec->v[2];
544 for(uint32_t i=0; i<4; i++) {
545 vecOut->v[i] = mat->m[0][i] * vec->v[0] +
546 mat->m[1][i] * vec->v[1] +
547 mat->m[2][i] * vec->v[2] +
570 inline void t3d_quat_identity(T3DQuat &quat) { t3d_quat_identity(&quat); }
578 inline void t3d_quat_slerp(T3DQuat &res,
const T3DQuat &a,
const T3DQuat &b,
float t) { t3d_quat_slerp(&res, &a, &b, t); }
587 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); }
596 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); }
602 inline void t3d_mat4_mul(T3DMat4 &matRes,
const T3DMat4 &matA,
const T3DMat4 &matB) {
t3d_mat4_mul(&matRes, &matA, &matB); }
void t3d_mat4_ortho(T3DMat4 *mat, float left, float right, float bottom, float top, float near, float far)
Definition t3dmath.c:209
static void t3d_mat4_translate(T3DMat4 *mat, float offsetX, float offsetY, float offsetZ)
Definition t3dmath.h:299
static void t3d_quat_normalize(T3DQuat *quat)
Definition t3dmath.h:233
static float s1616_to_float(int16_t partI, uint16_t partF)
Converts a 16.16 fixed-point number to a float.
Definition t3dmath.h:48
void t3d_mat4fp_from_srt_euler(T3DMat4FP *mat, const float scale[3], const float rot[3], const float translate[3])
Definition t3dmath.c:286
static void t3d_mat4_mul(T3DMat4 *matRes, const T3DMat4 *matA, const T3DMat4 *matB)
Multiplies the matrices 'matA' and 'matB' and stores it in 'matRes'.
Definition t3dmath.h:508
static void t3d_mat4fp_set_float(T3DMat4FP *mat, uint32_t column, uint32_t row, float val)
Sets a value in a fixed-point matrix.
Definition t3dmath.h:372
static void t3d_quat_from_rotation(T3DQuat *quat, float axis[3], float angleRad)
Definition t3dmath.h:181
bool t3d_frustum_vs_aabb(const T3DFrustum *frustum, const T3DVec3 *min, const T3DVec3 *max)
Definition t3dmath.c:92
void t3d_frustum_scale(T3DFrustum *frustum, float scale)
Definition t3dmath.c:84
void t3d_mat4fp_from_srt(T3DMat4FP *mat, const float scale[3], const float rotQuat[4], const float translate[3])
Definition t3dmath.c:292
static void t3d_vec3_lerp(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b, float t)
Linearly interpolates between 'a' and 'b' by 't' and stores it in 'res'.
Definition t3dmath.h:143
static float t3d_vec3_distance2(const T3DVec3 *vecA, const T3DVec3 *vecB)
Returns the squared distance between 'vecA' and 'vecB'.
Definition t3dmath.h:108
void t3d_mat4_from_srt_euler(T3DMat4 *mat, const float scale[3], const float rot[3], const float translate[3])
Definition t3dmath.c:249
static void t3d_quat_rotate_euler(T3DQuat *quat, float axis[3], float angleRad)
Definition t3dmath.h:210
static float t3d_lerp(float a, float b, float t)
Interpolates between two floats by 't'.
Definition t3dmath.h:56
bool t3d_frustum_vs_sphere(const T3DFrustum *frustum, const T3DVec3 *center, const float radius)
Definition t3dmath.c:140
void t3d_mat4_to_frustum(T3DFrustum *frustum, const T3DMat4 *mat)
Definition t3dmath.c:66
static float t3d_quat_dot(const T3DQuat *a, const T3DQuat *b)
Definition t3dmath.h:224
static void t3d_vec3_mul(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b)
Sets 'res' to 'a + b'.
Definition t3dmath.h:75
static void t3d_vec3_scale(T3DVec3 *res, const T3DVec3 *a, float s)
Sets 'res' to 'a * s'.
Definition t3dmath.h:82
static void t3d_quat_from_euler(T3DQuat *quat, const float rotEuler[3])
Definition t3dmath.h:160
static void t3d_vec3_norm(T3DVec3 *res)
Normalizes 'res', this does NOT check for a zero-length vector.
Definition t3dmath.h:120
static void t3d_mat4fp_set_pos(T3DMat4FP *mat, const float pos[3])
Definition t3dmath.h:385
void t3d_mat4_look_at(T3DMat4 *mat, const T3DVec3 *eye, const T3DVec3 *target, const T3DVec3 *up)
Creates a look-at matrix from an eye and target vector.
Definition t3dmath.c:42
void t3d_mat4_to_fixed(T3DMat4FP *matOut, const T3DMat4 *matIn)
Definition t3dmath.c:158
static void t3d_quat_mul(T3DQuat *res, T3DQuat *a, T3DQuat *b)
Definition t3dmath.h:196
static void t3d_mat4_mul_vec3(T3DVec4 *vecOut, const T3DMat4 *mat, const T3DVec3 *vec)
Definition t3dmath.h:542
static void t3d_mat4_scale(T3DMat4 *mat, float scaleX, float scaleY, float scaleZ)
Definition t3dmath.h:274
void t3d_mat4_to_fixed_3x4(T3DMat4FP *matOut, const T3DMat4 *matIn)
Definition t3dmath.c:183
static void t3d_vec3_add(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b)
Sets 'res' to 'a + b'.
Definition t3dmath.h:68
static void t3d_mat3_mul_vec3(T3DVec3 *vecOut, const T3DMat4 *mat, const T3DVec3 *vec)
Definition t3dmath.h:526
static float t3d_vec3_distance(const T3DVec3 *vecA, const T3DVec3 *vecB)
Returns the distance between 'vecA' and 'vecB'.
Definition t3dmath.h:115
void t3d_mat4_rotate(T3DMat4 *mat, const T3DVec3 *axis, float angleRad)
Definition t3dmath.c:298
bool t3d_frustum_vs_aabb_s16(const T3DFrustum *frustum, const int16_t min[3], const int16_t max[3])
Definition t3dmath.c:116
void t3d_mat4_perspective(T3DMat4 *mat, float fov, float aspect, float near, float far)
Definition t3dmath.c:148
static void t3d_vec3_cross(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b)
Crosses 'a' with 'b' and stores it in 'res'.
Definition t3dmath.h:129
static void t3d_vec3_diff(T3DVec3 *res, const T3DVec3 *a, const T3DVec3 *b)
Set 'res' to 'a - b'.
Definition t3dmath.h:89
static float t3d_vec3_dot(const T3DVec3 *a, const T3DVec3 *b)
Returns the dot product of 'a' and 'b'.
Definition t3dmath.h:136
static float t3d_mat4fp_get_float(const T3DMat4FP *mat, uint32_t y, uint32_t x)
Gets a value from a fixed-point matrix.
Definition t3dmath.h:398
void t3d_mat4_rot_from_dir(T3DMat4 *mat, const T3DVec3 *dir, const T3DVec3 *up)
Definition t3dmath.c:267
static void t3d_mat4_identity(T3DMat4 *mat)
Initializes a matrix to the identity matrix.
Definition t3dmath.h:257
void t3d_mat4_from_srt(T3DMat4 *mat, const float scale[3], const float quat[4], const float translate[3])
Definition t3dmath.c:220
static float t3d_vec3_len2(const T3DVec3 *vec)
Returns the squared length of 'v'.
Definition t3dmath.h:96
static float t3d_vec3_len(const T3DVec3 *vec)
Returns the length of 'v'.
Definition t3dmath.h:103
static float t3d_lerp_angle(float a, float b, float t)
Interpolates between two angles (radians) by 't'.
Definition t3dmath.h:61
void t3d_quat_nlerp(T3DQuat *res, const T3DQuat *a, const T3DQuat *b, float t)
Definition t3dmath.c:8