1 #ifdef HAVE_CONFIG_H
2 # include <config.h>
3 #endif
4
5 #include "ephysics_private.h"
6
7 #ifdef __cplusplus
8 extern "C" {
9 #endif
10
11 static void
_ephysics_quaternion_update(EPhysics_Quaternion * quat,btQuaternion * bt_quat)12 _ephysics_quaternion_update(EPhysics_Quaternion *quat, btQuaternion *bt_quat)
13 {
14 quat->x = bt_quat->x();
15 quat->y = bt_quat->y();
16 quat->z = bt_quat->z();
17 quat->w = bt_quat->getW();
18 }
19
20 static EPhysics_Quaternion *
_ephysics_quaternion_params_check(const EPhysics_Quaternion * quat1,const EPhysics_Quaternion * quat2,EPhysics_Quaternion * result)21 _ephysics_quaternion_params_check(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2, EPhysics_Quaternion *result)
22 {
23 if ((!quat1) || (!quat2))
24 {
25 ERR("Can't operate over null quaternions.");
26 return NULL;
27 }
28
29 if (result)
30 return result;
31
32 return ephysics_quaternion_new();
33 }
34
35 EAPI EPhysics_Quaternion *
ephysics_quaternion_new(void)36 ephysics_quaternion_new(void)
37 {
38 EPhysics_Quaternion *quat;
39
40 quat = (EPhysics_Quaternion *)calloc(1, sizeof(EPhysics_Quaternion));
41
42 if (!quat)
43 {
44 ERR("Could not allocate ephysics quaternion.");
45 return NULL;
46 }
47
48 quat->w = 1;
49 return quat;
50 }
51
52 EAPI void
ephysics_quaternion_get(const EPhysics_Quaternion * quat,double * x,double * y,double * z,double * w)53 ephysics_quaternion_get(const EPhysics_Quaternion *quat, double *x, double *y, double *z, double *w)
54 {
55 if (!quat)
56 {
57 ERR("Can't get quaternion's values, it is null.");
58 return;
59 }
60
61 if (x) *x = quat->x;
62 if (y) *y = quat->y;
63 if (z) *z = quat->z;
64 if (w) *w = quat->w;
65 }
66
67 EAPI void
ephysics_quaternion_axis_angle_get(const EPhysics_Quaternion * quat,double * nx,double * ny,double * nz,double * a)68 ephysics_quaternion_axis_angle_get(const EPhysics_Quaternion *quat, double *nx, double *ny, double *nz, double *a)
69 {
70 btQuaternion bt_quat;
71
72 if (!quat)
73 {
74 ERR("Can't get quaternion's values, it is null.");
75 return;
76 }
77
78 bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w);
79
80 if (nx) *nx = bt_quat.getAxis().getX();
81 if (ny) *ny = bt_quat.getAxis().getY();
82 if (nz) *nz = bt_quat.getAxis().getZ();
83 if (a) *a = bt_quat.getAngle() * RAD_TO_DEG;
84 }
85
86 EAPI void
ephysics_quaternion_set(EPhysics_Quaternion * quat,double x,double y,double z,double w)87 ephysics_quaternion_set(EPhysics_Quaternion *quat, double x, double y, double z, double w)
88 {
89 if (!quat)
90 {
91 ERR("Could not set a null quaternion.");
92 return;
93 }
94
95 quat->x = x;
96 quat->y = y;
97 quat->z = z;
98 quat->w = w;
99 }
100
101 EAPI void
ephysics_quaternion_axis_angle_set(EPhysics_Quaternion * quat,double nx,double ny,double nz,double a)102 ephysics_quaternion_axis_angle_set(EPhysics_Quaternion *quat, double nx, double ny, double nz, double a)
103 {
104 btQuaternion bt_quat;
105 btVector3 axis;
106
107 if (!quat)
108 {
109 ERR("Could not set a null quaternion.");
110 return;
111 }
112
113 axis = btVector3(nx, ny, nz);
114 bt_quat = btQuaternion(axis, a / RAD_TO_DEG);
115 _ephysics_quaternion_update(quat, &bt_quat);
116 }
117
118 EAPI void
ephysics_quaternion_euler_set(EPhysics_Quaternion * quat,double yaw,double pitch,double roll)119 ephysics_quaternion_euler_set(EPhysics_Quaternion *quat, double yaw, double pitch, double roll)
120 {
121 btQuaternion bt_quat;
122
123 if (!quat)
124 {
125 ERR("Could not set a null quaternion.");
126 return;
127 }
128
129 bt_quat.setEuler(yaw / RAD_TO_DEG, pitch / RAD_TO_DEG, roll / RAD_TO_DEG);
130 _ephysics_quaternion_update(quat, &bt_quat);
131 }
132
133 EAPI void
ephysics_quaternion_normalize(EPhysics_Quaternion * quat)134 ephysics_quaternion_normalize(EPhysics_Quaternion *quat)
135 {
136 btQuaternion bt_quat;
137
138 if (!quat)
139 {
140 ERR("Can't normalize a null quaternion.");
141 return;
142 }
143
144 bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w);
145 bt_quat.normalize();
146 _ephysics_quaternion_update(quat, &bt_quat);
147 }
148
149 EAPI void
ephysics_quaternion_invert(EPhysics_Quaternion * quat)150 ephysics_quaternion_invert(EPhysics_Quaternion *quat)
151 {
152 btQuaternion bt_quat;
153
154 if (!quat)
155 {
156 ERR("Can't normalize a null quaternion.");
157 return;
158 }
159
160 bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w);
161 bt_quat.inverse();
162 _ephysics_quaternion_update(quat, &bt_quat);
163 }
164
165 EAPI void
ephysics_quaternion_scale(EPhysics_Quaternion * quat,double scale)166 ephysics_quaternion_scale(EPhysics_Quaternion *quat, double scale)
167 {
168 btQuaternion bt_quat;
169
170 if (!quat)
171 {
172 ERR("Can't operate over a null quaternion.");
173 return;
174 }
175
176 bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w);
177 bt_quat *= scale;
178 _ephysics_quaternion_update(quat, &bt_quat);
179 }
180
181 EAPI void
ephysics_quaternion_inverse_scale(EPhysics_Quaternion * quat,double scale)182 ephysics_quaternion_inverse_scale(EPhysics_Quaternion *quat, double scale)
183 {
184 btQuaternion bt_quat;
185
186 if (!quat)
187 {
188 ERR("Can't operate over a null quaternion.");
189 return;
190 }
191
192 bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w);
193 bt_quat /= scale;
194 _ephysics_quaternion_update(quat, &bt_quat);
195 }
196
197 EAPI EPhysics_Quaternion *
ephysics_quaternion_sum(const EPhysics_Quaternion * quat1,const EPhysics_Quaternion * quat2,EPhysics_Quaternion * result)198 ephysics_quaternion_sum(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2, EPhysics_Quaternion *result)
199 {
200 btQuaternion bt_quat1, bt_quat2, bt_quat;
201 EPhysics_Quaternion *quat;
202
203 quat = _ephysics_quaternion_params_check(quat1, quat2, result);
204 if (!quat) return NULL;
205
206 bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w);
207 bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w);
208 bt_quat = bt_quat1 + bt_quat2;
209
210 _ephysics_quaternion_update(quat, &bt_quat);
211 return quat;
212 }
213
214 EAPI EPhysics_Quaternion *
ephysics_quaternion_diff(const EPhysics_Quaternion * quat1,const EPhysics_Quaternion * quat2,EPhysics_Quaternion * result)215 ephysics_quaternion_diff(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2, EPhysics_Quaternion *result)
216 {
217 btQuaternion bt_quat1, bt_quat2, bt_quat;
218 EPhysics_Quaternion *quat;
219
220 quat = _ephysics_quaternion_params_check(quat1, quat2, result);
221 if (!quat) return NULL;
222
223 bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w);
224 bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w);
225 bt_quat = bt_quat1 - bt_quat2;
226
227 _ephysics_quaternion_update(quat, &bt_quat);
228 return quat;
229 }
230
231 EAPI EPhysics_Quaternion *
ephysics_quaternion_multiply(const EPhysics_Quaternion * quat1,const EPhysics_Quaternion * quat2,EPhysics_Quaternion * result)232 ephysics_quaternion_multiply(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2, EPhysics_Quaternion *result)
233 {
234 btQuaternion bt_quat1, bt_quat2, bt_quat;
235 EPhysics_Quaternion *quat;
236
237 quat = _ephysics_quaternion_params_check(quat1, quat2, result);
238 if (!quat) return NULL;
239
240 bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w);
241 bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w);
242 bt_quat = bt_quat1 * bt_quat2;
243
244 _ephysics_quaternion_update(quat, &bt_quat);
245 return quat;
246 }
247
248 EAPI EPhysics_Quaternion *
ephysics_quaternion_slerp(const EPhysics_Quaternion * quat1,const EPhysics_Quaternion * quat2,double ratio,EPhysics_Quaternion * result)249 ephysics_quaternion_slerp(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2, double ratio, EPhysics_Quaternion *result)
250 {
251 btQuaternion bt_quat1, bt_quat2;
252 EPhysics_Quaternion *quat;
253
254 quat = _ephysics_quaternion_params_check(quat1, quat2, result);
255 if (!quat) return NULL;
256
257 bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w);
258 bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w);
259 bt_quat1.slerp(bt_quat2, ratio);
260
261 _ephysics_quaternion_update(quat, &bt_quat1);
262 return quat;
263 }
264
265 EAPI double
ephysics_quaternion_dot(const EPhysics_Quaternion * quat1,const EPhysics_Quaternion * quat2)266 ephysics_quaternion_dot(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2)
267 {
268 btQuaternion bt_quat1, bt_quat2;
269
270 if ((!quat1) || (!quat2))
271 {
272 ERR("Can't operate over null quaternions.");
273 return 0;
274 }
275
276 bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w);
277 bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w);
278
279 return bt_quat1.dot(bt_quat2);
280 }
281
282 EAPI double
ephysics_quaternion_angle_get(const EPhysics_Quaternion * quat1,const EPhysics_Quaternion * quat2)283 ephysics_quaternion_angle_get(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2)
284 {
285 btQuaternion bt_quat1, bt_quat2;
286
287 if ((!quat1) || (!quat2))
288 {
289 ERR("Can't operate over null quaternions.");
290 return 0;
291 }
292
293 bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w);
294 bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w);
295
296 return bt_quat1.angle(bt_quat2) * RAD_TO_DEG;
297 }
298
299 EAPI double
ephysics_quaternion_length_get(const EPhysics_Quaternion * quat)300 ephysics_quaternion_length_get(const EPhysics_Quaternion *quat)
301 {
302 btQuaternion bt_quat;
303
304 if (!quat)
305 {
306 ERR("Can't operate over a null quaternion.");
307 return 0;
308 }
309
310 bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w);
311 return bt_quat.length();
312 }
313
314 EAPI double
ephysics_quaternion_length2_get(const EPhysics_Quaternion * quat)315 ephysics_quaternion_length2_get(const EPhysics_Quaternion *quat)
316 {
317 btQuaternion bt_quat;
318
319 if (!quat)
320 {
321 ERR("Can't operate over a null quaternion.");
322 return 0;
323 }
324
325 bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w);
326 return bt_quat.length2();
327 }
328
329 #ifdef __cplusplus
330 }
331 #endif
332