foo
[vrlugburz] / libs / cgmath / cgmquat.inl
1 /* gph-cmath - C graphics math library
2  * Copyright (C) 2018 John Tsiombikas <nuclear@member.fsf.org>
3  *
4  * This program is free software. Feel free to use, modify, and/or redistribute
5  * it under the terms of the MIT/X11 license. See LICENSE for details.
6  * If you intend to redistribute parts of the code without the LICENSE file
7  * replace this paragraph with the full contents of the LICENSE file.
8  */
9 static inline void cgm_qcons(cgm_quat *q, float x, float y, float z, float w)
10 {
11         q->x = x;
12         q->y = y;
13         q->z = z;
14         q->w = w;
15 }
16
17
18 static inline void cgm_qneg(cgm_quat *q)
19 {
20         q->x = -q->x;
21         q->y = -q->y;
22         q->z = -q->z;
23         q->w = -q->w;
24 }
25
26 static inline void cgm_qadd(cgm_quat *a, const cgm_quat *b)
27 {
28         a->x += b->x;
29         a->y += b->y;
30         a->z += b->z;
31         a->w += b->w;
32 }
33
34 static inline void cgm_qsub(cgm_quat *a, const cgm_quat *b)
35 {
36         a->x -= b->x;
37         a->y -= b->y;
38         a->z -= b->z;
39         a->w -= b->w;
40 }
41
42 static inline void cgm_qmul(cgm_quat *a, const cgm_quat *b)
43 {
44         float x, y, z, dot;
45         cgm_vec3 cross;
46
47         dot = a->x * b->x + a->y * b->y + a->z * b->z;
48         cgm_vcross(&cross, (cgm_vec3*)a, (cgm_vec3*)b);
49
50         x = a->w * b->x + b->w * a->x + cross.x;
51         y = a->w * b->y + b->w * a->y + cross.y;
52         z = a->w * b->z + b->w * a->z + cross.z;
53         a->w = a->w * b->w - dot;
54         a->x = x;
55         a->y = y;
56         a->z = z;
57 }
58
59 static inline float cgm_qlength(const cgm_quat *q)
60 {
61         return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
62 }
63
64 static inline float cgm_qlength_sq(const cgm_quat *q)
65 {
66         return q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
67 }
68
69 static inline void cgm_qnormalize(cgm_quat *q)
70 {
71         float len = cgm_qlength(q);
72         if(len != 0.0f) {
73                 float s = 1.0f / len;
74                 q->x *= s;
75                 q->y *= s;
76                 q->z *= s;
77                 q->w *= s;
78         }
79 }
80
81 static inline void cgm_qconjugate(cgm_quat *q)
82 {
83         q->x = -q->x;
84         q->y = -q->y;
85         q->z = -q->z;
86 }
87
88 static inline void cgm_qinvert(cgm_quat *q)
89 {
90         float len_sq = cgm_qlength_sq(q);
91         cgm_qconjugate(q);
92         if(len_sq != 0.0f) {
93                 float s = 1.0f / len_sq;
94                 q->x *= s;
95                 q->y *= s;
96                 q->z *= s;
97                 q->w *= s;
98         }
99 }
100
101 static inline void cgm_qrotation(cgm_quat *q, float angle, float x, float y, float z)
102 {
103         float hangle = angle * 0.5f;
104         float sin_ha = sin(hangle);
105         q->w = cos(hangle);
106         q->x = x * sin_ha;
107         q->y = y * sin_ha;
108         q->z = z * sin_ha;
109 }
110
111 static inline void cgm_qrotate(cgm_quat *q, float angle, float x, float y, float z)
112 {
113         cgm_quat qrot;
114         cgm_qrotation(&qrot, angle, x, y, z);
115         cgm_qmul(q, &qrot);
116 }
117
118 static inline void cgm_qslerp(cgm_quat *res, const cgm_quat *quat1, const cgm_quat *q2, float t)
119 {
120         float angle, dot, a, b, sin_angle;
121         cgm_quat q1 = *quat1;
122
123         dot = quat1->x * q2->x + quat1->y * q2->y + quat1->z * q2->z + quat1->w * q2->w;
124         if(dot < 0.0f) {
125                 /* make sure we inteprolate across the shortest arc */
126                 cgm_qneg(&q1);
127                 dot = -dot;
128         }
129
130         /* clamp dot to [-1, 1] in order to avoid domain errors in acos due to
131          * floating point imprecisions
132          */
133         if(dot < -1.0f) dot = -1.0f;
134         if(dot > 1.0f) dot = 1.0f;
135         angle = acos(dot);
136
137         sin_angle = sin(angle);
138         if(sin_angle == 0.0f) {
139                 /* use linear interpolation to avoid div/zero */
140                 a = 1.0f;
141                 b = t;
142         } else {
143                 a = sin((1.0f - t) * angle) / sin_angle;
144                 b = sin(t * angle) / sin_angle;
145         }
146
147         res->x = q1.x * a + q2->x * b;
148         res->y = q1.y * a + q2->y * b;
149         res->z = q1.z * a + q2->z * b;
150         res->w = q1.w * a + q2->w * b;
151 }
152
153 static inline void cgm_qlerp(cgm_quat *res, const cgm_quat *a, const cgm_quat *b, float t)
154 {
155         res->x = a->x + (b->x - a->x) * t;
156         res->y = a->y + (b->y - a->y) * t;
157         res->z = a->z + (b->z - a->z) * t;
158         res->w = a->w + (b->w - a->w) * t;
159 }