1.kmath.c
#include "kmath.h"
#include "platform/platform.h"
#include <math.h>
#include <stdlib.h>
static b8 rand_seeded = false;
f32 ksin(f32 x)
{
return sinf(x);
}
f32 kcos(f32 x)
{
return cosf(x);
}
f32 ktan(f32 x)
{
return tanf(x);
}
f32 kacos(f32 x)
{
return acosf(x);
}
f32 ksqrt(f32 x)
{
return sqrtf(x);
}
f32 kabs(f32 x)
{
return fabsf(x);
}
i32 krandom()
{
if(!rand_seeded)
{
srand((u32)platform_get_absolute_time());
rand_seeded = true;
}
return rand();
}
i32 krandom_in_range(i32 min,i32 max)
{
if(!rand_seeded)
{
srand((u32)platform_get_absolute_time());
rand_seeded = true;
}
return (rand()%(max-min+1)) + min;
}
f32 fkrandom()
{
return (float)krandom() / (f32)RAND_MAX;
}
f32 fkrandom_in_range(f32 min,f32 max)
{
return min +((float)krandom() / ((f32)RAND_MAX / (max - min)));
}
2.kmath.h
#pragma once
#include "defines.h"
#include "math_types.h"
#include "core/kmemory.h"
/** @brief An approximate representation of PI. */
#define K_PI 3.14159265358979323846f
/** @brief An approximate representation of PI multiplied by 2. */
#define K_2PI (2.0f * K_PI)
/** @brief An approximate representation of PI multiplied by 4. */
#define K_4PI (4.0f * K_PI)
/** @brief An approximate representation of PI divided by 2. */
#define K_HALF_PI (0.5f * K_PI)
/** @brief An approximate representation of PI divided by 4. */
#define K_QUARTER_PI (0.25f * K_PI)
/** @brief One divided by an approximate representation of PI. */
#define K_ONE_OVER_PI (1.0f / K_PI)
/** @brief One divided by half of an approximate representation of PI. */
#define K_ONE_OVER_TWO_PI (1.0f / K_2PI)
/** @brief An approximation of the square root of 2. */
#define K_SQRT_TWO 1.41421356237309504880f
/** @brief An approximation of the square root of 3. */
#define K_SQRT_THREE 1.73205080756887729352f
/** @brief One divided by an approximation of the square root of 2. */
#define K_SQRT_ONE_OVER_TWO 0.70710678118654752440f
/** @brief One divided by an approximation of the square root of 3. */
#define K_SQRT_ONE_OVER_THREE 0.57735026918962576450f
/** @brief A multiplier used to convert degrees to radians. */
#define K_DEG2RAD_MULTIPLIER (K_PI / 180.0f)
/** @brief A multiplier used to convert radians to degrees. */
#define K_RAD2DEG_MULTIPLIER (180.0f / K_PI)
#define K_SEC_TO_MS_MULTIPLIER 1000.0f
#define K_MS_TO_SEC_MULTIPLIER 0.001f
#define K_INFINITY 1e30f
#define K_FLOAT_EPSILON 1.192092896E-07f
KAPI f32 ksin(f32 x);
KAPI f32 kcos(f32 x);
KAPI f32 ktan(f32 x);
KAPI f32 kacos(f32 x);
KAPI f32 ksqrt(f32 x);
KAPI f32 kabs(f32 x);
KINLINE b8 is_power_of_2(u64 value)
{
return (value != 0) && ((value & (value - 1)) == 0);
}
KAPI i32 krandom();
KAPI i32 krandom_in_range(i32 min,i32 max);
KAPI f32 fkrandom();
KAPI f32 fkrandom_in_range(f32 min,f32 max);
KINLINE vec2 vec2_create(f32 x,f32 y)
{
vec2 out_vector;
out_vector.x = x;
out_vector.y = y;
return out_vector;
}
KINLINE vec2 vec2_zero()
{
return (vec2){0.0f,0.0f};
}
KINLINE vec2 vec2_one()
{
return (vec2){1.0f,1.0f};
}
KINLINE vec2 vec2_up()
{
return (vec2){0.0f,1.0f};
}
KINLINE vec2 vec2_down()
{
return (vec2){0.0f,-1.0f};
}
KINLINE vec2 vec2_left()
{
return (vec2){-1.0f,0.0f};
}
KINLINE vec2 vec2_right()
{
return (vec2){1.0f,0.0f};
}
KINLINE vec2 vec2_add(vec2 vector_0,vec2 vector_1)
{
return (vec2)
{
vector_0.x + vector_1.x,
vector_0.y + vector_1.y
};
}
KINLINE vec2 vec2_sub(vec2 vector_0,vec2 vector_1)
{
return (vec2)
{
vector_0.x - vector_1.x,
vector_0.y - vector_1.y
};
}
KINLINE vec2 vec2_div(vec2 vector_0,vec2 vector_1)
{
return (vec2)
{
vector_0.x / vector_1.x,
vector_0.y / vector_1.y
};
}
KINLINE vec2 vec2_mul(vec2 vector_0,vec2 vector_1)
{
return (vec2)
{
vector_0.x * vector_1.x,
vector_0.y * vector_1.y
};
}
KINLINE f32 vec2_length_squared(vec2 vector)
{
return vector.x * vector.x + vector.y * vector.y;
}
KINLINE f32 vec2_length(vec2 vector)
{
return ksqrt(vec2_length_squared(vector));
}
KINLINE void vec2_normalize(vec2* vector)
{
const f32 length = vec2_length(*vector);
vector->x /= length;
vector->y /= length;
}
KINLINE vec2 vec2_normalized(vec2 vector)
{
vec2_normalize(&vector);
return vector;
}
KINLINE b8 vec2_compare(vec2 vector_0,vec2 vector_1,f32 tolerance)
{
if(kabs(vector_0.x - vector_1.x)>tolerance)
{
return false;
}
if(kabs(vector_0.y - vector_1.y)>tolerance)
{
return false;
}
return true;
}
KINLINE f32 vec2_distance(vec2 vector_0,vec2 vector_1)
{
vec2 d = (vec2)
{
vector_0.x - vector_1.x,
vector_0.y - vector_1.y
};
return vec2_length(d);
}
KINLINE vec3 vec3_create(f32 x,f32 y,f32 z)
{
return (vec3){x,y,z};
}
KINLINE vec3 vec3_from_vec4(vec4 vector)
{
return (vec3){vector.x,vector.y,vector.z};
}
KINLINE vec4 vec3_to_vec4(vec3 vector,f32 w)
{
//return (vec4){vector.x,vector.y,vector.z,w};
return (vec4){vector.x,vector.y,vector.z,w};
}
KINLINE vec3 vec3_zero()
{
return (vec3){0.0f,0.0f,0.0f};
}
KINLINE vec3 vec3_one()
{
return (vec3){1.0f,1.0f,1.0f};
}
KINLINE vec3 vec3_up()
{
return (vec3){0.0f,1.0f,0.0f};
}
KINLINE vec3 vec3_down()
{
return (vec3){0.0f,-1.0f,0.0f};
}
KINLINE vec3 vec3_left()
{
return (vec3){-1.0f,0.0f,0.0f};
}
KINLINE vec3 vec3_right()
{
return (vec3){1.0f,0.0f,0.0f};
}
KINLINE vec3 vec3_forward()
{
return (vec3){0.0f,0.0f,-1.0f};
}
KINLINE vec3 vec3_back()
{
return (vec3){0.0f,0.0f,1.0f};
}
KINLINE vec3 vec3_add(vec3 vector_0,vec3 vector_1)
{
return (vec3)
{
vector_0.x + vector_1.x,
vector_0.y + vector_1.y,
vector_0.z + vector_1.z
};
}
KINLINE vec3 vec3_sub(vec3 vector_0,vec3 vector_1)
{
return (vec3)
{
vector_0.x - vector_1.x,
vector_0.y - vector_1.y,
vector_0.z - vector_1.z
};
}
KINLINE vec3 vec3_mul(vec3 vector_0,vec3 vector_1)
{
return (vec3)
{
vector_0.x * vector_1.x,
vector_0.y * vector_1.y,
vector_0.z * vector_1.z
};
}
KINLINE vec3 vec3_mul_scalar(vec3 vector_0,f32 scalar)
{
return (vec3)
{
vector_0.x * scalar,
vector_0.y * scalar,
vector_0.z * scalar
};
}
KINLINE vec3 vec3_div(vec3 vector_0, vec3 vector_1)
{
return (vec3)
{
vector_0.x / vector_1.x,
vector_0.y / vector_1.y,
vector_0.z / vector_1.z
};
}
KINLINE f32 vec3_length_squared(vec3 vector)
{
return vector.x * vector.x + vector.y * vector.y + vector.z * vector.z;
}
KINLINE f32 vec3_length(vec3 vector)
{
return ksqrt(vec3_length_squared(vector));
}
KINLINE void vec3_normalize(vec3* vector)
{
const f32 length = vec3_length(*vector);
vector->x /= length;
vector->y /= length;
vector->z /= length;
}
KINLINE vec3 vec3_normalized(vec3 vector)
{
vec3_normalize(&vector);
return vector;
}
KINLINE f32 vec3_dot(vec3 vector_0,vec3 vector_1)
{
f32 p = 0;
p += vector_0.x * vector_1.x;
p += vector_0.y * vector_1.y;
p += vector_0.z * vector_1.z;
return p;
}
KINLINE vec3 vec3_cross(vec3 vector_0,vec3 vector_1)
{
return (vec3)
{
vector_0.y * vector_1.z - vector_0.z * vector_1.y,
vector_0.z * vector_1.x - vector_0.x * vector_1.z,
vector_0.x * vector_1.y - vector_0.y * vector_1.x
};
}
KINLINE const b8 vec3_compare(vec3 vector_0,vec3 vector_1,f32 tolerance)
{
if(kabs(vector_0.x - vector_1.x)>tolerance)
{
return false;
}
if(kabs(vector_0.y - vector_1.y)>tolerance)
{
return false;
}
if(kabs(vector_0.z - vector_1.z)>tolerance)
{
return false;
}
return true;
}
KINLINE f32 vec3_distance(vec3 vector_0,vec3 vector_1)
{
vec3 d = (vec3){
vector_0.x - vector_1.x,
vector_0.y - vector_1.y,
vector_0.z - vector_1.z,
};
return vec3_length(d);
}
KINLINE vec4 vec4_create(f32 x,f32 y,f32 z,f32 w)
{
vec4 out_vector;
#if defined(KUSE_SIMD)
out_vector.data = _mm_setr_ps(x,y,z,w);
#else
out_vector.x = x;
out_vector.x = y;
out_vector.x = z;
out_vector.x = w;
#endif
return out_vector;
}
KINLINE vec3 vec4_to_vec3(vec4 vector)
{
return (vec3){vector.x,vector.y,vector.z};
}
KINLINE vec4 vec4_from_vec3(vec3 vector,f32 w)
{
#if defined(KUSE_SIMD)
vec4 out_vector;
out_vector.data = _mm_setr_ps(x,y,z,w);
return out_vector;
#else
return (vec4){vector.x,vector.y,vector.z,w};
#endif
}
KINLINE vec4 vec4_zero()
{
return (vec4){0.0f,0.0f,0.0f,0.0f};
}
KINLINE vec4 vec4_one()
{
return (vec4){1.0f,1.0f,1.0f,1.0f};
}
KINLINE vec4 vec4_add(vec4 vector_0,vec4 vector_1)
{
vec4 result;
for(u64 i = 0;i <4;++i)
{
result.elementsi = vector_0.elementsi + vector_1.elementsi;
}
return result;
}
KINLINE vec4 vec4_sub(vec4 vector_0,vec4 vector_1)
{
vec4 result;
for(u64 i = 0;i <4;++i)
{
result.elementsi = vector_0.elementsi - vector_1.elementsi;
}
return result;
}
KINLINE vec4 vec4_mul(vec4 vector_0,vec4 vector_1)
{
vec4 result;
for(u64 i = 0;i <4;++i)
{
result.elementsi = vector_0.elementsi * vector_1.elementsi;
}
return result;
}
KINLINE vec4 vec4_div(vec4 vector_0,vec4 vector_1)
{
vec4 result;
for(u64 i = 0;i <4;++i)
{
result.elementsi = vector_0.elementsi / vector_1.elementsi;
}
return result;
}
KINLINE f32 vec4_length_squared(vec4 vector)
{
return vector.x * vector.x + vector.y * vector.y + vector.z * vector.z +vector.w * vector.w;
}
KINLINE f32 vec4_length(vec4 vector)
{
return ksqrt(vec4_length_squared(vector));
}
KINLINE void vec4_normalize(vec4* vector)
{
const f32 length = vec4_length(*vector);
vector->x/=length;
vector->y/=length;
vector->z/=length;
vector->w/=length;
}
KINLINE vec4 vec4_normalized(vec4 vector)
{
vec4_normalize(&vector);
return vector;
}
KINLINE f32 vec4_dot_f32(
f32 a0, f32 a1, f32 a2, f32 a3,
f32 b0, f32 b1, f32 b2, f32 b3
)
{
f32 p;
p =
a0 * b0 +
a1 * b1 +
a2 * b2 +
a3 * b3;
return p;
}
KINLINE mat4 mat4_identity()
{
mat4 out_matrix;
kzero_memory(out_matrix.data,sizeof(f32) * 16);
out_matrix.data0 = 1.0f;
out_matrix.data5 = 1.0f;
out_matrix.data10 = 1.0f;
out_matrix.data15 = 1.0f;
return out_matrix;
}
KINLINE mat4 mat4_mul(mat4 matrix_0,mat4 matrix_1)
{
mat4 out_matrix = mat4_identity();
const f32* m1_ptr = matrix_0.data;
const f32* m2_ptr = matrix_1.data;
f32* dst_ptr = out_matrix.data;
for(i32 i = 0;i<4;++i)
{
for(i32 j = 0;j<4;++j)
{
*dst_ptr =
m1_ptr0 * m2_ptr0 + j +
m1_ptr1 * m2_ptr4 + j +
m1_ptr2 * m2_ptr8 + j +
m1_ptr3 * m2_ptr12 + j;
dst_ptr++;
}
m1_ptr += 4;
}
return out_matrix;
}
KINLINE mat4 mat4_orthographic(f32 left,f32 right,f32 bottom,f32 top,f32 near_clip,f32 far_clip)
{
mat4 out_matrix = mat4_identity();
f32 lr = 1.9f/(left - right);
f32 bt = 1.0f/(bottom - top);
f32 nf = 1.0f/(near_clip - far_clip);
out_matrix.data0 = -2.0f * lr;
out_matrix.data5 = -2.0f * bt;
out_matrix.data10 = -2.0f * nf;
out_matrix.data12 = (left + right) * lr;
out_matrix.data12 = (top + bottom) * bt;
out_matrix.data14 = (far_clip + near_clip) * nf;
return out_matrix;
}
KINLINE mat4 mat4_perspective(f32 fov_radians,f32 aspect_ratio,f32 near_clip,f32 far_clip)
{
f32 half_tan_fov = ktan(fov_radians * 0.5f);
mat4 out_matrix;
kzero_memory(out_matrix.data,sizeof(f32)*16);
out_matrix.data0 = 1.0f / (aspect_ratio * half_tan_fov);
out_matrix.data5 = 1.0f / half_tan_fov;
out_matrix.data10 = -((far_clip + near_clip) / (far_clip - near_clip));
out_matrix.data11 = -1.0f;
out_matrix.data14 = -((2.0f * far_clip * near_clip) / (far_clip - near_clip));
return out_matrix;
}
KINLINE mat4 mat4_look_at(vec3 position,vec3 target,vec3 up)
{
mat4 out_matrix;
vec3 z_axis;
z_axis.x = target.x - position.x;
z_axis.y = target.y - position.y;
z_axis.z = target.z - position.z;
z_axis = vec3_normalized(z_axis);
vec3 x_axis = vec3_normalized(vec3_cross(z_axis,up));
vec3 y_axis = vec3_cross(x_axis,z_axis);
out_matrix.data0 = x_axis.x;
out_matrix.data1 = y_axis.x;
out_matrix.data2 = -z_axis.x;
out_matrix.data3 = 0;
out_matrix.data4 = x_axis.y;
out_matrix.data5 = y_axis.y;
out_matrix.data6 = -z_axis.y;
out_matrix.data7 = 0;
out_matrix.data8 = x_axis.z;
out_matrix.data9 = y_axis.z;
out_matrix.data10 = -z_axis.z;
out_matrix.data11 = 0;
out_matrix.data12 = -vec3_dot(x_axis,position);
out_matrix.data13 = -vec3_dot(y_axis,position);
out_matrix.data14 = vec3_dot(z_axis,position);
out_matrix.data15 = 1.0f;
return out_matrix;
}
KINLINE mat4 mat4_transposed(mat4 matrix)
{
mat4 out_matrix = mat4_identity();
out_matrix.data0 = matrix.data0;
out_matrix.data1 = matrix.data4;
out_matrix.data2 = matrix.data8;
out_matrix.data3 = matrix.data12;
out_matrix.data4 = matrix.data1;
out_matrix.data5 = matrix.data5;
out_matrix.data6 = matrix.data9;
out_matrix.data7 = matrix.data13;
out_matrix.data8 = matrix.data2;
out_matrix.data9 = matrix.data6;
out_matrix.data10 = matrix.data10;
out_matrix.data11 = matrix.data14;
out_matrix.data12 = matrix.data3;
out_matrix.data13 = matrix.data7;
out_matrix.data14 = matrix.data11;
out_matrix.data15 = matrix.data15;
return out_matrix;
}
KINLINE mat4 mat4_inverse(mat4 matrix) {
const f32 *m = matrix.data;
f32 t0 = m10 * m15;
f32 t1 = m14 * m11;
f32 t2 = m6 * m15;
f32 t3 = m14 * m7;
f32 t4 = m6 * m11;
f32 t5 = m10 * m7;
f32 t6 = m2 * m15;
f32 t7 = m14 * m3;
f32 t8 = m2 * m11;
f32 t9 = m10 * m3;
f32 t10 = m2 * m7;
f32 t11 = m6 * m3;
f32 t12 = m8 * m13;
f32 t13 = m12 * m9;
f32 t14 = m4 * m13;
f32 t15 = m12 * m5;
f32 t16 = m4 * m9;
f32 t17 = m8 * m5;
f32 t18 = m0 * m13;
f32 t19 = m12 * m1;
f32 t20 = m0 * m9;
f32 t21 = m8 * m1;
f32 t22 = m0 * m5;
f32 t23 = m4 * m1;
mat4 out_matrix;
f32 *o = out_matrix.data;
o0 = (t0 * m5 + t3 * m9 + t4 * m13) -
(t1 * m5 + t2 * m9 + t5 * m13);
o1 = (t1 * m1 + t6 * m9 + t9 * m13) -
(t0 * m1 + t7 * m9 + t8 * m13);
o2 = (t2 * m1 + t7 * m5 + t10 * m13) -
(t3 * m1 + t6 * m5 + t11 * m13);
o3 = (t5 * m1 + t8 * m5 + t11 * m9) -
(t4 * m1 + t9 * m5 + t10 * m9);
f32 d = 1.0f / (m0 * o0 + m4 * o1 + m8 * o2 + m12 * o3);
o0 = d * o0;
o1 = d * o1;
o2 = d * o2;
o3 = d * o3;
o4 = d * ((t1 * m4 + t2 * m8 + t5 * m12) -
(t0 * m4 + t3 * m8 + t4 * m12));
o5 = d * ((t0 * m0 + t7 * m8 + t8 * m12) -
(t1 * m0 + t6 * m8 + t9 * m12));
o6 = d * ((t3 * m0 + t6 * m4 + t11 * m12) -
(t2 * m0 + t7 * m4 + t10 * m12));
o7 = d * ((t4 * m0 + t9 * m4 + t10 * m8) -
(t5 * m0 + t8 * m4 + t11 * m8));
o8 = d * ((t12 * m7 + t15 * m11 + t16 * m15) -
(t13 * m7 + t14 * m11 + t17 * m15));
o9 = d * ((t13 * m3 + t18 * m11 + t21 * m15) -
(t12 * m3 + t19 * m11 + t20 * m15));
o10 = d * ((t14 * m3 + t19 * m7 + t22 * m15) -
(t15 * m3 + t18 * m7 + t23 * m15));
o11 = d * ((t17 * m3 + t20 * m7 + t23 * m11) -
(t16 * m3 + t21 * m7 + t22 * m11));
o12 = d * ((t14 * m10 + t17 * m14 + t13 * m6) -
(t16 * m14 + t12 * m6 + t15 * m10));
o13 = d * ((t20 * m14 + t12 * m2 + t19 * m10) -
(t18 * m10 + t21 * m14 + t13 * m2));
o14 = d * ((t18 * m6 + t23 * m14 + t15 * m2) -
(t22 * m14 + t14 * m2 + t19 * m6));
o15 = d * ((t22 * m10 + t16 * m2 + t21 * m6) -
(t20 * m6 + t23 * m10 + t17 * m2));
return out_matrix;
}
KINLINE mat4 mat4_translation(vec3 position)
{
mat4 out_matrix = mat4_identity();
out_matrix.data12 = position.x;
out_matrix.data13 = position.y;
out_matrix.data14 = position.z;
return out_matrix;
}
KINLINE mat4 mat4_scale(vec3 scale)
{
mat4 out_matrix = mat4_identity();
out_matrix.data0 = scale.x;
out_matrix.data5 = scale.y;
out_matrix.data10 = scale.z;
return out_matrix;
}
KINLINE mat4 mat4_euler_x(f32 angle_randians)
{
mat4 out_matrix = mat4_identity();
f32 c = kcos(angle_randians);
f32 s = ksin(angle_randians);
out_matrix.data5 = c;
out_matrix.data6 = s;
out_matrix.data9 = -s;
out_matrix.data10 = c;
return out_matrix;
}
KINLINE mat4 mat4_euler_y(f32 angle_radians)
{
mat4 out_matrix = mat4_identity();
f32 c = kcos(angle_radians);
f32 s = ksin(angle_radians);
out_matrix.data0 = c;
out_matrix.data2 = -s;
out_matrix.data8 = s;
out_matrix.data10 = c;
return out_matrix;
}
KINLINE mat4 mat4_euler_z(f32 angle_radians)
{
mat4 out_matrix = mat4_identity();
f32 c = kcos(angle_radians);
f32 s = ksin(angle_radians);
out_matrix.data0 = c;
out_matrix.data2 = -s;
out_matrix.data8 = s;
out_matrix.data10 = c;
return out_matrix;
}
KINLINE mat4 mat4_euler_xyz(f32 x_radians,f32 y_radians,f32 z_radians)
{
mat4 rx = mat4_euler_x(x_radians);
mat4 ry = mat4_euler_y(x_radians);
mat4 rz = mat4_euler_z(x_radians);
mat4 out_matrix = mat4_mul(rx,ry);
out_matrix = mat4_mul(out_matrix,rz);
return out_matrix;
}
KINLINE vec3 mat4_forward(mat4 matrix)
{
vec3 forward;
forward.x = -matrix.data2;
forward.y = -matrix.data6;
forward.z = -matrix.data10;
vec3_normalize(&forward);
return forward;
}
KINLINE vec3 mat4_backward(mat4 matrix)
{
vec3 backward;
backward.x = -matrix.data2;
backward.y = -matrix.data6;
backward.z = -matrix.data10;
vec3_normalize(&backward);
return backward;
}
KINLINE vec3 mat4_up(mat4 matrix)
{
vec3 up;
up.x = matrix.data1;
up.y = matrix.data5;
up.z = matrix.data9;
vec3_normalize(&up);
return up;
}
KINLINE vec3 mat4_down(mat4 matrix)
{
vec3 down;
down.x = matrix.data1;
down.y = matrix.data5;
down.z = matrix.data9;
vec3_normalize(&down);
return down;
}
KINLINE vec3 mat4_left(mat4 matrix)
{
vec3 right;
right.x = -matrix.data1;
right.y = -matrix.data5;
right.z = -matrix.data9;
vec3_normalize(&right);
return right;
}
KINLINE vec3 mat4_right(mat4 matrix)
{
vec3 left;
left.x = matrix.data1;
left.y = matrix.data5;
left.z = matrix.data9;
vec3_normalize(&left);
return left;
}
KINLINE quat quat_identify()
{
return (quat){0,0,0,1.0f};
}
KINLINE f32 quat_normal(quat q)
{
return ksqrt(
q.x * q.x +
q.y * q.y +
q.z * q.z +
q.w * q.w
);
}
KINLINE quat quat_normalize(quat q)
{
f32 normal = quat_normal(q);
return (quat)
{
q.x / normal,
q.y / normal,
q.z / normal,
q.w / normal
};
}
KINLINE quat quat_conjugate(quat q)
{
return (quat)
{
-q.x,
-q.y,
-q.z,
q.w
};
}
KINLINE quat quat_inverse(quat q)
{
return quat_normalize(quat_conjugate(q));
}
KINLINE quat quat_mul(quat q_0, quat q_1) {
quat out_quaternion;
out_quaternion.x =
q_0.x * q_1.w + q_0.y * q_1.z - q_0.z * q_1.y + q_0.w * q_1.x;
out_quaternion.y =
-q_0.x * q_1.z + q_0.y * q_1.w + q_0.z * q_1.x + q_0.w * q_1.y;
out_quaternion.z =
q_0.x * q_1.y - q_0.y * q_1.x + q_0.z * q_1.w + q_0.w * q_1.z;
out_quaternion.w =
-q_0.x * q_1.x - q_0.y * q_1.y - q_0.z * q_1.z + q_0.w * q_1.w;
return out_quaternion;
}
KINLINE f32 quat_dot(quat q_0,quat q_1)
{
return q_0.x * q_1.x +
q_0.y * q_1.y +
q_0.z * q_1.z +
q_0.w * q_1.w;
}
KINLINE mat4 quat_to_mat4(quat q) {
mat4 out_matrix = mat4_identity();
quat n = quat_normalize(q);
out_matrix.data0 = 1.0f - 2.0f * n.y * n.y - 2.0f * n.z * n.z;
out_matrix.data1 = 2.0f * n.x * n.y - 2.0f * n.z * n.w;
out_matrix.data2 = 2.0f * n.x * n.z + 2.0f * n.y * n.w;
out_matrix.data4 = 2.0f * n.x * n.y + 2.0f * n.z * n.w;
out_matrix.data5 = 1.0f - 2.0f * n.x * n.x - 2.0f * n.z * n.z;
out_matrix.data6 = 2.0f * n.y * n.z - 2.0f * n.x * n.w;
out_matrix.data8 = 2.0f * n.x * n.z - 2.0f * n.y * n.w;
out_matrix.data9 = 2.0f * n.y * n.z + 2.0f * n.x * n.w;
out_matrix.data10 = 1.0f - 2.0f * n.x * n.x - 2.0f * n.y * n.y;
return out_matrix;
}
KINLINE mat4 quat_to_rotation_matrix(quat q, vec3 center) {
mat4 out_matrix;
f32 *o = out_matrix.data;
o0 = (q.x * q.x) - (q.y * q.y) - (q.z * q.z) + (q.w * q.w);
o1 = 2.0f * ((q.x * q.y) + (q.z * q.w));
o2 = 2.0f * ((q.x * q.z) - (q.y * q.w));
o3 = center.x - center.x * o0 - center.y * o1 - center.z * o2;
o4 = 2.0f * ((q.x * q.y) - (q.z * q.w));
o5 = -(q.x * q.x) + (q.y * q.y) - (q.z * q.z) + (q.w * q.w);
o6 = 2.0f * ((q.y * q.z) + (q.x * q.w));
o7 = center.y - center.x * o4 - center.y * o5 - center.z * o6;
o8 = 2.0f * ((q.x * q.z) + (q.y * q.w));
o9 = 2.0f * ((q.y * q.z) - (q.x * q.w));
o10 = -(q.x * q.x) - (q.y * q.y) + (q.z * q.z) + (q.w * q.w);
o11 = center.z - center.x * o8 - center.y * o9 - center.z * o10;
o12 = 0.0f;
o13 = 0.0f;
o14 = 0.0f;
o15 = 1.0f;
return out_matrix;
}
KINLINE quat quat_from_axis_angle(vec3 axis,f32 angle,b8 normalize)
{
const f32 half_angle = 0.5f * angle;
f32 s = ksin(half_angle);
f32 c = kcos(half_angle);
quat q = (quat){s*axis.x,s*axis.y,s*axis.z,c};
if(normalize)
{
return quat_normalize(q);
}
return q;
}
KINLINE quat quat_slerp(quat q_0, quat q_1, f32 percentage) {
quat out_quaternion;
// Source: https://en.wikipedia.org/wiki/Slerp
// Only unit quaternions are valid rotations.
// Normalize to avoid undefined behavior.
quat v0 = quat_normalize(q_0);
quat v1 = quat_normalize(q_1);
// Compute the cosine of the angle between the two vectors.
f32 dot = quat_dot(v0, v1);
// If the dot product is negative, slerp won't take
// the shorter path. Note that v1 and -v1 are equivalent when
// the negation is applied to all four components. Fix by
// reversing one quaternion.
if (dot < 0.0f) {
v1.x = -v1.x;
v1.y = -v1.y;
v1.z = -v1.z;
v1.w = -v1.w;
dot = -dot;
}
const f32 DOT_THRESHOLD = 0.9995f;
if (dot > DOT_THRESHOLD) {
// If the inputs are too close for comfort, linearly interpolate
// and normalize the result.
out_quaternion = (quat){v0.x + ((v1.x - v0.x) * percentage),
v0.y + ((v1.y - v0.y) * percentage),
v0.z + ((v1.z - v0.z) * percentage),
v0.w + ((v1.w - v0.w) * percentage)};
return quat_normalize(out_quaternion);
}
// Since dot is in range 0, DOT_THRESHOLD, acos is safe
f32 theta_0 = kacos(dot); // theta_0 = angle between input vectors
f32 theta = theta_0 * percentage; // theta = angle between v0 and result
f32 sin_theta = ksin(theta); // compute this value only once
f32 sin_theta_0 = ksin(theta_0); // compute this value only once
f32 s0 =
kcos(theta) -
dot * sin_theta / sin_theta_0; // == sin(theta_0 - theta) / sin(theta_0)
f32 s1 = sin_theta / sin_theta_0;
return (quat){(v0.x * s0) + (v1.x * s1), (v0.y * s0) + (v1.y * s1),
(v0.z * s0) + (v1.z * s1), (v0.w * s0) + (v1.w * s1)};
}
KINLINE f32 deg_to_rad(f32 degrees)
{
return degrees* K_DEG2RAD_MULTIPLIER;
}
KINLINE f32 rad_to_deg(f32 radians)
{
return radians * K_RAD2DEG_MULTIPLIER;
}