2 * Copyright (c) 2014 Samsung Electronics Co., Ltd.
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
8 * http://www.apache.org/licenses/LICENSE-2.0
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
19 #include <dali/public-api/math/quaternion.h>
22 #include <dali/public-api/common/constants.h>
23 #include <dali/public-api/math/degree.h>
24 #include <dali/public-api/math/matrix.h>
25 #include <dali/public-api/math/radian.h>
26 #include <dali/public-api/math/math-utils.h>
27 #include <dali/internal/render/common/performance-monitor.h>
34 using Internal::PerformanceMonitor;
36 const Quaternion Quaternion::IDENTITY;
42 Quaternion::Quaternion()
43 : mVector(0.0f, 0.0f, 0.0f, 1.0f)
47 Quaternion::Quaternion(float cosThetaBy2, float iBySineTheta, float jBySineTheta, float kBySineTheta) :
48 mVector(iBySineTheta, jBySineTheta, kBySineTheta, cosThetaBy2)
52 Quaternion::Quaternion(const Vector4& vector)
57 Quaternion::Quaternion(float angle, const Vector3 &axis)
59 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
61 Vector3 tmpAxis = axis;
63 const float halfAngle = angle * 0.5f;
64 const float sinThetaByTwo = sinf(halfAngle);
65 const float cosThetaByTwo = cosf(halfAngle);
66 mVector.x = tmpAxis.x * sinThetaByTwo;
67 mVector.y = tmpAxis.y * sinThetaByTwo;
68 mVector.z = tmpAxis.z * sinThetaByTwo;
69 mVector.w = cosThetaByTwo;
72 Quaternion::Quaternion(float theta, const Vector4 &axis)
74 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
76 Vector4 tmpAxis = axis;
78 const float halfTheta = theta * 0.5f;
79 const float sinThetaByTwo = sinf(halfTheta);
80 const float cosThetaByTwo = cosf(halfTheta);
81 mVector.x = tmpAxis.x * sinThetaByTwo;
82 mVector.y = tmpAxis.y * sinThetaByTwo;
83 mVector.z = tmpAxis.z * sinThetaByTwo;
84 mVector.w = cosThetaByTwo;
87 Quaternion::Quaternion(float x, float y, float z)
92 Quaternion::Quaternion(const Matrix& matrix)
94 Vector3 xAxis( matrix.GetXAxis() );
95 Vector3 yAxis( matrix.GetYAxis() );
96 Vector3 zAxis( matrix.GetZAxis() );
98 SetFromAxes( xAxis, yAxis, zAxis );
101 Quaternion::Quaternion( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
103 SetFromAxes( xAxis, yAxis, zAxis );
106 Quaternion::Quaternion( const Vector3& v0, const Vector3& v1 )
108 float dot = v0.Dot(v1);
109 if( dot > 1.0f - Math::MACHINE_EPSILON_1 )
111 //Identity quaternion
112 mVector.x = mVector.y = mVector.z = 0.0f;
115 else if( dot < -1.0f + Math::MACHINE_EPSILON_1)
117 //180 degree rotation across the Z axis
118 mVector.x = mVector.y = mVector.w = 0.0f;
123 Vector3 w = v0.Cross(v1);
124 mVector.w = 1.0f + dot;
132 Quaternion Quaternion::FromAxisAngle(const Vector4 &axis, float angle)
134 return Quaternion(angle, axis);
137 Quaternion::~Quaternion()
141 bool Quaternion::IsIdentity() const
143 // start from w as its unlikely that any real rotation has w == 1
144 // Uses a relaxed epsilon, as composition of rotation introduces error
145 return ( ( fabsf( mVector.w - 1.0f ) < Math::MACHINE_EPSILON_10 )&&
146 ( fabsf( mVector.x ) < Math::MACHINE_EPSILON_10 )&&
147 ( fabsf( mVector.y ) < Math::MACHINE_EPSILON_10 )&&
148 ( fabsf( mVector.z ) < Math::MACHINE_EPSILON_10 ) );
151 bool Quaternion::ToAxisAngle(Vector3 &axis, float &angle) const
153 angle = acosf(mVector.w);
154 bool converted = false;
155 // pre-compute to save time
156 const float sine = sinf( angle );
158 // If sine(angle) is zero, conversion is not possible
160 if ( ! EqualsZero( sine ) )
162 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,3);
164 float sinf_theta_inv = 1.0f / sine;
166 axis.x = mVector.x*sinf_theta_inv;
167 axis.y = mVector.y*sinf_theta_inv;
168 axis.z = mVector.z*sinf_theta_inv;
175 bool Quaternion::ToAxisAngle(Vector4 &axis, float &angle) const
178 bool converted = ToAxisAngle(axis3, angle);
189 const Vector4& Quaternion::AsVector() const
194 void Quaternion::SetEuler(float x, float y, float z)
196 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,19);
198 const float halfX = 0.5f * x;
199 const float halfY = 0.5f * y;
200 const float halfZ = 0.5f * z;
202 float cosX2 = cosf(halfX);
203 float cosY2 = cosf(halfY);
204 float cosZ2 = cosf(halfZ);
206 float sinX2 = sinf(halfX);
207 float sinY2 = sinf(halfY);
208 float sinZ2 = sinf(halfZ);
210 mVector.w = cosZ2 * cosY2 * cosX2 + sinZ2 * sinY2 * sinX2;
211 mVector.x = cosZ2 * cosY2 * sinX2 - sinZ2 * sinY2 * cosX2;
212 mVector.y = cosZ2 * sinY2 * cosX2 + sinZ2 * cosY2 * sinX2;
213 mVector.z = sinZ2 * cosY2 * cosX2 - cosZ2 * sinY2 * sinX2;
216 Vector4 Quaternion::EulerAngles() const
218 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,13);
220 float sqw = mVector.w*mVector.w;
221 float sqx = mVector.x*mVector.x;
222 float sqy = mVector.y*mVector.y;
223 float sqz = mVector.z*mVector.z;
226 euler.x = atan2f(2.0f * (mVector.y*mVector.z + mVector.x*mVector.w), -sqx - sqy + sqz + sqw);
227 euler.y = asinf(-2.0f * (mVector.x*mVector.z - mVector.y*mVector.w));
228 euler.z = atan2f(2.0f * (mVector.x*mVector.y + mVector.z*mVector.w), sqx - sqy - sqz + sqw);
232 const Quaternion Quaternion::operator +(const Quaternion &other) const
234 return Quaternion(mVector + other.mVector);
237 const Quaternion Quaternion::operator -(const Quaternion &other) const
239 return Quaternion(mVector - other.mVector);
242 const Quaternion Quaternion::operator *(const Quaternion &other) const
244 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
246 return Quaternion(mVector.w * other.mVector.w - mVector.Dot(other.mVector),
247 mVector.y * other.mVector.z - mVector.z * other.mVector.y + mVector.w * other.mVector.x + mVector.x * other.mVector.w,
248 mVector.z * other.mVector.x - mVector.x * other.mVector.z + mVector.w * other.mVector.y + mVector.y * other.mVector.w,
249 mVector.x * other.mVector.y - mVector.y * other.mVector.x + mVector.w * other.mVector.z + mVector.z * other.mVector.w);
252 Vector3 Quaternion::operator *(const Vector3& v) const
254 // nVidia SDK implementation
256 Vector3 qvec(mVector.x, mVector.y, mVector.z);
258 uuv = qvec.Cross(uv);
259 uv *= (2.0f * mVector.w);
265 const Quaternion Quaternion::operator /(const Quaternion &q) const
272 const Quaternion Quaternion::operator *(float scale) const
274 return Quaternion(mVector*scale);
277 const Quaternion Quaternion::operator /(float scale) const
279 return Quaternion(mVector/scale);
282 Quaternion Quaternion::operator -() const
284 return Quaternion(-mVector.w, -mVector.x, -mVector.y, -mVector.z);
287 const Quaternion& Quaternion::operator +=(const Quaternion &q)
289 mVector += q.mVector; return *this;
292 const Quaternion& Quaternion::operator -=(const Quaternion &q)
294 mVector -= q.mVector; return *this;
297 const Quaternion& Quaternion::operator *=(const Quaternion &q)
299 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
301 float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
303 mVector.w = mVector.w * q.mVector.w - mVector.Dot(q.mVector);
304 mVector.x = y*q.mVector.z - z*q.mVector.y + w*q.mVector.x + x*q.mVector.w;
305 mVector.y = z*q.mVector.x - x*q.mVector.z + w*q.mVector.y + y*q.mVector.w;
306 mVector.z = x*q.mVector.y - y*q.mVector.x + w*q.mVector.z + z*q.mVector.w;
310 const Quaternion& Quaternion::operator *= (float scale)
312 mVector*=scale; return *this;
315 const Quaternion& Quaternion::operator /= (float scale)
317 mVector/=scale; return *this;
320 bool Quaternion::operator== (const Quaternion& rhs) const
322 return ( ( fabsf(mVector.x - rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
323 fabsf(mVector.y - rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
324 fabsf(mVector.z - rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
325 fabsf(mVector.w - rhs.mVector.w) < Math::MACHINE_EPSILON_1 ) ||
326 // Or equal to negation of rhs
327 ( fabsf(mVector.x + rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
328 fabsf(mVector.y + rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
329 fabsf(mVector.z + rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
330 fabsf(mVector.w + rhs.mVector.w) < Math::MACHINE_EPSILON_1 )
334 bool Quaternion::operator!= (const Quaternion& rhs) const
336 return !operator==(rhs);
339 float Quaternion::Length() const
341 return (float)sqrt(mVector.w * mVector.w + mVector.Dot(mVector));
344 float Quaternion::LengthSquared() const
346 return (float)(mVector.w * mVector.w + mVector.Dot(mVector));
349 void Quaternion::Normalize()
354 Quaternion Quaternion::Normalized() const
356 return *this/Length();
359 void Quaternion::Conjugate()
361 mVector.x = -mVector.x;
362 mVector.y = -mVector.y;
363 mVector.z = -mVector.z;
366 void Quaternion::Invert()
369 *this/=LengthSquared();
372 Quaternion Quaternion::Log() const
374 float a = acosf(mVector.w);
375 float sina = sinf(a);
379 if (fabsf(sina) >= Math::MACHINE_EPSILON_1)
381 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
383 float angleBySinAngle = a * (1.0f / sina);
384 ret.mVector.x = mVector.x * angleBySinAngle;
385 ret.mVector.y = mVector.y * angleBySinAngle;
386 ret.mVector.z = mVector.z * angleBySinAngle;
390 ret.mVector.x= ret.mVector.y= ret.mVector.z= 0;
395 Quaternion Quaternion::Exp() const
397 DALI_ASSERT_ALWAYS( EqualsZero( mVector.w ) && "Cannot perform Exponent" );
399 float a = mVector.Length();
400 float sina = sinf(a);
403 ret.mVector.w = cosf(a);
405 if (a >= Math::MACHINE_EPSILON_1)
407 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
409 float sinAOverA = sina * (1.0f / a);
410 ret.mVector.x = mVector.x * sinAOverA;
411 ret.mVector.y = mVector.y * sinAOverA;
412 ret.mVector.z = mVector.z * sinAOverA;
416 ret.mVector.x = ret.mVector.y = ret.mVector.z = 0.0f;
421 float Quaternion::Dot(const Quaternion &q1, const Quaternion &q2)
423 return q1.mVector.Dot4(q2.mVector);
426 Quaternion Quaternion::Lerp(const Quaternion &q1, const Quaternion &q2, float t)
428 return (q1*(1.0f-t) + q2*t).Normalized();
431 Quaternion Quaternion::Slerp(const Quaternion &q1, const Quaternion &q2, float progress)
434 float cosTheta = Quaternion::Dot(q1, q2);
437 * If cos(theta) < 0, q1 and q2 are more than 90 degrees apart,
438 * so invert one to reduce spinning.
442 cosTheta = -cosTheta;
450 if (fabsf(cosTheta) < 0.95f)
452 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,5);
455 float sine = sqrtf(1.0f - cosTheta*cosTheta);
456 float angle = atan2f(sine, cosTheta);
457 float invSine = 1.0f / sine;
458 float coeff0 = sinf((1.0f - progress) * angle) * invSine;
459 float coeff1 = sinf(progress * angle) * invSine;
461 return q1*coeff0 + q3*coeff1;
465 // If the angle is small, use linear interpolation
466 Quaternion result = q1*(1.0f - progress) + q3*progress;
468 return result.Normalized();
472 Quaternion Quaternion::SlerpNoInvert(const Quaternion &q1, const Quaternion &q2, float t)
474 float cosTheta = Quaternion::Dot(q1, q2);
476 if (cosTheta > -0.95f && cosTheta < 0.95f)
478 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
480 float theta = acosf(cosTheta);
481 return (q1*sinf(theta*(1.0f-t)) + q2*sinf(theta*t))/sinf(theta);
485 return Lerp(q1, q2, t);
489 Quaternion Quaternion::Squad(
490 const Quaternion &q1, // start
491 const Quaternion &q2, // end
492 const Quaternion &a, // ctrl pt for q1
493 const Quaternion &b, // ctrl pt for q2
496 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
498 Quaternion c = SlerpNoInvert(q1, q2, t);
499 Quaternion d = SlerpNoInvert(a, b, t);
500 return SlerpNoInvert(c, d, 2*t*(1-t));
503 float Quaternion::AngleBetween(const Quaternion &q1, const Quaternion &q2)
511 //Formula for angle θ between two quaternion is:
512 //θ = cos^−1 (2⟨q1,q2⟩^2 − 1), Where (q1,q2) is inner product of the quaternions.
513 float X = from.mVector.Dot4(to.mVector);
514 float theta = acos( (2 * X * X) - 1);
519 Vector4 Quaternion::Rotate(const Vector4 &v) const
521 Quaternion V(0.0f, v.x, v.y, v.z);
522 Quaternion conjugate(*this);
523 conjugate.Conjugate();
524 return (*this * V * conjugate).mVector;
527 Vector3 Quaternion::Rotate(const Vector3 &v) const
529 Quaternion V(0.0f, v.x, v.y, v.z);
530 Quaternion conjugate(*this);
531 conjugate.Conjugate();
532 return Vector3((*this * V * conjugate).mVector);
535 void Quaternion::SetFromAxes( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
537 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
539 float t = xAxis.x + yAxis.y + zAxis.z;
540 if ( t > 0.0f ) // w is largest
542 float root = sqrtf( t + 1.0f );
543 float one_over_4w = 0.5f / root;
544 mVector.x = ( yAxis.z - zAxis.y ) * one_over_4w;
545 mVector.y = ( zAxis.x - xAxis.z ) * one_over_4w;
546 mVector.z = ( xAxis.y - yAxis.x ) * one_over_4w;
547 mVector.w = root * 0.5f;
549 else if( zAxis.z > xAxis.x && zAxis.z > yAxis.y ) // z is largest
551 float root = sqrtf( zAxis.z - xAxis.x - yAxis.y + 1.0f );
552 float one_over_4w = 0.5f / root;
553 mVector.x = ( xAxis.z + zAxis.x ) * one_over_4w;
554 mVector.y = ( yAxis.z + zAxis.y ) * one_over_4w;
555 mVector.z = root * 0.5f;
556 mVector.w = ( xAxis.y - yAxis.x ) * one_over_4w;
558 else if( yAxis.y > xAxis.x ) // y is largest
560 float root = sqrtf(yAxis.y - zAxis.z - xAxis.x + 1.0f );
561 float one_over_4w = 0.5f / root;
563 mVector.x = ( xAxis.y + yAxis.x ) * one_over_4w;
564 mVector.y = root * 0.5f;
565 mVector.z = ( zAxis.y + yAxis.z ) * one_over_4w;
566 mVector.w = ( zAxis.x - xAxis.z ) * one_over_4w;
570 float root = sqrtf( xAxis.x - yAxis.y - zAxis.z + 1.0f );
571 float one_over_4w = 0.5f / root;
572 mVector.x = root * 0.5f;
573 mVector.y = ( yAxis.x + xAxis.y ) * one_over_4w;
574 mVector.z = ( zAxis.x + xAxis.z ) * one_over_4w;
575 mVector.w = ( yAxis.z - zAxis.y ) * one_over_4w;
581 std::ostream& operator<< (std::ostream& o, const Quaternion& quaternion)
586 quaternion.ToAxisAngle( axis, angleRadians );
587 Degree degrees = Radian(angleRadians);
589 return o << "[ Axis: [" << axis.x << ", " << axis.y << ", " << axis.z << "], Angle: " << degrees << " degrees ]";