2 * Copyright (c) 2015 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>
25 #include <dali/public-api/common/constants.h>
26 #include <dali/public-api/math/degree.h>
27 #include <dali/public-api/math/matrix.h>
28 #include <dali/public-api/math/radian.h>
29 #include <dali/public-api/math/math-utils.h>
30 #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( Radian angle, const Vector3& axis )
59 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
61 Vector3 tmpAxis = axis;
63 const float halfAngle = angle.radian * 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( Radian pitch, Radian yaw, Radian roll )
74 SetEuler( pitch, yaw, roll );
77 Quaternion::Quaternion( const Matrix& matrix )
79 Vector3 xAxis( matrix.GetXAxis() );
80 Vector3 yAxis( matrix.GetYAxis() );
81 Vector3 zAxis( matrix.GetZAxis() );
83 SetFromAxes( xAxis, yAxis, zAxis );
86 Quaternion::Quaternion( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
88 SetFromAxes( xAxis, yAxis, zAxis );
91 Quaternion::Quaternion( const Vector3& v0, const Vector3& v1 )
93 float dot = v0.Dot(v1);
94 if( dot > 1.0f - Math::MACHINE_EPSILON_1 )
97 mVector.x = mVector.y = mVector.z = 0.0f;
100 else if( dot < -1.0f + Math::MACHINE_EPSILON_1)
102 //180 degree rotation across the Z axis
103 mVector.x = mVector.y = mVector.w = 0.0f;
108 Vector3 w = v0.Cross(v1);
109 mVector.w = 1.0f + dot;
117 Quaternion::~Quaternion()
121 bool Quaternion::IsIdentity() const
123 // start from w as its unlikely that any real rotation has w == 1
124 // Uses a relaxed epsilon, as composition of rotation introduces error
125 return ( ( fabsf( mVector.w - 1.0f ) < Math::MACHINE_EPSILON_10 )&&
126 ( fabsf( mVector.x ) < Math::MACHINE_EPSILON_10 )&&
127 ( fabsf( mVector.y ) < Math::MACHINE_EPSILON_10 )&&
128 ( fabsf( mVector.z ) < Math::MACHINE_EPSILON_10 ) );
131 bool Quaternion::ToAxisAngle(Vector3& axis, Radian& angle) const
133 angle = acosf(mVector.w);
134 bool converted = false;
135 // pre-compute to save time
136 const float sine = sinf( angle.radian );
138 // If sine(angle) is zero, conversion is not possible
140 if ( ! EqualsZero( sine ) )
142 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,3);
144 float sinf_theta_inv = 1.0f / sine;
146 axis.x = mVector.x*sinf_theta_inv;
147 axis.y = mVector.y*sinf_theta_inv;
148 axis.z = mVector.z*sinf_theta_inv;
149 angle.radian *= 2.0f;
155 const Vector4& Quaternion::AsVector() const
160 void Quaternion::SetEuler( Radian pitch, Radian yaw, Radian roll )
162 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,19);
164 const float halfX = 0.5f * pitch.radian;
165 const float halfY = 0.5f * yaw.radian;
166 const float halfZ = 0.5f * roll.radian;
168 float cosX2 = cosf(halfX);
169 float cosY2 = cosf(halfY);
170 float cosZ2 = cosf(halfZ);
172 float sinX2 = sinf(halfX);
173 float sinY2 = sinf(halfY);
174 float sinZ2 = sinf(halfZ);
176 mVector.w = cosZ2 * cosY2 * cosX2 + sinZ2 * sinY2 * sinX2;
177 mVector.x = cosZ2 * cosY2 * sinX2 - sinZ2 * sinY2 * cosX2;
178 mVector.y = cosZ2 * sinY2 * cosX2 + sinZ2 * cosY2 * sinX2;
179 mVector.z = sinZ2 * cosY2 * cosX2 - cosZ2 * sinY2 * sinX2;
182 Vector4 Quaternion::EulerAngles() const
184 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,13);
186 float sqw = mVector.w*mVector.w;
187 float sqx = mVector.x*mVector.x;
188 float sqy = mVector.y*mVector.y;
189 float sqz = mVector.z*mVector.z;
192 euler.x = atan2f(2.0f * (mVector.y*mVector.z + mVector.x*mVector.w), -sqx - sqy + sqz + sqw);
193 euler.y = asinf(-2.0f * (mVector.x*mVector.z - mVector.y*mVector.w));
194 euler.z = atan2f(2.0f * (mVector.x*mVector.y + mVector.z*mVector.w), sqx - sqy - sqz + sqw);
198 const Quaternion Quaternion::operator+( const Quaternion& other ) const
200 return Quaternion(mVector + other.mVector);
203 const Quaternion Quaternion::operator-( const Quaternion& other ) const
205 return Quaternion(mVector - other.mVector);
208 const Quaternion Quaternion::operator*( const Quaternion& other ) const
210 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
212 return Quaternion(mVector.w * other.mVector.w - mVector.Dot(other.mVector),
213 mVector.y * other.mVector.z - mVector.z * other.mVector.y + mVector.w * other.mVector.x + mVector.x * other.mVector.w,
214 mVector.z * other.mVector.x - mVector.x * other.mVector.z + mVector.w * other.mVector.y + mVector.y * other.mVector.w,
215 mVector.x * other.mVector.y - mVector.y * other.mVector.x + mVector.w * other.mVector.z + mVector.z * other.mVector.w);
218 Vector3 Quaternion::operator*( const Vector3& other ) const
220 Vector3 qvec(mVector.x, mVector.y, mVector.z);
221 Vector3 uv = qvec.Cross( other );
222 Vector3 uuv = qvec.Cross(uv);
223 uv *= (2.0f * mVector.w);
226 return other + uv + uuv;
229 const Quaternion Quaternion::operator/( const Quaternion& q ) const
236 const Quaternion Quaternion::operator*( float scale ) const
238 return Quaternion(mVector*scale);
241 const Quaternion Quaternion::operator/( float scale ) const
243 return Quaternion(mVector/scale);
246 Quaternion Quaternion::operator-() const
248 return Quaternion(-mVector.w, -mVector.x, -mVector.y, -mVector.z);
251 const Quaternion& Quaternion::operator+=( const Quaternion& q )
253 mVector += q.mVector; return *this;
256 const Quaternion& Quaternion::operator-=( const Quaternion& q )
258 mVector -= q.mVector; return *this;
261 const Quaternion& Quaternion::operator*=( const Quaternion& q )
263 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
265 float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
267 mVector.w = mVector.w * q.mVector.w - mVector.Dot(q.mVector);
268 mVector.x = y*q.mVector.z - z*q.mVector.y + w*q.mVector.x + x*q.mVector.w;
269 mVector.y = z*q.mVector.x - x*q.mVector.z + w*q.mVector.y + y*q.mVector.w;
270 mVector.z = x*q.mVector.y - y*q.mVector.x + w*q.mVector.z + z*q.mVector.w;
274 const Quaternion& Quaternion::operator*=( float scale )
276 mVector*=scale; return *this;
279 const Quaternion& Quaternion::operator/=( float scale )
281 mVector/=scale; return *this;
284 bool Quaternion::operator==( const Quaternion& rhs ) const
286 return ( ( fabsf(mVector.x - rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
287 fabsf(mVector.y - rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
288 fabsf(mVector.z - rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
289 fabsf(mVector.w - rhs.mVector.w) < Math::MACHINE_EPSILON_1 ) ||
290 // Or equal to negation of rhs
291 ( fabsf(mVector.x + rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
292 fabsf(mVector.y + rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
293 fabsf(mVector.z + rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
294 fabsf(mVector.w + rhs.mVector.w) < Math::MACHINE_EPSILON_1 )
298 bool Quaternion::operator!=( const Quaternion& rhs ) const
300 return !operator==(rhs);
303 float Quaternion::Length() const
305 return (float)sqrt(mVector.w * mVector.w + mVector.Dot(mVector));
308 float Quaternion::LengthSquared() const
310 return (float)(mVector.w * mVector.w + mVector.Dot(mVector));
313 void Quaternion::Normalize()
318 Quaternion Quaternion::Normalized() const
320 return *this/Length();
323 void Quaternion::Conjugate()
325 mVector.x = -mVector.x;
326 mVector.y = -mVector.y;
327 mVector.z = -mVector.z;
330 void Quaternion::Invert()
333 *this/=LengthSquared();
336 Quaternion Quaternion::Log() const
338 float a = acosf(mVector.w);
339 float sina = sinf(a);
343 if (fabsf(sina) >= Math::MACHINE_EPSILON_1)
345 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
347 float angleBySinAngle = a * (1.0f / sina);
348 ret.mVector.x = mVector.x * angleBySinAngle;
349 ret.mVector.y = mVector.y * angleBySinAngle;
350 ret.mVector.z = mVector.z * angleBySinAngle;
354 ret.mVector.x= ret.mVector.y= ret.mVector.z= 0;
359 Quaternion Quaternion::Exp() const
361 DALI_ASSERT_ALWAYS( EqualsZero( mVector.w ) && "Cannot perform Exponent" );
363 float a = mVector.Length();
364 float sina = sinf(a);
367 ret.mVector.w = cosf(a);
369 if (a >= Math::MACHINE_EPSILON_1)
371 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
373 float sinAOverA = sina * (1.0f / a);
374 ret.mVector.x = mVector.x * sinAOverA;
375 ret.mVector.y = mVector.y * sinAOverA;
376 ret.mVector.z = mVector.z * sinAOverA;
380 ret.mVector.x = ret.mVector.y = ret.mVector.z = 0.0f;
385 float Quaternion::Dot( const Quaternion& q1, const Quaternion& q2 )
387 return q1.mVector.Dot4(q2.mVector);
390 Quaternion Quaternion::Lerp(const Quaternion& q1, const Quaternion& q2, float t )
392 return (q1*(1.0f-t) + q2*t).Normalized();
395 Quaternion Quaternion::Slerp( const Quaternion& q1, const Quaternion& q2, float progress )
398 float cosTheta = Quaternion::Dot(q1, q2);
401 * If cos(theta) < 0, q1 and q2 are more than 90 degrees apart,
402 * so invert one to reduce spinning.
406 cosTheta = -cosTheta;
414 if (fabsf(cosTheta) < 0.95f)
416 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,5);
419 float sine = sqrtf(1.0f - cosTheta*cosTheta);
420 float angle = atan2f(sine, cosTheta);
421 float invSine = 1.0f / sine;
422 float coeff0 = sinf((1.0f - progress) * angle) * invSine;
423 float coeff1 = sinf(progress * angle) * invSine;
425 return q1*coeff0 + q3*coeff1;
429 // If the angle is small, use linear interpolation
430 Quaternion result = q1*(1.0f - progress) + q3*progress;
432 return result.Normalized();
436 Quaternion Quaternion::SlerpNoInvert( const Quaternion& q1, const Quaternion& q2, float t )
438 float cosTheta = Quaternion::Dot(q1, q2);
440 if (cosTheta > -0.95f && cosTheta < 0.95f)
442 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
444 float theta = acosf(cosTheta);
445 return (q1*sinf(theta*(1.0f-t)) + q2*sinf(theta*t))/sinf(theta);
449 return Lerp(q1, q2, t);
453 Quaternion Quaternion::Squad( const Quaternion& start, const Quaternion& end, const Quaternion& ctrl1, const Quaternion& ctrl2, float t )
455 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
457 Quaternion c = SlerpNoInvert( start, end, t );
458 Quaternion d = SlerpNoInvert( ctrl1, ctrl2, t );
459 return SlerpNoInvert( c, d, 2*t*(1-t) );
462 float Quaternion::AngleBetween( const Quaternion& q1, const Quaternion& q2 )
470 //Formula for angle θ between two quaternion is:
471 //θ = cos^−1 (2⟨q1,q2⟩^2 − 1), Where (q1,q2) is inner product of the quaternions.
472 float X = from.mVector.Dot4(to.mVector);
473 float theta = acos( (2 * X * X) - 1);
478 Vector4 Quaternion::Rotate( const Vector4& vector ) const
480 Quaternion V(0.0f, vector.x, vector.y, vector.z);
481 Quaternion conjugate(*this);
482 conjugate.Conjugate();
483 return (*this * V * conjugate).mVector;
486 Vector3 Quaternion::Rotate( const Vector3& vector ) const
488 Quaternion V(0.0f, vector.x, vector.y, vector.z);
489 Quaternion conjugate(*this);
490 conjugate.Conjugate();
491 return Vector3((*this * V * conjugate).mVector);
494 void Quaternion::SetFromAxes( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
496 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
498 float t = xAxis.x + yAxis.y + zAxis.z;
499 if ( t > 0.0f ) // w is largest
501 float root = sqrtf( t + 1.0f );
502 float one_over_4w = 0.5f / root;
503 mVector.x = ( yAxis.z - zAxis.y ) * one_over_4w;
504 mVector.y = ( zAxis.x - xAxis.z ) * one_over_4w;
505 mVector.z = ( xAxis.y - yAxis.x ) * one_over_4w;
506 mVector.w = root * 0.5f;
508 else if( zAxis.z > xAxis.x && zAxis.z > yAxis.y ) // z is largest
510 float root = sqrtf( zAxis.z - xAxis.x - yAxis.y + 1.0f );
511 float one_over_4w = 0.5f / root;
512 mVector.x = ( xAxis.z + zAxis.x ) * one_over_4w;
513 mVector.y = ( yAxis.z + zAxis.y ) * one_over_4w;
514 mVector.z = root * 0.5f;
515 mVector.w = ( xAxis.y - yAxis.x ) * one_over_4w;
517 else if( yAxis.y > xAxis.x ) // y is largest
519 float root = sqrtf(yAxis.y - zAxis.z - xAxis.x + 1.0f );
520 float one_over_4w = 0.5f / root;
522 mVector.x = ( xAxis.y + yAxis.x ) * one_over_4w;
523 mVector.y = root * 0.5f;
524 mVector.z = ( zAxis.y + yAxis.z ) * one_over_4w;
525 mVector.w = ( zAxis.x - xAxis.z ) * one_over_4w;
529 float root = sqrtf( xAxis.x - yAxis.y - zAxis.z + 1.0f );
530 float one_over_4w = 0.5f / root;
531 mVector.x = root * 0.5f;
532 mVector.y = ( yAxis.x + xAxis.y ) * one_over_4w;
533 mVector.z = ( zAxis.x + xAxis.z ) * one_over_4w;
534 mVector.w = ( yAxis.z - zAxis.y ) * one_over_4w;
540 std::ostream& operator<<( std::ostream& o, const Quaternion& quaternion )
545 quaternion.ToAxisAngle( axis, angleRadians );
546 Degree degrees( angleRadians );
548 return o << "[ Axis: [" << axis.x << ", " << axis.y << ", " << axis.z << "], Angle: " << degrees.degree << " degrees ]";