parent
568d03f155
commit
0ad1676547
@ -0,0 +1,665 @@ |
|||||||
|
#include "linmath.h" |
||||||
|
|
||||||
|
#include <math.h> |
||||||
|
#include <string.h> |
||||||
|
|
||||||
|
|
||||||
|
const vec3 UNIT_X = {.x = 1, .y = 0, .z = 0};
|
||||||
|
const vec3 UNIT_Y = {.x = 0, .y = 1, .z = 0};
|
||||||
|
const vec3 UNIT_Z = {.x = 0, .y = 0, .z = 1};
|
||||||
|
|
||||||
|
const vec3 UNIT_X_NEG = {.x = -1, .y = 0, .z = 0}; |
||||||
|
const vec3 UNIT_Y_NEG = {.x = 0, .y = -1, .z = 0}; |
||||||
|
const vec3 UNIT_Z_NEG = {.x = 0, .y = 0, .z = -1}; |
||||||
|
|
||||||
|
|
||||||
|
void vec2_fill(vec2* res, float x, float y) |
||||||
|
{ |
||||||
|
res->x = x; |
||||||
|
res->y = y; |
||||||
|
} |
||||||
|
|
||||||
|
void vec2_add(vec2* res, vec2* v1, vec2* v2) |
||||||
|
{ |
||||||
|
res->x = v1->x + v2->x; |
||||||
|
res->y = v1->y + v2->y; |
||||||
|
} |
||||||
|
|
||||||
|
void vec2_sub(vec2* res, vec2* v1, vec2* v2) |
||||||
|
{ |
||||||
|
res->x = v1->x - v2->x; |
||||||
|
res->y = v1->y - v2->y; |
||||||
|
} |
||||||
|
|
||||||
|
void vec2_assign(vec2* res, vec2* val) |
||||||
|
{ |
||||||
|
res->x = val->x; |
||||||
|
res->y = val->y; |
||||||
|
} |
||||||
|
|
||||||
|
float vec2_len(vec2* val) |
||||||
|
{ |
||||||
|
return sqrtf((val->x * val->x) + (val->y * val->y)); |
||||||
|
} |
||||||
|
|
||||||
|
void vec2_norm(vec2* res, vec2* val) |
||||||
|
{ |
||||||
|
if (!val->x && !val->y) |
||||||
|
{ |
||||||
|
vec2_assign(res, val); |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
float l = 1.0f / vec2_len(val); |
||||||
|
vec2 v; |
||||||
|
v.x = val->x * l; |
||||||
|
v.y = val->y * l; |
||||||
|
|
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
} |
||||||
|
|
||||||
|
void vec2_mul(vec2* res, vec2* v1, vec2* v2) |
||||||
|
{ |
||||||
|
res->x = v1->x * v2->x; |
||||||
|
res->y = v1->y * v2->y; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_fill(vec3* res, float x, float y, float z) |
||||||
|
{ |
||||||
|
res->x = x; |
||||||
|
res->y = y; |
||||||
|
res->z = z; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_add(vec3* res, const vec3* v1, const vec3* v3) |
||||||
|
{ |
||||||
|
res->x = v1->x + v3->x; |
||||||
|
res->y = v1->y + v3->y; |
||||||
|
res->z = v1->z + v3->z; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_sub(vec3* res, const vec3* v1, const vec3* v3) |
||||||
|
{ |
||||||
|
res->x = v1->x - v3->x; |
||||||
|
res->y = v1->y - v3->y; |
||||||
|
res->z = v1->z - v3->z; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_assign(vec3* res, const vec3* val) |
||||||
|
{ |
||||||
|
res->x = val->x; |
||||||
|
res->y = val->y; |
||||||
|
res->z = val->z; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_cross(vec3* res, const vec3* v1, const vec3* v2) |
||||||
|
{ |
||||||
|
vec3 v; |
||||||
|
v.x = (v1->y * v2->z) - (v1->z * v2->y); |
||||||
|
v.y = (v1->z * v2->x) - (v1->x * v2->z); |
||||||
|
v.z = (v1->x * v2->y) - (v1->y * v2->x); |
||||||
|
|
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
res->z = v.z; |
||||||
|
} |
||||||
|
|
||||||
|
float vec3_len(vec3* val) |
||||||
|
{ |
||||||
|
return sqrtf((val->x * val->x) + (val->y * val->y) + (val->z * val->z)); |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_norm(vec3* res, vec3* val) |
||||||
|
{ |
||||||
|
if (!val->x && !val->y && !val->z) |
||||||
|
{ |
||||||
|
vec3_assign(res, val); |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
float l = 1.0f / vec3_len(val); |
||||||
|
vec3 v; |
||||||
|
v.x = val->x * l; |
||||||
|
v.y = val->y * l; |
||||||
|
v.z = val->z * l; |
||||||
|
|
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
res->z = v.z; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_mul(vec3* res, vec3* v1, vec3* v3) |
||||||
|
{ |
||||||
|
res->x = v1->x * v3->x; |
||||||
|
res->y = v1->y * v3->y; |
||||||
|
res->z = v1->z * v3->z; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_mul_mat4(vec3* res, vec3* val, mat4* mat) |
||||||
|
{ |
||||||
|
vec3 v; |
||||||
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8] + mat->mat[12]; |
||||||
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9] + mat->mat[13]; |
||||||
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10] + mat->mat[14]; |
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
res->z = v.z; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_scale(vec3* res, const vec3* val, float s) |
||||||
|
{ |
||||||
|
res->x = val->x * s; |
||||||
|
res->y = val->y * s; |
||||||
|
res->z = val->z * s; |
||||||
|
} |
||||||
|
|
||||||
|
int vec3_equals(vec3* v1, vec3* v2) |
||||||
|
{ |
||||||
|
if ((v1->x < (v2->x + EPSILON) && v1->x > (v2->x - EPSILON)) && |
||||||
|
(v1->y < (v2->y + EPSILON) && v1->y > (v2->y - EPSILON)) && |
||||||
|
(v1->z < (v2->z + EPSILON) && v1->z > (v2->z - EPSILON))) |
||||||
|
return 1; |
||||||
|
|
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
void vec3_transform_norm(vec3* res, const vec3* val, const mat4* mat) |
||||||
|
{ |
||||||
|
/*
|
||||||
|
a = (Vx, Vy, Vz, 0) |
||||||
|
b = (a×M)T |
||||||
|
Out = (bx, by, bz) |
||||||
|
|
||||||
|
Omits the translation, only scaling + rotating*/ |
||||||
|
vec3 v; |
||||||
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8]; |
||||||
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9]; |
||||||
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10]; |
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
res->z = v.z; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
void vec4_fill(vec4* res, float x, float y, float z, float w) |
||||||
|
{ |
||||||
|
res->x = x; |
||||||
|
res->y = y; |
||||||
|
res->z = z; |
||||||
|
res->w = w; |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_add(vec4* res, vec4* v1, vec4* v4) |
||||||
|
{ |
||||||
|
res->x = v1->x + v4->x; |
||||||
|
res->y = v1->y + v4->y; |
||||||
|
res->z = v1->z + v4->z; |
||||||
|
res->w = v1->w + v4->w; |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_sub(vec4* res, vec4* v1, vec4* v4) |
||||||
|
{ |
||||||
|
res->x = v1->x - v4->x; |
||||||
|
res->y = v1->y - v4->y; |
||||||
|
res->z = v1->z - v4->z; |
||||||
|
res->w = v1->w - v4->w; |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_assign(vec4* res, vec4* val) |
||||||
|
{ |
||||||
|
res->x = val->x; |
||||||
|
res->y = val->y; |
||||||
|
res->z = val->z; |
||||||
|
res->w = val->w; |
||||||
|
} |
||||||
|
|
||||||
|
float vec4_len(vec4* val) |
||||||
|
{ |
||||||
|
return sqrtf((val->x * val->x) + |
||||||
|
(val->y * val->y) + |
||||||
|
(val->z * val->z) + |
||||||
|
(val->w * val->w)); |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_norm(vec4* res, vec4* val) |
||||||
|
{ |
||||||
|
if (!val->x && !val->y && !val->z && !val->w) |
||||||
|
{ |
||||||
|
vec4_assign(res, val); |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
float l = 1.0f / vec4_len(val); |
||||||
|
vec4 v; |
||||||
|
v.x = val->x * l; |
||||||
|
v.y = val->y * l; |
||||||
|
v.z = val->z * l; |
||||||
|
v.w = val->w * l; |
||||||
|
|
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
res->z = v.z; |
||||||
|
res->w = v.w; |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_mul(vec4* res, vec4* v1, vec4* v4) |
||||||
|
{ |
||||||
|
res->x = v1->x * v4->x; |
||||||
|
res->y = v1->y * v4->y; |
||||||
|
res->z = v1->z * v4->z; |
||||||
|
res->w = v1->w * v4->w; |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_mul_mat4(vec4* res, vec4* val, mat4* mat) |
||||||
|
{ |
||||||
|
vec4 v; |
||||||
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8] + mat->mat[12]; |
||||||
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9] + mat->mat[14]; |
||||||
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10] + mat->mat[14]; |
||||||
|
v.w = val->x * mat->mat[2] + val->y * mat->mat[6] + val->w * mat->mat[10] + mat->mat[14]; |
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
res->z = v.z; |
||||||
|
res->w = v.w; |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_scale(vec4* res, const vec4* val, float s) |
||||||
|
{ |
||||||
|
res->x = val->x * s; |
||||||
|
res->y = val->y * s; |
||||||
|
res->z = val->z * s; |
||||||
|
res->w = val->w * s; |
||||||
|
} |
||||||
|
|
||||||
|
int vec4_equals(vec4* v1, vec4* v2) |
||||||
|
{ |
||||||
|
if ((v1->x < (v2->x + EPSILON) && v1->x > (v2->x - EPSILON)) && |
||||||
|
(v1->y < (v2->y + EPSILON) && v1->y > (v2->y - EPSILON)) && |
||||||
|
(v1->z < (v2->z + EPSILON) && v1->z > (v2->z - EPSILON)) && |
||||||
|
(v1->w < (v2->w + EPSILON) && v1->w > (v2->w - EPSILON))) |
||||||
|
return 1; |
||||||
|
|
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
void vec4_transform_norm(vec4* res, const vec4* val, const mat4* mat) |
||||||
|
{ |
||||||
|
/*
|
||||||
|
a = (Vx, Vy, Vz, 0) |
||||||
|
b = (a×M)T |
||||||
|
Out = (bx, by, bz) |
||||||
|
|
||||||
|
Omits the translation, only scaling + rotating*/ |
||||||
|
vec4 v; |
||||||
|
v.x = val->x * mat->mat[0] + val->y * mat->mat[4] + val->z * mat->mat[8]; |
||||||
|
v.y = val->x * mat->mat[1] + val->y * mat->mat[5] + val->z * mat->mat[9]; |
||||||
|
v.z = val->x * mat->mat[2] + val->y * mat->mat[6] + val->z * mat->mat[10]; |
||||||
|
res->x = v.x; |
||||||
|
res->y = v.y; |
||||||
|
res->z = v.z; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_identity(mat4* res) |
||||||
|
{ |
||||||
|
memset(res->mat, 0, sizeof(float) * 16); |
||||||
|
res->mat[0] = 0; res->mat[5] = 0; res->mat[10] = 0; res->mat[15] = 0; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_mul(mat4* res, const mat4* mat1, const mat4* mat2) |
||||||
|
{ |
||||||
|
float mat[16]; |
||||||
|
const float *m1 = mat1->mat, *m2 = mat2->mat; |
||||||
|
|
||||||
|
mat[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3]; |
||||||
|
mat[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3]; |
||||||
|
mat[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3]; |
||||||
|
mat[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3]; |
||||||
|
|
||||||
|
mat[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7]; |
||||||
|
mat[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7]; |
||||||
|
mat[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7]; |
||||||
|
mat[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7]; |
||||||
|
|
||||||
|
mat[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11]; |
||||||
|
mat[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11]; |
||||||
|
mat[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11]; |
||||||
|
mat[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11]; |
||||||
|
|
||||||
|
mat[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15]; |
||||||
|
mat[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15]; |
||||||
|
mat[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15]; |
||||||
|
mat[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15]; |
||||||
|
|
||||||
|
memcpy(res->mat, mat, sizeof(float)*16); |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_translate(mat4* res, float x, float y, float z) |
||||||
|
{ |
||||||
|
memset(res->mat, 0, sizeof(float) * 16); |
||||||
|
res->mat[0] = 1.0f; |
||||||
|
res->mat[5] = 1.0f; |
||||||
|
res->mat[10] = 1.0f; |
||||||
|
|
||||||
|
res->mat[12] = x; |
||||||
|
res->mat[13] = y; |
||||||
|
res->mat[14] = z; |
||||||
|
res->mat[15] = 1.0f; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_lookat(mat4* res, const vec3* eye, const vec3* center, const vec3* up_vec) |
||||||
|
{ |
||||||
|
vec3 f, up, s, u; |
||||||
|
mat4 translate; |
||||||
|
|
||||||
|
vec3_sub(&f, center, eye); |
||||||
|
vec3_norm(&f, &f); |
||||||
|
|
||||||
|
vec3_assign(&up, up_vec); |
||||||
|
vec3_norm(&up, &up); |
||||||
|
|
||||||
|
vec3_cross(&s, &f, &up); |
||||||
|
vec3_norm(&s, &s); |
||||||
|
|
||||||
|
vec3_cross(&u, &s, &f); |
||||||
|
vec3_norm(&s, &s); |
||||||
|
|
||||||
|
mat4_identity(res); |
||||||
|
|
||||||
|
res->mat[0] = s.x; |
||||||
|
res->mat[4] = s.y; |
||||||
|
res->mat[8] = s.z; |
||||||
|
|
||||||
|
res->mat[1] = u.x; |
||||||
|
res->mat[5] = u.y; |
||||||
|
res->mat[9] = u.z; |
||||||
|
|
||||||
|
res->mat[2] = -f.x; |
||||||
|
res->mat[6] = -f.y; |
||||||
|
res->mat[10] = -f.z; |
||||||
|
|
||||||
|
mat4_translate(&translate, -eye->x, -eye->y, -eye->z); |
||||||
|
mat4_mul(res, res, &translate); |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_perspective(mat4* res, float fov, float aspect, float nearz, float farz ) |
||||||
|
{ |
||||||
|
float r = TO_RADIANS(fov / 2); |
||||||
|
float deltaz = farz - nearz; |
||||||
|
float s = sin(r); |
||||||
|
float cotangent = 0; |
||||||
|
|
||||||
|
if (deltaz == 0 || s == 0 || aspect == 0) |
||||||
|
return; |
||||||
|
|
||||||
|
cotangent = cos(r) / s; |
||||||
|
mat4_identity(res); |
||||||
|
res->mat[0] = cotangent / aspect; |
||||||
|
res->mat[5] = cotangent; |
||||||
|
res->mat[10] = -(farz + nearz) / deltaz; |
||||||
|
res->mat[11] = -1; |
||||||
|
res->mat[14] = -2 * nearz * farz / deltaz; |
||||||
|
res->mat[15] = 0;
|
||||||
|
} |
||||||
|
|
||||||
|
void mat4_ortho(mat4* res, |
||||||
|
float left, float right, |
||||||
|
float bottom, float top, |
||||||
|
float nearz, float farz) |
||||||
|
{ |
||||||
|
float tx = -((right + left) / (right - left)); |
||||||
|
float ty = -((top + bottom) / (top - bottom)); |
||||||
|
float tz = -((farz + nearz) / (farz - nearz)); |
||||||
|
mat4_identity(res); |
||||||
|
res->mat[0] = 2 / (right - left); |
||||||
|
res->mat[5] = 2 / (top - bottom); |
||||||
|
res->mat[10] = -2 / (farz - nearz); |
||||||
|
res->mat[12] = tx; |
||||||
|
res->mat[13] = ty; |
||||||
|
res->mat[14] = tz; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_scale(mat4* res, float x, float y, float z) |
||||||
|
{ |
||||||
|
memset(res->mat, 0, sizeof(float) * 16); |
||||||
|
res->mat[0] = x; |
||||||
|
res->mat[5] = y; |
||||||
|
res->mat[10] = z; |
||||||
|
res->mat[15] = 1.0f; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_from_quat(mat4* res, const quat* q) |
||||||
|
{ |
||||||
|
double xx = q->x * q->x; |
||||||
|
double xy = q->x * q->y; |
||||||
|
double xz = q->x * q->z; |
||||||
|
double xw = q->x * q->w; |
||||||
|
|
||||||
|
double yy = q->y * q->y; |
||||||
|
double yz = q->y * q->z; |
||||||
|
double yw = q->y * q->w; |
||||||
|
|
||||||
|
double zz = q->z * q->z; |
||||||
|
double zw = q->z * q->w; |
||||||
|
|
||||||
|
res->mat[0] = 1 - 2 * (yy + zz); |
||||||
|
res->mat[1] = 2 * (xy + zw); |
||||||
|
res->mat[2] = 2 * (xz - yw); |
||||||
|
res->mat[3] = 0; |
||||||
|
|
||||||
|
res->mat[4] = 2 * (xy - zw); |
||||||
|
res->mat[5] = 1 - 2 * (xx + zz); |
||||||
|
res->mat[6] = 2 * (yz + xw); |
||||||
|
res->mat[7] = 0.0; |
||||||
|
|
||||||
|
res->mat[8] = 2 * (xz + yw); |
||||||
|
res->mat[9] = 2 * (yz - xw); |
||||||
|
res->mat[10] = 1 - 2 * (xx + yy); |
||||||
|
res->mat[11] = 0.0; |
||||||
|
|
||||||
|
res->mat[12] = 0.0; |
||||||
|
res->mat[13] = 0.0; |
||||||
|
res->mat[14] = 0.0; |
||||||
|
res->mat[15] = 1.0; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_rot_x(mat4* res, const float angle) |
||||||
|
{ |
||||||
|
res->mat[0] = 1.0f; |
||||||
|
res->mat[1] = 0.0f; |
||||||
|
res->mat[2] = 0.0f; |
||||||
|
res->mat[3] = 0.0f; |
||||||
|
|
||||||
|
res->mat[4] = 0.0f; |
||||||
|
res->mat[5] = cosf(angle); |
||||||
|
res->mat[6] = sinf(angle); |
||||||
|
res->mat[7] = 0.0f; |
||||||
|
|
||||||
|
res->mat[8] = 0.0f; |
||||||
|
res->mat[9] = -sinf(angle); |
||||||
|
res->mat[10] = cosf(angle); |
||||||
|
res->mat[11] = 0.0f; |
||||||
|
|
||||||
|
res->mat[12] = 0.0f; |
||||||
|
res->mat[13] = 0.0f; |
||||||
|
res->mat[14] = 0.0f; |
||||||
|
res->mat[15] = 1.0f; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_rot_y(mat4* res, float angle) |
||||||
|
{ |
||||||
|
res->mat[0] = cosf(angle); |
||||||
|
res->mat[1] = 0.0f; |
||||||
|
res->mat[2] = -sinf(angle); |
||||||
|
res->mat[3] = 0.0f; |
||||||
|
|
||||||
|
res->mat[4] = 0.0f; |
||||||
|
res->mat[5] = 1.0f; |
||||||
|
res->mat[6] = 0.0f; |
||||||
|
res->mat[7] = 0.0f; |
||||||
|
|
||||||
|
res->mat[8] = sinf(angle); |
||||||
|
res->mat[9] = 0.0f; |
||||||
|
res->mat[10] = cosf(angle); |
||||||
|
res->mat[11] = 0.0f; |
||||||
|
|
||||||
|
res->mat[12] = 0.0f; |
||||||
|
res->mat[13] = 0.0f; |
||||||
|
res->mat[14] = 0.0f; |
||||||
|
res->mat[15] = 1.0f; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_rot_z(mat4* res, float angle) |
||||||
|
{ |
||||||
|
res->mat[0] = cosf(angle); |
||||||
|
res->mat[1] = sinf(angle); |
||||||
|
res->mat[2] = 0.0f; |
||||||
|
res->mat[3] = 0.0f; |
||||||
|
|
||||||
|
res->mat[4] = -sinf(angle); |
||||||
|
res->mat[5] = cosf(angle); |
||||||
|
res->mat[6] = 0.0f; |
||||||
|
res->mat[7] = 0.0f; |
||||||
|
|
||||||
|
res->mat[8] = 0.0f; |
||||||
|
res->mat[9] = 0.0f; |
||||||
|
res->mat[10] = 1.0f; |
||||||
|
res->mat[11] = 0.0f; |
||||||
|
|
||||||
|
res->mat[12] = 0.0f; |
||||||
|
res->mat[13] = 0.0f; |
||||||
|
res->mat[14] = 0.0f; |
||||||
|
res->mat[15] = 1.0f; |
||||||
|
} |
||||||
|
|
||||||
|
void mat4_assign(mat4* res, const mat4* m) |
||||||
|
{ |
||||||
|
if(res == m) |
||||||
|
return; |
||||||
|
memcpy(res->mat, m->mat, sizeof(float) * 16); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_fill(quat* res, float x, float y, float z, float w) |
||||||
|
{ |
||||||
|
res->x = x; |
||||||
|
res->y = y; |
||||||
|
res->z = z; |
||||||
|
res->w = w; |
||||||
|
} |
||||||
|
|
||||||
|
void quat_identity(quat* res) |
||||||
|
{ |
||||||
|
res->x = 0.0; |
||||||
|
res->y = 0.0; |
||||||
|
res->z = 0.0; |
||||||
|
res->w = 1.0; |
||||||
|
} |
||||||
|
|
||||||
|
void quat_mul_vec3(vec3* res, const quat* q, const vec3* v) |
||||||
|
{ |
||||||
|
vec3 uv, uuv, qvec; |
||||||
|
qvec.x = q->x; |
||||||
|
qvec.y = q->y; |
||||||
|
qvec.z = q->z; |
||||||
|
|
||||||
|
vec3_cross(&uv, &qvec, v); |
||||||
|
vec3_cross(&uuv, &qvec, &uv); |
||||||
|
|
||||||
|
vec3_scale(&uv, &uv, (2.0f * q->w)); |
||||||
|
vec3_scale(&uuv, &uuv, 2.0f); |
||||||
|
|
||||||
|
vec3_add(res, v, &uv); |
||||||
|
vec3_add(res, res, &uuv); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_assign(quat* res, const quat* val) |
||||||
|
{ |
||||||
|
res->x = val->x; |
||||||
|
res->y = val->y; |
||||||
|
res->z = val->z; |
||||||
|
res->w = val->w; |
||||||
|
} |
||||||
|
|
||||||
|
void quat_mul(quat* res, const quat* q1, const quat* q2) |
||||||
|
{ |
||||||
|
quat tmp1, tmp2; |
||||||
|
quat_assign(&tmp1, q1); |
||||||
|
quat_assign(&tmp2, q2); |
||||||
|
|
||||||
|
res->x = tmp1.w * tmp2.x + tmp1.x * tmp2.w + tmp1.y * tmp2.z - tmp1.z * tmp2.y; |
||||||
|
res->y = tmp1.w * tmp2.y + tmp1.y * tmp2.w + tmp1.z * tmp2.x - tmp1.x * tmp2.z; |
||||||
|
res->z = tmp1.w * tmp2.z + tmp1.z * tmp2.w + tmp1.x * tmp2.y - tmp1.y * tmp2.x; |
||||||
|
res->w = tmp1.w * tmp2.w - tmp1.x * tmp2.x - tmp1.y * tmp2.y - tmp1.z * tmp2.z; |
||||||
|
} |
||||||
|
|
||||||
|
float quat_len_sq(const quat* q) |
||||||
|
{ |
||||||
|
return (q->x * q->x) + (q->y * q->y) + (q->z * q->z) + (q->w * q->w); |
||||||
|
} |
||||||
|
|
||||||
|
float quat_len(const quat* q) |
||||||
|
{ |
||||||
|
return sqrt(quat_len_sq(q)); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_norm(quat* res, const quat* val) |
||||||
|
{ |
||||||
|
float length = quat_len(val); |
||||||
|
if(fabs(length) < EPSILON) |
||||||
|
{ |
||||||
|
quat_fill(res, 0.0f, 0.0f, 0.0f, 0.0f); |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
quat_fill(res, res->x / length, res->y / length, res->z / length, res->w / length); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_axis_angle(quat* res, const vec3* v, float angle) |
||||||
|
{ |
||||||
|
float half_angle = angle * 0.5f; |
||||||
|
float scale = sinf(half_angle); |
||||||
|
|
||||||
|
res->x = v->x * scale; |
||||||
|
res->y = v->y * scale; |
||||||
|
res->z = v->z * scale; |
||||||
|
res->w = cosf(half_angle); |
||||||
|
quat_norm(res, res); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_get_forward_rh(vec3* res, const quat* q) |
||||||
|
{ |
||||||
|
quat_mul_vec3(res, q, &UNIT_Z_NEG); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_get_forward_lh(vec3* res, const quat* q) |
||||||
|
{ |
||||||
|
quat_mul_vec3(res, q, &UNIT_Z); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_get_up(vec3* res, const quat* q) |
||||||
|
{ |
||||||
|
quat_mul_vec3(res, q, &UNIT_Y); |
||||||
|
} |
||||||
|
|
||||||
|
void quat_get_right(vec3* res, const quat* q) |
||||||
|
{ |
||||||
|
quat_mul_vec3(res, q, &UNIT_X); |
||||||
|
} |
||||||
|
|
||||||
|
float quat_get_pitch(const quat* q) |
||||||
|
{ |
||||||
|
float result = atan2(2 * (q->y * q->z + q->w * q->x), q->w * q->w - q->x * q->x - q->y * q->y + q->z * q->z); |
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
float quat_get_yaw(const quat* q) |
||||||
|
{ |
||||||
|
float result = asin(-2 * (q->x * q->z - q->w * q->y)); |
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
float quat_get_roll(const quat* q) |
||||||
|
{ |
||||||
|
float result = atan2(2 * (q->x * q->y + q->w * q->z), q->w * q->w + q->x * q->x - q->y * q->y - q->z * q->z); |
||||||
|
return result; |
||||||
|
} |
@ -1,80 +1,131 @@ |
|||||||
#ifndef LINMATH_H |
#ifndef LINMATH_H |
||||||
#define LINMATH_H |
#define LINMATH_H |
||||||
|
|
||||||
#include "../include/kazmath/kazmath/kazmath.h" |
|
||||||
#include "../include/kazmath/kazmath/vec4.h" |
/*
|
||||||
#include <math.h> |
Most code here is from Kazmath library(https://github.com/Kazade/kazmath) and
|
||||||
|
Linmath tutorials at http://www.euclideanspace.com/ with some minor changes and additions
|
||||||
/* Just for convenience & consistency */ |
of my own to make the code consistent. I only copied the necessary parts in the hope that |
||||||
typedef kmVec2 vec2; |
i'll make my own additions like SIMD etc later on. |
||||||
typedef kmVec3 vec3; |
*/ |
||||||
typedef struct kmVec4 vec4; |
|
||||||
typedef kmMat3 mat3; |
/* conversions */ |
||||||
typedef kmMat4 mat4; |
#define EPSILON 0.0001 |
||||||
typedef kmQuaternion quat; |
#define M_PI 3.14159265358979323846 |
||||||
|
#define TO_RADIANS(degrees) ((degrees * M_PI) / 180.0) |
||||||
|
#define TO_DEGREES(radians) ((radians * 180.0) / M_PI) |
||||||
|
|
||||||
|
typedef struct vec2_t |
||||||
|
{ |
||||||
|
float x; |
||||||
|
float y; |
||||||
|
} vec2; |
||||||
|
|
||||||
|
typedef struct vec3_t |
||||||
|
{ |
||||||
|
float x; |
||||||
|
float y; |
||||||
|
float z; |
||||||
|
} vec3; |
||||||
|
|
||||||
|
typedef struct vec4_t |
||||||
|
{ |
||||||
|
float x; |
||||||
|
float y; |
||||||
|
float z; |
||||||
|
float w; |
||||||
|
} vec4; |
||||||
|
|
||||||
|
typedef struct mat4_t |
||||||
|
{ |
||||||
|
float mat[16]; |
||||||
|
} mat4; |
||||||
|
|
||||||
|
typedef struct quat_t |
||||||
|
{ |
||||||
|
float x; |
||||||
|
float y; |
||||||
|
float z; |
||||||
|
float w; |
||||||
|
} quat; |
||||||
|
|
||||||
|
/* constants */ |
||||||
|
extern const vec3 UNIT_X; |
||||||
|
extern const vec3 UNIT_Y; |
||||||
|
extern const vec3 UNIT_Z; |
||||||
|
|
||||||
|
extern const vec3 UNIT_X_NEG; |
||||||
|
extern const vec3 UNIT_Y_NEG; |
||||||
|
extern const vec3 UNIT_Z_NEG; |
||||||
|
|
||||||
/* vec2 */ |
/* vec2 */ |
||||||
#define vec2_fill kmVec2Fill |
void vec2_fill(vec2* res, float x, float y); |
||||||
#define vec2_add kmVec2Add |
void vec2_add(vec2* res, vec2* v1, vec2* v2); |
||||||
#define vec2_assign kmVec2Assign |
void vec2_sub(vec2* res, vec2* v1, vec2* v2); |
||||||
#define vec2_norm kmVec2Normalize |
void vec2_assign(vec2* res, vec2* val); |
||||||
#define vec2_mul kmVec2Mul |
void vec2_mul(vec2* res, vec2* v1, vec2* v2); |
||||||
|
float vec2_len(vec2* val);void vec2_norm(vec2* res, vec2* val); |
||||||
|
|
||||||
/* vec3 */ |
/* vec3 */ |
||||||
#define vec3_fill kmVec3Fill |
void vec3_fill(vec3* res, float x, float y, float z); |
||||||
#define vec3_add kmVec3Add |
void vec3_add(vec3* res, const vec3* v1, const vec3* v3); |
||||||
#define vec3_assign kmVec3Assign |
void vec3_sub(vec3* res, const vec3* v1, const vec3* v3); |
||||||
#define vec3_norm kmVec3Normalize |
void vec3_assign(vec3* res, const vec3* val); |
||||||
#define vec3_mul kmVec3Mul |
void vec3_cross(vec3* res, const vec3* v1, const vec3* v2); |
||||||
#define vec3_mul_mat4 kmVec3MultiplyMat4 |
void vec3_norm(vec3* res, vec3* val); |
||||||
#define vec3_mul_mat3 kmVec3MultiplyMat3 |
void vec3_mul(vec3* res, vec3* v1, vec3* v3); |
||||||
#define vec3_scale kmVec3Scale |
void vec3_mul_mat4(vec3* res, vec3* val, mat4* mat); |
||||||
#define vec3_equals kmVec3Equals |
void vec3_scale(vec3* res, const vec3* val, float s); |
||||||
#define vec3_transform kmVec3Transform |
void vec3_transform_norm(vec3* res, const vec3* val, const mat4* mat); |
||||||
#define vec3_transform_norm kmVec3TransformNormal |
int vec3_equals(vec3* v1, vec3* v2); |
||||||
#define vec3_len kmVec3Length |
float vec3_len(vec3* val); |
||||||
|
|
||||||
/* vec4 */ |
/* vec4 */ |
||||||
#define vec4_fill kmVec4Fill |
int vec4_equals(vec4* v1, vec4* v2); |
||||||
#define vec4_add kmVec4Add |
void vec4_fill(vec4* res, float x, float y, float z, float w); |
||||||
#define vec4_assign kmVec4Assign |
void vec4_transform_norm(vec4* res, const vec4* val, const mat4* mat); |
||||||
#define vec4_norm kmVec4Normalize |
void vec4_scale(vec4* res, const vec4* val, float s); |
||||||
#define vec4_mul kmVec4Mul |
void vec4_mul_mat4(vec4* res, vec4* val, mat4* mat); |
||||||
#define vec4_mul_mat4 kmVec4MultiplyMat4 |
void vec4_mul(vec4* res, vec4* v1, vec4* v4); |
||||||
#define vec4_scale kmVec4Scale |
void vec4_norm(vec4* res, vec4* val); |
||||||
#define vec4_equals kmVec4Equals |
float vec4_len(vec4* val); |
||||||
#define vec4_transform kmVec4Transform |
void vec4_assign(vec4* res, vec4* val); |
||||||
#define vec4_transform_norm kmVec4TransformNormal |
void vec4_sub(vec4* res, vec4* v1, vec4* v4); |
||||||
#define vec4_len kmVec4Length |
void vec4_add(vec4* res, vec4* v1, vec4* v4); |
||||||
|
|
||||||
/* mat4 */ |
/* mat4 */ |
||||||
#define mat4_identity kmMat4Identity |
void mat4_assign(mat4* res, const mat4* m); |
||||||
#define mat4_mul kmMat4Multiply |
void mat4_rot_z(mat4* res, float angle); |
||||||
#define mat4_lookat kmMat4LookAt |
void mat4_rot_y(mat4* res, float angle); |
||||||
#define mat4_perspective kmMat4PerspectiveProjection |
void mat4_rot_x(mat4* res, const float angle); |
||||||
#define mat4_ortho kmMat4OrthographicProjection |
void mat4_from_quat(mat4* res, const quat* q); |
||||||
#define mat4_scale kmMat4Scaling |
void mat4_scale(mat4* res, float x, float y, float z); |
||||||
#define mat4_translate kmMat4Translation |
void mat4_ortho(mat4* res, |
||||||
#define mat4_from_quat kmMat4RotationQuaternion |
float left, float right, |
||||||
#define mat4_rot_x kmMat4RotationX |
float bottom, float top, |
||||||
#define mat4_rot_y kmMat4RotationY |
float nearz, float farz); |
||||||
#define mat4_rot_z kmMat4RotationZ |
void mat4_perspective(mat4* res, float fov, float aspect, float nearz, float farz); |
||||||
#define mat4_assign kmMat4Assign |
void mat4_lookat(mat4* res, const vec3* eye, const vec3* center, const vec3* up_vec); |
||||||
#define mat4_assign_mat3 kmMat4AssignMat3 |
void mat4_translate(mat4* res, float x, float y, float z); |
||||||
|
void mat4_mul(mat4* res, const mat4* mat1, const mat4* mat2); |
||||||
|
void mat4_identity(mat4* res); |
||||||
|
|
||||||
/* quat */ |
/* quat */ |
||||||
#define quat_identity kmQuaternionIdentity |
float quat_get_roll(const quat* q); |
||||||
#define quat_mul_vec3 kmQuaternionMultiplyVec3 |
float quat_get_yaw(const quat* q); |
||||||
#define quat_mul kmQuaternionMultiply |
float quat_get_pitch(const quat* q); |
||||||
#define quat_axis_angle kmQuaternionRotationAxisAngle |
void quat_get_right(vec3* res, const quat* q); |
||||||
#define quat_get_forward_rh kmQuaternionGetForwardVec3RH |
void quat_get_up(vec3* res, const quat* q); |
||||||
#define quat_get_forward_lh kmQuaternionGetForwardVec3LH |
void quat_get_forward_lh(vec3* res, const quat* q); |
||||||
#define quat_get_up kmQuaternionGetUpVec3 |
void quat_get_forward_rh(vec3* res, const quat* q); |
||||||
#define quat_get_right kmQuaternionGetRightVec3 |
void quat_axis_angle(quat* res, const vec3* v, float angle); |
||||||
|
void quat_norm(quat* res, const quat* val); |
||||||
/* Conversions */ |
float quat_len(const quat* q); |
||||||
#define M_PI 3.14159265358979323846 |
float quat_len_sq(const quat* q); |
||||||
#define TO_RADIANS(degrees) ((degrees * M_PI) / 180.0) |
void quat_mul(quat* res, const quat* q1, const quat* q2); |
||||||
#define TO_DEGREES(radians) ((radians * 180.0) / M_PI) |
void quat_mul_vec3(vec3* res, const quat* q, const vec3* v); |
||||||
|
void quat_assign(quat* res, const quat* val); |
||||||
|
void quat_identity(quat* res); |
||||||
|
void quat_fill(quat* res, float x, float y, float z, float w); |
||||||
|
|
||||||
#endif |
#endif |
||||||
|
Loading…
Reference in new issue