--- /dev/null
+*.o
+*.swp
+*.d
--- /dev/null
+Copyright (C) 2016 John Tsiombikas <nuclear@member.fsf.org>
+
+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.
-/* C version of the graphene math library */
+/* C version of the graphene math library
+ * Copyright (C) 2018 John Tsiombikas <nuclear@member.fsf.org>
+ *
+ * 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 <math.h>
+#include <string.h>
typedef struct {
float x, y, z;
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);
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);
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_ */
+++ /dev/null
-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;
-}
--- /dev/null
+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;
+}
--- /dev/null
+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;
+}
--- /dev/null
+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;
+}
--- /dev/null
+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;
+}
projects / gph-cmath / commitdiff