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,2006 by Jeroen van der Zijp. All Rights Reserved. *
7 *********************************************************************************
8 * This library is free software; you can redistribute it and/or *
9 * modify it under the terms of the GNU Lesser General Public *
10 * License as published by the Free Software Foundation; either *
11 * version 2.1 of the License, or (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 GNU *
16 * Lesser General Public License for more details. *
17 * *
18 * You should have received a copy of the GNU Lesser General Public *
19 * License along with this library; if not, write to the Free Software *
20 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. *
21 *********************************************************************************
22 * $Id: FXQuatf.cpp,v 1.31 2006/01/22 17:58:37 fox Exp $ *
23 ********************************************************************************/
24 #include "xincs.h"
25 #include "fxver.h"
26 #include "fxdefs.h"
27 #include "FXHash.h"
28 #include "FXStream.h"
29 #include "FXObject.h"
30 #include "FXVec2f.h"
31 #include "FXVec3f.h"
32 #include "FXVec4f.h"
33 #include "FXQuatf.h"
34 #include "FXMat3f.h"
35
36
37 using namespace FX;
38
39 /*******************************************************************************/
40
41 namespace FX {
42
43 // Construct from angle and axis
FXQuatf(const FXVec3f & axis,FXfloat phi)44 FXQuatf::FXQuatf(const FXVec3f& axis,FXfloat phi){
45 setAxisAngle(axis,phi);
46 }
47
48
49 // Construct from roll, pitch, yaw
FXQuatf(FXfloat roll,FXfloat pitch,FXfloat yaw)50 FXQuatf::FXQuatf(FXfloat roll,FXfloat pitch,FXfloat yaw){
51 setRollPitchYaw(roll,pitch,yaw);
52 }
53
54
55 // Construct quaternion from two unit vectors
FXQuatf(const FXVec3f & fr,const FXVec3f & to)56 FXQuatf::FXQuatf(const FXVec3f& fr,const FXVec3f& to){
57 arc(fr,to);
58 }
59
60
61 // Construct quaternion from axes
FXQuatf(const FXVec3f & ex,const FXVec3f & ey,const FXVec3f & ez)62 FXQuatf::FXQuatf(const FXVec3f& ex,const FXVec3f& ey,const FXVec3f& ez){
63 setAxes(ex,ey,ez);
64 }
65
66
67 // Construct quaternion from 3x3 matrix
FXQuatf(const FXMat3f & mat)68 FXQuatf::FXQuatf(const FXMat3f& mat){
69 setAxes(mat[0],mat[1],mat[2]);
70 }
71
72
73 // Adjust quaternion length
adjust()74 FXQuatf& FXQuatf::adjust(){
75 register FXfloat t=x*x+y*y+z*z+w*w;
76 register FXfloat f;
77 if(t>0.0f){
78 f=1.0f/sqrtf(t);
79 x*=f;
80 y*=f;
81 z*=f;
82 w*=f;
83 }
84 return *this;
85 }
86
87
88 // Set axis and angle
setAxisAngle(const FXVec3f & axis,FXfloat phi)89 void FXQuatf::setAxisAngle(const FXVec3f& axis,FXfloat phi){
90 register FXfloat a=0.5f*phi;
91 register FXfloat s=sinf(a)/axis.length();
92 x=axis.x*s;
93 y=axis.y*s;
94 z=axis.z*s;
95 w=cosf(a);
96 }
97
98
99 // Obtain axis and angle
100 // Remeber that: q = sin(A/2)*(x*i+y*j+z*k)+cos(A/2)
101 // for unit quaternion |q| == 1
getAxisAngle(FXVec3f & axis,FXfloat & phi) const102 void FXQuatf::getAxisAngle(FXVec3f& axis,FXfloat& phi) const {
103 register FXfloat n=sqrtf(x*x+y*y+z*z);
104 if(n>0.0f){
105 axis.x=x/n;
106 axis.y=y/n;
107 axis.z=z/n;
108 phi=2.0f*acosf(w);
109 }
110 else{
111 axis.x=1.0f;
112 axis.y=0.0f;
113 axis.z=0.0f;
114 phi=0.0f;
115 }
116 }
117
118
119 // Set quaternion from roll (x), pitch (y), yaw (z)
setRollPitchYaw(FXfloat roll,FXfloat pitch,FXfloat yaw)120 void FXQuatf::setRollPitchYaw(FXfloat roll,FXfloat pitch,FXfloat yaw){
121 register FXfloat sr,cr,sp,cp,sy,cy;
122 register FXfloat rr=0.5f*roll;
123 register FXfloat pp=0.5f*pitch;
124 register FXfloat yy=0.5f*yaw;
125 sr=sinf(rr); cr=cosf(rr);
126 sp=sinf(pp); cp=cosf(pp);
127 sy=sinf(yy); cy=cosf(yy);
128 x=sr*cp*cy-cr*sp*sy;
129 y=cr*sp*cy+sr*cp*sy;
130 z=cr*cp*sy-sr*sp*cy;
131 w=cr*cp*cy+sr*sp*sy;
132 }
133
134
135 // Set quaternion from yaw (z), pitch (y), roll (x)
setYawPitchRoll(FXfloat yaw,FXfloat pitch,FXfloat roll)136 void FXQuatf::setYawPitchRoll(FXfloat yaw,FXfloat pitch,FXfloat roll){
137 register FXfloat sr,cr,sp,cp,sy,cy;
138 register FXfloat rr=0.5f*roll;
139 register FXfloat pp=0.5f*pitch;
140 register FXfloat yy=0.5f*yaw;
141 sr=sinf(rr); cr=cosf(rr);
142 sp=sinf(pp); cp=cosf(pp);
143 sy=sinf(yy); cy=cosf(yy);
144 x=sr*cp*cy+cr*sp*sy;
145 y=cr*sp*cy-sr*cp*sy;
146 z=cr*cp*sy+sr*sp*cy;
147 w=cr*cp*cy-sr*sp*sy;
148 }
149
150
151 // Set quaternion from roll (x), yaw (z), pitch (y)
setRollYawPitch(FXfloat roll,FXfloat yaw,FXfloat pitch)152 void FXQuatf::setRollYawPitch(FXfloat roll,FXfloat yaw,FXfloat pitch){
153 register FXfloat sr,cr,sp,cp,sy,cy;
154 register FXfloat rr=0.5f*roll;
155 register FXfloat pp=0.5f*pitch;
156 register FXfloat yy=0.5f*yaw;
157 sr=sinf(rr); cr=cosf(rr);
158 sp=sinf(pp); cp=cosf(pp);
159 sy=sinf(yy); cy=cosf(yy);
160 x=cp*cy*sr+sp*sy*cr;
161 y=sp*cy*cr+cp*sy*sr;
162 z=cp*sy*cr-sp*cy*sr;
163 w=cp*cy*cr-sp*sy*sr;
164 }
165
166
167 // Set quaternion from pitch (y), roll (x),yaw (z)
setPitchRollYaw(FXfloat pitch,FXfloat roll,FXfloat yaw)168 void FXQuatf::setPitchRollYaw(FXfloat pitch,FXfloat roll,FXfloat yaw){
169 register FXfloat sr,cr,sp,cp,sy,cy;
170 register FXfloat rr=0.5f*roll;
171 register FXfloat pp=0.5f*pitch;
172 register FXfloat yy=0.5f*yaw;
173 sr=sinf(rr); cr=cosf(rr);
174 sp=sinf(pp); cp=cosf(pp);
175 sy=sinf(yy); cy=cosf(yy);
176 x=cy*sr*cp-sy*cr*sp;
177 y=cy*cr*sp+sy*sr*cp;
178 z=cy*sr*sp+sy*cr*cp;
179 w=cy*cr*cp-sy*sr*sp;
180 }
181
182
183 // Set quaternion from pitch (y), yaw (z), roll (x)
setPitchYawRoll(FXfloat pitch,FXfloat yaw,FXfloat roll)184 void FXQuatf::setPitchYawRoll(FXfloat pitch,FXfloat yaw,FXfloat roll){
185 register FXfloat sr,cr,sp,cp,sy,cy;
186 register FXfloat rr=0.5f*roll;
187 register FXfloat pp=0.5f*pitch;
188 register FXfloat yy=0.5f*yaw;
189 sr=sinf(rr); cr=cosf(rr);
190 sp=sinf(pp); cp=cosf(pp);
191 sy=sinf(yy); cy=cosf(yy);
192 x=sr*cy*cp-cr*sy*sp;
193 y=cr*cy*sp-sr*sy*cp;
194 z=sr*cy*sp+cr*sy*cp;
195 w=cr*cy*cp+sr*sy*sp;
196 }
197
198
199 // Set quaternion from yaw (z), roll (x), pitch (y)
setYawRollPitch(FXfloat yaw,FXfloat roll,FXfloat pitch)200 void FXQuatf::setYawRollPitch(FXfloat yaw,FXfloat roll,FXfloat pitch){
201 register FXfloat sr,cr,sp,cp,sy,cy;
202 register FXfloat rr=0.5f*roll;
203 register FXfloat pp=0.5f*pitch;
204 register FXfloat yy=0.5f*yaw;
205 sr=sinf(rr); cr=cosf(rr);
206 sp=sinf(pp); cp=cosf(pp);
207 sy=sinf(yy); cy=cosf(yy);
208 x=cp*sr*cy+sp*cr*sy;
209 y=sp*cr*cy-cp*sr*sy;
210 z=cp*cr*sy-sp*sr*cy;
211 w=cp*cr*cy+sp*sr*sy;
212 }
213
214
215
216 // Obtain roll, pitch, yaw
217 // Math is from "3D Game Engine Design" by David Eberly pp 19-20.
218 // However, instead of testing asin(Sy) against -PI/2 and PI/2, I
219 // test Sy against -1 and 1; this is numerically more stable, as
220 // asin doesn't like arguments outside [-1,1].
getRollPitchYaw(FXfloat & roll,FXfloat & pitch,FXfloat & yaw) const221 void FXQuatf::getRollPitchYaw(FXfloat& roll,FXfloat& pitch,FXfloat& yaw) const {
222 register FXfloat s=-2.0f*(x*z-w*y);
223 if(s<1.0f){
224 if(-1.0f<s){
225 roll=atan2f(2.0f*(y*z+w*x),1.0f-2.0f*(x*x+y*y));
226 pitch=asinf(s);
227 yaw=atan2f(2.0f*(x*y+w*z),1.0f-2.0f*(y*y+z*z));
228 }
229 else{
230 roll=0.0f;
231 pitch=-1.57079632679489661923f;
232 yaw=-atan2f(-2.0f*(x*y-w*z),2.0f*(x*z+w*y));
233 }
234 }
235 else{
236 roll=0.0f;
237 pitch=1.57079632679489661923f;
238 yaw=atan2f(-2.0f*(x*y-w*z),2.0f*(x*z+w*y));
239 }
240 }
241
242
243 // Obtain yaw, pitch, and roll
getYawPitchRoll(FXfloat & yaw,FXfloat & pitch,FXfloat & roll) const244 void FXQuatf::getYawPitchRoll(FXfloat& yaw,FXfloat& pitch,FXfloat& roll) const {
245 register FXfloat s=2.0f*(x*z+w*y);
246 if(s<1.0f){
247 if(-1.0f<s){
248 yaw=atan2f(-2.0f*(x*y-w*z),1.0f-2.0f*(y*y+z*z));
249 pitch=asinf(s);
250 roll=atan2f(-2.0f*(y*z-w*x),1.0f-2.0f*(x*x+y*y));
251 }
252 else{
253 yaw=0.0f;
254 pitch=-1.57079632679489661923f;
255 roll=-atan2f(2.0f*(x*y+w*z),1.0f-2.0f*(x*x+z*z));
256 }
257 }
258 else{
259 yaw=0.0f;
260 pitch=1.57079632679489661923f;
261 roll=atan2f(2.0f*(x*y+w*z),1.0f-2.0f*(x*x+z*z));
262 }
263 }
264
265
266 // Obtain roll, yaw, pitch
getRollYawPitch(FXfloat & roll,FXfloat & yaw,FXfloat & pitch) const267 void FXQuatf::getRollYawPitch(FXfloat& roll,FXfloat& yaw,FXfloat& pitch) const {
268 register FXfloat s=2.0f*(x*y+w*z);
269 if(s<1.0f){
270 if(-1.0f<s){
271 roll=atan2f(-2.0f*(y*z-w*x),1.0f-2.0f*(x*x+z*z));
272 yaw=asinf(s);
273 pitch=atan2f(-2.0f*(x*z-w*y),1.0f-2.0f*(y*y+z*z));
274 }
275 else{
276 roll=0.0f;
277 yaw=-1.57079632679489661923f;
278 pitch=-atan2f(2.0f*(y*z+w*x),1.0f-2.0f*(x*x+y*y));
279 }
280 }
281 else{
282 roll=0.0f;
283 yaw=1.57079632679489661923f;
284 pitch=atan2f(2.0f*(y*z+w*x),1.0f-2.0f*(x*x+y*y));
285 }
286 }
287
288
289 // Obtain pitch, roll, yaw
getPitchRollYaw(FXfloat & pitch,FXfloat & roll,FXfloat & yaw) const290 void FXQuatf::getPitchRollYaw(FXfloat& pitch,FXfloat& roll,FXfloat& yaw) const {
291 register FXfloat s=2.0f*(y*z+w*x);
292 if(s<1.0f){
293 if(-1.0f<s){
294 pitch=atan2f(-2.0f*(x*z-w*y),1.0f-2.0f*(x*x+y*y));
295 roll=asinf(s);
296 yaw=atan2f(-2.0f*(x*y-w*z),1.0f-2.0f*(x*x+z*z));
297 }
298 else{
299 pitch=0.0f;
300 roll=-1.57079632679489661923f;
301 yaw=-atan2f(2.0f*(x*z+w*y),1.0f-2.0f*(y*y+z*z));
302 }
303 }
304 else{
305 pitch=0.0f;
306 roll=1.57079632679489661923f;
307 yaw=atan2f(2.0f*(x*z+w*y),1.0f-2.0f*(y*y+z*z));
308 }
309 }
310
311
312 // Obtain pitch, yaw, roll
getPitchYawRoll(FXfloat & pitch,FXfloat & yaw,FXfloat & roll) const313 void FXQuatf::getPitchYawRoll(FXfloat& pitch,FXfloat& yaw,FXfloat& roll) const {
314 register FXfloat s=-2.0f*(x*y-w*z);
315 if(s<1.0f){
316 if(-1.0f<s){
317 pitch=atan2f(2.0f*(x*z+w*y),1.0f-2.0f*(y*y+z*z));
318 yaw=asinf(s);
319 roll=atan2f(2.0f*(y*z+w*x),1.0f-2.0f*(x*x+z*z));
320 }
321 else{
322 pitch=0.0f;
323 yaw=-1.57079632679489661923f;
324 roll=-atan2f(-2.0f*(x*z-w*y),1.0f-2.0f*(x*x+y*y));
325 }
326 }
327 else{
328 pitch=0.0f;
329 yaw=1.57079632679489661923f;
330 roll=atan2f(-2.0f*(x*z-w*y),1.0f-2.0f*(x*x+y*y));
331 }
332 }
333
334
335 // Obtain yaw, roll, pitch
getYawRollPitch(FXfloat & yaw,FXfloat & roll,FXfloat & pitch) const336 void FXQuatf::getYawRollPitch(FXfloat& yaw,FXfloat& roll,FXfloat& pitch) const {
337 register FXfloat s=-2.0f*(y*z-w*x);
338 if(s<1.0f){
339 if(-1.0f<s){
340 yaw=atan2f(2.0f*(x*y+w*z),1.0f-2.0f*(x*x+z*z));
341 roll=asinf(s);
342 pitch=atan2f(2.0f*(x*z+w*y),1.0f-2.0f*(x*x+y*y));
343 }
344 else{
345 yaw=0.0f;
346 roll=-1.57079632679489661923f;
347 pitch=-atan2f(-2.0f*(x*y-w*z),1.0f-2.0f*(y*y+z*z));
348 }
349 }
350 else{
351 yaw=0.0f;
352 roll=1.57079632679489661923f;
353 pitch=atan2f(-2.0f*(x*y-w*z),1.0f-2.0f*(y*y+z*z));
354 }
355 }
356
357
358 // Set quaternion from axes
setAxes(const FXVec3f & ex,const FXVec3f & ey,const FXVec3f & ez)359 void FXQuatf::setAxes(const FXVec3f& ex,const FXVec3f& ey,const FXVec3f& ez){
360 register FXfloat trace=ex.x+ey.y+ez.z;
361 register FXfloat scale;
362 if(trace>0.0f){
363 scale=sqrtf(1.0f+trace);
364 w=0.5f*scale;
365 scale=0.5f/scale;
366 x=(ey.z-ez.y)*scale;
367 y=(ez.x-ex.z)*scale;
368 z=(ex.y-ey.x)*scale;
369 }
370 else if(ex.x>ey.y && ex.x>ez.z){
371 scale=2.0f*sqrtf(1.0f+ex.x-ey.y-ez.z);
372 x=0.25f*scale;
373 y=(ex.y+ey.x)/scale;
374 z=(ex.z+ez.x)/scale;
375 w=(ey.z-ez.y)/scale;
376 }
377 else if(ey.y>ez.z){
378 scale=2.0f*sqrtf(1.0f+ey.y-ex.x-ez.z);
379 y=0.25f*scale;
380 x=(ex.y+ey.x)/scale;
381 z=(ey.z+ez.y)/scale;
382 w=(ez.x-ex.z)/scale;
383 }
384 else{
385 scale=2.0f*sqrtf(1.0f+ez.z-ex.x-ey.y);
386 z=0.25f*scale;
387 x=(ex.z+ez.x)/scale;
388 y=(ey.z+ez.y)/scale;
389 w=(ex.y-ey.x)/scale;
390 }
391 }
392
393
394 // Get quaternion axes
getAxes(FXVec3f & ex,FXVec3f & ey,FXVec3f & ez) const395 void FXQuatf::getAxes(FXVec3f& ex,FXVec3f& ey,FXVec3f& ez) const {
396 register FXfloat tx=2.0f*x;
397 register FXfloat ty=2.0f*y;
398 register FXfloat tz=2.0f*z;
399 register FXfloat twx=tx*w;
400 register FXfloat twy=ty*w;
401 register FXfloat twz=tz*w;
402 register FXfloat txx=tx*x;
403 register FXfloat txy=ty*x;
404 register FXfloat txz=tz*x;
405 register FXfloat tyy=ty*y;
406 register FXfloat tyz=tz*y;
407 register FXfloat tzz=tz*z;
408 ex.x=1.0f-tyy-tzz;
409 ex.y=txy+twz;
410 ex.z=txz-twy;
411 ey.x=txy-twz;
412 ey.y=1.0f-txx-tzz;
413 ey.z=tyz+twx;
414 ez.x=txz+twy;
415 ez.y=tyz-twx;
416 ez.z=1.0f-txx-tyy;
417 }
418
419
420 // Obtain local x axis
getXAxis() const421 FXVec3f FXQuatf::getXAxis() const {
422 register FXfloat ty=2.0f*y;
423 register FXfloat tz=2.0f*z;
424 return FXVec3f(1.0f-ty*y-tz*z,ty*x+tz*w,tz*x-ty*w);
425 }
426
427
428 // Obtain local y axis
getYAxis() const429 FXVec3f FXQuatf::getYAxis() const {
430 register FXfloat tx=2.0f*x;
431 register FXfloat tz=2.0f*z;
432 return FXVec3f(tx*y-tz*w,1.0f-tx*x-tz*z,tz*y+tx*w);
433 }
434
435
436 // Obtain local z axis
getZAxis() const437 FXVec3f FXQuatf::getZAxis() const {
438 register FXfloat tx=2.0f*x;
439 register FXfloat ty=2.0f*y;
440 return FXVec3f(tx*z+ty*w,ty*z-tx*w,1.0f-tx*x-ty*y);
441 }
442
443
444 // Exponentiate unit quaternion
445 // Given q = theta*(x*i+y*j+z*k), where length of (x,y,z) is 1,
446 // then exp(q) = sin(theta)*(x*i+y*j+z*k)+cos(theta).
exp() const447 FXQuatf FXQuatf::exp() const {
448 register FXfloat theta=sqrtf(x*x+y*y+z*z);
449 register FXfloat scale;
450 FXQuatf result(x,y,z,cosf(theta));
451 if(theta>0.000001f){
452 scale=sinf(theta)/theta;
453 result.x*=scale;
454 result.y*=scale;
455 result.z*=scale;
456 }
457 return result;
458 }
459
460
461 // Take logarithm of unit quaternion
462 // Given q = sin(theta)*(x*i+y*j+z*k)+cos(theta), length of (x,y,z) is 1,
463 // then log(q) = theta*(x*i+y*j+z*k).
log() const464 FXQuatf FXQuatf::log() const {
465 register FXfloat scale=sqrtf(x*x+y*y+z*z);
466 register FXfloat theta=atan2f(scale,w);
467 FXQuatf result(x,y,z,0.0f);
468 if(scale>0.0f){
469 scale=theta/scale;
470 result.x*=scale;
471 result.y*=scale;
472 result.z*=scale;
473 }
474 return result;
475 }
476
477
478 // Invert quaternion
invert() const479 FXQuatf FXQuatf::invert() const {
480 register FXfloat n=x*x+y*y+z*z+w*w;
481 return FXQuatf(-x/n,-y/n,-z/n,w/n);
482 }
483
484
485 // Invert unit quaternion
unitinvert() const486 FXQuatf FXQuatf::unitinvert() const {
487 return FXQuatf(-x,-y,-z,w);
488 }
489
490
491 // Conjugate quaternion
conj() const492 FXQuatf FXQuatf::conj() const {
493 return FXQuatf(-x,-y,-z,w);
494 }
495
496
497 // Construct quaternion from arc a->b on unit sphere.
498 //
499 // Explanation: a quaternion which rotates by angle theta about unit axis a
500 // is specified as:
501 //
502 // q = (a * sin(theta/2), cos(theta/2)).
503 //
504 // Assuming is f and t are unit length, we have:
505 //
506 // sin(theta) = | f x t |
507 //
508 // and
509 //
510 // cos(theta) = f . t
511 //
512 // Using sin(2 * x) = 2 * sin(x) * cos(x), we get:
513 //
514 // a * sin(theta/2) = (f x t) * sin(theta/2) / (2 * sin(theta/2) * cos(theta/2))
515 //
516 // = (f x t) / (2 * cos(theta/2))
517 //
518 // Using cos^2(x)=(1 + cos(2 * x)) / 2, we get:
519 //
520 // 4 * cos^2(theta/2) = 2 + 2 * cos(theta)
521 //
522 // = 2 + 2 * (f . t)
523 // Ergo:
524 //
525 // 2 * cos(theta/2) = sqrt(2 + 2 * (f . t))
526 //
arc(const FXVec3f & f,const FXVec3f & t)527 FXQuatf& FXQuatf::arc(const FXVec3f& f,const FXVec3f& t){
528 register FXfloat dot=f.x*t.x+f.y*t.y+f.z*t.z,div;
529 if(dot> 0.999999f){ // Unit quaternion
530 x= 0.0f;
531 y= 0.0f;
532 z= 0.0f;
533 w= 1.0f;
534 }
535 else if(dot<-0.999999f){ // 180 quaternion
536 if(fabsf(f.z)<fabsf(f.x) && fabsf(f.z)<fabs(f.y)){ // x, y largest magnitude
537 x= f.x*f.z-f.z*f.y;
538 y= f.z*f.x+f.y*f.z;
539 z=-f.y*f.y-f.x*f.x;
540 }
541 else if(fabsf(f.y)<fabsf(f.x)){ // y, z largest magnitude
542 x= f.y*f.z-f.x*f.y;
543 y= f.x*f.x+f.z*f.z;
544 z=-f.z*f.y-f.y*f.x;
545 }
546 else{ // x, z largest magnitude
547 x=-f.z*f.z-f.y*f.y;
548 y= f.y*f.x-f.x*f.z;
549 z= f.x*f.y+f.z*f.x;
550 }
551 dot=x*x+y*y+z*z;
552 div=sqrtf(dot);
553 x/=div;
554 y/=div;
555 z/=div;
556 w=0.0f;
557 }
558 else{
559 div=sqrtf((dot+1.0f)*2.0f);
560 x=(f.y*t.z-f.z*t.y)/div;
561 y=(f.z*t.x-f.x*t.z)/div;
562 z=(f.x*t.y-f.y*t.x)/div;
563 w=div*0.5f;
564 }
565 return *this;
566 }
567
568
569 // Spherical lerp
lerp(const FXQuatf & u,const FXQuatf & v,FXfloat f)570 FXQuatf& FXQuatf::lerp(const FXQuatf& u,const FXQuatf& v,FXfloat f){
571 register FXfloat alpha,beta,theta,sin_t,cos_t;
572 register FXint flip=0;
573 cos_t = u.x*v.x+u.y*v.y+u.z*v.z+u.w*v.w;
574 if(cos_t<0.0f){ cos_t = -cos_t; flip=1; }
575 if((1.0f-cos_t)<0.000001f){
576 beta = 1.0f-f;
577 alpha = f;
578 }
579 else{
580 theta = acosf(cos_t);
581 sin_t = sinf(theta);
582 beta = sinf(theta-f*theta)/sin_t;
583 alpha = sinf(f*theta)/sin_t;
584 }
585 if(flip) alpha = -alpha;
586 x=beta*u.x+alpha*v.x;
587 y=beta*u.y+alpha*v.y;
588 z=beta*u.z+alpha*v.z;
589 w=beta*u.w+alpha*v.w;
590 return *this;
591 }
592
593
594 // Multiply quaternions
operator *(const FXQuatf & q) const595 FXQuatf FXQuatf::operator*(const FXQuatf& q) const {
596 return FXQuatf(w*q.x+x*q.w+y*q.z-z*q.y, w*q.y+y*q.w+z*q.x-x*q.z, w*q.z+z*q.w+x*q.y-y*q.x, w*q.w-x*q.x-y*q.y-z*q.z);
597 }
598
599
600 /*
601
602 According to someone on gdalgorithms list, this is faster:
603
604 V' = V - 2 * cross( cross( q.xyz, V ) - q.w * V ), q.xyz )
605
606 ==
607 v' = q.v.q*
608
609 expand to get:
610
611 v' = (s.v)s + w(wv + s x v) + s x (wv + s x v)
612
613 Where s is the vector part of the quaternion and w is the scalar part (q = (w, s))
614
615 Expanding this out further gives:
616
617 v' = (s.v)s + (w^2)v + 2w(s x v) + s x (s x v)
618
619 Use the vector triple product identity:
620
621 a x (b x c) = b(a.c) - c(a.b)
622
623 To give
624
625 v' + s(s.v) - v(s.s) = (s.v)s + (w^2)v + 2w(s x v) + 2(s x (s x v))
626
627 Cancelling to:
628
629 v' = (w^2 + s.s)v + 2(s x (s x v + wv))
630
631 From the definition of a quaternion representing a rotation,
632 w = cos(a/2), s = sin(a/2)*r where r is a unit vector representing the
633 axis of the rotation. So (w^2 + s.s) = (cos(a/2))^2 + (sin(a/2))^2 = 1,
634 giving us the final formula:
635
636 v' = v + 2(s x (s x v + vw))
637 */
638
639
640 // Rotation of a vector by a quaternion; this is defined as q.v.q*
641 // where q* is the conjugate of q.
operator *(const FXVec3f & v) const642 FXVec3f FXQuatf::operator*(const FXVec3f& v) const {
643 return v*FXMat3f(*this);
644 }
645
646
647 }
648
649