Quat.cpp

1 /* Copyright 2011 Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file Quat.cpp
16         @author Jukka Jyl�nki
17         @brief */
18 #include <stdlib.h>
19 #include "Math/Quat.h"
20 #include "Math/float3.h"
21 #include "Math/float4.h"
22 #include "Math/float3x3.h"
23 #include "Math/float3x4.h"
24 #include "Math/float4x4.h"
25 #include "Algorithm/Random/LCG.h"
26 #include "assume.h"
27 #include "Math/MathFunc.h"
28
29 MATH_BEGIN_NAMESPACE
30
31 Quat::Quat(const float *data)
32 :x(data[0]),
33 y(data[1]),
34 z(data[2]),
35 w(data[3])
36 {
37 }
38
39 Quat::Quat(const float3x3 &rotationMatrix)
40 {
41         Set(rotationMatrix);
42 }
43
44 Quat::Quat(const float3x4 &rotationMatrix)
45 {
46         Set(rotationMatrix);
47 }
48
49 Quat::Quat(const float4x4 &rotationMatrix)
50 {
51         Set(rotationMatrix);
52 }
53
54 Quat::Quat(float x_, float y_, float z_, float w_)
55 :x(x_), y(y_), z(z_), w(w_)
56 {
57 }
58
59 Quat::Quat(const float3 &rotationAxis, float rotationAngle)
60 {
61         SetFromAxisAngle(rotationAxis, rotationAngle);
62 }
63
64 float3 Quat::WorldX() const
65 {
66         return this->Transform(1.f, 0.f, 0.f);
67 }
68
69 float3 Quat::WorldY() const
70 {
71         return this->Transform(0.f, 1.f, 0.f);
72 }
73
74 float3 Quat::WorldZ() const
75 {
76         return this->Transform(0.f, 0.f, 1.f);
77 }
78
79 float3 Quat::Axis() const
80 {
81         float3 axis;
82         float angle;
83         ToAxisAngle(axis, angle);
84         return axis;
85 }
86
87 float Quat::Angle() const
88 {
89         return acos(w) * 2.f;
90 }
91
92 float Quat::Dot(const Quat &rhs) const
93 {
94         return x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w;
95 }
96
97 float Quat::LengthSq() const
98 {
99         return x*x + y*y + z*z + w*w;
100 }
101
102 float Quat::Length() const
103 {
104         return sqrtf(LengthSq());
105 }
106
107 float Quat::Normalize()
108 {
109         float length = Length();
110         if (length < 1e-4f)
111                 return 0.f;
112         float rcpLength = 1.f / length;
113         x *= rcpLength;
114         y *= rcpLength;
115         z *= rcpLength;
116         w *= rcpLength;
117         return length;
118 }
119
120 Quat Quat::Normalized() const
121 {
122         Quat copy = *this;
123         float success = copy.Normalize();
124         assume(success > 0 && "Quat::Normalized failed!");
125         return copy;
126 }
127
128 bool Quat::IsNormalized(float epsilon) const
129 {
130         return EqualAbs(LengthSq(), 1.f, epsilon);
131 }
132
133 bool Quat::IsInvertible(float epsilon) const
134 {
135         return LengthSq() > epsilon && IsFinite();
136 }
137
138 bool Quat::IsFinite() const
139 {
140         return isfinite(x) && isfinite(y) && isfinite(z) && isfinite(w);
141 }
142
143 bool Quat::Equals(const Quat &rhs, float epsilon) const
144 {
145         return EqualAbs(x, rhs.x, epsilon) && EqualAbs(y, rhs.y, epsilon) && EqualAbs(z, rhs.z, epsilon) && EqualAbs(w, rhs.w, epsilon);
146 }
147
148 float *Quat::ptr()
149 {
150         return &x;
151 }
152
153 const float *Quat::ptr() const
154 {
155         return &x;
156 }
157
158 void Quat::Inverse()
159 {
160         assume(IsNormalized());
161         assume(IsInvertible());
162         Conjugate();
163 }
164
165 Quat Quat::Inverted() const
166 {
167         assume(IsNormalized());
168         assume(IsInvertible());
169         return Conjugated();
170 }
171
172 float Quat::InverseAndNormalize()
173 {
174         Conjugate();
175         return Normalize();
176 }
177
178 void Quat::Conjugate()
179 {
180         x = -x;
181         y = -y;
182         z = -z;
183 }
184
185 Quat Quat::Conjugated() const
186 {
187         Quat copy = *this;
188         copy.Conjugate();
189         return copy;
190 }
191
192 float3 Quat::Transform(const float3 &vec) const
193 {
194         assume(this->IsNormalized());
195         float3x3 mat = this->ToFloat3x3();
196         return mat * vec;
197 }
198
199 float3 Quat::Transform(float x, float y, float z) const
200 {
201         return Transform(float3(x, y, z));
202 }
203
204 float4 Quat::Transform(const float4 &vec) const
205 {
206         assume(vec.IsWZeroOrOne());
207
208         return float4(Transform(vec.x, vec.y, vec.z), vec.w);
209 }
210
211 Quat Quat::Lerp(const Quat &b, float t) const
212 {
213         assume(0.f <= t && t <= 1.f);
214         return *this * (1.f - t) + b * t;
215 }
216
217 Quat Quat::Lerp(const Quat &a, const Quat &b, float t)
218 {
219         return a.Lerp(b, t);
220 }
221
222 /** Implementation based on the math in the book Watt, Policarpo. 3D Games: Real-time rendering and Software Technology, pp. 383-386. */
223 Quat Quat::Slerp(const Quat &q2, float t) const
224 {
225         assume(0.f <= t && t <= 1.f);
226         assume(IsNormalized());
227         assume(q2.IsNormalized());
228
229         float angle = this->Dot(q2);
230         float sign = 1.f; // Multiply by a sign of +/-1 to guarantee we rotate the shorter arc.
231         if (angle < 0.f)
232         {
233                 angle = -angle;
234                 sign = -1.f;
235         }
236
237         float a;
238         float b;
239         if (angle <= 0.97f) // perform spherical linear interpolation.
240         {
241                 angle = acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
242
243                 float c = 1.f / sin(angle);
244                 a = sin((1.f - t) * angle) * c;
245                 b = sin(angle * t) * c;
246         }
247         else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
248         {
249                 a = 1.f - t;
250                 b = t;
251         }
252
253         return (*this * (a * sign) + q2 * b).Normalized();
254 }
255
256 Quat Quat::Slerp(const Quat &a, const Quat &b, float t)
257 {
258         return a.Slerp(b, t);
259 }
260
261 Quat Lerp(const Quat &a, const Quat &b, float t)
262 {
263         return a.Lerp(b, t);
264 }
265
266 Quat Slerp(const Quat &a, const Quat &b, float t)
267 {
268         return a.Slerp(b, t);
269 }
270
271 float Quat::AngleBetween(const Quat &target) const
272 {
273         assume(this->IsInvertible());
274         Quat q = target / *this;
275         return q.Angle();
276 }
277
278 float3 Quat::AxisFromTo(const Quat &target) const
279 {
280         assume(this->IsInvertible());
281         Quat q = target / *this;
282         return q.Axis();
283 }
284
285 void Quat::ToAxisAngle(float3 &axis, float &angle) const
286 {
287         angle = acos(w) * 2.f;
288         float sinz = Sin(angle/2.f);
289         if (fabs(sinz) > 1e-4f)
290         {
291                 sinz = 1.f / sinz;
292                 axis = float3(x * sinz, y * sinz, z * sinz);
293         }
294         else
295         {
296                 // The quaternion does not produce any rotation. Still, explicitly
297                 // set the axis so that the user gets a valid normalized vector back.
298                 angle = 0.f;
299                 axis = float3(1.f, 0.f, 0.f);
300         }
301 }
302
303 void Quat::SetFromAxisAngle(const float3 &axis, float angle)
304 {
305         assume(axis.IsNormalized());
306         assume(isfinite(angle));
307         float cosz = Cos(angle/2.f);
308         float sinz = Sin(angle/2.f);
309         x = axis.x * sinz;
310         y = axis.y * sinz;
311         z = axis.z * sinz;
312         w = cosz;
313 }
314
315 /// See Schneider, Eberly. Geometric Tools for Computer Graphics, p. 861.
316 template<typename M>
317 void SetQuatFrom(Quat &q, const M &m)
318 {
319         // The rotation matrix is of form: (Eric Lengyel's Mathematics for 3D Game Programming and Computer Graphics 2nd ed., p. 92)
320         // 1 - 2y^2 - 2z^2        2xy - 2wz            2xz + 2wy
321         //    2xy + 2wz        1 - 2x^2 - 2z^2         2yz - 2wx
322         //    2xz - 2wy           2yz + 2wx         1 - 2x^2 - 2y^2
323
324         float r = m[0][0] + m[1][1] + m[2][2]; // The element w is easiest picked up as a sum of the diagonals.
325         // Above, r == 3 - 4(x^2+y^2+z^2) == 4(1-x^2-y^2-z^2) - 1 == 4*w^2 - 1.
326         if (r > 0) // In this case, |w| > 1/2.
327         {
328                 q.w = sqrtf(r + 1.f) * 0.5f; // We have two choices for the sign of w, arbitrarily pick the positive.
329                 float inv4w = 1.f / (4.f * q.w);
330                 q.x = (m[2][1] - m[1][2]) * inv4w;
331                 q.y = (m[0][2] - m[2][0]) * inv4w;
332                 q.z = (m[1][0] - m[0][1]) * inv4w;
333         }
334         else if (m[0][0] > m[1][1] && m[0][0] > m[2][2]) // If |q.x| is larger than |q.y| and |q.z|, extract it first. This gives
335         {                                                // best stability, and we know below x can't be zero.
336                 q.x = sqrtf(1.f + m[0][0] - m[1][1] - m[2][2]) * 0.5f; // We have two choices for the sign of x, arbitrarily pick the positive.
337                 const float x4 = 1.f / (4.f * q.x);
338                 q.y = (m[0][1] + m[1][0]) * x4;
339                 q.z = (m[0][2] + m[2][0]) * x4;
340                 q.w = (m[2][1] - m[1][2]) * x4;
341         }
342         else if (m[1][1] > m[2][2]) // |q.y| is larger than |q.x| and |q.z|
343         {
344                 q.y = sqrtf(1.f + m[1][1] - m[0][0] - m[2][2]) * 0.5f; // We have two choices for the sign of y, arbitrarily pick the positive.
345                 const float y4 = 1.f / (4.f * q.y);
346                 q.x = (m[0][1] + m[1][0]) * y4;
347                 q.z = (m[1][2] + m[2][1]) * y4;
348                 q.w = (m[0][2] - m[2][0]) * y4;
349         }
350         else // |q.z| is larger than |q.x| or |q.y|
351         {
352                 q.z = sqrtf(1.f + m[2][2] - m[0][0] - m[1][1]) * 0.5f; // We have two choices for the sign of z, arbitrarily pick the positive.
353                 const float z4 = 1.f / (4.f * q.z);
354                 q.x = (m[0][2] + m[2][0]) * z4;
355                 q.y = (m[1][2] + m[2][1]) * z4;
356                 q.w = (m[1][0] - m[0][1]) * z4;
357         }
358         float oldLength = q.Normalize();
359         assume(oldLength > 0.f);
360 }
361
362 void Quat::Set(const float3x3 &m)
363 {
364         assume(m.IsColOrthogonal());
365         assume(m.HasUnitaryScale());
366         assume(!m.HasNegativeScale());
367         SetQuatFrom(*this, m);
368
369 #ifdef MATH_ASSERT_CORRECTNESS
370         // Test that the conversion float3x3->Quat->float3x3 is correct.
371         float3x3 f = this->ToFloat3x3();
372         mathassert(this->ToFloat3x3().Equals(m, 0.01f));
373 #endif
374 }
375
376 void Quat::Set(const float3x4 &m)
377 {
378         assume(m.IsColOrthogonal());
379         assume(m.HasUnitaryScale());
380         assume(!m.HasNegativeScale());
381         SetQuatFrom(*this, m);
382
383 #ifdef MATH_ASSERT_CORRECTNESS
384         // Test that the conversion float3x3->Quat->float3x3 is correct.
385         mathassert(this->ToFloat3x3().Equals(m.Float3x3Part(), 0.01f));
386 #endif
387 }
388
389 void Quat::Set(const float4x4 &m)
390 {
391         assume(m.IsColOrthogonal3());
392         assume(m.HasUnitaryScale());
393         assume(!m.HasNegativeScale());
394         assume(m.Row(3).Equals(0,0,0,1));
395         SetQuatFrom(*this, m);
396
397 #ifdef MATH_ASSERT_CORRECTNESS
398         // Test that the conversion float3x3->Quat->float3x3 is correct.
399         mathassert(this->ToFloat3x3().Equals(m.Float3x3Part(), 0.01f));
400 #endif
401 }
402
403 void Quat::Set(float x_, float y_, float z_, float w_)
404 {
405         x = x_;
406         y = y_;
407         z = z_;
408         w = w_;
409 }
410
411 Quat Quat::LookAt(const float3 &localForward, const float3 &targetDirection, const float3 &localUp, const float3 &worldUp)
412 {
413         return float3x3::LookAt(localForward, targetDirection, localUp, worldUp).ToQuat();
414 }
415
416 Quat Quat::RotateX(float angle)
417 {
418         return Quat(float3(1,0,0), angle);
419 }
420
421 Quat Quat::RotateY(float angle)
422 {
423         return Quat(float3(0,1,0), angle);
424 }
425
426 Quat Quat::RotateZ(float angle)
427 {
428         return Quat(float3(0,0,1), angle);
429 }
430
431 Quat Quat::RotateAxisAngle(const float3 &axis, float angle)
432 {
433         return Quat(axis, angle);
434 }
435
436 Quat Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
437 {
438         assume(sourceDirection.IsNormalized());
439         assume(targetDirection.IsNormalized());
440         float angle = sourceDirection.AngleBetweenNorm(targetDirection);
441         assume(angle >= 0.f);
442         // If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis.
443         float3 axis = sourceDirection.Cross(targetDirection);
444         float oldLength = axis.Normalize();
445         if (oldLength == 0)
446                 axis = float3(1, 0, 0);
447         return Quat(axis, angle);
448 }
449
450 Quat Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection,
451         const float3 &sourceDirection2, const float3 &targetDirection2)
452 {
453         return LookAt(sourceDirection, targetDirection, sourceDirection2, targetDirection2);
454 }
455
456 Quat Quat::FromEulerXYX(float x2, float y, float x) { return (Quat::RotateX(x2) * Quat::RotateY(y) * Quat::RotateX(x)).Normalized(); }
457 Quat Quat::FromEulerXZX(float x2, float z, float x) { return (Quat::RotateX(x2) * Quat::RotateZ(z) * Quat::RotateX(x)).Normalized(); }
458 Quat Quat::FromEulerYXY(float y2, float x, float y) { return (Quat::RotateY(y2) * Quat::RotateX(x) * Quat::RotateY(y)).Normalized(); }
459 Quat Quat::FromEulerYZY(float y2, float z, float y) { return (Quat::RotateY(y2) * Quat::RotateZ(z) * Quat::RotateY(y)).Normalized(); }
460 Quat Quat::FromEulerZXZ(float z2, float x, float z) { return (Quat::RotateZ(z2) * Quat::RotateX(x) * Quat::RotateZ(z)).Normalized(); }
461 Quat Quat::FromEulerZYZ(float z2, float y, float z) { return (Quat::RotateZ(z2) * Quat::RotateY(y) * Quat::RotateZ(z)).Normalized(); }
462 Quat Quat::FromEulerXYZ(float x, float y, float z) { return (Quat::RotateX(x) * Quat::RotateY(y) * Quat::RotateZ(z)).Normalized(); }
463 Quat Quat::FromEulerXZY(float x, float z, float y) { return (Quat::RotateX(x) * Quat::RotateZ(z) * Quat::RotateY(y)).Normalized(); }
464 Quat Quat::FromEulerYXZ(float y, float x, float z) { return (Quat::RotateY(y) * Quat::RotateX(x) * Quat::RotateZ(z)).Normalized(); }
465 Quat Quat::FromEulerYZX(float y, float z, float x) { return (Quat::RotateY(y) * Quat::RotateZ(z) * Quat::RotateX(x)).Normalized(); }
466 Quat Quat::FromEulerZXY(float z, float x, float y) { return (Quat::RotateZ(z) * Quat::RotateX(x) * Quat::RotateY(y)).Normalized(); }
467 Quat Quat::FromEulerZYX(float z, float y, float x) { return (Quat::RotateZ(z) * Quat::RotateY(y) * Quat::RotateX(x)).Normalized(); }
468
469 Quat Quat::RandomRotation(LCG &lcg)
470 {
471         // Generate a random point on the 4D unitary hypersphere using the rejection sampling method.
472         for(int i = 0; i < 1000; ++i)
473         {
474                 float x = lcg.Float(-1, 1);
475                 float y = lcg.Float(-1, 1);
476                 float z = lcg.Float(-1, 1);
477                 float w = lcg.Float(-1, 1);
478                 float lenSq = x*x + y*y + z*z + w*w;
479                 if (lenSq >= 1e-6f && lenSq <= 1.f)
480                         return Quat(x, y, z, w) / sqrt(lenSq);
481         }
482         assume(false && "Quat::RandomRotation failed!");
483         return Quat::identity;
484 }
485
486 ///@todo the following could be heavily optimized. Don't route through float3x3 conversion.
487
488 float3 Quat::ToEulerXYX() const { return ToFloat3x3().ToEulerXYX(); }
489 float3 Quat::ToEulerXZX() const { return ToFloat3x3().ToEulerXZX(); }
490 float3 Quat::ToEulerYXY() const { return ToFloat3x3().ToEulerYXY(); }
491 float3 Quat::ToEulerYZY() const { return ToFloat3x3().ToEulerYZY(); }
492 float3 Quat::ToEulerZXZ() const { return ToFloat3x3().ToEulerZXZ(); }
493 float3 Quat::ToEulerZYZ() const { return ToFloat3x3().ToEulerZYZ(); }
494 float3 Quat::ToEulerXYZ() const { return ToFloat3x3().ToEulerXYZ(); }
495 float3 Quat::ToEulerXZY() const { return ToFloat3x3().ToEulerXZY(); }
496 float3 Quat::ToEulerYXZ() const { return ToFloat3x3().ToEulerYXZ(); }
497 float3 Quat::ToEulerYZX() const { return ToFloat3x3().ToEulerYZX(); }
498 float3 Quat::ToEulerZXY() const { return ToFloat3x3().ToEulerZXY(); }
499 float3 Quat::ToEulerZYX() const { return ToFloat3x3().ToEulerZYX(); }
500
501 float3x3 Quat::ToFloat3x3() const
502 {
503         return float3x3(*this);
504 }
505
506 float3x4 Quat::ToFloat3x4() const
507 {
508         return float3x4(*this);
509 }
510
511 float4x4 Quat::ToFloat4x4() const
512 {
513         return float4x4(*this);
514 }
515
516 #ifdef MATH_ENABLE_STL_SUPPORT
517 std::string Quat::ToString() const
518 {
519         char str[256];
520         sprintf(str, "(%.3f, %.3f, %.3f, %.3f)", x, y, z, w);
521         return str;
522 }
523
524 std::string Quat::ToString2() const
525 {
526         float3 axis;
527         float angle;
528         ToAxisAngle(axis, angle);
529         char str[256];
530         sprintf(str, "Quat(axis:(%.2f,%.2f,%.2f) angle:%2.f)", axis.x, axis.y, axis.z, RadToDeg(angle));
531         return str;
532 }
533
534 std::string Quat::SerializeToString() const
535 {
536         char str[256];
537         sprintf(str, "%f %f %f %f", x, y, z, w);
538         return std::string(str);
539 }
540 #endif
541
542 Quat Quat::FromString(const char *str)
543 {
544         assume(str);
545         if (!str)
546                 return Quat();
547         if (*str == '(')
548                 ++str;
549         Quat q;
550         q.x = (float)strtod(str, const_cast<char**>(&str));
551         if (*str == ',' || *str == ';')
552                 ++str;
553         q.y = (float)strtod(str, const_cast<char**>(&str));
554         if (*str == ',' || *str == ';')
555                 ++str;
556         q.z = (float)strtod(str, const_cast<char**>(&str));
557         if (*str == ',' || *str == ';')
558                 ++str;
559         q.w = (float)strtod(str, const_cast<char**>(&str));
560         return q;
561 }
562
563 Quat Quat::operator +(const Quat &rhs) const
564 {
565         return Quat(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
566 }
567
568 Quat Quat::operator -(const Quat &rhs) const
569 {
570         return Quat(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
571 }
572
573 Quat Quat::operator *(float scalar) const
574 {
575         return Quat(x * scalar, y * scalar, z * scalar, w * scalar);
576 }
577
578 float3 Quat::operator *(const float3 &rhs) const
579 {
580         return Mul(rhs);
581 }
582
583 Quat Quat::operator /(float scalar) const
584 {
585         assume(!EqualAbs(scalar, 0.f));
586
587         return *this * (1.f / scalar);
588 }
589
590 Quat Quat::operator *(const Quat &r) const
591 {
592         return Quat(w*r.x + x*r.w + y*r.z - z*r.y,
593                     w*r.y - x*r.z + y*r.w + z*r.x,
594                     w*r.z + x*r.y - y*r.x + z*r.w,
595                     w*r.w - x*r.x - y*r.y - z*r.z);
596 }
597
598 Quat Quat::operator /(const Quat &rhs) const
599 {
600         Quat inverse = rhs.Inverted();
601         return *this * inverse;
602 }
603
604 #ifdef MATH_ENABLE_STL_SUPPORT
605 std::ostream &operator <<(std::ostream &out, const Quat &rhs)
606 {
607         out << rhs.ToString();
608         return out;
609 }
610 #endif
611
612 Quat Quat::Mul(const Quat &rhs) const { return *this * rhs; }
613 Quat Quat::Mul(const float3x3 &rhs) const { return *this * Quat(rhs); }
614 float3 Quat::Mul(const float3 &vector) const { return this->Transform(vector); }
615 float4 Quat::Mul(const float4 &vector) const { return this->Transform(vector); }
616
617 const Quat Quat::identity = Quat(0.f, 0.f, 0.f, 1.f);
618 const Quat Quat::nan = Quat(FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN);
619
620 MATH_END_NAMESPACE