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 namespace Dali
  30 {
  31 using Internal::PerformanceMonitor;
  32
  33 const Quaternion Quaternion::IDENTITY;
  34
  35
  36 /**
  37  * Default Constructor
  38  */
  39 Quaternion::Quaternion()
  40   : mVector(0.0f, 0.0f, 0.0f, 1.0f)
  41 {
  42 }
  43
  44 Quaternion::Quaternion(float cosThetaBy2, float iBySineTheta, float jBySineTheta, float kBySineTheta) :
  45   mVector(iBySineTheta, jBySineTheta, kBySineTheta, cosThetaBy2)
  46 {
  47 }
  48
  49 Quaternion::Quaternion(const Vector4& vector)
  50 {
  51   mVector = vector;
  52 }
  53
  54 Quaternion::Quaternion(float angle, const Vector3 &axis)
  55 {
  56   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
  57
  58   Vector3 tmpAxis = axis;
  59   tmpAxis.Normalize();
  60   const float halfAngle = angle * 0.5f;
  61   const float sinThetaByTwo = sinf(halfAngle);
  62   const float cosThetaByTwo = cosf(halfAngle);
  63   mVector.x = tmpAxis.x * sinThetaByTwo;
  64   mVector.y = tmpAxis.y * sinThetaByTwo;
  65   mVector.z = tmpAxis.z * sinThetaByTwo;
  66   mVector.w = cosThetaByTwo;
  67 }
  68
  69 Quaternion::Quaternion(float theta, const Vector4 &axis)
  70 {
  71   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
  72
  73   Vector4 tmpAxis = axis;
  74   tmpAxis.Normalize();
  75   const float halfTheta = theta * 0.5f;
  76   const float sinThetaByTwo = sinf(halfTheta);
  77   const float cosThetaByTwo = cosf(halfTheta);
  78   mVector.x = tmpAxis.x * sinThetaByTwo;
  79   mVector.y = tmpAxis.y * sinThetaByTwo;
  80   mVector.z = tmpAxis.z * sinThetaByTwo;
  81   mVector.w = cosThetaByTwo;
  82 }
  83
  84 Quaternion::Quaternion(float x, float y, float z)
  85 {
  86   SetEuler(x,y,z);
  87 }
  88
  89 Quaternion::Quaternion(const Matrix& matrix)
  90 {
  91   Vector3 xAxis( matrix.GetXAxis() );
  92   Vector3 yAxis( matrix.GetYAxis() );
  93   Vector3 zAxis( matrix.GetZAxis() );
  94
  95   SetFromAxes( xAxis, yAxis, zAxis );
  96 }
  97
  98 Quaternion::Quaternion( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
  99 {
 100   SetFromAxes( xAxis, yAxis, zAxis );
 101 }
 102
 103 Quaternion::Quaternion( const Vector3& v0, const Vector3& v1 )
 104 {
 105   float dot = v0.Dot(v1);
 106   if( dot > 1.0f - Math::MACHINE_EPSILON_1 )
 107   {
 108     //Identity quaternion
 109     mVector.x = mVector.y = mVector.z = 0.0f;
 110     mVector.w = 1.0f;
 111   }
 112   else if( dot < -1.0f + Math::MACHINE_EPSILON_1)
 113   {
 114     //180 degree rotation across the Z axis
 115     mVector.x = mVector.y = mVector.w = 0.0f;
 116     mVector.z = 1.0f;
 117   }
 118   else
 119   {
 120     Vector3 w = v0.Cross(v1);
 121     mVector.w = 1.0f + dot;
 122     mVector.x = w.x;
 123     mVector.y = w.y;
 124     mVector.z = w.z;
 125     Normalize();
 126   }
 127 }
 128
 129 Quaternion Quaternion::FromAxisAngle(const Vector4 &axis, float angle)
 130 {
 131   return Quaternion(angle, axis);
 132 }
 133
 134 Quaternion::~Quaternion()
 135 {
 136 }
 137
 138 bool Quaternion::ToAxisAngle(Vector3 &axis, float &angle) const
 139 {
 140   angle = acosf(mVector.w);
 141   bool converted = false;
 142   // pre-compute to save time
 143   const float sine = sinf( angle );
 144
 145   // If sine(angle) is zero, conversion is not possible
 146
 147   if ( ! EqualsZero( sine ) )
 148   {
 149     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,3);
 150
 151     float sinf_theta_inv = 1.0f / sine;
 152
 153     axis.x = mVector.x*sinf_theta_inv;
 154     axis.y = mVector.y*sinf_theta_inv;
 155     axis.z = mVector.z*sinf_theta_inv;
 156     angle*=2.0f;
 157     converted = true;
 158   }
 159   return converted;
 160 }
 161
 162 bool Quaternion::ToAxisAngle(Vector4 &axis, float &angle) const
 163 {
 164   Vector3 axis3;
 165   bool converted = ToAxisAngle(axis3, angle);
 166   if(converted)
 167   {
 168     axis.x = axis3.x;
 169     axis.y = axis3.y;
 170     axis.z = axis3.z;
 171     axis.w = 0;
 172   }
 173   return converted;
 174 }
 175
 176 const Vector4& Quaternion::AsVector() const
 177 {
 178   return mVector;
 179 }
 180
 181 void Quaternion::SetEuler(float x, float y, float z)
 182 {
 183   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,19);
 184
 185   const float halfX = 0.5f * x;
 186   const float halfY = 0.5f * y;
 187   const float halfZ = 0.5f * z;
 188
 189   float cosX2 = cosf(halfX);
 190   float cosY2 = cosf(halfY);
 191   float cosZ2 = cosf(halfZ);
 192
 193   float sinX2 = sinf(halfX);
 194   float sinY2 = sinf(halfY);
 195   float sinZ2 = sinf(halfZ);
 196
 197   mVector.w = cosZ2 * cosY2 * cosX2 + sinZ2 * sinY2 * sinX2;
 198   mVector.x = cosZ2 * cosY2 * sinX2 - sinZ2 * sinY2 * cosX2;
 199   mVector.y = cosZ2 * sinY2 * cosX2 + sinZ2 * cosY2 * sinX2;
 200   mVector.z = sinZ2 * cosY2 * cosX2 - cosZ2 * sinY2 * sinX2;
 201 }
 202
 203 Vector4 Quaternion::EulerAngles() const
 204 {
 205   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,13);
 206
 207   float sqw = mVector.w*mVector.w;
 208   float sqx = mVector.x*mVector.x;
 209   float sqy = mVector.y*mVector.y;
 210   float sqz = mVector.z*mVector.z;
 211
 212   Vector4 euler;
 213   euler.x = atan2f(2.0f * (mVector.y*mVector.z + mVector.x*mVector.w), -sqx - sqy + sqz + sqw);
 214   euler.y = asinf(-2.0f * (mVector.x*mVector.z - mVector.y*mVector.w));
 215   euler.z = atan2f(2.0f * (mVector.x*mVector.y + mVector.z*mVector.w), sqx - sqy - sqz + sqw);
 216   return euler;
 217 }
 218
 219 const Quaternion Quaternion::operator +(const Quaternion &other) const
 220 {
 221   return Quaternion(mVector + other.mVector);
 222 }
 223
 224 const Quaternion Quaternion::operator -(const Quaternion &other) const
 225 {
 226   return Quaternion(mVector - other.mVector);
 227 }
 228
 229 const Quaternion Quaternion::operator *(const Quaternion &other) const
 230 {
 231   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
 232
 233   return Quaternion(mVector.w * other.mVector.w - mVector.Dot(other.mVector),
 234                     mVector.y * other.mVector.z - mVector.z * other.mVector.y + mVector.w * other.mVector.x + mVector.x * other.mVector.w,
 235                     mVector.z * other.mVector.x - mVector.x * other.mVector.z + mVector.w * other.mVector.y + mVector.y * other.mVector.w,
 236                     mVector.x * other.mVector.y - mVector.y * other.mVector.x + mVector.w * other.mVector.z + mVector.z * other.mVector.w);
 237 }
 238
 239 Vector3 Quaternion::operator *(const Vector3& v) const
 240 {
 241   // nVidia SDK implementation
 242   Vector3 uv, uuv;
 243   Vector3 qvec(mVector.x, mVector.y, mVector.z);
 244   uv = qvec.Cross(v);
 245   uuv = qvec.Cross(uv);
 246   uv *= (2.0f * mVector.w);
 247   uuv *= 2.0f;
 248
 249   return v + uv + uuv;
 250 }
 251
 252 const Quaternion Quaternion::operator /(const Quaternion &q) const
 253 {
 254   Quaternion p(q);
 255   p.Invert();
 256   return *this * p;
 257 }
 258
 259 const Quaternion Quaternion::operator *(float scale) const
 260 {
 261   return Quaternion(mVector*scale);
 262 }
 263
 264 const Quaternion Quaternion::operator /(float scale) const
 265 {
 266   return Quaternion(mVector/scale);
 267 }
 268
 269 Quaternion Quaternion::operator -() const
 270 {
 271   return Quaternion(-mVector.w, -mVector.x, -mVector.y, -mVector.z);
 272 }
 273
 274 const Quaternion& Quaternion::operator +=(const Quaternion &q)
 275 {
 276   mVector += q.mVector; return *this;
 277 }
 278
 279 const Quaternion& Quaternion::operator -=(const Quaternion &q)
 280 {
 281   mVector -= q.mVector; return *this;
 282 }
 283
 284 const Quaternion& Quaternion::operator *=(const Quaternion &q)
 285 {
 286   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,12);
 287
 288   float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
 289
 290   mVector.w = mVector.w * q.mVector.w - mVector.Dot(q.mVector);
 291   mVector.x = y*q.mVector.z - z*q.mVector.y + w*q.mVector.x + x*q.mVector.w;
 292   mVector.y = z*q.mVector.x - x*q.mVector.z + w*q.mVector.y + y*q.mVector.w;
 293   mVector.z = x*q.mVector.y - y*q.mVector.x + w*q.mVector.z + z*q.mVector.w;
 294   return *this;
 295 }
 296
 297 const Quaternion& Quaternion::operator *= (float scale)
 298 {
 299   mVector*=scale; return *this;
 300 }
 301
 302 const Quaternion& Quaternion::operator /= (float scale)
 303 {
 304   mVector/=scale; return *this;
 305 }
 306
 307 bool Quaternion::operator== (const Quaternion& rhs) const
 308 {
 309   return ( ( fabsf(mVector.x - rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
 310              fabsf(mVector.y - rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
 311              fabsf(mVector.z - rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
 312              fabsf(mVector.w - rhs.mVector.w) < Math::MACHINE_EPSILON_1 ) ||
 313            // Or equal to negation of rhs
 314            ( fabsf(mVector.x + rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
 315              fabsf(mVector.y + rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
 316              fabsf(mVector.z + rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
 317              fabsf(mVector.w + rhs.mVector.w) < Math::MACHINE_EPSILON_1 )
 318          );
 319 }
 320
 321 bool Quaternion::operator!= (const Quaternion& rhs) const
 322 {
 323   return !operator==(rhs);
 324 }
 325
 326 float Quaternion::Length() const
 327 {
 328   return (float)sqrt(mVector.w * mVector.w + mVector.Dot(mVector));
 329 }
 330
 331 float Quaternion::LengthSquared() const
 332 {
 333   return (float)(mVector.w * mVector.w + mVector.Dot(mVector));
 334 }
 335
 336 void Quaternion::Normalize()
 337 {
 338   *this/=Length();
 339 }
 340
 341 Quaternion Quaternion::Normalized() const
 342 {
 343   return  *this/Length();
 344 }
 345
 346 void Quaternion::Conjugate()
 347 {
 348   mVector.x = -mVector.x;
 349   mVector.y = -mVector.y;
 350   mVector.z = -mVector.z;
 351 }
 352
 353 void Quaternion::Invert()
 354 {
 355   Conjugate();
 356   *this/=LengthSquared();
 357 }
 358
 359 Quaternion Quaternion::Log() const
 360 {
 361   float a = acosf(mVector.w);
 362   float sina = sinf(a);
 363   Quaternion ret;
 364
 365   ret.mVector.w = 0;
 366   if (fabsf(sina) >= Math::MACHINE_EPSILON_1)
 367   {
 368     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 369
 370     float angleBySinAngle = a * (1.0f / sina);
 371     ret.mVector.x = mVector.x * angleBySinAngle;
 372     ret.mVector.y = mVector.y * angleBySinAngle;
 373     ret.mVector.z = mVector.z * angleBySinAngle;
 374   }
 375   else
 376   {
 377     ret.mVector.x= ret.mVector.y= ret.mVector.z= 0;
 378   }
 379   return ret;
 380 }
 381
 382 Quaternion Quaternion::Exp() const
 383 {
 384   DALI_ASSERT_ALWAYS( EqualsZero( mVector.w ) && "Cannot perform Exponent" );
 385
 386   float a = mVector.Length();
 387   float sina = sinf(a);
 388   Quaternion ret;
 389
 390   ret.mVector.w = cosf(a);
 391
 392   if (a >= Math::MACHINE_EPSILON_1)
 393   {
 394     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 395
 396     float sinAOverA = sina * (1.0f / a);
 397     ret.mVector.x = mVector.x * sinAOverA;
 398     ret.mVector.y = mVector.y * sinAOverA;
 399     ret.mVector.z = mVector.z * sinAOverA;
 400   }
 401   else
 402   {
 403     ret.mVector.x = ret.mVector.y = ret.mVector.z = 0.0f;
 404   }
 405   return ret;
 406 }
 407
 408 float Quaternion::Dot(const Quaternion &q1, const Quaternion &q2)
 409 {
 410   return q1.mVector.Dot4(q2.mVector);
 411 }
 412
 413 Quaternion Quaternion::Lerp(const Quaternion &q1, const Quaternion &q2, float t)
 414 {
 415   return (q1*(1.0f-t) + q2*t).Normalized();
 416 }
 417
 418 Quaternion Quaternion::Slerp(const Quaternion &q1, const Quaternion &q2, float progress)
 419 {
 420   Quaternion q3;
 421   float cosTheta = Quaternion::Dot(q1, q2);
 422
 423   /**
 424    * If cos(theta) < 0, q1 and q2 are more than 90 degrees apart,
 425    * so invert one to reduce spinning.
 426    */
 427   if (cosTheta < 0.0f)
 428   {
 429     cosTheta = -cosTheta;
 430     q3 = -q2;
 431   }
 432   else
 433   {
 434     q3 = q2;
 435   }
 436
 437   if (fabsf(cosTheta) < 0.95f)
 438   {
 439     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,5);
 440
 441     // Normal SLERP
 442     float sine = sqrtf(1.0f - cosTheta*cosTheta);
 443     float angle = atan2f(sine, cosTheta);
 444     float invSine = 1.0f / sine;
 445     float coeff0 = sinf((1.0f - progress) * angle) * invSine;
 446     float coeff1 = sinf(progress * angle) * invSine;
 447
 448     return q1*coeff0 + q3*coeff1;
 449   }
 450   else
 451   {
 452     // If the angle is small, use linear interpolation
 453     Quaternion result = q1*(1.0f - progress) + q3*progress;
 454
 455     return result.Normalized();
 456   }
 457 }
 458
 459 Quaternion Quaternion::SlerpNoInvert(const Quaternion &q1, const Quaternion &q2, float t)
 460 {
 461   float cosTheta = Quaternion::Dot(q1, q2);
 462
 463   if (cosTheta > -0.95f && cosTheta < 0.95f)
 464   {
 465     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
 466
 467     float theta = acosf(cosTheta);
 468     return (q1*sinf(theta*(1.0f-t)) + q2*sinf(theta*t))/sinf(theta);
 469   }
 470   else
 471   {
 472     return Lerp(q1, q2, t);
 473   }
 474 }
 475
 476 Quaternion Quaternion::Squad(
 477   const Quaternion &q1, // start
 478   const Quaternion &q2, // end
 479   const Quaternion &a,  // ctrl pt for q1
 480   const Quaternion &b,  // ctrl pt for q2
 481   float t)
 482 {
 483   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,2);
 484
 485   Quaternion c = SlerpNoInvert(q1, q2, t);
 486   Quaternion d = SlerpNoInvert(a, b, t);
 487   return SlerpNoInvert(c, d, 2*t*(1-t));
 488 }
 489
 490 float Quaternion::AngleBetween(const Quaternion &q1, const Quaternion &q2)
 491 {
 492   Quaternion from(q1);
 493   Quaternion to(q2);
 494
 495   from.Normalize();
 496   to.Normalize();
 497
 498   //Formula for angle θ between two quaternion is:
 499   //θ = cos^−1 (2⟨q1,q2⟩^2 − 1), Where (q1,q2) is inner product of the quaternions.
 500   float X = from.mVector.Dot4(to.mVector);
 501   float theta = acos( (2 * X * X) - 1);
 502
 503   return theta;
 504 }
 505
 506 Vector4 Quaternion::Rotate(const Vector4 &v) const
 507 {
 508   Quaternion V(0.0f, v.x, v.y, v.z);
 509   Quaternion conjugate(*this);
 510   conjugate.Conjugate();
 511   return (*this * V * conjugate).mVector;
 512 }
 513
 514 Vector3 Quaternion::Rotate(const Vector3 &v) const
 515 {
 516   Quaternion V(0.0f, v.x, v.y, v.z);
 517   Quaternion conjugate(*this);
 518   conjugate.Conjugate();
 519   return Vector3((*this * V * conjugate).mVector);
 520 }
 521
 522 void Quaternion::SetFromAxes( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
 523 {
 524   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY,4);
 525
 526   float t = xAxis.x + yAxis.y + zAxis.z;
 527   if ( t > 0.0f )                                      // w is largest
 528   {
 529     float root = sqrtf( t + 1.0f );
 530     float one_over_4w = 0.5f / root;
 531     mVector.x = ( yAxis.z - zAxis.y ) * one_over_4w;
 532     mVector.y = ( zAxis.x - xAxis.z ) * one_over_4w;
 533     mVector.z = ( xAxis.y - yAxis.x ) * one_over_4w;
 534     mVector.w = root * 0.5f;
 535   }
 536   else if( zAxis.z > xAxis.x && zAxis.z > yAxis.y )    // z is largest
 537   {
 538     float root = sqrtf( zAxis.z - xAxis.x - yAxis.y + 1.0f );
 539     float one_over_4w = 0.5f / root;
 540     mVector.x = ( xAxis.z + zAxis.x ) * one_over_4w;
 541     mVector.y = ( yAxis.z + zAxis.y ) * one_over_4w;
 542     mVector.z = root * 0.5f;
 543     mVector.w = ( xAxis.y - yAxis.x ) * one_over_4w;
 544   }
 545   else if( yAxis.y > xAxis.x )                         // y is largest
 546   {
 547     float root = sqrtf(yAxis.y - zAxis.z - xAxis.x + 1.0f );
 548     float one_over_4w = 0.5f / root;
 549
 550     mVector.x = ( xAxis.y + yAxis.x ) * one_over_4w;
 551     mVector.y = root * 0.5f;
 552     mVector.z = ( zAxis.y + yAxis.z ) * one_over_4w;
 553     mVector.w = ( zAxis.x - xAxis.z ) * one_over_4w;
 554   }
 555   else                                                 // x is largest
 556   {
 557     float root = sqrtf( xAxis.x - yAxis.y - zAxis.z + 1.0f );
 558     float one_over_4w = 0.5f / root;
 559     mVector.x = root * 0.5f;
 560     mVector.y = ( yAxis.x + xAxis.y ) * one_over_4w;
 561     mVector.z = ( zAxis.x + xAxis.z ) * one_over_4w;
 562     mVector.w = ( yAxis.z - zAxis.y ) * one_over_4w;
 563   }
 564
 565   Normalize();
 566 }
 567
 568 std::ostream& operator<< (std::ostream& o, const Quaternion& quaternion)
 569 {
 570   Vector3 axis;
 571   float angleRadians;
 572
 573   quaternion.ToAxisAngle( axis, angleRadians );
 574   Degree degrees = Radian(angleRadians);
 575
 576   return o << "[ Axis: [" << axis.x << ", " << axis.y << ", " << axis.z << "], Angle: " << degrees << " degrees ]";
 577 }
 578
 579 } // namespace Dali
 580