1 static inline void cgm_qcons(cgm_quat *q, float x, float y, float z, float w)
10 static inline void cgm_qneg(cgm_quat *q)
18 static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b)
26 static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b)
34 static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b)
39 dot = a->x * b->x + a->y * b->y + a->z * b->z;
40 cgm_vcross(&cross, (cgm_vec3*)a, (cgm_vec3*)b);
42 x = a->w * b->x + b->w * a->x + cross.x;
43 y = a->w * b->y + b->w * a->y + cross.y;
44 z = a->w * b->z + b->w * a->z + cross.z;
45 a->w = a->w * b->w - dot;
51 static inline float cgm_qlength(const cgm_quat *q)
53 return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
56 static inline float cgm_qlength_sq(const cgm_quat *q)
58 return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
61 static inline void cgm_qnormalize(cgm_quat *q)
63 float len = cgm_qlength(q);
73 static inline void cgm_qconjugate(cgm_quat *q)
80 static inline void cgm_qinvert(cgm_quat *q)
82 float len_sq = cgm_qlength_sq(q);
85 float s = 1.0f / len_sq;
93 static inline void cgm_qrotation(cgm_quat *q, const cgm_vec3 *axis, float angle)
95 float hangle = angle * 0.5f;
96 float sin_ha = sin(hangle);
98 q->x = axis->x * sin_ha;
99 q->y = axis->y * sin_ha;
100 q->z = axis->z * sin_ha;
103 static inline void cgm_qrotate(cgm_quat *q, const cgm_vec3 *axis, float angle)
106 cgm_qrotation(&qrot, axis, angle);
110 static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *quat1, const cgm_quat *q2, float t)
112 float angle, dot, a, b, sin_angle;
113 cgm_quat q1 = *quat1;
115 dot = quat1->x * q2->x + quat1->y * q2->y + quat1->z * q2->z + quat1->w * q2->w;
117 /* make sure we inteprolate across the shortest arc */
122 /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
123 * floating point imprecisions
125 if(dot < -1.0f) dot = -1.0f;
126 if(dot > 1.0f) dot = 1.0f;
129 sin_angle = sin(angle);
130 if(sin_angle == 0.0f) {
131 /* use linear interpolation to avoid div/zero */
135 a = sin((1.0f - t) * angle) / sin_angle;
136 b = sin(t * angle) / sin_angle;
139 res->x = q1.x * a + q2->x * b;
140 res->y = q1.y * a + q2->y * b;
141 res->z = q1.z * a + q2->z * b;
142 res->w = q1.w * a + q2->w * b;
145 static inline void cgm_qlerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t)
147 res->x = a->x + (b->x - a->x) * t;
148 res->y = a->y + (b->y - a->y) * t;
149 res->z = a->z + (b->z - a->z) * t;
150 res->w = a->w + (b->w - a->w) * t;