Line data Source code
1 0 : /*
2 :
3 : OOQuaternion.m
4 :
5 : Oolite
6 : Copyright (C) 2004-2013 Giles C Williams and contributors
7 :
8 : This program is free software; you can redistribute it and/or
9 : modify it under the terms of the GNU General Public License
10 : as published by the Free Software Foundation; either version 2
11 : of the License, or (at your option) any later version.
12 :
13 : This program is distributed in the hope that it will be useful,
14 : but WITHOUT ANY WARRANTY; without even the implied warranty of
15 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 : GNU General Public License for more details.
17 :
18 : You should have received a copy of the GNU General Public License
19 : along with this program; if not, write to the Free Software
20 : Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
21 : MA 02110-1301, USA.
22 :
23 : */
24 :
25 :
26 : #include "OOMaths.h"
27 :
28 :
29 0 : const Quaternion kIdentityQuaternion = { 1.0f, 0.0f, 0.0f, 0.0f };
30 0 : const Quaternion kZeroQuaternion = { 0.0f, 0.0f, 0.0f, 0.0f };
31 :
32 :
33 0 : Quaternion quaternion_multiply(Quaternion q1, Quaternion q2)
34 : {
35 : Quaternion result;
36 : result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
37 : result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
38 : result.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
39 : result.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
40 : return result;
41 : }
42 :
43 :
44 : #if !OOMATHS_STANDALONE
45 : // NOTE: this is broken - its distribution is weighted towards corners of the hypercube. Probably doesn't matter, though.
46 0 : void quaternion_set_random(Quaternion *quat)
47 : {
48 : quat->w = (OOScalar)(Ranrot() % 1024) - 511.5f; // -511.5 to +511.5
49 : quat->x = (OOScalar)(Ranrot() % 1024) - 511.5f; // -511.5 to +511.5
50 : quat->y = (OOScalar)(Ranrot() % 1024) - 511.5f; // -511.5 to +511.5
51 : quat->z = (OOScalar)(Ranrot() % 1024) - 511.5f; // -511.5 to +511.5
52 : quaternion_normalize(quat);
53 : }
54 : #endif
55 :
56 :
57 0 : Vector vector_forward_from_quaternion(Quaternion quat)
58 : {
59 : OOScalar w, wy, wx;
60 : OOScalar x, xz, xx;
61 : OOScalar y, yz, yy;
62 : OOScalar z, zz;
63 : Vector res;
64 :
65 : w = quat.w;
66 : z = quat.z;
67 : y = quat.y;
68 : x = quat.x;
69 :
70 : xx = 2.0f * x; yy = 2.0f * y; zz = 2.0f * z;
71 : wx = w * xx; wy = w * yy;
72 : xx = x * xx; xz = x * zz;
73 : yy = y * yy; yz = y * zz;
74 :
75 : res.x = xz - wy;
76 : res.y = yz + wx;
77 : res.z = 1.0f - xx - yy;
78 :
79 : if (res.x||res.y||res.z) return vector_normal(res);
80 : else return make_vector(0.0f, 0.0f, 1.0f);
81 : }
82 :
83 0 : HPVector HPvector_forward_from_quaternion(Quaternion quat)
84 : {
85 : // HPVect: profile this later
86 : return vectorToHPVector(vector_forward_from_quaternion(quat));
87 : }
88 :
89 0 : Vector vector_up_from_quaternion(Quaternion quat)
90 : {
91 : OOScalar w, wz, wx;
92 : OOScalar x, xy, xx;
93 : OOScalar y, yz, yy;
94 : OOScalar z, zz;
95 : Vector res;
96 :
97 : w = quat.w;
98 : z = quat.z;
99 : y = quat.y;
100 : x = quat.x;
101 :
102 : xx = 2.0f * x; yy = 2.0f * y; zz = 2.0f * z;
103 : wx = w * xx; wz = w * zz;
104 : xx = x * xx; xy = x * yy;
105 : yz = y * zz;
106 : zz = z * zz;
107 :
108 : res.x = xy + wz;
109 : res.y = 1.0f - xx - zz;
110 : res.z = yz - wx;
111 :
112 : if (res.x||res.y||res.z) return vector_normal(res);
113 : else return make_vector(0.0f, 1.0f, 0.0f);
114 : }
115 :
116 :
117 0 : Vector vector_right_from_quaternion(Quaternion quat)
118 : {
119 : OOScalar w, wz, wy;
120 : OOScalar x, xz, xy;
121 : OOScalar y, yy;
122 : OOScalar z, zz;
123 : Vector res;
124 :
125 : w = quat.w;
126 : z = quat.z;
127 : y = quat.y;
128 : x = quat.x;
129 :
130 : yy = 2.0f * y; zz = 2.0f * z;
131 : wy = w * yy; wz = w * zz;
132 : xy = x * yy; xz = x * zz;
133 : yy = y * yy;
134 : zz = z * zz;
135 :
136 : res.x = 1.0f - yy - zz;
137 : res.y = xy - wz;
138 : res.z = xz + wy;
139 :
140 : if (res.x||res.y||res.z) return vector_normal(res);
141 : else return make_vector(1.0f, 0.0f, 0.0f);
142 : }
143 :
144 :
145 0 : void basis_vectors_from_quaternion(Quaternion quat, Vector *outRight, Vector *outUp, Vector *outForward)
146 : {
147 : OOScalar w, wz, wy, wx;
148 : OOScalar x, xz, xy, xx;
149 : OOScalar y, yz, yy;
150 : OOScalar z, zz;
151 :
152 : w = quat.w;
153 : z = quat.z;
154 : y = quat.y;
155 : x = quat.x;
156 :
157 : xx = 2.0f * x; yy = 2.0f * y; zz = 2.0f * z;
158 : wx = w * xx; wy = w * yy; wz = w * zz;
159 : xx = x * xx; xy = x * yy; xz = x * zz;
160 : yy = y * yy; yz = y * zz;
161 : zz = z * zz;
162 :
163 : if (outRight != NULL)
164 : {
165 : outRight->x = 1.0f - yy - zz;
166 : outRight->y = xy - wz;
167 : outRight->z = xz + wy;
168 :
169 : if (outRight->x || outRight->y || outRight->z) *outRight = vector_normal(*outRight);
170 : else *outRight = make_vector(1.0f, 0.0f, 0.0f);
171 : }
172 :
173 : if (outUp != NULL)
174 : {
175 : outUp->x = xy + wz;
176 : outUp->y = 1.0f - xx - zz;
177 : outUp->z = yz - wx;
178 :
179 : if (outUp->x || outUp->y || outUp->z) *outUp = vector_normal(*outUp);
180 : else *outUp = make_vector(0.0f, 1.0f, 0.0f);
181 : }
182 :
183 : if (outForward != NULL)
184 : {
185 : outForward->x = xz - wy;
186 : outForward->y = yz + wx;
187 : outForward->z = 1.0f - xx - yy;
188 :
189 : if (outForward->x || outForward->y || outForward->z) *outForward = vector_normal(*outForward);
190 : else *outForward = make_vector(0.0f, 0.0f, 1.0f);
191 : }
192 : }
193 :
194 :
195 0 : Quaternion quaternion_rotation_between(Vector v0, Vector v1)
196 : {
197 : Quaternion q;
198 : OOScalar s = sqrt((1.0f + v0.x * v1.x + v0.y * v1.y + v0.z * v1.z) * 2.0f);
199 : if (EXPECT(s > 0.0f))
200 : {
201 : OOScalar is = 1.0f / s;
202 : q.x = (v0.y * v1.z - v0.z * v1.y) * is;
203 : q.y = (v0.z * v1.x - v0.x * v1.z) * is;
204 : q.z = (v0.x * v1.y - v0.y * v1.x) * is;
205 : q.w = s * 0.5f;
206 : }
207 : else
208 : {
209 : // Is this actually a problem?
210 : if (vector_equal(v1, kBasisZVector) || vector_equal(v0, kBasisZVector))
211 : {
212 : q = make_quaternion(0, 1, 0, 0);
213 : }
214 : else
215 : {
216 : q = kIdentityQuaternion;
217 : }
218 : // We arrive here for antiparallel vectors. Rotation axis is then undefined, but not rotating is
219 : // wrong. Probably the calling function should exclude this situation. For current
220 : // in-game use of this function we return (0,1,0,0), but generally that is also wrong.
221 : }
222 : return q;
223 : }
224 :
225 :
226 0 : Quaternion quaternion_rotation_betweenHP(HPVector v0, HPVector v1)
227 : {
228 : Quaternion q;
229 : OOHPScalar s = sqrt((1.0 + v0.x * v1.x + v0.y * v1.y + v0.z * v1.z) * 2.0);
230 : if (EXPECT(s > 0.0))
231 : {
232 : OOHPScalar is = 1.0 / s;
233 : q.x = (v0.y * v1.z - v0.z * v1.y) * is;
234 : q.y = (v0.z * v1.x - v0.x * v1.z) * is;
235 : q.z = (v0.x * v1.y - v0.y * v1.x) * is;
236 : q.w = s * 0.5;
237 : }
238 : else
239 : {
240 : // Is this actually a problem?
241 : if (HPvector_equal(v1, kBasisZHPVector) || HPvector_equal(v0, kBasisZHPVector))
242 : {
243 : q = make_quaternion(0, 1, 0, 0);
244 : }
245 : else
246 : {
247 : q = kIdentityQuaternion;
248 : }
249 : // We arrive here for antiparallel vectors. Rotation axis is then undefined, but not rotating is
250 : // wrong. Probably the calling function should exclude this situation. For current
251 : // in-game use of this function we return (0,1,0,0), but generally that is also wrong.
252 : }
253 : return q;
254 : }
255 :
256 :
257 0 : Quaternion quaternion_limited_rotation_between(Vector v0, Vector v1, float maxArc) // vectors both normalised
258 : {
259 : Quaternion q;
260 : OOScalar min_s = 2.0f * cosf(0.5f * maxArc);
261 : OOScalar s = sqrtf((1.0f + v0.x * v1.x + v0.y * v1.y + v0.z * v1.z) * 2.0f);
262 : // for some antiparallel vectors, s returns a NaN instead of 0. Testing s > 0 catches both.
263 : if (EXPECT(s > 0.0f))
264 : {
265 : if (s < min_s) // larger angle => smaller cos
266 : {
267 : OOScalar a = maxArc * 0.5f;
268 : OOScalar w = cosf(a);
269 : OOScalar scale = sinf(a);
270 : q.x = (v0.y * v1.z - v0.z * v1.y) * scale;
271 : q.y = (v0.z * v1.x - v0.x * v1.z) * scale;
272 : q.z = (v0.x * v1.y - v0.y * v1.x) * scale;
273 : q.w = w;
274 : }
275 : else
276 : {
277 : OOScalar is = 1.0f / s;
278 : q.x = (v0.y * v1.z - v0.z * v1.y) * is;
279 : q.y = (v0.z * v1.x - v0.x * v1.z) * is;
280 : q.z = (v0.x * v1.y - v0.y * v1.x) * is;
281 : q.w = s * 0.5f;
282 : }
283 : }
284 : else
285 : {
286 : // Is this actually a problem?
287 : q = kIdentityQuaternion;
288 : }
289 : return q;
290 : }
291 :
292 :
293 0 : void quaternion_rotate_about_x(Quaternion *quat, OOScalar angle)
294 : {
295 : Quaternion result;
296 : OOScalar a = angle * 0.5f;
297 : OOScalar w = cosf(a);
298 : OOScalar scale = sinf(a);
299 :
300 : result.w = quat->w * w - quat->x * scale;
301 : result.x = quat->w * scale + quat->x * w;
302 : result.y = quat->y * w + quat->z * scale;
303 : result.z = quat->z * w - quat->y * scale;
304 :
305 : quat->w = result.w;
306 : quat->x = result.x;
307 : quat->y = result.y;
308 : quat->z = result.z;
309 : }
310 :
311 :
312 0 : void quaternion_rotate_about_y(Quaternion *quat, OOScalar angle)
313 : {
314 : Quaternion result;
315 : OOScalar a = angle * 0.5f;
316 : OOScalar w = cosf(a);
317 : OOScalar scale = sinf(a);
318 :
319 : result.w = quat->w * w - quat->y * scale;
320 : result.x = quat->x * w - quat->z * scale;
321 : result.y = quat->w * scale + quat->y * w;
322 : result.z = quat->z * w + quat->x * scale;
323 :
324 : quat->w = result.w;
325 : quat->x = result.x;
326 : quat->y = result.y;
327 : quat->z = result.z;
328 : }
329 :
330 :
331 0 : void quaternion_rotate_about_z(Quaternion *quat, OOScalar angle)
332 : {
333 : Quaternion result;
334 : OOScalar a = angle * 0.5f;
335 : OOScalar w = cosf(a);
336 : OOScalar scale = sinf(a);
337 :
338 : result.w = quat->w * w - quat->z * scale;
339 : result.x = quat->x * w + quat->y * scale;
340 : result.y = quat->y * w - quat->x * scale;
341 : result.z = quat->w * scale + quat->z * w;
342 :
343 : quat->w = result.w;
344 : quat->x = result.x;
345 : quat->y = result.y;
346 : quat->z = result.z;
347 : }
348 :
349 :
350 0 : void quaternion_rotate_about_axis(Quaternion *quat, Vector axis, OOScalar angle)
351 : {
352 : Quaternion q2 /*, result */;
353 : OOScalar a = angle * 0.5f;
354 : OOScalar w = cosf(a);
355 : OOScalar scale = sinf(a);
356 :
357 : q2.w = w;
358 : q2.x = axis.x * scale;
359 : q2.y = axis.y * scale;
360 : q2.z = axis.z * scale;
361 :
362 : *quat = quaternion_multiply(*quat, q2);
363 : }
364 :
365 :
366 : #if __OBJC__
367 : NSString *QuaternionDescription(Quaternion quaternion)
368 : {
369 : float x, y, z;
370 : char xs, ys, zs;
371 :
372 : x = fabsf(quaternion.x);
373 : y = fabsf(quaternion.y);
374 : z = fabsf(quaternion.z);
375 :
376 : xs = (quaternion.x >= 0.0f) ? '+' : '-';
377 : ys = (quaternion.y >= 0.0f) ? '+' : '-';
378 : zs = (quaternion.z >= 0.0f) ? '+' : '-';
379 :
380 : return [NSString stringWithFormat:@"(%g %c %gi %c %gj %c %gk)", quaternion.w, xs, x, ys, y, zs, z];
381 : }
382 : #endif
383 :
384 :
385 0 : Vector quaternion_rotate_vector(Quaternion q, Vector v)
386 : {
387 : Quaternion qv;
388 :
389 : qv.w = 0.0f - q.x * v.x - q.y * v.y - q.z * v.z;
390 : qv.x = -q.w * v.x + q.y * v.z - q.z * v.y;
391 : qv.y = -q.w * v.y + q.z * v.x - q.x * v.z;
392 : qv.z = -q.w * v.z + q.x * v.y - q.y * v.x;
393 :
394 : v.x = qv.w * -q.x + qv.x * -q.w + qv.y * -q.z - qv.z * -q.y;
395 : v.y = qv.w * -q.y + qv.y * -q.w + qv.z * -q.x - qv.x * -q.z;
396 : v.z = qv.w * -q.z + qv.z * -q.w + qv.x * -q.y - qv.y * -q.x;
397 :
398 : return v;
399 : }
400 :
401 0 : HPVector quaternion_rotate_HPvector(Quaternion q, HPVector v)
402 : {
403 : OOHPScalar qvw, qvx, qvy, qvz;
404 :
405 : qvw = 0.0 - q.x * v.x - q.y * v.y - q.z * v.z;
406 : qvx = -q.w * v.x + q.y * v.z - q.z * v.y;
407 : qvy = -q.w * v.y + q.z * v.x - q.x * v.z;
408 : qvz = -q.w * v.z + q.x * v.y - q.y * v.x;
409 :
410 : v.x = qvw * -q.x + qvx * -q.w + qvy * -q.z - qvz * -q.y;
411 : v.y = qvw * -q.y + qvy * -q.w + qvz * -q.x - qvx * -q.z;
412 : v.z = qvw * -q.z + qvz * -q.w + qvx * -q.y - qvy * -q.x;
413 :
414 : return v;
415 : }
|
