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