From: John Tsiombikas Date: Mon, 1 Oct 2018 13:50:16 +0000 (+0300) Subject: Merge branch 'master' of photon:code/graphics/gph-cmath X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?p=gph-cmath;a=commitdiff_plain;h=b0bbed094191f5bea8fb436cd5403d32c479ec81;hp=63b42239ff7d94b05e909419e570e7c525082b3b Merge branch 'master' of photon:code/graphics/gph-cmath --- diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5880c41 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*.o +*.swp +*.d diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..536e666 --- /dev/null +++ b/LICENSE @@ -0,0 +1,20 @@ +Copyright (C) 2016 John Tsiombikas + +Permission is hereby granted, free of charge, to any person obtaining +a copy of this software and associated documentation files (the +"Software"), to deal in the Software without restriction, including +without limitation the rights to use, copy, modify, merge, publish, +distribute, sublicense, and/or sell copies of the Software, and to +permit persons to whom the Software is furnished to do so, subject to +the following conditions: + +The above copyright notice and this permission notice shall be +included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY +CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, +TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/src/cgmath.h b/src/cgmath.h index 60b4b92..36632f2 100644 --- a/src/cgmath.h +++ b/src/cgmath.h @@ -1,8 +1,29 @@ -/* C version of the graphene math library */ +/* C version of the graphene math library + * Copyright (C) 2018 John Tsiombikas + * + * This program is free software. Feel free to use, modify, and/or redistribute + * it under the terms of the MIT/X11 license. See LICENSE for details. + * If you intend to redistribute parts of the code without the LICENSE file + * replace this paragraph with the full contents of the LICENSE file. + * + * Function prefixes signify the data type of their operand(s): + * - cgm_v... functions are operations on cgm_vec3 vectors + * - cgm_w... functions are operations on cgm_vec4 vectors + * - cgm_q... functions are operations on cgm_quat quaternions (w + xi + yj + zk) + * - cgm_m... functions are operations on 4x4 matrices (stored as linear 16 float arrays) + * + * NOTE: *ALL* matrix arguments are pointers to 16 floats. Even the functions + * which operate on 3x3 matrices, actually use the upper 3x3 of a 4x4 matrix, + * and still expect an array of 16 floats. + * + * NOTE: matrices are treated by all operations as column-major, to match OpenGL + * conventions, so everything is pretty much transposed. + */ #ifndef CGMATH_H_ #define CGMATH_H_ #include +#include typedef struct { float x, y, z; @@ -10,16 +31,38 @@ typedef struct { typedef struct { float x, y, z, w; -} cgm_quat; - -/* vectors */ -static inline void cgm_vzero(cgm_vec3 *v); -static inline void cgm_vone(cgm_vec3 *v); +} cgm_vec4, cgm_quat; + +typedef enum cgm_euler_mode { + CGM_EULER_XYZ, + CGM_EULER_XZY, + CGM_EULER_YXZ, + CGM_EULER_YZX, + CGM_EULER_ZXY, + CGM_EULER_ZYX, + CGM_EULER_ZXZ, + CGM_EULER_ZYZ, + CGM_EULER_YXY, + CGM_EULER_YZY, + CGM_EULER_XYX, + CGM_EULER_XZX +} cgm_euler_mode; + +#ifdef __cplusplus +extern "C" { +#endif + +/* --- operations on cgm_vec3 --- */ +static inline void cgm_vcons(cgm_vec3 *v, float x, float y, float z); static inline void cgm_vadd(cgm_vec3 *a, const cgm_vec3 *b); static inline void cgm_vsub(cgm_vec3 *a, const cgm_vec3 *b); static inline void cgm_vmul(cgm_vec3 *a, const cgm_vec3 *b); static inline void cgm_vscale(cgm_vec3 *v, float s); +static inline void cgm_vmul_m4v3(cgm_vec3 *v, const float *m); /* m4x4 * v */ +static inline void cgm_vmul_v3m4(cgm_vec3 *v, const float *m); /* v * m4x4 */ +static inline void cgm_vmul_m3v3(cgm_vec3 *v, const float *m); /* m3x3 * v (m still 16 floats) */ +static inline void cgm_vmul_v3m3(cgm_vec3 *v, const float *m); /* v * m3x3 (m still 16 floats) */ static inline float cgm_vdot(const cgm_vec3 *a, const cgm_vec3 *b); static inline void cgm_vcross(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b); @@ -29,8 +72,40 @@ static inline float cgm_vdist(const cgm_vec3 *a, const cgm_vec3 *b); static inline float cgm_vdist_sq(const cgm_vec3 *a, const cgm_vec3 *b); static inline void cgm_vnormalize(cgm_vec3 *v); -/* quaternions */ -static inline void cgm_qident(cgm_quat *q); +static inline void cgm_vreflect(cgm_vec3 *v, const cgm_vec3 *n); +static inline void cgm_vrefract(cgm_vec3 *v, const cgm_vec3 *n, float ior); + +static inline void cgm_vrotate_quat(cgm_vec3 *v, const cgm_quat *q); +static inline void cgm_vrotate_axis(cgm_vec3 *v, const cgm_vec3 *axis, float angle); +static inline void cgm_vrotate_euler(cgm_vec3 *v, const cgm_vec3 *euler, enum cgm_euler_mode mode); + +static inline void cgm_vlerp(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b, float t); + +/* --- operations on cgm_vec4 --- */ +static inline void cgm_wcons(cgm_vec4 *v, float x, float y, float z, float w); + +static inline void cgm_wadd(cgm_vec4 *a, const cgm_vec4 *b); +static inline void cgm_wsub(cgm_vec4 *a, const cgm_vec4 *b); +static inline void cgm_wmul(cgm_vec4 *a, const cgm_vec4 *b); +static inline void cgm_wscale(cgm_vec4 *v, float s); + +static inline void cgm_wmul_m4v4(cgm_vec4 *v, const float *m); +static inline void cgm_wmul_v4m4(cgm_vec4 *v, const float *m); +static inline void cgm_wmul_m34v4(cgm_vec4 *v, const float *m); /* doesn't affect w */ +static inline void cgm_wmul_v4m43(cgm_vec4 *v, const float *m); /* doesn't affect w */ +static inline void cgm_wmul_m3v4(cgm_vec4 *v, const float *m); /* (m still 16 floats) */ +static inline void cgm_wmul_v4m3(cgm_vec4 *v, const float *m); /* (m still 16 floats) */ + +static inline float cgm_wlength(const cgm_vec4 *v); +static inline float cgm_wlength_sq(const cgm_vec4 *v); +static inline float cgm_wdist(const cgm_vec4 *a, const cgm_vec4 *b); +static inline float cgm_wdist_sq(const cgm_vec4 *a, const cgm_vec4 *b); +static inline void cgm_wnormalize(cgm_vec4 *v); + +static inline void cgm_wlerp(cgm_vec4 *res, const cgm_vec4 *a, const cgm_vec4 *b, float t); + +/* --- operations on quaternions --- */ +static inline void cgm_qcons(cgm_quat *q, float x, float y, float z, float w); static inline void cgm_qneg(cgm_quat *q); static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b); @@ -39,19 +114,39 @@ static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b); static inline float cgm_qlength(const cgm_quat *q); static inline float cgm_qlength_sq(const cgm_quat *q); - static inline void cgm_qnormalize(cgm_quat *q); - static inline void cgm_qconjugate(cgm_quat *q); static inline void cgm_qinvert(cgm_quat *q); -static inline void cgm_qrot_axis(cgm_quat *q, float axis, float x, float y, float z); -static inline void cgm_qrot_quat(cgm_quat *q, const cgm_quat *rq); - -static inline void cgm_qmatrix(float *mat, const cgm_quat *q); +static inline void cgm_qrotation(cgm_quat *q, const cgm_vec3 *axis, float angle); +static inline void cgm_qrotate(cgm_quat *q, const cgm_vec3 *axis, float angle); static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t); - -#include "cgmath.inl" +static inline void cgm_qlerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t); + +/* --- operations on matrices --- */ +static inline void cgm_mcopy(float *dest, const float *src); +static inline void cgm_mzero(float *m); +static inline void cgm_midentity(float *m); + +static inline void cgm_msetrow_v3(float *m, int idx, const cgm_vec3 *v); +static inline void cgm_msetrow_v4(float *m, int idx, const cgm_vec4 *v); +static inline void cgm_msetcol_v3(float *m, int idx, const cgm_vec3 *v); +static inline void cgm_msetcol_v4(float *m, int idx, const cgm_vec4 *v); +static inline void cgm_mgetrow_v3(cgm_vec3 *v, const float *m, int idx); +static inline void cgm_mgetrow_v4(cgm_vec4 *v, const float *m, int idx); +static inline void cgm_mgetcol_v3(cgm_vec3 *v, const float *m, int idx); +static inline void cgm_mgetcol_v4(cgm_vec4 *v, const float *m, int idx); + +static inline void cgm_mupper3(float *m); + +#include "cgmvec3.inl" +#include "cgmvec4.inl" +#include "cgmquat.inl" +#include "cgmmat.inl" + +#ifdef __cplusplus +} +#endif #endif /* CGMATH_H_ */ diff --git a/src/cgmath.inl b/src/cgmath.inl deleted file mode 100644 index 7ea5817..0000000 --- a/src/cgmath.inl +++ /dev/null @@ -1,258 +0,0 @@ -static inline void cgm_vzero(cgm_vec3 *v) -{ - v->x = v->y = v->z = 0.0f; -} - -static inline void cgm_vone(cgm_vec3 *v) -{ - v->x = v->y = v->z = 1.0f; -} - -static inline void cgm_vadd(cgm_vec3 *a, const cgm_vec3 *b) -{ - a->x += b->x; - a->y += b->y; - a->z += b->z; -} - -static inline void cgm_vsub(cgm_vec3 *a, const cgm_vec3 *b) -{ - a->x -= b->x; - a->y -= b->y; - a->z -= b->z; -} - -static inline void cgm_vmul(cgm_vec3 *a, const cgm_vec3 *b) -{ - a->x *= b->x; - a->y *= b->y; - a->z *= b->z; -} - -static inline void cgm_vscale(cgm_vec3 *v, float s) -{ - v->x *= s; - v->y *= s; - v->z *= s; -} - -static inline float cgm_vdot(const cgm_vec3 *a, const cgm_vec3 *b) -{ - return a->x * b->x + a->y * b->y + a->z * b->z; -} - -static inline void cgm_vcross(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b) -{ - res->x = a->y * b->z - a->z * b->y; - res->y = a->z * b->x - a->x * b->z; - res->z = a->x * b->y - a->y * b->x; -} - -static inline float cgm_vlength(const cgm_vec3 *v) -{ - return sqrt(v->x * v->x + v->y * v->y + v->z * v->z); -} - -static inline float cgm_vlength_sq(const cgm_vec3 *v) -{ - return v->x * v->x + v->y * v->y + v->z * v->z; -} - -static inline float cgm_vdist(const cgm_vec3 *a, const cgm_vec3 *b) -{ - float dx = a->x - b->x; - float dy = a->y - b->y; - float dz = a->z - b->z; - return sqrt(dx * dx + dy * dy + dz * dz); -} - -static inline float cgm_vdist_sq(const cgm_vec3 *a, const cgm_vec3 *b) -{ - float dx = a->x - b->x; - float dy = a->y - b->y; - float dz = a->z - b->z; - return dx * dx + dy * dy + dz * dz; -} - -static inline void cgm_vnormalize(cgm_vec3 *v) -{ - float len = cgm_vlength(v); - if(len != 0.0f) { - float s = 1.0f / len; - v->x *= s; - v->y *= s; - v->z *= s; - } -} - -static inline void cgm_qident(cgm_quat *q) -{ - q->x = q->y = q->z = 0.0f; - q->w = 1.0f; -} - -static inline void cgm_qneg(cgm_quat *q) -{ - q->x = -q->x; - q->y = -q->y; - q->z = -q->z; - q->w = -q->w; -} - -static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b) -{ - a->x += b->x; - a->y += b->y; - a->z += b->z; - a->w += b->w; -} - -static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b) -{ - a->x -= b->x; - a->y -= b->y; - a->z -= b->z; - a->w -= b->w; -} - -static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b) -{ - float axb_x = a->y * b->z - a->z * b->y; - float axb_y = a->z * b->x - a->x * b->z; - float axb_z = a->x * b->y - a->y * b->x; - - float im_x = a->w * b->x + b->w * a->x + axb_x; - float im_y = a->w * b->y + b->w * a->y + axb_y; - float im_z = a->w * b->z + b->w * a->z + axb_z; - - a->w = a->w * b->w - (a->x * b->x + a->y * b->y + a->z * b->z); - a->x = im_x; - a->y = im_y; - a->z = im_z; -} - -static inline float cgm_qlength(const cgm_quat *q) -{ - return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w); -} - -static inline float cgm_qlength_sq(const cgm_quat *q) -{ - return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w; -} - -static inline void cgm_qnormalize(cgm_quat *q) -{ - float len = cgm_qlength(q); - if(len != 0.0f) { - float inv_len = 1.0f / len; - q->x *= inv_len; - q->y *= inv_len; - q->z *= inv_len; - q->w *= inv_len; - } -} - -static inline void cgm_qconjugate(cgm_quat *q) -{ - q->x = -q->x; - q->y = -q->y; - q->z = -q->z; -} - -static inline void cgm_qinvert(cgm_quat *q) -{ - cgm_qconjugate(q); - float len_sq = cgm_qlength_sq(q); - if(len_sq != 0.0) { - float s = 1.0f / len_sq; - q->x *= s; - q->y *= s; - q->z *= s; - q->w *= s; - } -} - -static inline void cgm_qrot_axis(cgm_quat *q, float angle, float x, float y, float z) -{ - float half_angle = angle * 0.5f; - float sin_ha = sin(half_angle); - q->w = cos(half_angle); - q->x = x * sin_ha; - q->y = y * sin_ha; - q->z = z * sin_ha; -} - -static inline void cgm_qrot_quat(cgm_quat *q, const cgm_quat *rq) -{ - cgm_quat qrot = *rq; - cgm_quat rqconj = *rq; - cgm_qconjugate(&rqconj); - cgm_qmul(&qrot, q); - cgm_qmul(&qrot, &rqconj); - *q = qrot; -} - -static inline void cgm_qmatrix(float *mat, const cgm_quat *q) -{ - float xsq2 = 2.0f * q->x * q->x; - float ysq2 = 2.0f * q->y * q->y; - float zsq2 = 2.0f * q->z * q->z; - float sx = 1.0f - ysq2 - zsq2; - float sy = 1.0f - xsq2 - zsq2; - float sz = 1.0f - xsq2 - ysq2; - - mat[0] = sx; - mat[1] = 2.0f * q->x * q->y + 2.0f * q->w * q->z; - mat[2] = 2.0f * q->z * q->x - 2.0f * q->w * q->y; - mat[3] = 0.0f; - - mat[4] = 2.0f * q->x * q->y - 2.0f * q->w * q->z; - mat[5] = sy; - mat[6] = 2.0f * q->y * q->z + 2.0f * q->w * q->x; - mat[7] = 0.0f; - - mat[8] = 2.0f * q->z * q->x + 2.0f * q->w * q->y; - mat[9] = 2.0f * q->y * q->z - 2.0f * q->w * q->x; - mat[10] = sz; - mat[11] = 0.0f; - - mat[12] = mat[13] = mat[14] = 0.0f; - mat[15] = 1.0f; -} - -static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t) -{ - cgm_quat qa = *a; - float dot = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w; - float angle, sin_angle, ta, tb; - - if(dot < 0.0f) { - /* make sure we interpolate across the shortest arc */ - qa.x = -qa.x; - qa.y = -qa.y; - qa.z = -qa.z; - qa.w = -qa.w; - dot = -dot; - } - - /* clamp dot to [-1, 1] in order to avoid domain errors in acos */ - if(dot < -1.0f) dot = -1.0f; - if(dot > 1.0f) dot = 1.0f; - angle = acos(dot); - sin_angle = sin(angle); - - if(sin_angle == 0.0f) { - /* use linear interpolation to avoid div/zero */ - ta = 1.0f - t; - tb = t; - } else { - ta = sin((1.0f - t) * angle) / sin_angle; - tb = sin(t * angle) / sin_angle; - } - - res->x = a->x * ta + b->x * tb; - res->y = a->y * ta + b->y * tb; - res->z = a->z * ta + b->z * tb; - res->w = a->w * ta + b->w * tb; -} diff --git a/src/cgmmat.inl b/src/cgmmat.inl new file mode 100644 index 0000000..86dd63d --- /dev/null +++ b/src/cgmmat.inl @@ -0,0 +1,88 @@ +static inline void cgm_mcopy(float *dest, const float *src) +{ + memcpy(dest, src, 16 * sizeof(float)); +} + +static inline void cgm_mzero(float *m) +{ + static float z[16]; + cgm_mcopy(m, z); +} + +static inline void cgm_midentity(float *m) +{ + static float id[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; + cgm_mcopy(m, id); +} + +static inline void cgm_msetrow_v3(float *m, int idx, const cgm_vec3 *v) +{ + m[idx] = v->x; + m[idx + 4] = v->y; + m[idx + 8] = v->z; + m[idx + 12] = 0.0f; +} + +static inline void cgm_msetrow_v4(float *m, int idx, const cgm_vec4 *v) +{ + m[idx] = v->x; + m[idx + 4] = v->y; + m[idx + 8] = v->z; + m[idx + 12] = v->w; +} + +static inline void cgm_msetcol_v3(float *m, int idx, const cgm_vec3 *v) +{ + m[idx * 4] = v->x; + m[idx * 4 + 1] = v->y; + m[idx * 4 + 2] = v->z; + m[idx * 4 + 3] = 0.0f; +} + +static inline void cgm_msetcol_v4(float *m, int idx, const cgm_vec4 *v) +{ + m[idx * 4] = v->x; + m[idx * 4 + 1] = v->y; + m[idx * 4 + 2] = v->z; + m[idx * 4 + 3] = v->w; +} + +static inline void cgm_mgetrow_v3(cgm_vec3 *v, const float *m, int idx) +{ + v->x = m[idx]; + v->y = m[idx + 4]; + v->z = m[idx + 8]; +} + +static inline void cgm_mgetrow_v4(cgm_vec4 *v, const float *m, int idx) +{ + v->x = m[idx]; + v->y = m[idx + 4]; + v->z = m[idx + 8]; + v->w = m[idx + 12]; +} + +static inline void cgm_mgetcol_v3(cgm_vec3 *v, const float *m, int idx) +{ + v->x = m[idx * 4]; + v->y = m[idx * 4 + 1]; + v->z = m[idx * 4 + 2]; +} + +static inline void cgm_mgetcol_v4(cgm_vec4 *v, const float *m, int idx) +{ + v->x = m[idx * 4]; + v->y = m[idx * 4 + 1]; + v->z = m[idx * 4 + 2]; + v->w = m[idx * 4 + 3]; +} + +static inline void cgm_msubmatrix(float *m, int row, int col) +{ +} + +static inline void cgm_mupper3(float *m) +{ + m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f; + m[15] = 1.0f; +} diff --git a/src/cgmquat.inl b/src/cgmquat.inl new file mode 100644 index 0000000..f0b0ee3 --- /dev/null +++ b/src/cgmquat.inl @@ -0,0 +1,151 @@ +static inline void cgm_qcons(cgm_quat *q, float x, float y, float z, float w) +{ + q->x = x; + q->y = y; + q->z = z; + q->w = w; +} + + +static inline void cgm_qneg(cgm_quat *q) +{ + q->x = -q->x; + q->y = -q->y; + q->z = -q->z; + q->w = -q->w; +} + +static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b) +{ + a->x += b->x; + a->y += b->y; + a->z += b->z; + a->w += b->w; +} + +static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b) +{ + a->x -= b->x; + a->y -= b->y; + a->z -= b->z; + a->w -= b->w; +} + +static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b) +{ + float x, y, z, dot; + cgm_vec3 cross; + + dot = a->x * b->x + a->y * b->y + a->z * b->z; + cgm_vcross(&cross, (cgm_vec3*)a, (cgm_vec3*)b); + + x = a->w * b->x + b->w * a->x + cross.x; + y = a->w * b->y + b->w * a->y + cross.y; + z = a->w * b->z + b->w * a->z + cross.z; + a->w = a->w * b->w - dot; + a->x = x; + a->y = y; + a->z = z; +} + +static inline float cgm_qlength(const cgm_quat *q) +{ + return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w); +} + +static inline float cgm_qlength_sq(const cgm_quat *q) +{ + return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w; +} + +static inline void cgm_qnormalize(cgm_quat *q) +{ + float len = cgm_qlength(q); + if(len != 0.0f) { + float s = 1.0f / len; + q->x *= s; + q->y *= s; + q->z *= s; + q->w *= s; + } +} + +static inline void cgm_qconjugate(cgm_quat *q) +{ + q->x = -q->x; + q->y = -q->y; + q->z = -q->z; +} + +static inline void cgm_qinvert(cgm_quat *q) +{ + float len_sq = cgm_qlength_sq(q); + cgm_qconjugate(q); + if(len_sq != 0.0f) { + float s = 1.0f / len_sq; + q->x *= s; + q->y *= s; + q->z *= s; + q->w *= s; + } +} + +static inline void cgm_qrotation(cgm_quat *q, const cgm_vec3 *axis, float angle) +{ + float hangle = angle * 0.5f; + float sin_ha = sin(hangle); + q->w = cos(hangle); + q->x = axis->x * sin_ha; + q->y = axis->y * sin_ha; + q->z = axis->z * sin_ha; +} + +static inline void cgm_qrotate(cgm_quat *q, const cgm_vec3 *axis, float angle) +{ + cgm_quat qrot; + cgm_qrotation(&qrot, axis, angle); + cgm_qmul(q, &qrot); +} + +static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *quat1, const cgm_quat *q2, float t) +{ + float angle, dot, a, b, sin_angle; + cgm_quat q1 = *quat1; + + dot = quat1->x * q2->x + quat1->y * q2->y + quat1->z * q2->z + quat1->w * q2->w; + if(dot < 0.0f) { + /* make sure we inteprolate across the shortest arc */ + cgm_qneg(&q1); + dot = -dot; + } + + /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to + * floating point imprecisions + */ + if(dot < -1.0f) dot = -1.0f; + if(dot > 1.0f) dot = 1.0f; + angle = acos(dot); + + sin_angle = sin(angle); + if(sin_angle == 0.0f) { + /* use linear interpolation to avoid div/zero */ + a = 1.0f; + b = t; + } else { + a = sin((1.0f - t) * angle) / sin_angle; + b = sin(t * angle) / sin_angle; + } + + res->x = q1.x * a + q2->x * b; + res->y = q1.y * a + q2->y * b; + res->z = q1.z * a + q2->z * b; + res->w = q1.w * a + q2->w * b; +} + +static inline void cgm_qlerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t) +{ + res->x = a->x + (b->x - a->x) * t; + res->y = a->y + (b->y - a->y) * t; + res->z = a->z + (b->z - a->z) * t; + res->w = a->w + (b->w - a->w) * t; +} diff --git a/src/cgmvec3.inl b/src/cgmvec3.inl new file mode 100644 index 0000000..1320c08 --- /dev/null +++ b/src/cgmvec3.inl @@ -0,0 +1,173 @@ +static inline void cgm_vcons(cgm_vec3 *v, float x, float y, float z) +{ + v->x = x; + v->y = y; + v->z = z; +} + +static inline void cgm_vadd(cgm_vec3 *a, const cgm_vec3 *b) +{ + a->x += b->x; + a->y += b->y; + a->z += b->z; +} + +static inline void cgm_vsub(cgm_vec3 *a, const cgm_vec3 *b) +{ + a->x -= b->x; + a->y -= b->y; + a->z -= b->z; +} + +static inline void cgm_vmul(cgm_vec3 *a, const cgm_vec3 *b) +{ + a->x *= b->x; + a->y *= b->y; + a->z *= b->z; +} + +static inline void cgm_vscale(cgm_vec3 *v, float s) +{ + v->x *= s; + v->y *= s; + v->z *= s; +} + +static inline void cgm_vmul_m4v3(cgm_vec3 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[4] + v->z * m[8] + m[12]; + float y = v->x * m[1] + v->y * m[5] + v->z * m[9] + m[13]; + v->z = v->x * m[2] + v->y * m[6] + v->z * m[10] + m[14]; + v->x = x; + v->y = y; +} + +static inline void cgm_vmul_v3m4(cgm_vec3 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[1] + v->z * m[2] + m[3]; + float y = v->x * m[4] + v->y * m[5] + v->z * m[6] + m[7]; + v->z = v->x * m[8] + v->y * m[9] + v->z * m[10] + m[11]; + v->x = x; + v->y = y; +} + +static inline void cgm_vmul_m3v3(cgm_vec3 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[4] + v->z * m[8]; + float y = v->x * m[1] + v->y * m[5] + v->z * m[9]; + v->z = v->x * m[2] + v->y * m[6] + v->z * m[10]; + v->x = x; + v->y = y; +} + +static inline void cgm_vmul_v3m3(cgm_vec3 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[1] + v->z * m[2]; + float y = v->x * m[4] + v->y * m[5] + v->z * m[6]; + v->z = v->x * m[8] + v->y * m[9] + v->z * m[10]; + v->x = x; + v->y = y; +} + +static inline float cgm_vdot(const cgm_vec3 *a, const cgm_vec3 *b) +{ + return a->x * b->x + a->y * b->y + a->z * b->z; +} + +static inline void cgm_vcross(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b) +{ + res->x = a->y * b->z - a->z * b->y; + res->y = a->z * b->x - a->x * b->z; + res->z = a->x * b->y - a->y * b->x; +} + +static inline float cgm_vlength(const cgm_vec3 *v) +{ + return sqrt(v->x * v->x + v->y * v->y + v->z * v->z); +} + +static inline float cgm_vlength_sq(const cgm_vec3 *v) +{ + return v->x * v->x + v->y * v->y + v->z * v->z; +} + +static inline float cgm_vdist(const cgm_vec3 *a, const cgm_vec3 *b) +{ + float dx = a->x - b->x; + float dy = a->y - b->y; + float dz = a->z - b->z; + return sqrt(dx * dx + dy * dy + dz * dz); +} + +static inline float cgm_vdist_sq(const cgm_vec3 *a, const cgm_vec3 *b) +{ + float dx = a->x - b->x; + float dy = a->y - b->y; + float dz = a->z - b->z; + return dx * dx + dy * dy + dz * dz; +} + +static inline void cgm_vnormalize(cgm_vec3 *v) +{ + float len = cgm_vlength(v); + if(len != 0.0f) { + float s = 1.0f / len; + v->x *= s; + v->y *= s; + v->z *= s; + } +} + +static inline void cgm_vreflect(cgm_vec3 *v, const cgm_vec3 *n) +{ + float ndotv2 = cgm_vdot(v, n) * 2.0f; + v->x -= n->x * ndotv2; + v->y -= n->y * ndotv2; + v->z -= n->z * ndotv2; +} + +static inline void cgm_vrefract(cgm_vec3 *v, const cgm_vec3 *n, float ior) +{ + float ndotv = cgm_vdot(v, n); + float k = 1.0f - ior * ior * (1.0f - ndotv * ndotv); + if(k < 0.0f) { + cgm_vreflect(v, n); /* TIR */ + } else { + float sqrt_k = sqrt(k); + v->x = ior * v->x - (ior * ndotv + sqrt_k) * n->x; + v->y = ior * v->y - (ior * ndotv + sqrt_k) * n->y; + v->z = ior * v->z - (ior * ndotv + sqrt_k) * n->z; + } +} + +static inline void cgm_vrotate_quat(cgm_vec3 *v, const cgm_quat *q) +{ + cgm_quat vq, inv_q = *q, tmp_q = *q; + + cgm_qcons(&vq, v->x, v->y, v->z, 0.0f); + cgm_qinvert(&inv_q); + cgm_qmul(&tmp_q, &vq); + cgm_qmul(&vq, &inv_q); + cgm_vcons(v, vq.x, vq.y, vq.z); +} + +static inline void cgm_vrotate_axis(cgm_vec3 *v, const cgm_vec3 *axis, float angle) +{ + float m[16]; + cgm_mrotation_axis(m, axis, angle); + cgm_vmul_m3v3(v, m); +} + +static inline void cgm_vrotate_euler(cgm_vec3 *v, const cgm_vec3 *euler, enum cgm_euler_mode mode) +{ + float m[16]; + cgm_mrotation_euler(m, euler, mode); + cgm_vmul_m3v3(v, m); +} + +static inline void cgm_vlerp(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b, float t) +{ + res->x = a->x * (b->x - a->x) * t; + res->y = a->y * (b->y - a->y) * t; + res->z = a->z * (b->z - a->z) * t; +} diff --git a/src/cgmvec4.inl b/src/cgmvec4.inl new file mode 100644 index 0000000..f333403 --- /dev/null +++ b/src/cgmvec4.inl @@ -0,0 +1,145 @@ +static inline void cgm_wcons(cgm_vec4 *v, float x, float y, float z, float w) +{ + v->x = x; + v->y = y; + v->z = z; + v->w = w; +} + +static inline void cgm_wadd(cgm_vec4 *a, const cgm_vec4 *b) +{ + a->x += b->x; + a->y += b->y; + a->z += b->z; + a->w += b->w; +} + +static inline void cgm_wsub(cgm_vec4 *a, const cgm_vec4 *b) +{ + a->x -= b->x; + a->y -= b->y; + a->z -= b->z; + a->w -= b->w; +} + +static inline void cgm_wmul(cgm_vec4 *a, const cgm_vec4 *b) +{ + a->x *= b->x; + a->y *= b->y; + a->z *= b->z; + a->w *= b->w; +} + +static inline void cgm_wscale(cgm_vec4 *v, float s) +{ + v->x *= s; + v->y *= s; + v->z *= s; + v->w *= s; +} + +static inline void cgm_wmul_m4v4(cgm_vec4 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[4] + v->z * m[8] + v->w * m[12]; + float y = v->x * m[1] + v->y * m[5] + v->z * m[9] + v->w * m[13]; + float z = v->x * m[2] + v->y * m[6] + v->z * m[10] + v->w * m[14]; + v->w = v->x * m[3] + v->y * m[7] + v->z * m[11] + v->w * m[15]; + v->x = x; + v->y = y; + v->z = z; +} + +static inline void cgm_wmul_v4m4(cgm_vec4 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[1] + v->z * m[2] + v->w * m[3]; + float y = v->x * m[4] + v->y * m[5] + v->z * m[6] + v->w * m[7]; + float z = v->x * m[8] + v->y * m[9] + v->z * m[10] + v->w * m[11]; + v->w = v->x * m[12] + v->y * m[13] + v->z * m[14] + v->w * m[15]; + v->x = x; + v->y = y; + v->z = z; +} + +static inline void cgm_wmul_m34v4(cgm_vec4 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[4] + v->z * m[8] + v->w * m[12]; + float y = v->x * m[1] + v->y * m[5] + v->z * m[9] + v->w * m[13]; + v->z = v->x * m[2] + v->y * m[6] + v->z * m[10] + v->w * m[14]; + v->x = x; + v->y = y; +} + +static inline void cgm_wmul_v4m43(cgm_vec4 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[1] + v->z * m[2] + v->w * m[3]; + float y = v->x * m[4] + v->y * m[5] + v->z * m[6] + v->w * m[7]; + v->z = v->x * m[8] + v->y * m[9] + v->z * m[10] + v->w * m[11]; + v->x = x; + v->y = y; +} + +static inline void cgm_wmul_m3v4(cgm_vec4 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[4] + v->z * m[8]; + float y = v->x * m[1] + v->y * m[5] + v->z * m[9]; + v->z = v->x * m[2] + v->y * m[6] + v->z * m[10]; + v->x = x; + v->y = y; +} + +static inline void cgm_wmul_v4m3(cgm_vec4 *v, const float *m) +{ + float x = v->x * m[0] + v->y * m[1] + v->z * m[2]; + float y = v->x * m[4] + v->y * m[5] + v->z * m[6]; + v->z = v->x * m[8] + v->y * m[9] + v->z * m[10]; + v->x = x; + v->y = y; +} + +static inline float cgm_wlength(const cgm_vec4 *v) +{ + return sqrt(v->x * v->x + v->y * v->y + v->z * v->z + v->w * v->w); +} + +static inline float cgm_wlength_sq(const cgm_vec4 *v) +{ + return v->x * v->x + v->y * v->y + v->z * v->z + v->w * v->w; +} + +static inline float cgm_wdist(const cgm_vec4 *a, const cgm_vec4 *b) +{ + float dx = a->x - b->x; + float dy = a->y - b->y; + float dz = a->z - b->z; + float dw = a->w - b->w; + return sqrt(dx * dx + dy * dy + dz * dz + dw * dw); +} + +static inline float cgm_wdist_sq(const cgm_vec4 *a, const cgm_vec4 *b) +{ + float dx = a->x - b->x; + float dy = a->y - b->y; + float dz = a->z - b->z; + float dw = a->w - b->w; + return dx * dx + dy * dy + dz * dz + dw * dw; +} + +static inline void cgm_wnormalize(cgm_vec4 *v) +{ + float len = cgm_wlength(v); + if(len != 0.0f) { + float s = 1.0f / len; + v->x *= s; + v->y *= s; + v->z *= s; + v->w *= s; + } +} + +static inline void cgm_wlerp(cgm_vec4 *res, const cgm_vec4 *a, const cgm_vec4 *b, float t) +{ + res->x = a->x + (b->x - a->x) * t; + res->y = a->y + (b->y - a->y) * t; + res->z = a->z + (b->z - a->z) * t; + res->w = a->w + (b->w - a->w) * t; +}