dali/public-api/math/quaternion.cpp

   1 /*
   2  * Copyright (c) 2018 Samsung Electronics Co., Ltd.
   3  *
   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
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  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.
  15  *
  16  */
  17
  18 // CLASS HEADER
  19 #include <dali/public-api/math/quaternion.h>
  20
  21 // EXTERNAL INCLUDES
  22 #include <ostream>
  23
  24 // INTERNAL INCLUDES
  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>
  31
  32 namespace Dali
  33 {
  34 using Internal::PerformanceMonitor;
  35
  36 const Quaternion Quaternion::IDENTITY;
  37
  38
  39 /**
  40  * Default Constructor
  41  */
  42 Quaternion::Quaternion()
  43 : mVector( 0.0f, 0.0f, 0.0f, 1.0f )
  44 {
  45 }
  46
  47 Quaternion::Quaternion( float cosThetaBy2, float iBySineTheta, float jBySineTheta, float kBySineTheta )
  48 : mVector( iBySineTheta, jBySineTheta, kBySineTheta, cosThetaBy2 )
  49 {
  50 }
  51
  52 Quaternion::Quaternion( const Vector4& vector )
  53 : mVector( vector )
  54 {
  55 }
  56
  57 Quaternion::Quaternion( Radian angle, const Vector3& axis )
  58 {
  59   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
  60
  61   Vector3 tmpAxis = axis;
  62   tmpAxis.Normalize();
  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;
  70 }
  71
  72 Quaternion::Quaternion( Radian pitch, Radian yaw, Radian roll )
  73 {
  74   SetEuler( pitch, yaw, roll );
  75 }
  76
  77 Quaternion::Quaternion( const Matrix& matrix )
  78 {
  79   Vector3 xAxis( matrix.GetXAxis() );
  80   Vector3 yAxis( matrix.GetYAxis() );
  81   Vector3 zAxis( matrix.GetZAxis() );
  82
  83   SetFromAxes( xAxis, yAxis, zAxis );
  84 }
  85
  86 Quaternion::Quaternion( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
  87 {
  88   SetFromAxes( xAxis, yAxis, zAxis );
  89 }
  90
  91 Quaternion::Quaternion( const Vector3& v0, const Vector3& v1 )
  92 {
  93   float dot = v0.Dot(v1);
  94   if( dot > 1.0f - Math::MACHINE_EPSILON_1 )
  95   {
  96     //Identity quaternion
  97     mVector.x = mVector.y = mVector.z = 0.0f;
  98     mVector.w = 1.0f;
  99   }
 100   else if( dot < -1.0f + Math::MACHINE_EPSILON_1)
 101   {
 102     //180 degree rotation across the Z axis
 103     mVector.x = mVector.y = mVector.w = 0.0f;
 104     mVector.z = 1.0f;
 105   }
 106   else
 107   {
 108     Vector3 w = v0.Cross(v1);
 109     mVector.w = 1.0f + dot;
 110     mVector.x = w.x;
 111     mVector.y = w.y;
 112     mVector.z = w.z;
 113     Normalize();
 114   }
 115 }
 116
 117 Quaternion::~Quaternion()
 118 {
 119 }
 120
 121 bool Quaternion::IsIdentity() const
 122 {
 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 ) );
 129 }
 130
 131 bool Quaternion::ToAxisAngle(Vector3& axis, Radian& angle) const
 132 {
 133   angle = acosf(mVector.w);
 134   bool converted = false;
 135   // pre-compute to save time
 136   const float sine = sinf( angle.radian );
 137
 138   // If sine(angle) is zero, conversion is not possible
 139
 140   if ( ! EqualsZero( sine ) )
 141   {
 142     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,3);
 143
 144     float sinf_theta_inv = 1.0f / sine;
 145
 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;
 150     converted = true;
 151   }
 152   return converted;
 153 }
 154
 155 const Vector4& Quaternion::AsVector() const
 156 {
 157   return mVector;
 158 }
 159
 160 void Quaternion::SetEuler( Radian pitch, Radian yaw, Radian roll )
 161 {
 162   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,19);
 163
 164   const float halfX = 0.5f * pitch.radian;
 165   const float halfY = 0.5f * yaw.radian;
 166   const float halfZ = 0.5f * roll.radian;
 167
 168   float cosX2 = cosf(halfX);
 169   float cosY2 = cosf(halfY);
 170   float cosZ2 = cosf(halfZ);
 171
 172   float sinX2 = sinf(halfX);
 173   float sinY2 = sinf(halfY);
 174   float sinZ2 = sinf(halfZ);
 175
 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;
 180 }
 181
 182 Vector4 Quaternion::EulerAngles() const
 183 {
 184   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,13);
 185
 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;
 190
 191   Vector4 euler;
 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);
 195   return euler;
 196 }
 197
 198 const Quaternion Quaternion::operator+( const Quaternion& other ) const
 199 {
 200   return Quaternion(mVector + other.mVector);
 201 }
 202
 203 const Quaternion Quaternion::operator-( const Quaternion& other ) const
 204 {
 205   return Quaternion(mVector - other.mVector);
 206 }
 207
 208 const Quaternion Quaternion::operator*( const Quaternion& other ) const
 209 {
 210   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
 211
 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);
 216 }
 217
 218 Vector3 Quaternion::operator*( const Vector3& other ) const
 219 {
 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);
 224   uuv *= 2.0f;
 225
 226   return other + uv + uuv;
 227 }
 228
 229 const Quaternion Quaternion::operator/( const Quaternion& q ) const
 230 {
 231   Quaternion p(q);
 232   p.Invert();
 233   return *this * p;
 234 }
 235
 236 const Quaternion Quaternion::operator*( float scale ) const
 237 {
 238   return Quaternion(mVector*scale);
 239 }
 240
 241 const Quaternion Quaternion::operator/( float scale ) const
 242 {
 243   return Quaternion(mVector/scale);
 244 }
 245
 246 Quaternion Quaternion::operator-() const
 247 {
 248   return Quaternion(-mVector.w, -mVector.x, -mVector.y, -mVector.z);
 249 }
 250
 251 const Quaternion& Quaternion::operator+=( const Quaternion& q )
 252 {
 253   mVector += q.mVector; return *this;
 254 }
 255
 256 const Quaternion& Quaternion::operator-=( const Quaternion& q )
 257 {
 258   mVector -= q.mVector; return *this;
 259 }
 260
 261 const Quaternion& Quaternion::operator*=( const Quaternion& q )
 262 {
 263   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
 264
 265   float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
 266
 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;
 271   return *this;
 272 }
 273
 274 const Quaternion& Quaternion::operator*=( float scale )
 275 {
 276   mVector*=scale; return *this;
 277 }
 278
 279 const Quaternion& Quaternion::operator/=( float scale )
 280 {
 281   mVector/=scale; return *this;
 282 }
 283
 284 bool Quaternion::operator==( const Quaternion& rhs ) const
 285 {
 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 )
 295          );
 296 }
 297
 298 bool Quaternion::operator!=( const Quaternion& rhs ) const
 299 {
 300   return !operator==(rhs);
 301 }
 302
 303 float Quaternion::Length() const
 304 {
 305   return static_cast< float >( sqrt(mVector.w * mVector.w + mVector.Dot(mVector) ) );
 306 }
 307
 308 float Quaternion::LengthSquared() const
 309 {
 310   return static_cast< float >( mVector.w * mVector.w + mVector.Dot(mVector) );
 311 }
 312
 313 void Quaternion::Normalize()
 314 {
 315   *this/=Length();
 316 }
 317
 318 Quaternion Quaternion::Normalized() const
 319 {
 320   return  *this/Length();
 321 }
 322
 323 void Quaternion::Conjugate()
 324 {
 325   mVector.x = -mVector.x;
 326   mVector.y = -mVector.y;
 327   mVector.z = -mVector.z;
 328 }
 329
 330 void Quaternion::Invert()
 331 {
 332   Conjugate();
 333   *this/=LengthSquared();
 334 }
 335
 336 Quaternion Quaternion::Log() const
 337 {
 338   float a = acosf(mVector.w);
 339   float sina = sinf(a);
 340   Quaternion ret;
 341
 342   ret.mVector.w = 0;
 343   if (fabsf(sina) >= Math::MACHINE_EPSILON_1)
 344   {
 345     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 346
 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;
 351   }
 352   else
 353   {
 354     ret.mVector.x= ret.mVector.y= ret.mVector.z= 0;
 355   }
 356   return ret;
 357 }
 358
 359 Quaternion Quaternion::Exp() const
 360 {
 361   DALI_ASSERT_ALWAYS( EqualsZero( mVector.w ) && "Cannot perform Exponent" );
 362
 363   float a = mVector.Length();
 364   float sina = sinf(a);
 365   Quaternion ret;
 366
 367   ret.mVector.w = cosf(a);
 368
 369   if (a >= Math::MACHINE_EPSILON_1)
 370   {
 371     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 372
 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;
 377   }
 378   else
 379   {
 380     ret.mVector.x = ret.mVector.y = ret.mVector.z = 0.0f;
 381   }
 382   return ret;
 383 }
 384
 385 float Quaternion::Dot( const Quaternion& q1, const Quaternion& q2 )
 386 {
 387   return q1.mVector.Dot4(q2.mVector);
 388 }
 389
 390 Quaternion Quaternion::Lerp(const Quaternion& q1, const Quaternion& q2, float t )
 391 {
 392   return (q1*(1.0f-t) + q2*t).Normalized();
 393 }
 394
 395 Quaternion Quaternion::Slerp( const Quaternion& q1, const Quaternion& q2, float progress )
 396 {
 397   Quaternion q3;
 398   float cosTheta = Quaternion::Dot(q1, q2);
 399
 400   /**
 401    * If cos(theta) < 0, q1 and q2 are more than 90 degrees apart,
 402    * so invert one to reduce spinning.
 403    */
 404   if (cosTheta < 0.0f)
 405   {
 406     cosTheta = -cosTheta;
 407     q3 = -q2;
 408   }
 409   else
 410   {
 411     q3 = q2;
 412   }
 413
 414   if (fabsf(cosTheta) < 0.95f)
 415   {
 416     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,5);
 417
 418     // Normal SLERP
 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;
 424
 425     return q1*coeff0 + q3*coeff1;
 426   }
 427   else
 428   {
 429     // If the angle is small, use linear interpolation
 430     Quaternion result = q1*(1.0f - progress) + q3*progress;
 431
 432     return result.Normalized();
 433   }
 434 }
 435
 436 Quaternion Quaternion::SlerpNoInvert( const Quaternion& q1, const Quaternion& q2, float t )
 437 {
 438   float cosTheta = Quaternion::Dot(q1, q2);
 439
 440   if (cosTheta > -0.95f && cosTheta < 0.95f)
 441   {
 442     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
 443
 444     float theta = acosf(cosTheta);
 445     return (q1*sinf(theta*(1.0f-t)) + q2*sinf(theta*t))/sinf(theta);
 446   }
 447   else
 448   {
 449     return Lerp(q1, q2, t);
 450   }
 451 }
 452
 453 Quaternion Quaternion::Squad( const Quaternion& start, const Quaternion& end,  const Quaternion& ctrl1,  const Quaternion& ctrl2, float t )
 454 {
 455   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
 456
 457   Quaternion c = SlerpNoInvert( start, end, t );
 458   Quaternion d = SlerpNoInvert( ctrl1, ctrl2, t );
 459   return SlerpNoInvert( c, d, 2*t*(1-t) );
 460 }
 461
 462 float Quaternion::AngleBetween( const Quaternion& q1, const Quaternion& q2 )
 463 {
 464   Quaternion from(q1);
 465   Quaternion to(q2);
 466
 467   from.Normalize();
 468   to.Normalize();
 469
 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 = acosf( (2 * X * X) - 1); // float arc cosine
 474
 475   return theta;
 476 }
 477
 478 Vector4 Quaternion::Rotate( const Vector4& vector ) const
 479 {
 480   Quaternion V(0.0f, vector.x, vector.y, vector.z);
 481   Quaternion conjugate(*this);
 482   conjugate.Conjugate();
 483   return (*this * V * conjugate).mVector;
 484 }
 485
 486 Vector3 Quaternion::Rotate( const Vector3& vector ) const
 487 {
 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);
 492 }
 493
 494 void Quaternion::SetFromAxes( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
 495 {
 496   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 497
 498   float t = xAxis.x + yAxis.y + zAxis.z;
 499   if ( t > 0.0f )                                      // w is largest
 500   {
 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;
 507   }
 508   else if( zAxis.z > xAxis.x && zAxis.z > yAxis.y )    // z is largest
 509   {
 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;
 516   }
 517   else if( yAxis.y > xAxis.x )                         // y is largest
 518   {
 519     float root = sqrtf(yAxis.y - zAxis.z - xAxis.x + 1.0f );
 520     float one_over_4w = 0.5f / root;
 521
 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;
 526   }
 527   else                                                 // x is largest
 528   {
 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;
 535   }
 536
 537   Normalize();
 538 }
 539
 540 std::ostream& operator<<( std::ostream& o, const Quaternion& quaternion )
 541 {
 542   Vector3 axis;
 543   Radian angleRadians;
 544
 545   quaternion.ToAxisAngle( axis, angleRadians );
 546   Degree degrees( angleRadians );
 547
 548   return o << "[ Axis: [" << axis.x << ", " << axis.y << ", " << axis.z << "], Angle: " << degrees.degree << " degrees ]";
 549 }
 550
 551 } // namespace Dali