1 /********************************************************************************
2 * *
3 * S i n g l e - P r e c i s i o n Q u a t e r n i o n *
4 * *
5 *********************************************************************************
6 * Copyright (C) 1994,2021 by Jeroen van der Zijp. All Rights Reserved. *
7 *********************************************************************************
8 * This library is free software; you can redistribute it and/or modify *
9 * it under the terms of the GNU Lesser General Public License as published by *
10 * the Free Software Foundation; either version 3 of the License, or *
11 * (at your option) any later version. *
12 * *
13 * This library 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 Lesser General Public License for more details. *
17 * *
18 * You should have received a copy of the GNU Lesser General Public License *
19 * along with this program. If not, see <http://www.gnu.org/licenses/> *
20 ********************************************************************************/
21 #include "xincs.h"
22 #include "fxver.h"
23 #include "fxdefs.h"
24 #include "fxmath.h"
25 #include "FXArray.h"
26 #include "FXHash.h"
27 #include "FXStream.h"
28 #include "FXObject.h"
29 #include "FXVec2f.h"
30 #include "FXVec3f.h"
31 #include "FXVec4f.h"
32 #include "FXQuatf.h"
33 #include "FXMat3f.h"
34 #include "FXMat4f.h"
35
36 /*
37 Notes:
38
39 - Quaternion represents a rotation as follows:
40
41 phi axis phi
42 Q = ( sin ( ----- ) * ------ , cos ( ----- ) )
43 2 |axis| 2
44
45 - Typically, |Q| == 1. But this is not always a given.
46 - Repeated operations should periodically fix Q to maintain |Q| == 1, using
47 the adjust() API.
48 */
49
50 using namespace FX;
51
52 /*******************************************************************************/
53
54 namespace FX {
55
56 // Construct from rotation axis and angle in radians
FXQuatf(const FXVec3f & axis,FXfloat phi)57 FXQuatf::FXQuatf(const FXVec3f& axis,FXfloat phi){
58 setAxisAngle(axis,phi);
59 }
60
61
62 // Construct quaternion from arc between two unit vectors fm and to
FXQuatf(const FXVec3f & fr,const FXVec3f & to)63 FXQuatf::FXQuatf(const FXVec3f& fr,const FXVec3f& to){
64 set(arc(fr,to));
65 }
66
67
68 // Construct from euler angles yaw (z), pitch (y), and roll (x)
FXQuatf(FXfloat roll,FXfloat pitch,FXfloat yaw)69 FXQuatf::FXQuatf(FXfloat roll,FXfloat pitch,FXfloat yaw){
70 setRollPitchYaw(roll,pitch,yaw);
71 }
72
73
74 // Construct quaternion from three orthogonal unit vectors
FXQuatf(const FXVec3f & ex,const FXVec3f & ey,const FXVec3f & ez)75 FXQuatf::FXQuatf(const FXVec3f& ex,const FXVec3f& ey,const FXVec3f& ez){
76 setAxes(ex,ey,ez);
77 }
78
79
80 // Construct quaternion from rotation vector rot, representing a rotation
81 // by |rot| radians about a unit vector rot/|rot|.
FXQuatf(const FXVec3f & rot)82 FXQuatf::FXQuatf(const FXVec3f& rot){
83 setRotation(rot);
84 }
85
86
87 // Adjust quaternion length
adjust()88 FXQuatf& FXQuatf::adjust(){
89 FXfloat mag2(x*x+y*y+z*z+w*w);
90 if(__likely(0.0f<mag2)){
91 FXfloat s(1.0f/Math::sqrt(mag2));
92 x*=s;
93 y*=s;
94 z*=s;
95 w*=s;
96 }
97 return *this;
98 }
99
100
101 // Set axis and angle
setAxisAngle(const FXVec3f & axis,FXfloat phi)102 void FXQuatf::setAxisAngle(const FXVec3f& axis,FXfloat phi){
103 FXfloat mag2(axis.length2());
104 if(__likely(0.0f<mag2)){
105 FXfloat arg(0.5f*phi);
106 FXfloat mag(Math::sqrt(mag2));
107 FXfloat s(Math::sin(arg)/mag);
108 FXfloat c(Math::cos(arg));
109 x=axis.x*s;
110 y=axis.y*s;
111 z=axis.z*s;
112 w=c;
113 }
114 else{
115 x=0.0f;
116 y=0.0f;
117 z=0.0f;
118 w=1.0f;
119 }
120 }
121
122
123 // Obtain axis and angle
124 // Remeber that: q = sin(A/2)*(x*i+y*j+z*k)+cos(A/2)
getAxisAngle(FXVec3f & axis,FXfloat & phi) const125 void FXQuatf::getAxisAngle(FXVec3f& axis,FXfloat& phi) const {
126 FXfloat mag2(x*x+y*y+z*z);
127 if(0.0f<mag2){
128 FXfloat mag(Math::sqrt(mag2));
129 axis.x=x/mag;
130 axis.y=y/mag;
131 axis.z=z/mag;
132 phi=2.0f*Math::atan2(mag,w);
133 }
134 else{
135 axis.x=1.0f;
136 axis.y=0.0f;
137 axis.z=0.0f;
138 phi=0.0f;
139 }
140 }
141
142
143 // Set quaternion from rotation vector rot
144 //
145 // |rot| rot |rot|
146 // Q = ( sin ( ------- ) * ------- , cos ( ------- ) )
147 // 2 |rot| 2
148 //
setRotation(const FXVec3f & rot)149 void FXQuatf::setRotation(const FXVec3f& rot){
150 FXfloat mag2(rot.length2());
151 if(0.0f<mag2){
152 FXfloat mag(Math::sqrt(mag2));
153 FXfloat arg(mag*0.5f);
154 FXfloat s(Math::sin(arg)/mag);
155 FXfloat c(Math::cos(arg));
156 x=rot.x*s;
157 y=rot.y*s;
158 z=rot.z*s;
159 w=c;
160 }
161 else{
162 x=0.0f;
163 y=0.0f;
164 z=0.0f;
165 w=1.0f;
166 }
167 }
168
169
170 // Set unit quaternion to modified rodrigues parameters.
171 // Modified Rodriques parameters are defined as MRP = tan(theta/4)*E,
172 // where theta is rotation angle (radians), and E is unit axis of rotation.
173 // Reference: "A survey of Attitude Representations", Malcolm D. Shuster,
174 // Journal of Astronautical sciences, Vol. 41, No. 4, Oct-Dec. 1993, pp. 476,
175 // Equations (253).
setMRP(const FXVec3f & m)176 void FXQuatf::setMRP(const FXVec3f& m){
177 FXfloat mm=m[0]*m[0]+m[1]*m[1]+m[2]*m[2];
178 FXfloat D=1.0f/(1.0f+mm);
179 x=m[0]*2.0f*D;
180 y=m[1]*2.0f*D;
181 z=m[2]*2.0f*D;
182 w=(1.0f-mm)*D;
183 }
184
185
186 // Return modified rodrigues parameters from unit quaternion.
187 // Reference: "A survey of Attitude Representations", Malcolm D. Shuster,
188 // Journal of Astronautical sciences, Vol. 41, No. 4, Oct-Dec. 1993, pp. 475,
189 // Equations (249). (250).
getMRP() const190 FXVec3f FXQuatf::getMRP() const {
191 FXfloat m=(0.0f<w)?1.0f/(1.0f+w):-1.0f/(1.0f-w);
192 return FXVec3f(x*m,y*m,z*m);
193 }
194
195
196 // Get rotation vector from quaternion
getRotation() const197 FXVec3f FXQuatf::getRotation() const {
198 FXVec3f rot(0.0f,0.0f,0.0f);
199 FXfloat mag2(x*x+y*y+z*z);
200 if(0.0f<mag2){
201 FXfloat mag(Math::sqrt(mag2));
202 FXfloat phi(2.0f*Math::atan2(mag,w)/mag);
203 rot.x=x*phi*mag;
204 rot.y=y*phi*mag;
205 rot.z=z*phi*mag;
206 }
207 return rot;
208 }
209
210
211 // Set quaternion from roll (x), pitch (y), yaw (z)
setRollPitchYaw(FXfloat roll,FXfloat pitch,FXfloat yaw)212 void FXQuatf::setRollPitchYaw(FXfloat roll,FXfloat pitch,FXfloat yaw){
213 FXfloat sr,cr,sp,cp,sy,cy;
214 FXfloat rr=0.5f*roll;
215 FXfloat pp=0.5f*pitch;
216 FXfloat yy=0.5f*yaw;
217 sr=Math::sin(rr); cr=Math::cos(rr);
218 sp=Math::sin(pp); cp=Math::cos(pp);
219 sy=Math::sin(yy); cy=Math::cos(yy);
220 x=sr*cp*cy-cr*sp*sy;
221 y=cr*sp*cy+sr*cp*sy;
222 z=cr*cp*sy-sr*sp*cy;
223 w=cr*cp*cy+sr*sp*sy;
224 }
225
226
227 // Set quaternion from yaw (z), pitch (y), roll (x)
setYawPitchRoll(FXfloat yaw,FXfloat pitch,FXfloat roll)228 void FXQuatf::setYawPitchRoll(FXfloat yaw,FXfloat pitch,FXfloat roll){
229 FXfloat sr,cr,sp,cp,sy,cy;
230 FXfloat rr=0.5f*roll;
231 FXfloat pp=0.5f*pitch;
232 FXfloat yy=0.5f*yaw;
233 sr=Math::sin(rr); cr=Math::cos(rr);
234 sp=Math::sin(pp); cp=Math::cos(pp);
235 sy=Math::sin(yy); cy=Math::cos(yy);
236 x=sr*cp*cy+cr*sp*sy;
237 y=cr*sp*cy-sr*cp*sy;
238 z=cr*cp*sy+sr*sp*cy;
239 w=cr*cp*cy-sr*sp*sy;
240 }
241
242
243 // Set quaternion from roll (x), yaw (z), pitch (y)
setRollYawPitch(FXfloat roll,FXfloat yaw,FXfloat pitch)244 void FXQuatf::setRollYawPitch(FXfloat roll,FXfloat yaw,FXfloat pitch){
245 FXfloat sr,cr,sp,cp,sy,cy;
246 FXfloat rr=0.5f*roll;
247 FXfloat pp=0.5f*pitch;
248 FXfloat yy=0.5f*yaw;
249 sr=Math::sin(rr); cr=Math::cos(rr);
250 sp=Math::sin(pp); cp=Math::cos(pp);
251 sy=Math::sin(yy); cy=Math::cos(yy);
252 x=cp*cy*sr+sp*sy*cr;
253 y=sp*cy*cr+cp*sy*sr;
254 z=cp*sy*cr-sp*cy*sr;
255 w=cp*cy*cr-sp*sy*sr;
256 }
257
258
259 // Set quaternion from pitch (y), roll (x),yaw (z)
setPitchRollYaw(FXfloat pitch,FXfloat roll,FXfloat yaw)260 void FXQuatf::setPitchRollYaw(FXfloat pitch,FXfloat roll,FXfloat yaw){
261 FXfloat sr,cr,sp,cp,sy,cy;
262 FXfloat rr=0.5f*roll;
263 FXfloat pp=0.5f*pitch;
264 FXfloat yy=0.5f*yaw;
265 sr=Math::sin(rr); cr=Math::cos(rr);
266 sp=Math::sin(pp); cp=Math::cos(pp);
267 sy=Math::sin(yy); cy=Math::cos(yy);
268 x=cy*sr*cp-sy*cr*sp;
269 y=cy*cr*sp+sy*sr*cp;
270 z=cy*sr*sp+sy*cr*cp;
271 w=cy*cr*cp-sy*sr*sp;
272 }
273
274
275 // Set quaternion from pitch (y), yaw (z), roll (x)
setPitchYawRoll(FXfloat pitch,FXfloat yaw,FXfloat roll)276 void FXQuatf::setPitchYawRoll(FXfloat pitch,FXfloat yaw,FXfloat roll){
277 FXfloat sr,cr,sp,cp,sy,cy;
278 FXfloat rr=0.5f*roll;
279 FXfloat pp=0.5f*pitch;
280 FXfloat yy=0.5f*yaw;
281 sr=Math::sin(rr); cr=Math::cos(rr);
282 sp=Math::sin(pp); cp=Math::cos(pp);
283 sy=Math::sin(yy); cy=Math::cos(yy);
284 x=sr*cy*cp-cr*sy*sp;
285 y=cr*cy*sp-sr*sy*cp;
286 z=sr*cy*sp+cr*sy*cp;
287 w=cr*cy*cp+sr*sy*sp;
288 }
289
290
291 // Set quaternion from yaw (z), roll (x), pitch (y)
setYawRollPitch(FXfloat yaw,FXfloat roll,FXfloat pitch)292 void FXQuatf::setYawRollPitch(FXfloat yaw,FXfloat roll,FXfloat pitch){
293 FXfloat sr,cr,sp,cp,sy,cy;
294 FXfloat rr=0.5f*roll;
295 FXfloat pp=0.5f*pitch;
296 FXfloat yy=0.5f*yaw;
297 sr=Math::sin(rr); cr=Math::cos(rr);
298 sp=Math::sin(pp); cp=Math::cos(pp);
299 sy=Math::sin(yy); cy=Math::cos(yy);
300 x=cp*sr*cy+sp*cr*sy;
301 y=sp*cr*cy-cp*sr*sy;
302 z=cp*cr*sy-sp*sr*cy;
303 w=cp*cr*cy+sp*sr*sy;
304 }
305
306
307 // Obtain roll, pitch, yaw
getRollPitchYaw(FXfloat & roll,FXfloat & pitch,FXfloat & yaw) const308 void FXQuatf::getRollPitchYaw(FXfloat& roll,FXfloat& pitch,FXfloat& yaw) const {
309 roll=Math::atan2(2.0f*(y*z+w*x),1.0f-2.0f*(x*x+y*y));
310 pitch=Math::asin(Math::fclamp(-1.0f,-2.0f*(x*z-w*y),1.0f));
311 yaw=Math::atan2(2.0f*(x*y+w*z),1.0f-2.0f*(y*y+z*z));
312 }
313
314
315 // Obtain yaw, pitch, and roll
getYawPitchRoll(FXfloat & yaw,FXfloat & pitch,FXfloat & roll) const316 void FXQuatf::getYawPitchRoll(FXfloat& yaw,FXfloat& pitch,FXfloat& roll) const {
317 yaw=Math::atan2(-2.0f*(x*y-w*z),1.0f-2.0f*(y*y+z*z));
318 pitch=Math::asin(Math::fclamp(-1.0f,2.0f*(x*z+w*y),1.0f));
319 roll=Math::atan2(-2.0f*(y*z-w*x),1.0f-2.0f*(x*x+y*y));
320 }
321
322
323 // Obtain roll, yaw, pitch
getRollYawPitch(FXfloat & roll,FXfloat & yaw,FXfloat & pitch) const324 void FXQuatf::getRollYawPitch(FXfloat& roll,FXfloat& yaw,FXfloat& pitch) const {
325 roll=Math::atan2(-2.0f*(y*z-w*x),1.0f-2.0f*(x*x+z*z));
326 yaw=Math::asin(Math::fclamp(-1.0f,2.0f*(x*y+w*z),1.0f));
327 pitch=Math::atan2(-2.0f*(x*z-w*y),1.0f-2.0f*(y*y+z*z));
328 }
329
330
331 // Obtain pitch, roll, yaw
getPitchRollYaw(FXfloat & pitch,FXfloat & roll,FXfloat & yaw) const332 void FXQuatf::getPitchRollYaw(FXfloat& pitch,FXfloat& roll,FXfloat& yaw) const {
333 pitch=Math::atan2(-2.0f*(x*z-w*y),1.0f-2.0f*(x*x+y*y));
334 roll=Math::asin(Math::fclamp(-1.0f,2.0f*(y*z+w*x),1.0f));
335 yaw=Math::atan2(-2.0f*(x*y-w*z),1.0f-2.0f*(x*x+z*z));
336 }
337
338
339 // Obtain pitch, yaw, roll
getPitchYawRoll(FXfloat & pitch,FXfloat & yaw,FXfloat & roll) const340 void FXQuatf::getPitchYawRoll(FXfloat& pitch,FXfloat& yaw,FXfloat& roll) const {
341 pitch=Math::atan2(2.0f*(x*z+w*y),1.0f-2.0f*(y*y+z*z));
342 yaw=Math::asin(Math::fclamp(-1.0f,-2.0f*(x*y-w*z),1.0f));
343 roll=Math::atan2(2.0f*(y*z+w*x),1.0f-2.0f*(x*x+z*z));
344 }
345
346
347 // Obtain yaw, roll, pitch
getYawRollPitch(FXfloat & yaw,FXfloat & roll,FXfloat & pitch) const348 void FXQuatf::getYawRollPitch(FXfloat& yaw,FXfloat& roll,FXfloat& pitch) const {
349 yaw=Math::atan2(2.0f*(x*y+w*z),1.0f-2.0f*(x*x+z*z));
350 roll=Math::asin(Math::fclamp(-1.0f,-2.0f*(y*z-w*x),1.0f));
351 pitch=Math::atan2(2.0f*(x*z+w*y),1.0f-2.0f*(x*x+y*y));
352 }
353
354
355 // Set quaternion from axes
setAxes(const FXVec3f & ex,const FXVec3f & ey,const FXVec3f & ez)356 void FXQuatf::setAxes(const FXVec3f& ex,const FXVec3f& ey,const FXVec3f& ez){
357 FXfloat trace=ex.x+ey.y+ez.z;
358 FXfloat scale;
359 if(trace>0.0f){
360 scale=Math::sqrt(1.0f+trace);
361 w=0.5f*scale;
362 scale=0.5f/scale;
363 x=(ey.z-ez.y)*scale;
364 y=(ez.x-ex.z)*scale;
365 z=(ex.y-ey.x)*scale;
366 }
367 else if(ex.x>ey.y && ex.x>ez.z){
368 scale=2.0f*Math::sqrt(1.0f+ex.x-ey.y-ez.z);
369 x=0.25f*scale;
370 y=(ex.y+ey.x)/scale;
371 z=(ex.z+ez.x)/scale;
372 w=(ey.z-ez.y)/scale;
373 }
374 else if(ey.y>ez.z){
375 scale=2.0f*Math::sqrt(1.0f+ey.y-ex.x-ez.z);
376 y=0.25f*scale;
377 x=(ex.y+ey.x)/scale;
378 z=(ey.z+ez.y)/scale;
379 w=(ez.x-ex.z)/scale;
380 }
381 else{
382 scale=2.0f*Math::sqrt(1.0f+ez.z-ex.x-ey.y);
383 z=0.25f*scale;
384 x=(ex.z+ez.x)/scale;
385 y=(ey.z+ez.y)/scale;
386 w=(ex.y-ey.x)/scale;
387 }
388 }
389
390
391 // Get quaternion axes
getAxes(FXVec3f & ex,FXVec3f & ey,FXVec3f & ez) const392 void FXQuatf::getAxes(FXVec3f& ex,FXVec3f& ey,FXVec3f& ez) const {
393 FXfloat tx=2.0f*x;
394 FXfloat ty=2.0f*y;
395 FXfloat tz=2.0f*z;
396 FXfloat twx=tx*w;
397 FXfloat twy=ty*w;
398 FXfloat twz=tz*w;
399 FXfloat txx=tx*x;
400 FXfloat txy=ty*x;
401 FXfloat txz=tz*x;
402 FXfloat tyy=ty*y;
403 FXfloat tyz=tz*y;
404 FXfloat tzz=tz*z;
405 ex.x=1.0f-tyy-tzz;
406 ex.y=txy+twz;
407 ex.z=txz-twy;
408 ey.x=txy-twz;
409 ey.y=1.0f-txx-tzz;
410 ey.z=tyz+twx;
411 ez.x=txz+twy;
412 ez.y=tyz-twx;
413 ez.z=1.0f-txx-tyy;
414 }
415
416
417 // Obtain local x axis
getXAxis() const418 FXVec3f FXQuatf::getXAxis() const {
419 FXfloat ty=2.0f*y;
420 FXfloat tz=2.0f*z;
421 return FXVec3f(1.0f-ty*y-tz*z,ty*x+tz*w,tz*x-ty*w);
422 }
423
424
425 // Obtain local y axis
getYAxis() const426 FXVec3f FXQuatf::getYAxis() const {
427 FXfloat tx=2.0f*x;
428 FXfloat tz=2.0f*z;
429 return FXVec3f(tx*y-tz*w,1.0f-tx*x-tz*z,tz*y+tx*w);
430 }
431
432
433 // Obtain local z axis
getZAxis() const434 FXVec3f FXQuatf::getZAxis() const {
435 FXfloat tx=2.0f*x;
436 FXfloat ty=2.0f*y;
437 return FXVec3f(tx*z+ty*w,ty*z-tx*w,1.0f-tx*x-ty*y);
438 }
439
440
441 // Exponentiate unit quaternion
442 // Given q = theta*(x*i+y*j+z*k), where length of (x,y,z) is 1,
443 // then exp(q) = sin(theta)*(x*i+y*j+z*k)+cos(theta).
exp() const444 FXQuatf FXQuatf::exp() const {
445 FXQuatf result(0.0f,0.0f,0.0f,1.0f);
446 FXfloat mag2(x*x+y*y+z*z);
447 if(0.0f<mag2){
448 FXfloat mag(Math::sqrt(mag2));
449 FXfloat s(Math::sin(mag)/mag);
450 FXfloat c(Math::cos(mag));
451 result.x=x*s;
452 result.y=y*s;
453 result.z=z*s;
454 result.w=c;
455 }
456 return result;
457 }
458
459
460 // Take logarithm of unit quaternion
461 // Given q = sin(theta)*(x*i+y*j+z*k)+cos(theta), length of (x,y,z) is 1,
462 // then log(q) = theta*(x*i+y*j+z*k).
log() const463 FXQuatf FXQuatf::log() const {
464 FXQuatf result(0.0f,0.0f,0.0f,0.0f);
465 FXfloat mag2(x*x+y*y+z*z);
466 if(0.0f<mag2){
467 FXfloat mag(Math::sqrt(mag2));
468 FXfloat phi(Math::atan2(mag,w)/mag);
469 result.x=x*phi;
470 result.y=y*phi;
471 result.z=z*phi;
472 }
473 return result;
474 }
475
476
477 // Power of quaternion
pow(FXfloat t) const478 FXQuatf FXQuatf::pow(FXfloat t) const {
479 return (log()*t).exp();
480 }
481
482
483 // Rotation unit-quaternion and vector v . q = (q . v . q*) where q* is
484 // the conjugate of q.
485 //
486 // The Rodriques Formula for rotating a vector V about a unit-axis K is:
487 //
488 // V' = K (K . V) + (K x V) sin(A) - K x (K x V) cos(A)
489 //
490 // Given UNIT quaternion q=(S,c), i.e. |q|=1, defined as a imaginary part S with
491 // |S|=K sin(A/2), where K is a unit vector, and a real part c=cos(A/2), we can obtain,
492 // after some (well, a LOT of) manipulation:
493 //
494 // V' = S (S . V) + c (c V + S x V) + S x (c V + S x V)
495 //
496 // Using:
497 //
498 // A x (B x C) = B (A . C) - C (A . B)
499 //
500 // We can further simplify:
501 //
502 // V' = V + 2 S x (S x V + c V)
503 //
operator *(const FXVec3f & v,const FXQuatf & q)504 FXVec3f operator*(const FXVec3f& v,const FXQuatf& q){
505 FXVec3f s(q.x,q.y,q.z);
506 return v+(s^((s^v)+(v*q.w)))*2.0;
507 }
508
509
510 // Rotation unit-quaternion and vector q . v = (q* . v . q)
operator *(const FXQuatf & q,const FXVec3f & v)511 FXVec3f operator*(const FXQuatf& q,const FXVec3f& v){
512 FXVec3f s(q.x,q.y,q.z);
513 return v+(((v^s)+(v*q.w))^s)*2.0; // Yes, -a^b is b^a!
514 }
515
516
517 // Construct quaternion from arc a->b on unit sphere.
518 //
519 // Explanation: a quaternion which rotates by angle theta about unit axis a
520 // is specified as:
521 //
522 // q = (a * sin(theta/2), cos(theta/2)).
523 //
524 // Assuming is f and t are unit length, we have:
525 //
526 // sin(theta) = | f x t |
527 //
528 // and
529 //
530 // cos(theta) = f . t
531 //
532 // Using sin(2 * x) = 2 * sin(x) * cos(x), we get:
533 //
534 // a * sin(theta/2) = (f x t) * sin(theta/2) / (2 * sin(theta/2) * cos(theta/2))
535 //
536 // = (f x t) / (2 * cos(theta/2))
537 //
538 // Using cos^2(x)=(1 + cos(2 * x)) / 2, we get:
539 //
540 // 4 * cos^2(theta/2) = 2 + 2 * cos(theta)
541 //
542 // = 2 + 2 * (f . t)
543 // Ergo:
544 //
545 // 2 * cos(theta/2) = sqrt(2 + 2 * (f . t))
546 //
arc(const FXVec3f & f,const FXVec3f & t)547 FXQuatf arc(const FXVec3f& f,const FXVec3f& t){
548 FXfloat dot=f.x*t.x+f.y*t.y+f.z*t.z,div;
549 FXQuatf result;
550 if(__unlikely(dot> 0.999999f)){ // Unit quaternion
551 result.x=0.0f;
552 result.y=0.0f;
553 result.z=0.0f;
554 result.w=1.0f;
555 }
556 else if(__unlikely(dot<-0.999999f)){ // 180 quaternion (Stephen Hardy)
557 if(Math::fabs(f.z)<Math::fabs(f.x) && Math::fabs(f.z)<Math::fabs(f.y)){ // x, y largest magnitude
558 result.x= f.x*f.z-f.z*f.y;
559 result.y= f.z*f.x+f.y*f.z;
560 result.z=-f.y*f.y-f.x*f.x;
561 }
562 else if(Math::fabs(f.y)<Math::fabs(f.x)){ // y, z largest magnitude
563 result.x= f.y*f.z-f.x*f.y;
564 result.y= f.x*f.x+f.z*f.z;
565 result.z=-f.z*f.y-f.y*f.x;
566 }
567 else{ // x, z largest magnitude
568 result.x=-f.z*f.z-f.y*f.y;
569 result.y= f.y*f.x-f.x*f.z;
570 result.z= f.x*f.y+f.z*f.x;
571 }
572 dot=result.x*result.x+result.y*result.y+result.z*result.z;
573 div=Math::sqrt(dot);
574 result.x/=div;
575 result.y/=div;
576 result.z/=div;
577 result.w=0.0f;
578 }
579 else{
580 div=Math::sqrt((dot+1.0f)*2.0f);
581 result.x=(f.y*t.z-f.z*t.y)/div;
582 result.y=(f.z*t.x-f.x*t.z)/div;
583 result.z=(f.x*t.y-f.y*t.x)/div;
584 result.w=div*0.5f;
585 }
586 return result;
587 }
588
589
590 // Spherical lerp of unit quaternions u,v
591 // This is equivalent to: u * (u.unitinvert()*v).pow(f)
lerp(const FXQuatf & u,const FXQuatf & v,FXfloat f)592 FXQuatf lerp(const FXQuatf& u,const FXQuatf& v,FXfloat f){
593 FXfloat dot=u.x*v.x+u.y*v.y+u.z*v.z+u.w*v.w;
594 FXfloat to=Math::fblend(dot,0.0f,-f,f);
595 FXfloat fr=1.0f-f;
596 FXfloat cost=Math::fabs(dot);
597 FXfloat sint;
598 FXfloat theta;
599 FXQuatf result;
600 if(__likely(cost<0.999999f)){
601 sint=Math::sqrt(1.0f-cost*cost);
602 theta=Math::atan2(sint,cost);
603 fr=Math::sin(fr*theta)/sint;
604 to=Math::sin(to*theta)/sint;
605 }
606 result.x=fr*u.x+to*v.x;
607 result.y=fr*u.y+to*v.y;
608 result.z=fr*u.z+to*v.z;
609 result.w=fr*u.w+to*v.w;
610 return result;
611 }
612
613
614 // Derivative of spherical lerp of unit quaternions u,v
615 // This is equivalent to: u * (u.unitinvert()*v).pow(f) * (u.unitinvert()*v).log(),
616 // which is itself equivalent to: lerp(u,v,f) * (u.unitinvert()*v).log()
lerpdot(const FXQuatf & u,const FXQuatf & v,FXfloat f)617 FXQuatf lerpdot(const FXQuatf& u,const FXQuatf& v,FXfloat f){
618 FXfloat dot=u.x*v.x+u.y*v.y+u.z*v.z+u.w*v.w;
619 FXfloat cost=Math::fabs(dot);
620 FXfloat sint;
621 FXfloat fr=1.0f-f;
622 FXfloat to=f;
623 FXfloat theta;
624 FXQuatf result;
625 if(__likely(cost<0.999999f)){
626 sint=Math::sqrt(1.0f-cost*cost);
627 theta=Math::atan2(sint,cost);
628 fr=-theta*Math::cos(fr*theta)/sint;
629 to=theta*Math::cos(to*theta)/sint;
630 }
631 result.x=fr*u.x+to*v.x;
632 result.y=fr*u.y+to*v.y;
633 result.z=fr*u.z+to*v.z;
634 result.w=fr*u.w+to*v.w;
635 return result;
636 }
637
638
639 // Cubic hermite quaternion interpolation
hermite(const FXQuatf & q0,const FXVec3f & r0,const FXQuatf & q1,const FXVec3f & r1,FXfloat t)640 FXQuatf hermite(const FXQuatf& q0,const FXVec3f& r0,const FXQuatf& q1,const FXVec3f& r1,FXfloat t){
641 FXQuatf w1(r0[0]*0.333333333333333333f,r0[1]*0.333333333333333333f,r0[2]*0.333333333333333333f,0.0f);
642 FXQuatf w3(r1[0]*0.333333333333333333f,r1[1]*0.333333333333333333f,r1[2]*0.333333333333333333f,0.0f);
643 FXQuatf w2((w1.exp().unitinvert()*q0.unitinvert()*q1*w3.exp().unitinvert()).log());
644 FXfloat t2=t*t;
645 FXfloat beta3=t2*t;
646 FXfloat beta1=3.0f*t-3.0f*t2+beta3;
647 FXfloat beta2=3.0f*t2-2.0f*beta3;
648 return q0*(w1*beta1).exp()*(w2*beta2).exp()*(w3*beta3).exp();
649 }
650
651
652 // Estimate angular body rates omega from unit quaternions Q0 and Q1 separated by time dt
653 //
654 // -1
655 // 2 * log ( Q0 * Q1 )
656 // w = ---------------------
657 // b dt
658 //
659 // Be aware that we don't know how many full revolutions happened between q0 and q1;
660 // there may be aliasing!
omegaBody(const FXQuatf & q0,const FXQuatf & q1,FXfloat dt)661 FXVec3f omegaBody(const FXQuatf& q0,const FXQuatf& q1,FXfloat dt){
662 FXVec3f omega(0.0f,0.0f,0.0f);
663 FXQuatf diff(q0.unitinvert()*q1);
664 FXfloat mag2(diff.x*diff.x+diff.y*diff.y+diff.z*diff.z);
665 if(0.0f<mag2){
666 FXfloat mag(Math::sqrt(mag2));
667 FXfloat phi(2.0f*Math::atan2(mag,diff.w));
668 FXfloat s(Math::wrap(phi)/(mag*dt)); // Wrap angle -PI*2...PI*2 to -PI...PI range
669 omega.x=diff.x*s;
670 omega.y=diff.y*s;
671 omega.z=diff.z*s;
672 }
673 return omega;
674 }
675
676
677 // Derivative q' of orientation quaternion q with angular body rates omega (rad/s)
678 //
679 // .
680 // Q = 0.5 * Q * w
681 //
quatDot(const FXQuatf & q,const FXVec3f & omega)682 FXQuatf quatDot(const FXQuatf& q,const FXVec3f& omega){
683 return FXQuatf( 0.5f*(omega.x*q.w-omega.y*q.z+omega.z*q.y),
684 0.5f*(omega.x*q.z+omega.y*q.w-omega.z*q.x),
685 0.5f*(omega.y*q.x+omega.z*q.w-omega.x*q.y),
686 -0.5f*(omega.x*q.x+omega.y*q.y+omega.z*q.z));
687 }
688
689
690
691
692 // Calculate angular acceleration of a body with inertial moments tensor M
693 // Rotationg about its axes with angular rates omega, under a torque torq.
694 // Formula is:
695 //
696 // . -1
697 // w = M * ( T - w x ( M * w )
698 // b b b
699 //
700 // Where M is inertia tensor:
701 //
702 // ( Ixx Ixy Ixz ) T
703 // M = ( Iyx Iyy Iyz ) , with Ixy == Iyz, Ixz == Izx, Iyz == Izy, i.e. M == M
704 // ( Izx Izy Izz )
705 //
706 // In the unlikely case that M is singular, return angular acceleration of 0.
omegaDot(const FXMat3f & M,const FXVec3f & omega,const FXVec3f & torq)707 FXVec3f omegaDot(const FXMat3f& M,const FXVec3f& omega,const FXVec3f& torq){
708 FXVec3f result(0.0f,0.0f,0.0f);
709 FXfloat Ixx=M[0][0];
710 FXfloat Ixy=M[0][1];
711 FXfloat Ixz=M[0][2];
712 FXfloat Iyy=M[1][1];
713 FXfloat Iyz=M[1][2];
714 FXfloat Izz=M[2][2];
715 FXfloat m00=Iyy*Izz-Iyz*Iyz; // Cofactors of M
716 FXfloat m01=Ixz*Iyz-Ixy*Izz;
717 FXfloat m02=Ixy*Iyz-Ixz*Iyy;
718 FXfloat m11=Ixx*Izz-Ixz*Ixz;
719 FXfloat m12=Ixy*Ixz-Ixx*Iyz;
720 FXfloat m22=Ixx*Iyy-Ixy*Ixy;
721 FXfloat det=Ixx*m00+Ixy*m01+Ixz*m02;
722 FXASSERT(M[0][1]==M[1][0]);
723 FXASSERT(M[0][2]==M[2][0]);
724 FXASSERT(M[1][2]==M[2][1]);
725 if(__likely(det!=0.0f)){ // Check if M is singular
726 FXfloat ox=omega.x;
727 FXfloat oy=omega.y;
728 FXfloat oz=omega.z;
729 FXfloat mox=Ixx*ox+Ixy*oy+Ixz*oz; // M * w
730 FXfloat moy=Ixy*ox+Iyy*oy+Iyz*oz;
731 FXfloat moz=Ixz*ox+Iyz*oy+Izz*oz;
732 FXfloat rhx=torq.x-(oy*moz-oz*moy); // R = T - w x (M * w)
733 FXfloat rhy=torq.y-(oz*mox-ox*moz);
734 FXfloat rhz=torq.z-(ox*moy-oy*mox);
735 result.x=(m00*rhx+m01*rhy+m02*rhz)/det; // -1
736 result.y=(m01*rhx+m11*rhy+m12*rhz)/det; // M * R
737 result.z=(m02*rhx+m12*rhy+m22*rhz)/det; //
738 }
739 return result;
740 }
741
742
743 // Save vector to stream
operator <<(FXStream & store,const FXQuatf & v)744 FXStream& operator<<(FXStream& store,const FXQuatf& v){
745 store << v.x << v.y << v.z << v.w;
746 return store;
747 }
748
749 // Load vector from stream
operator >>(FXStream & store,FXQuatf & v)750 FXStream& operator>>(FXStream& store,FXQuatf& v){
751 store >> v.x >> v.y >> v.z >> v.w;
752 return store;
753 }
754
755 }
756
757