dali/public-api/math/quaternion.cpp

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