first render

[nexus3d] / src / cgmath2 / cgmquat.inl

diff --git a/src/cgmath2/cgmquat.inl b/src/cgmath2/cgmquat.inl
new file mode 100644 (file)
index 0000000..e238006
--- /dev/null
+++ b/src/cgmath2/cgmquat.inl
@@ -0,0 +1,151 @@
+static CGM_INLINE cgm_quat cgm_qcons(float x, float y, float z, float w)
+{
+       cgm_quat q;
+       q.x = x;
+       q.y = y;
+       q.z = z;
+       q.w = w;
+       return q;
+}
+
+static CGM_INLINE cgm_quat cgm_qneg(cgm_quat q)
+{
+       return cgm_qcons(-q.x, -q.y, -q.z, -q.w);
+}
+
+static CGM_INLINE cgm_quat cgm_qadd(cgm_quat a, cgm_quat b)
+{
+       return cgm_qcons(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+
+static CGM_INLINE cgm_quat cgm_qsub(cgm_quat a, cgm_quat b)
+{
+       return cgm_qcons(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+
+static CGM_INLINE cgm_quat cgm_qmul(cgm_quat a, cgm_quat b)
+{
+       cgm_quat res;
+       float dot;
+       cgm_vec3 cross;
+
+       dot = a.x * b.x + a.y * b.y + a.z * b.z;
+       cross = cgm_vcross(cgm_vcons(a.x, a.y, a.z), cgm_vcons(b.x, b.y, b.z));
+       res.x = a.w * b.x + b.w * a.x + cross.x;
+       res.y = a.w * b.y + b.w * a.y + cross.y;
+       res.z = a.w * b.z + b.w * a.z + cross.z;
+       res.w = a.w * b.w - dot;
+       return res;
+}
+
+static CGM_INLINE float cgm_qlength(cgm_quat q)
+{
+       return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+}
+
+static CGM_INLINE float cgm_qlength_sq(cgm_quat q)
+{
+       return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+}
+
+static CGM_INLINE cgm_quat cgm_qnormalize(cgm_quat q)
+{
+       cgm_quat res;
+       float len, s;
+
+       len = cgm_qlength(q);
+       if(len == 0.0f) {
+               return q;
+       }
+       s = 1.0f / len;
+       res.x = q.x * s;
+       res.y = q.y * s;
+       res.z = q.z * s;
+       res.w = q.w * s;
+       return res;
+}
+
+static CGM_INLINE cgm_quat cgm_qconjugate(cgm_quat q)
+{
+       return cgm_qcons(-q.x, -q.y, -q.z, q.w);
+}
+
+static CGM_INLINE cgm_quat cgm_qinvert(cgm_quat q)
+{
+       cgm_quat res;
+       float len_sq, s;
+
+       if((len_sq = cgm_qlength_sq(q)) == 0.0f) {
+               return q;
+       }
+       s = 1.0f / len_sq;
+       res.x = -q.x * s;
+       res.y = -q.y * s;
+       res.z = -q.z * s;
+       res.w = q.w * s;
+       return res;
+}
+
+static CGM_INLINE cgm_quat cgm_qrotation(float angle, float x, float y, float z)
+{
+       cgm_quat res;
+       float hangle = angle * 0.5f;
+       float sin_ha = sin(hangle);
+       res.w = cos(hangle);
+       res.x = x * sin_ha;
+       res.y = y * sin_ha;
+       res.z = z * sin_ha;
+       return res;
+}
+
+static CGM_INLINE void cgm_qrotate(cgm_quat *q, float angle, float x, float y, float z)
+{
+       cgm_quat qrot = cgm_qrotation(angle, x, y, z);
+       *q = cgm_qmul(*q, qrot);
+}
+
+static CGM_INLINE cgm_quat cgm_qslerp(cgm_quat q1, cgm_quat q2, float t)
+{
+       cgm_quat res;
+       float angle, dot, a, b, sin_angle;
+
+       dot = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
+       if(dot < 0.0f) {
+               /* make sure we inteprolate across the shortest arc */
+               q1 = 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;
+       return res;
+}
+
+static CGM_INLINE cgm_quat cgm_qlerp(cgm_quat a, cgm_quat b, float t)
+{
+       cgm_quat res;
+       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;
+       return res;
+}