dali/public-api/math/quaternion.cpp

   1 /*
   2  * Copyright (c) 2014 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 // INTERNAL INCLUDES
  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>
  28
  29 // EXTERNAL INCLUDES
  30 #include <iostream>
  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 {
  54   mVector = vector;
  55 }
  56
  57 Quaternion::Quaternion(float 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 * 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(float theta, const Vector4 &axis)
  73 {
  74   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
  75
  76   Vector4 tmpAxis = axis;
  77   tmpAxis.Normalize();
  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;
  85 }
  86
  87 Quaternion::Quaternion(float x, float y, float z)
  88 {
  89   SetEuler(x,y,z);
  90 }
  91
  92 Quaternion::Quaternion(const Matrix& matrix)
  93 {
  94   Vector3 xAxis( matrix.GetXAxis() );
  95   Vector3 yAxis( matrix.GetYAxis() );
  96   Vector3 zAxis( matrix.GetZAxis() );
  97
  98   SetFromAxes( xAxis, yAxis, zAxis );
  99 }
 100
 101 Quaternion::Quaternion( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
 102 {
 103   SetFromAxes( xAxis, yAxis, zAxis );
 104 }
 105
 106 Quaternion::Quaternion( const Vector3& v0, const Vector3& v1 )
 107 {
 108   float dot = v0.Dot(v1);
 109   if( dot > 1.0f - Math::MACHINE_EPSILON_1 )
 110   {
 111     //Identity quaternion
 112     mVector.x = mVector.y = mVector.z = 0.0f;
 113     mVector.w = 1.0f;
 114   }
 115   else if( dot < -1.0f + Math::MACHINE_EPSILON_1)
 116   {
 117     //180 degree rotation across the Z axis
 118     mVector.x = mVector.y = mVector.w = 0.0f;
 119     mVector.z = 1.0f;
 120   }
 121   else
 122   {
 123     Vector3 w = v0.Cross(v1);
 124     mVector.w = 1.0f + dot;
 125     mVector.x = w.x;
 126     mVector.y = w.y;
 127     mVector.z = w.z;
 128     Normalize();
 129   }
 130 }
 131
 132 Quaternion Quaternion::FromAxisAngle(const Vector4 &axis, float angle)
 133 {
 134   return Quaternion(angle, axis);
 135 }
 136
 137 Quaternion::~Quaternion()
 138 {
 139 }
 140
 141 bool Quaternion::IsIdentity() const
 142 {
 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 ) );
 149 }
 150
 151 bool Quaternion::ToAxisAngle(Vector3 &axis, float &angle) const
 152 {
 153   angle = acosf(mVector.w);
 154   bool converted = false;
 155   // pre-compute to save time
 156   const float sine = sinf( angle );
 157
 158   // If sine(angle) is zero, conversion is not possible
 159
 160   if ( ! EqualsZero( sine ) )
 161   {
 162     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,3);
 163
 164     float sinf_theta_inv = 1.0f / sine;
 165
 166     axis.x = mVector.x*sinf_theta_inv;
 167     axis.y = mVector.y*sinf_theta_inv;
 168     axis.z = mVector.z*sinf_theta_inv;
 169     angle*=2.0f;
 170     converted = true;
 171   }
 172   return converted;
 173 }
 174
 175 bool Quaternion::ToAxisAngle(Vector4 &axis, float &angle) const
 176 {
 177   Vector3 axis3;
 178   bool converted = ToAxisAngle(axis3, angle);
 179   if(converted)
 180   {
 181     axis.x = axis3.x;
 182     axis.y = axis3.y;
 183     axis.z = axis3.z;
 184     axis.w = 0;
 185   }
 186   return converted;
 187 }
 188
 189 const Vector4& Quaternion::AsVector() const
 190 {
 191   return mVector;
 192 }
 193
 194 void Quaternion::SetEuler(float x, float y, float z)
 195 {
 196   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,19);
 197
 198   const float halfX = 0.5f * x;
 199   const float halfY = 0.5f * y;
 200   const float halfZ = 0.5f * z;
 201
 202   float cosX2 = cosf(halfX);
 203   float cosY2 = cosf(halfY);
 204   float cosZ2 = cosf(halfZ);
 205
 206   float sinX2 = sinf(halfX);
 207   float sinY2 = sinf(halfY);
 208   float sinZ2 = sinf(halfZ);
 209
 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;
 214 }
 215
 216 Vector4 Quaternion::EulerAngles() const
 217 {
 218   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,13);
 219
 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;
 224
 225   Vector4 euler;
 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);
 229   return euler;
 230 }
 231
 232 const Quaternion Quaternion::operator +(const Quaternion &other) const
 233 {
 234   return Quaternion(mVector + other.mVector);
 235 }
 236
 237 const Quaternion Quaternion::operator -(const Quaternion &other) const
 238 {
 239   return Quaternion(mVector - other.mVector);
 240 }
 241
 242 const Quaternion Quaternion::operator *(const Quaternion &other) const
 243 {
 244   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
 245
 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);
 250 }
 251
 252 Vector3 Quaternion::operator *(const Vector3& v) const
 253 {
 254   // nVidia SDK implementation
 255   Vector3 uv, uuv;
 256   Vector3 qvec(mVector.x, mVector.y, mVector.z);
 257   uv = qvec.Cross(v);
 258   uuv = qvec.Cross(uv);
 259   uv *= (2.0f * mVector.w);
 260   uuv *= 2.0f;
 261
 262   return v + uv + uuv;
 263 }
 264
 265 const Quaternion Quaternion::operator /(const Quaternion &q) const
 266 {
 267   Quaternion p(q);
 268   p.Invert();
 269   return *this * p;
 270 }
 271
 272 const Quaternion Quaternion::operator *(float scale) const
 273 {
 274   return Quaternion(mVector*scale);
 275 }
 276
 277 const Quaternion Quaternion::operator /(float scale) const
 278 {
 279   return Quaternion(mVector/scale);
 280 }
 281
 282 Quaternion Quaternion::operator -() const
 283 {
 284   return Quaternion(-mVector.w, -mVector.x, -mVector.y, -mVector.z);
 285 }
 286
 287 const Quaternion& Quaternion::operator +=(const Quaternion &q)
 288 {
 289   mVector += q.mVector; return *this;
 290 }
 291
 292 const Quaternion& Quaternion::operator -=(const Quaternion &q)
 293 {
 294   mVector -= q.mVector; return *this;
 295 }
 296
 297 const Quaternion& Quaternion::operator *=(const Quaternion &q)
 298 {
 299   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
 300
 301   float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
 302
 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;
 307   return *this;
 308 }
 309
 310 const Quaternion& Quaternion::operator *= (float scale)
 311 {
 312   mVector*=scale; return *this;
 313 }
 314
 315 const Quaternion& Quaternion::operator /= (float scale)
 316 {
 317   mVector/=scale; return *this;
 318 }
 319
 320 bool Quaternion::operator== (const Quaternion& rhs) const
 321 {
 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 )
 331          );
 332 }
 333
 334 bool Quaternion::operator!= (const Quaternion& rhs) const
 335 {
 336   return !operator==(rhs);
 337 }
 338
 339 float Quaternion::Length() const
 340 {
 341   return (float)sqrt(mVector.w * mVector.w + mVector.Dot(mVector));
 342 }
 343
 344 float Quaternion::LengthSquared() const
 345 {
 346   return (float)(mVector.w * mVector.w + mVector.Dot(mVector));
 347 }
 348
 349 void Quaternion::Normalize()
 350 {
 351   *this/=Length();
 352 }
 353
 354 Quaternion Quaternion::Normalized() const
 355 {
 356   return  *this/Length();
 357 }
 358
 359 void Quaternion::Conjugate()
 360 {
 361   mVector.x = -mVector.x;
 362   mVector.y = -mVector.y;
 363   mVector.z = -mVector.z;
 364 }
 365
 366 void Quaternion::Invert()
 367 {
 368   Conjugate();
 369   *this/=LengthSquared();
 370 }
 371
 372 Quaternion Quaternion::Log() const
 373 {
 374   float a = acosf(mVector.w);
 375   float sina = sinf(a);
 376   Quaternion ret;
 377
 378   ret.mVector.w = 0;
 379   if (fabsf(sina) >= Math::MACHINE_EPSILON_1)
 380   {
 381     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 382
 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;
 387   }
 388   else
 389   {
 390     ret.mVector.x= ret.mVector.y= ret.mVector.z= 0;
 391   }
 392   return ret;
 393 }
 394
 395 Quaternion Quaternion::Exp() const
 396 {
 397   DALI_ASSERT_ALWAYS( EqualsZero( mVector.w ) && "Cannot perform Exponent" );
 398
 399   float a = mVector.Length();
 400   float sina = sinf(a);
 401   Quaternion ret;
 402
 403   ret.mVector.w = cosf(a);
 404
 405   if (a >= Math::MACHINE_EPSILON_1)
 406   {
 407     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 408
 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;
 413   }
 414   else
 415   {
 416     ret.mVector.x = ret.mVector.y = ret.mVector.z = 0.0f;
 417   }
 418   return ret;
 419 }
 420
 421 float Quaternion::Dot(const Quaternion &q1, const Quaternion &q2)
 422 {
 423   return q1.mVector.Dot4(q2.mVector);
 424 }
 425
 426 Quaternion Quaternion::Lerp(const Quaternion &q1, const Quaternion &q2, float t)
 427 {
 428   return (q1*(1.0f-t) + q2*t).Normalized();
 429 }
 430
 431 Quaternion Quaternion::Slerp(const Quaternion &q1, const Quaternion &q2, float progress)
 432 {
 433   Quaternion q3;
 434   float cosTheta = Quaternion::Dot(q1, q2);
 435
 436   /**
 437    * If cos(theta) < 0, q1 and q2 are more than 90 degrees apart,
 438    * so invert one to reduce spinning.
 439    */
 440   if (cosTheta < 0.0f)
 441   {
 442     cosTheta = -cosTheta;
 443     q3 = -q2;
 444   }
 445   else
 446   {
 447     q3 = q2;
 448   }
 449
 450   if (fabsf(cosTheta) < 0.95f)
 451   {
 452     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,5);
 453
 454     // Normal SLERP
 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;
 460
 461     return q1*coeff0 + q3*coeff1;
 462   }
 463   else
 464   {
 465     // If the angle is small, use linear interpolation
 466     Quaternion result = q1*(1.0f - progress) + q3*progress;
 467
 468     return result.Normalized();
 469   }
 470 }
 471
 472 Quaternion Quaternion::SlerpNoInvert(const Quaternion &q1, const Quaternion &q2, float t)
 473 {
 474   float cosTheta = Quaternion::Dot(q1, q2);
 475
 476   if (cosTheta > -0.95f && cosTheta < 0.95f)
 477   {
 478     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
 479
 480     float theta = acosf(cosTheta);
 481     return (q1*sinf(theta*(1.0f-t)) + q2*sinf(theta*t))/sinf(theta);
 482   }
 483   else
 484   {
 485     return Lerp(q1, q2, t);
 486   }
 487 }
 488
 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
 494   float t)
 495 {
 496   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
 497
 498   Quaternion c = SlerpNoInvert(q1, q2, t);
 499   Quaternion d = SlerpNoInvert(a, b, t);
 500   return SlerpNoInvert(c, d, 2*t*(1-t));
 501 }
 502
 503 float Quaternion::AngleBetween(const Quaternion &q1, const Quaternion &q2)
 504 {
 505   Quaternion from(q1);
 506   Quaternion to(q2);
 507
 508   from.Normalize();
 509   to.Normalize();
 510
 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);
 515
 516   return theta;
 517 }
 518
 519 Vector4 Quaternion::Rotate(const Vector4 &v) const
 520 {
 521   Quaternion V(0.0f, v.x, v.y, v.z);
 522   Quaternion conjugate(*this);
 523   conjugate.Conjugate();
 524   return (*this * V * conjugate).mVector;
 525 }
 526
 527 Vector3 Quaternion::Rotate(const Vector3 &v) const
 528 {
 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);
 533 }
 534
 535 void Quaternion::SetFromAxes( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
 536 {
 537   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 538
 539   float t = xAxis.x + yAxis.y + zAxis.z;
 540   if ( t > 0.0f )                                      // w is largest
 541   {
 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;
 548   }
 549   else if( zAxis.z > xAxis.x && zAxis.z > yAxis.y )    // z is largest
 550   {
 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;
 557   }
 558   else if( yAxis.y > xAxis.x )                         // y is largest
 559   {
 560     float root = sqrtf(yAxis.y - zAxis.z - xAxis.x + 1.0f );
 561     float one_over_4w = 0.5f / root;
 562
 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;
 567   }
 568   else                                                 // x is largest
 569   {
 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;
 576   }
 577
 578   Normalize();
 579 }
 580
 581 std::ostream& operator<< (std::ostream& o, const Quaternion& quaternion)
 582 {
 583   Vector3 axis;
 584   float angleRadians;
 585
 586   quaternion.ToAxisAngle( axis, angleRadians );
 587   Degree degrees = Radian(angleRadians);
 588
 589   return o << "[ Axis: [" << axis.x << ", " << axis.y << ", " << axis.z << "], Angle: " << degrees << " degrees ]";
 590 }
 591
 592 } // namespace Dali
 593