1 static inline void cgm_vzero(cgm_vec3 *v)
3 v->x = v->y = v->z = 0.0f;
6 static inline void cgm_vone(cgm_vec3 *v)
8 v->x = v->y = v->z = 1.0f;
11 static inline void cgm_vadd(cgm_vec3 *a, const cgm_vec3 *b)
18 static inline void cgm_vsub(cgm_vec3 *a, const cgm_vec3 *b)
25 static inline void cgm_vmul(cgm_vec3 *a, const cgm_vec3 *b)
32 static inline void cgm_vscale(cgm_vec3 *v, float s)
39 static inline float cgm_vdot(const cgm_vec3 *a, const cgm_vec3 *b)
41 return a->x * b->x + a->y * b->y + a->z * b->z;
44 static inline void cgm_vcross(cgm_vec3 *res, const cgm_vec3 *a, const cgm_vec3 *b)
46 res->x = a->y * b->z - a->z * b->y;
47 res->y = a->z * b->x - a->x * b->z;
48 res->z = a->x * b->y - a->y * b->x;
51 static inline float cgm_vlength(const cgm_vec3 *v)
53 return sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
56 static inline float cgm_vlength_sq(const cgm_vec3 *v)
58 return v->x * v->x + v->y * v->y + v->z * v->z;
61 static inline float cgm_vdist(const cgm_vec3 *a, const cgm_vec3 *b)
63 float dx = a->x - b->x;
64 float dy = a->y - b->y;
65 float dz = a->z - b->z;
66 return sqrt(dx * dx + dy * dy + dz * dz);
69 static inline float cgm_vdist_sq(const cgm_vec3 *a, const cgm_vec3 *b)
71 float dx = a->x - b->x;
72 float dy = a->y - b->y;
73 float dz = a->z - b->z;
74 return dx * dx + dy * dy + dz * dz;
77 static inline void cgm_vnormalize(cgm_vec3 *v)
79 float len = cgm_vlength(v);
88 static inline void cgm_qident(cgm_quat *q)
90 q->x = q->y = q->z = 0.0f;
94 static inline void cgm_qneg(cgm_quat *q)
102 static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b)
110 static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b)
118 static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b)
120 float axb_x = a->y * b->z - a->z * b->y;
121 float axb_y = a->z * b->x - a->x * b->z;
122 float axb_z = a->x * b->y - a->y * b->x;
124 float im_x = a->w * b->x + b->w * a->x + axb_x;
125 float im_y = a->w * b->y + b->w * a->y + axb_y;
126 float im_z = a->w * b->z + b->w * a->z + axb_z;
128 a->w = a->w * b->w - (a->x * b->x + a->y * b->y + a->z * b->z);
134 static inline float cgm_qlength(const cgm_quat *q)
136 return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
139 static inline float cgm_qlength_sq(const cgm_quat *q)
141 return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
144 static inline void cgm_qnormalize(cgm_quat *q)
146 float len = cgm_qlength(q);
148 float inv_len = 1.0f / len;
156 static inline void cgm_qconjugate(cgm_quat *q)
163 static inline void cgm_qinvert(cgm_quat *q)
166 float len_sq = cgm_qlength_sq(q);
168 float s = 1.0f / len_sq;
176 static inline void cgm_qrot_axis(cgm_quat *q, float angle, float x, float y, float z)
178 float half_angle = angle * 0.5f;
179 float sin_ha = sin(half_angle);
180 q->w = cos(half_angle);
186 static inline void cgm_qrot_quat(cgm_quat *q, const cgm_quat *rq)
189 cgm_quat rqconj = *rq;
190 cgm_qconjugate(&rqconj);
192 cgm_qmul(&qrot, &rqconj);
196 static inline void cgm_qmatrix(float *mat, const cgm_quat *q)
198 float xsq2 = 2.0f * q->x * q->x;
199 float ysq2 = 2.0f * q->y * q->y;
200 float zsq2 = 2.0f * q->z * q->z;
201 float sx = 1.0f - ysq2 - zsq2;
202 float sy = 1.0f - xsq2 - zsq2;
203 float sz = 1.0f - xsq2 - ysq2;
206 mat[1] = 2.0f * q->x * q->y + 2.0f * q->w * q->z;
207 mat[2] = 2.0f * q->z * q->x - 2.0f * q->w * q->y;
210 mat[4] = 2.0f * q->x * q->y - 2.0f * q->w * q->z;
212 mat[6] = 2.0f * q->y * q->z + 2.0f * q->w * q->x;
215 mat[8] = 2.0f * q->z * q->x + 2.0f * q->w * q->y;
216 mat[9] = 2.0f * q->y * q->z - 2.0f * q->w * q->x;
220 mat[12] = mat[13] = mat[14] = 0.0f;
224 static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t)
227 float dot = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w;
228 float angle, sin_angle, ta, tb;
231 /* make sure we interpolate across the shortest arc */
239 /* clamp dot to [-1, 1] in order to avoid domain errors in acos */
240 if(dot < -1.0f) dot = -1.0f;
241 if(dot > 1.0f) dot = 1.0f;
243 sin_angle = sin(angle);
245 if(sin_angle == 0.0f) {
246 /* use linear interpolation to avoid div/zero */
250 ta = sin((1.0f - t) * angle) / sin_angle;
251 tb = sin(t * angle) / sin_angle;
254 res->x = a->x * ta + b->x * tb;
255 res->y = a->y * ta + b->y * tb;
256 res->z = a->z * ta + b->z * tb;
257 res->w = a->w * ta + b->w * tb;