Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

OgreQuaternion.cpp @ 9926

Last change on this file since 9926 was 7908, checked in by rgrieder, 14 years ago
Stripped down trunk to form a new light sandbox.
Property svn:eol-style set to `native`
File size: 19.4 KB

Line
1	/*
2	-----------------------------------------------------------------------------
3	This source file is part of OGRE
4	(Object-oriented Graphics Rendering Engine)
5	For the latest info, see http://www.ogre3d.org/
6
7	Copyright (c) 2000-2006 Torus Knot Software Ltd
8	Also see acknowledgements in Readme.html
9
10	This program is free software; you can redistribute it and/or modify it under
11	the terms of the GNU Lesser General Public License as published by the Free Software
12	Foundation; either version 2 of the License, or (at your option) any later
13	version.
14
15	This program is distributed in the hope that it will be useful, but WITHOUT
16	ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17	FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
18
19	You should have received a copy of the GNU Lesser General Public License along with
20	this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21	Place - Suite 330, Boston, MA 02111-1307, USA, or go to
22	http://www.gnu.org/copyleft/lesser.txt.
23
24	You may alternatively use this source under the terms of a specific version of
25	the OGRE Unrestricted License provided you have obtained such a license from
26	Torus Knot Software Ltd.
27	-----------------------------------------------------------------------------
28	*/
29	// NOTE THAT THIS FILE IS BASED ON MATERIAL FROM:
30
31	// Magic Software, Inc.
32	// http://www.geometrictools.com
33	// Copyright (c) 2000, All Rights Reserved
34	//
35	// Source code from Magic Software is supplied under the terms of a license
36	// agreement and may not be copied or disclosed except in accordance with the
37	// terms of that agreement. The various license agreements may be found at
38	// the Magic Software web site. This file is subject to the license
39	//
40	// FREE SOURCE CODE
41	// http://www.geometrictools.com/License/WildMagic3License.pdf
42
43	#include "OgreQuaternion.h"
44
45	#include "OgreMath.h"
46	#include "OgreMatrix3.h"
47	#include "OgreVector3.h"
48
49	namespace Ogre {
50
51	const Real Quaternion::ms_fEpsilon = 1e-03;
52	const Quaternion Quaternion::ZERO(0.0,0.0,0.0,0.0);
53	const Quaternion Quaternion::IDENTITY(1.0,0.0,0.0,0.0);
54
55	//-----------------------------------------------------------------------
56	void Quaternion::FromRotationMatrix (const Matrix3& kRot)
57	{
58	// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
59	// article "Quaternion Calculus and Fast Animation".
60
61	Real fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
62	Real fRoot;
63
64	if ( fTrace > 0.0 )
65	{
66	// \|w\| > 1/2, may as well choose w > 1/2
67	fRoot = Math::Sqrt(fTrace + 1.0); // 2w
68	w = 0.5*fRoot;
69	fRoot = 0.5/fRoot; // 1/(4w)
70	x = (kRot[2][1]-kRot[1][2])*fRoot;
71	y = (kRot[0][2]-kRot[2][0])*fRoot;
72	z = (kRot[1][0]-kRot[0][1])*fRoot;
73	}
74	else
75	{
76	// \|w\| <= 1/2
77	static size_t s_iNext[3] = { 1, 2, 0 };
78	size_t i = 0;
79	if ( kRot[1][1] > kRot[0][0] )
80	i = 1;
81	if ( kRot[2][2] > kRot[i][i] )
82	i = 2;
83	size_t j = s_iNext[i];
84	size_t k = s_iNext[j];
85
86	fRoot = Math::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0);
87	Real* apkQuat[3] = { &x, &y, &z };
88	apkQuat[i] = 0.5fRoot;
89	fRoot = 0.5/fRoot;
90	w = (kRot[k][j]-kRot[j][k])*fRoot;
91	apkQuat[j] = (kRot[j][i]+kRot[i][j])fRoot;
92	apkQuat[k] = (kRot[k][i]+kRot[i][k])fRoot;
93	}
94	}
95	//-----------------------------------------------------------------------
96	void Quaternion::ToRotationMatrix (Matrix3& kRot) const
97	{
98	Real fTx = 2.0*x;
99	Real fTy = 2.0*y;
100	Real fTz = 2.0*z;
101	Real fTwx = fTx*w;
102	Real fTwy = fTy*w;
103	Real fTwz = fTz*w;
104	Real fTxx = fTx*x;
105	Real fTxy = fTy*x;
106	Real fTxz = fTz*x;
107	Real fTyy = fTy*y;
108	Real fTyz = fTz*y;
109	Real fTzz = fTz*z;
110
111	kRot[0][0] = 1.0-(fTyy+fTzz);
112	kRot[0][1] = fTxy-fTwz;
113	kRot[0][2] = fTxz+fTwy;
114	kRot[1][0] = fTxy+fTwz;
115	kRot[1][1] = 1.0-(fTxx+fTzz);
116	kRot[1][2] = fTyz-fTwx;
117	kRot[2][0] = fTxz-fTwy;
118	kRot[2][1] = fTyz+fTwx;
119	kRot[2][2] = 1.0-(fTxx+fTyy);
120	}
121	//-----------------------------------------------------------------------
122	void Quaternion::FromAngleAxis (const Radian& rfAngle,
123	const Vector3& rkAxis)
124	{
125	// assert: axis[] is unit length
126	//
127	// The quaternion representing the rotation is
128	// q = cos(A/2)+sin(A/2)(xi+yj+zk)
129
130	Radian fHalfAngle ( 0.5*rfAngle );
131	Real fSin = Math::Sin(fHalfAngle);
132	w = Math::Cos(fHalfAngle);
133	x = fSin*rkAxis.x;
134	y = fSin*rkAxis.y;
135	z = fSin*rkAxis.z;
136	}
137	//-----------------------------------------------------------------------
138	void Quaternion::ToAngleAxis (Radian& rfAngle, Vector3& rkAxis) const
139	{
140	// The quaternion representing the rotation is
141	// q = cos(A/2)+sin(A/2)(xi+yj+zk)
142
143	Real fSqrLength = xx+yy+z*z;
144	if ( fSqrLength > 0.0 )
145	{
146	rfAngle = 2.0*Math::ACos(w);
147	Real fInvLength = Math::InvSqrt(fSqrLength);
148	rkAxis.x = x*fInvLength;
149	rkAxis.y = y*fInvLength;
150	rkAxis.z = z*fInvLength;
151	}
152	else
153	{
154	// angle is 0 (mod 2*pi), so any axis will do
155	rfAngle = Radian(0.0);
156	rkAxis.x = 1.0;
157	rkAxis.y = 0.0;
158	rkAxis.z = 0.0;
159	}
160	}
161	//-----------------------------------------------------------------------
162	void Quaternion::FromAxes (const Vector3* akAxis)
163	{
164	Matrix3 kRot;
165
166	for (size_t iCol = 0; iCol < 3; iCol++)
167	{
168	kRot[0][iCol] = akAxis[iCol].x;
169	kRot[1][iCol] = akAxis[iCol].y;
170	kRot[2][iCol] = akAxis[iCol].z;
171	}
172
173	FromRotationMatrix(kRot);
174	}
175	//-----------------------------------------------------------------------
176	void Quaternion::FromAxes (const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
177	{
178	Matrix3 kRot;
179
180	kRot[0][0] = xaxis.x;
181	kRot[1][0] = xaxis.y;
182	kRot[2][0] = xaxis.z;
183
184	kRot[0][1] = yaxis.x;
185	kRot[1][1] = yaxis.y;
186	kRot[2][1] = yaxis.z;
187
188	kRot[0][2] = zaxis.x;
189	kRot[1][2] = zaxis.y;
190	kRot[2][2] = zaxis.z;
191
192	FromRotationMatrix(kRot);
193
194	}
195	//-----------------------------------------------------------------------
196	void Quaternion::ToAxes (Vector3* akAxis) const
197	{
198	Matrix3 kRot;
199
200	ToRotationMatrix(kRot);
201
202	for (size_t iCol = 0; iCol < 3; iCol++)
203	{
204	akAxis[iCol].x = kRot[0][iCol];
205	akAxis[iCol].y = kRot[1][iCol];
206	akAxis[iCol].z = kRot[2][iCol];
207	}
208	}
209	//-----------------------------------------------------------------------
210	Vector3 Quaternion::xAxis(void) const
211	{
212	//Real fTx = 2.0*x;
213	Real fTy = 2.0*y;
214	Real fTz = 2.0*z;
215	Real fTwy = fTy*w;
216	Real fTwz = fTz*w;
217	Real fTxy = fTy*x;
218	Real fTxz = fTz*x;
219	Real fTyy = fTy*y;
220	Real fTzz = fTz*z;
221
222	return Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
223	}
224	//-----------------------------------------------------------------------
225	Vector3 Quaternion::yAxis(void) const
226	{
227	Real fTx = 2.0*x;
228	Real fTy = 2.0*y;
229	Real fTz = 2.0*z;
230	Real fTwx = fTx*w;
231	Real fTwz = fTz*w;
232	Real fTxx = fTx*x;
233	Real fTxy = fTy*x;
234	Real fTyz = fTz*y;
235	Real fTzz = fTz*z;
236
237	return Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
238	}
239	//-----------------------------------------------------------------------
240	Vector3 Quaternion::zAxis(void) const
241	{
242	Real fTx = 2.0*x;
243	Real fTy = 2.0*y;
244	Real fTz = 2.0*z;
245	Real fTwx = fTx*w;
246	Real fTwy = fTy*w;
247	Real fTxx = fTx*x;
248	Real fTxz = fTz*x;
249	Real fTyy = fTy*y;
250	Real fTyz = fTz*y;
251
252	return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
253	}
254	//-----------------------------------------------------------------------
255	void Quaternion::ToAxes (Vector3& xaxis, Vector3& yaxis, Vector3& zaxis) const
256	{
257	Matrix3 kRot;
258
259	ToRotationMatrix(kRot);
260
261	xaxis.x = kRot[0][0];
262	xaxis.y = kRot[1][0];
263	xaxis.z = kRot[2][0];
264
265	yaxis.x = kRot[0][1];
266	yaxis.y = kRot[1][1];
267	yaxis.z = kRot[2][1];
268
269	zaxis.x = kRot[0][2];
270	zaxis.y = kRot[1][2];
271	zaxis.z = kRot[2][2];
272	}
273
274	//-----------------------------------------------------------------------
275	Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
276	{
277	return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
278	}
279	//-----------------------------------------------------------------------
280	Quaternion Quaternion::operator- (const Quaternion& rkQ) const
281	{
282	return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
283	}
284	//-----------------------------------------------------------------------
285	Quaternion Quaternion::operator* (const Quaternion& rkQ) const
286	{
287	// NOTE: Multiplication is not generally commutative, so in most
288	// cases pq != qp.
289
290	return Quaternion
291	(
292	w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
293	w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
294	w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
295	w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
296	);
297	}
298	//-----------------------------------------------------------------------
299	Quaternion Quaternion::operator* (Real fScalar) const
300	{
301	return Quaternion(fScalarw,fScalarx,fScalary,fScalarz);
302	}
303	//-----------------------------------------------------------------------
304	Quaternion operator* (Real fScalar, const Quaternion& rkQ)
305	{
306	return Quaternion(fScalarrkQ.w,fScalarrkQ.x,fScalar*rkQ.y,
307	fScalar*rkQ.z);
308	}
309	//-----------------------------------------------------------------------
310	Quaternion Quaternion::operator- () const
311	{
312	return Quaternion(-w,-x,-y,-z);
313	}
314	//-----------------------------------------------------------------------
315	Real Quaternion::Dot (const Quaternion& rkQ) const
316	{
317	return wrkQ.w+xrkQ.x+yrkQ.y+zrkQ.z;
318	}
319	//-----------------------------------------------------------------------
320	Real Quaternion::Norm () const
321	{
322	return ww+xx+yy+zz;
323	}
324	//-----------------------------------------------------------------------
325	Quaternion Quaternion::Inverse () const
326	{
327	Real fNorm = ww+xx+yy+zz;
328	if ( fNorm > 0.0 )
329	{
330	Real fInvNorm = 1.0/fNorm;
331	return Quaternion(wfInvNorm,-xfInvNorm,-yfInvNorm,-zfInvNorm);
332	}
333	else
334	{
335	// return an invalid result to flag the error
336	return ZERO;
337	}
338	}
339	//-----------------------------------------------------------------------
340	Quaternion Quaternion::UnitInverse () const
341	{
342	// assert: 'this' is unit length
343	return Quaternion(w,-x,-y,-z);
344	}
345	//-----------------------------------------------------------------------
346	Quaternion Quaternion::Exp () const
347	{
348	// If q = A(xi+yj+zk) where (x,y,z) is unit length, then
349	// exp(q) = cos(A)+sin(A)(xi+yj+zk). If sin(A) is near zero,
350	// use exp(q) = cos(A)+A(xi+yj+zk) since A/sin(A) has limit 1.
351
352	Radian fAngle ( Math::Sqrt(xx+yy+z*z) );
353	Real fSin = Math::Sin(fAngle);
354
355	Quaternion kResult;
356	kResult.w = Math::Cos(fAngle);
357
358	if ( Math::Abs(fSin) >= ms_fEpsilon )
359	{
360	Real fCoeff = fSin/(fAngle.valueRadians());
361	kResult.x = fCoeff*x;
362	kResult.y = fCoeff*y;
363	kResult.z = fCoeff*z;
364	}
365	else
366	{
367	kResult.x = x;
368	kResult.y = y;
369	kResult.z = z;
370	}
371
372	return kResult;
373	}
374	//-----------------------------------------------------------------------
375	Quaternion Quaternion::Log () const
376	{
377	// If q = cos(A)+sin(A)(xi+yj+zk) where (x,y,z) is unit length, then
378	// log(q) = A(xi+yj+zk). If sin(A) is near zero, use log(q) =
379	// sin(A)(xi+yj+zk) since sin(A)/A has limit 1.
380
381	Quaternion kResult;
382	kResult.w = 0.0;
383
384	if ( Math::Abs(w) < 1.0 )
385	{
386	Radian fAngle ( Math::ACos(w) );
387	Real fSin = Math::Sin(fAngle);
388	if ( Math::Abs(fSin) >= ms_fEpsilon )
389	{
390	Real fCoeff = fAngle.valueRadians()/fSin;
391	kResult.x = fCoeff*x;
392	kResult.y = fCoeff*y;
393	kResult.z = fCoeff*z;
394	return kResult;
395	}
396	}
397
398	kResult.x = x;
399	kResult.y = y;
400	kResult.z = z;
401
402	return kResult;
403	}
404	//-----------------------------------------------------------------------
405	Vector3 Quaternion::operator* (const Vector3& v) const
406	{
407	// nVidia SDK implementation
408	Vector3 uv, uuv;
409	Vector3 qvec(x, y, z);
410	uv = qvec.crossProduct(v);
411	uuv = qvec.crossProduct(uv);
412	uv = (2.0f w);
413	uuv *= 2.0f;
414
415	return v + uv + uuv;
416
417	}
418	//-----------------------------------------------------------------------
419	bool Quaternion::equals(const Quaternion& rhs, const Radian& tolerance) const
420	{
421	Real fCos = Dot(rhs);
422	Radian angle = Math::ACos(fCos);
423
424	return (Math::Abs(angle.valueRadians()) <= tolerance.valueRadians())
425	\|\| Math::RealEqual(angle.valueRadians(), Math::PI, tolerance.valueRadians());
426
427
428	}
429	//-----------------------------------------------------------------------
430	Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP,
431	const Quaternion& rkQ, bool shortestPath)
432	{
433	Real fCos = rkP.Dot(rkQ);
434	Quaternion rkT;
435
436	// Do we need to invert rotation?
437	if (fCos < 0.0f && shortestPath)
438	{
439	fCos = -fCos;
440	rkT = -rkQ;
441	}
442	else
443	{
444	rkT = rkQ;
445	}
446
447	if (Math::Abs(fCos) < 1 - ms_fEpsilon)
448	{
449	// Standard case (slerp)
450	Real fSin = Math::Sqrt(1 - Math::Sqr(fCos));
451	Radian fAngle = Math::ATan2(fSin, fCos);
452	Real fInvSin = 1.0f / fSin;
453	Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle) * fInvSin;
454	Real fCoeff1 = Math::Sin(fT * fAngle) * fInvSin;
455	return fCoeff0 * rkP + fCoeff1 * rkT;
456	}
457	else
458	{
459	// There are two situations:
460	// 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
461	// interpolation safely.
462	// 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
463	// are an infinite number of possibilities interpolation. but we haven't
464	// have method to fix this case, so just use linear interpolation here.
465	Quaternion t = (1.0f - fT) * rkP + fT * rkT;
466	// taking the complement requires renormalisation
467	t.normalise();
468	return t;
469	}
470	}
471	//-----------------------------------------------------------------------
472	Quaternion Quaternion::SlerpExtraSpins (Real fT,
473	const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins)
474	{
475	Real fCos = rkP.Dot(rkQ);
476	Radian fAngle ( Math::ACos(fCos) );
477
478	if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon )
479	return rkP;
480
481	Real fSin = Math::Sin(fAngle);
482	Radian fPhase ( Math::PIiExtraSpinsfT );
483	Real fInvSin = 1.0/fSin;
484	Real fCoeff0 = Math::Sin((1.0-fT)fAngle - fPhase)fInvSin;
485	Real fCoeff1 = Math::Sin(fTfAngle + fPhase)fInvSin;
486	return fCoeff0rkP + fCoeff1rkQ;
487	}
488	//-----------------------------------------------------------------------
489	void Quaternion::Intermediate (const Quaternion& rkQ0,
490	const Quaternion& rkQ1, const Quaternion& rkQ2,
491	Quaternion& rkA, Quaternion& rkB)
492	{
493	// assert: q0, q1, q2 are unit quaternions
494
495	Quaternion kQ0inv = rkQ0.UnitInverse();
496	Quaternion kQ1inv = rkQ1.UnitInverse();
497	Quaternion rkP0 = kQ0inv*rkQ1;
498	Quaternion rkP1 = kQ1inv*rkQ2;
499	Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
500	Quaternion kMinusArg = -kArg;
501
502	rkA = rkQ1*kArg.Exp();
503	rkB = rkQ1*kMinusArg.Exp();
504	}
505	//-----------------------------------------------------------------------
506	Quaternion Quaternion::Squad (Real fT,
507	const Quaternion& rkP, const Quaternion& rkA,
508	const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath)
509	{
510	Real fSlerpT = 2.0fT(1.0-fT);
511	Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
512	Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
513	return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
514	}
515	//-----------------------------------------------------------------------
516	Real Quaternion::normalise(void)
517	{
518	Real len = Norm();
519	Real factor = 1.0f / Math::Sqrt(len);
520	this = this * factor;
521	return len;
522	}
523	//-----------------------------------------------------------------------
524	Radian Quaternion::getRoll(bool reprojectAxis) const
525	{
526	if (reprojectAxis)
527	{
528	// roll = atan2(localx.y, localx.x)
529	// pick parts of xAxis() implementation that we need
530	Real fTx = 2.0*x;
531	Real fTy = 2.0*y;
532	Real fTz = 2.0*z;
533	Real fTwz = fTz*w;
534	Real fTxy = fTy*x;
535	Real fTyy = fTy*y;
536	Real fTzz = fTz*z;
537
538	// Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
539
540	return Radian(Math::ATan2(fTxy+fTwz, 1.0-(fTyy+fTzz)));
541
542	}
543	else
544	{
545	return Radian(Math::ATan2(2(xy + wz), ww + xx - yy - z*z));
546	}
547	}
548	//-----------------------------------------------------------------------
549	Radian Quaternion::getPitch(bool reprojectAxis) const
550	{
551	if (reprojectAxis)
552	{
553	// pitch = atan2(localy.z, localy.y)
554	// pick parts of yAxis() implementation that we need
555	Real fTx = 2.0*x;
556	Real fTy = 2.0*y;
557	Real fTz = 2.0*z;
558	Real fTwx = fTx*w;
559	Real fTxx = fTx*x;
560	Real fTyz = fTz*y;
561	Real fTzz = fTz*z;
562
563	// Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
564	return Radian(Math::ATan2(fTyz+fTwx, 1.0-(fTxx+fTzz)));
565	}
566	else
567	{
568	// internal version
569	return Radian(Math::ATan2(2(yz + wx), ww - xx - yy + z*z));
570	}
571	}
572	//-----------------------------------------------------------------------
573	Radian Quaternion::getYaw(bool reprojectAxis) const
574	{
575	if (reprojectAxis)
576	{
577	// yaw = atan2(localz.x, localz.z)
578	// pick parts of zAxis() implementation that we need
579	Real fTx = 2.0*x;
580	Real fTy = 2.0*y;
581	Real fTz = 2.0*z;
582	Real fTwy = fTy*w;
583	Real fTxx = fTx*x;
584	Real fTxz = fTz*x;
585	Real fTyy = fTy*y;
586
587	// Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
588
589	return Radian(Math::ATan2(fTxz+fTwy, 1.0-(fTxx+fTyy)));
590
591	}
592	else
593	{
594	// internal version
595	return Radian(Math::ASin(-2(xz - w*y)));
596	}
597	}
598	//-----------------------------------------------------------------------
599	Quaternion Quaternion::nlerp(Real fT, const Quaternion& rkP,
600	const Quaternion& rkQ, bool shortestPath)
601	{
602	Quaternion result;
603	Real fCos = rkP.Dot(rkQ);
604	if (fCos < 0.0f && shortestPath)
605	{
606	result = rkP + fT * ((-rkQ) - rkP);
607	}
608	else
609	{
610	result = rkP + fT * (rkQ - rkP);
611	}
612	result.normalise();
613	return result;
614	}
615	}

Note: See TracBrowser for help on using the repository browser.

Context Navigation

source: code/forks/sandbox_light/src/external/ogremath/OgreQuaternion.cpp @ 9926

Download in other formats: