dali/public-api/math/quaternion.cpp

   1 /*
   2  * Copyright (c) 2020 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/internal/render/common/performance-monitor.h>
  26 #include <dali/public-api/common/constants.h>
  27 #include <dali/public-api/math/degree.h>
  28 #include <dali/public-api/math/math-utils.h>
  29 #include <dali/public-api/math/matrix.h>
  30 #include <dali/public-api/math/radian.h>
  31
  32 namespace Dali
  33 {
  34 using Internal::PerformanceMonitor;
  35
  36 const Quaternion Quaternion::IDENTITY;
  37
  38 /**
  39  * Default Constructor
  40  */
  41 Quaternion::Quaternion()
  42 : mVector(0.0f, 0.0f, 0.0f, 1.0f)
  43 {
  44 }
  45
  46 Quaternion::Quaternion(float cosThetaBy2, float iBySineTheta, float jBySineTheta, float kBySineTheta)
  47 : mVector(iBySineTheta, jBySineTheta, kBySineTheta, cosThetaBy2)
  48 {
  49 }
  50
  51 Quaternion::Quaternion(const Vector4& vector)
  52 : mVector(vector)
  53 {
  54 }
  55
  56 Quaternion::Quaternion(Radian angle, const Vector3& axis)
  57 {
  58   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 4);
  59
  60   Vector3 tmpAxis = axis;
  61   tmpAxis.Normalize();
  62   const float halfAngle     = angle.radian * 0.5f;
  63   const float sinThetaByTwo = sinf(halfAngle);
  64   const float cosThetaByTwo = cosf(halfAngle);
  65   mVector.x                 = tmpAxis.x * sinThetaByTwo;
  66   mVector.y                 = tmpAxis.y * sinThetaByTwo;
  67   mVector.z                 = tmpAxis.z * sinThetaByTwo;
  68   mVector.w                 = cosThetaByTwo;
  69 }
  70
  71 Quaternion::Quaternion(Radian pitch, Radian yaw, Radian roll)
  72 {
  73   SetEuler(pitch, yaw, roll);
  74 }
  75
  76 Quaternion::Quaternion(const Matrix& matrix)
  77 {
  78   Vector3 xAxis(matrix.GetXAxis());
  79   Vector3 yAxis(matrix.GetYAxis());
  80   Vector3 zAxis(matrix.GetZAxis());
  81
  82   SetFromAxes(xAxis, yAxis, zAxis);
  83 }
  84
  85 Quaternion::Quaternion(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
  86 {
  87   SetFromAxes(xAxis, yAxis, zAxis);
  88 }
  89
  90 Quaternion::Quaternion(const Vector3& v0, const Vector3& v1)
  91 {
  92   float dot = v0.Dot(v1);
  93   if(dot > 1.0f - Math::MACHINE_EPSILON_1)
  94   {
  95     //Identity quaternion
  96     mVector.x = mVector.y = mVector.z = 0.0f;
  97     mVector.w                         = 1.0f;
  98   }
  99   else if(dot < -1.0f + Math::MACHINE_EPSILON_1)
 100   {
 101     //180 degree rotation across the Z axis
 102     mVector.x = mVector.y = mVector.w = 0.0f;
 103     mVector.z                         = 1.0f;
 104   }
 105   else
 106   {
 107     Vector3 w = v0.Cross(v1);
 108     mVector.w = 1.0f + dot;
 109     mVector.x = w.x;
 110     mVector.y = w.y;
 111     mVector.z = w.z;
 112     Normalize();
 113   }
 114 }
 115
 116 Quaternion::~Quaternion() = default;
 117
 118 bool Quaternion::IsIdentity() const
 119 {
 120   // start from w as its unlikely that any real rotation has w == 1
 121   // Uses a relaxed epsilon, as composition of rotation introduces error
 122   return ((fabsf(mVector.w - 1.0f) < Math::MACHINE_EPSILON_10) &&
 123           (fabsf(mVector.x) < Math::MACHINE_EPSILON_10) &&
 124           (fabsf(mVector.y) < Math::MACHINE_EPSILON_10) &&
 125           (fabsf(mVector.z) < Math::MACHINE_EPSILON_10));
 126 }
 127
 128 bool Quaternion::ToAxisAngle(Vector3& axis, Radian& angle) const
 129 {
 130   angle          = acosf(mVector.w);
 131   bool converted = false;
 132   // pre-compute to save time
 133   const float sine = sinf(angle.radian);
 134
 135   // If sine(angle) is zero, conversion is not possible
 136
 137   if(!EqualsZero(sine))
 138   {
 139     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 3);
 140
 141     float sinf_theta_inv = 1.0f / sine;
 142
 143     axis.x = mVector.x * sinf_theta_inv;
 144     axis.y = mVector.y * sinf_theta_inv;
 145     axis.z = mVector.z * sinf_theta_inv;
 146     angle.radian *= 2.0f;
 147     converted = true;
 148   }
 149   return converted;
 150 }
 151
 152 const Vector4& Quaternion::AsVector() const
 153 {
 154   return mVector;
 155 }
 156
 157 void Quaternion::SetEuler(Radian pitch, Radian yaw, Radian roll)
 158 {
 159   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 19);
 160
 161   const float halfX = 0.5f * pitch.radian;
 162   const float halfY = 0.5f * yaw.radian;
 163   const float halfZ = 0.5f * roll.radian;
 164
 165   float cosX2 = cosf(halfX);
 166   float cosY2 = cosf(halfY);
 167   float cosZ2 = cosf(halfZ);
 168
 169   float sinX2 = sinf(halfX);
 170   float sinY2 = sinf(halfY);
 171   float sinZ2 = sinf(halfZ);
 172
 173   mVector.w = cosZ2 * cosY2 * cosX2 + sinZ2 * sinY2 * sinX2;
 174   mVector.x = cosZ2 * cosY2 * sinX2 - sinZ2 * sinY2 * cosX2;
 175   mVector.y = cosZ2 * sinY2 * cosX2 + sinZ2 * cosY2 * sinX2;
 176   mVector.z = sinZ2 * cosY2 * cosX2 - cosZ2 * sinY2 * sinX2;
 177 }
 178
 179 Vector4 Quaternion::EulerAngles() const
 180 {
 181   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 13);
 182
 183   float sqw = mVector.w * mVector.w;
 184   float sqx = mVector.x * mVector.x;
 185   float sqy = mVector.y * mVector.y;
 186   float sqz = mVector.z * mVector.z;
 187
 188   Vector4 euler;
 189   euler.x = atan2f(2.0f * (mVector.y * mVector.z + mVector.x * mVector.w), -sqx - sqy + sqz + sqw);
 190   euler.y = asinf(-2.0f * (mVector.x * mVector.z - mVector.y * mVector.w));
 191   euler.z = atan2f(2.0f * (mVector.x * mVector.y + mVector.z * mVector.w), sqx - sqy - sqz + sqw);
 192   return euler;
 193 }
 194
 195 const Quaternion Quaternion::operator+(const Quaternion& other) const
 196 {
 197   return Quaternion(mVector + other.mVector);
 198 }
 199
 200 const Quaternion Quaternion::operator-(const Quaternion& other) const
 201 {
 202   return Quaternion(mVector - other.mVector);
 203 }
 204
 205 const Quaternion Quaternion::operator*(const Quaternion& other) const
 206 {
 207   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 12);
 208
 209   return Quaternion(mVector.w * other.mVector.w - mVector.Dot(other.mVector),
 210                     mVector.y * other.mVector.z - mVector.z * other.mVector.y + mVector.w * other.mVector.x + mVector.x * other.mVector.w,
 211                     mVector.z * other.mVector.x - mVector.x * other.mVector.z + mVector.w * other.mVector.y + mVector.y * other.mVector.w,
 212                     mVector.x * other.mVector.y - mVector.y * other.mVector.x + mVector.w * other.mVector.z + mVector.z * other.mVector.w);
 213 }
 214
 215 Vector3 Quaternion::operator*(const Vector3& other) const
 216 {
 217   Vector3 qvec(mVector.x, mVector.y, mVector.z);
 218   Vector3 uv  = qvec.Cross(other);
 219   Vector3 uuv = qvec.Cross(uv);
 220   uv *= (2.0f * mVector.w);
 221   uuv *= 2.0f;
 222
 223   return other + 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;
 251   return *this;
 252 }
 253
 254 const Quaternion& Quaternion::operator-=(const Quaternion& q)
 255 {
 256   mVector -= q.mVector;
 257   return *this;
 258 }
 259
 260 const Quaternion& Quaternion::operator*=(const Quaternion& q)
 261 {
 262   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 12);
 263
 264   float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
 265
 266   mVector.w = mVector.w * q.mVector.w - mVector.Dot(q.mVector);
 267   mVector.x = y * q.mVector.z - z * q.mVector.y + w * q.mVector.x + x * q.mVector.w;
 268   mVector.y = z * q.mVector.x - x * q.mVector.z + w * q.mVector.y + y * q.mVector.w;
 269   mVector.z = x * q.mVector.y - y * q.mVector.x + w * q.mVector.z + z * q.mVector.w;
 270   return *this;
 271 }
 272
 273 const Quaternion& Quaternion::operator*=(float scale)
 274 {
 275   mVector *= scale;
 276   return *this;
 277 }
 278
 279 const Quaternion& Quaternion::operator/=(float scale)
 280 {
 281   mVector /= scale;
 282   return *this;
 283 }
 284
 285 bool Quaternion::operator==(const Quaternion& rhs) const
 286 {
 287   return ((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           // Or equal to negation of rhs
 292           (fabsf(mVector.x + rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
 293            fabsf(mVector.y + rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
 294            fabsf(mVector.z + rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
 295            fabsf(mVector.w + rhs.mVector.w) < Math::MACHINE_EPSILON_1));
 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