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()
 117 {
 118 }
 119
 120 bool Quaternion::IsIdentity() const
 121 {
 122   // start from w as its unlikely that any real rotation has w == 1
 123   // Uses a relaxed epsilon, as composition of rotation introduces error
 124   return ((fabsf(mVector.w - 1.0f) < Math::MACHINE_EPSILON_10) &&
 125           (fabsf(mVector.x) < Math::MACHINE_EPSILON_10) &&
 126           (fabsf(mVector.y) < Math::MACHINE_EPSILON_10) &&
 127           (fabsf(mVector.z) < Math::MACHINE_EPSILON_10));
 128 }
 129
 130 bool Quaternion::ToAxisAngle(Vector3& axis, Radian& angle) const
 131 {
 132   angle          = acosf(mVector.w);
 133   bool converted = false;
 134   // pre-compute to save time
 135   const float sine = sinf(angle.radian);
 136
 137   // If sine(angle) is zero, conversion is not possible
 138
 139   if(!EqualsZero(sine))
 140   {
 141     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 3);
 142
 143     float sinf_theta_inv = 1.0f / sine;
 144
 145     axis.x = mVector.x * sinf_theta_inv;
 146     axis.y = mVector.y * sinf_theta_inv;
 147     axis.z = mVector.z * sinf_theta_inv;
 148     angle.radian *= 2.0f;
 149     converted = true;
 150   }
 151   return converted;
 152 }
 153
 154 const Vector4& Quaternion::AsVector() const
 155 {
 156   return mVector;
 157 }
 158
 159 void Quaternion::SetEuler(Radian pitch, Radian yaw, Radian roll)
 160 {
 161   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 19);
 162
 163   const float halfX = 0.5f * pitch.radian;
 164   const float halfY = 0.5f * yaw.radian;
 165   const float halfZ = 0.5f * roll.radian;
 166
 167   float cosX2 = cosf(halfX);
 168   float cosY2 = cosf(halfY);
 169   float cosZ2 = cosf(halfZ);
 170
 171   float sinX2 = sinf(halfX);
 172   float sinY2 = sinf(halfY);
 173   float sinZ2 = sinf(halfZ);
 174
 175   mVector.w = cosZ2 * cosY2 * cosX2 + sinZ2 * sinY2 * sinX2;
 176   mVector.x = cosZ2 * cosY2 * sinX2 - sinZ2 * sinY2 * cosX2;
 177   mVector.y = cosZ2 * sinY2 * cosX2 + sinZ2 * cosY2 * sinX2;
 178   mVector.z = sinZ2 * cosY2 * cosX2 - cosZ2 * sinY2 * sinX2;
 179 }
 180
 181 Vector4 Quaternion::EulerAngles() const
 182 {
 183   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 13);
 184
 185   float sqw = mVector.w * mVector.w;
 186   float sqx = mVector.x * mVector.x;
 187   float sqy = mVector.y * mVector.y;
 188   float sqz = mVector.z * mVector.z;
 189
 190   Vector4 euler;
 191   euler.x = atan2f(2.0f * (mVector.y * mVector.z + mVector.x * mVector.w), -sqx - sqy + sqz + sqw);
 192   euler.y = asinf(-2.0f * (mVector.x * mVector.z - mVector.y * mVector.w));
 193   euler.z = atan2f(2.0f * (mVector.x * mVector.y + mVector.z * mVector.w), sqx - sqy - sqz + sqw);
 194   return euler;
 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   return Quaternion(mVector - other.mVector);
 205 }
 206
 207 const Quaternion Quaternion::operator*(const Quaternion& other) const
 208 {
 209   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 12);
 210
 211   return Quaternion(mVector.w * other.mVector.w - mVector.Dot(other.mVector),
 212                     mVector.y * other.mVector.z - mVector.z * other.mVector.y + mVector.w * other.mVector.x + mVector.x * other.mVector.w,
 213                     mVector.z * other.mVector.x - mVector.x * other.mVector.z + mVector.w * other.mVector.y + mVector.y * other.mVector.w,
 214                     mVector.x * other.mVector.y - mVector.y * other.mVector.x + mVector.w * other.mVector.z + mVector.z * other.mVector.w);
 215 }
 216
 217 Vector3 Quaternion::operator*(const Vector3& other) const
 218 {
 219   Vector3 qvec(mVector.x, mVector.y, mVector.z);
 220   Vector3 uv  = qvec.Cross(other);
 221   Vector3 uuv = qvec.Cross(uv);
 222   uv *= (2.0f * mVector.w);
 223   uuv *= 2.0f;
 224
 225   return other + uv + uuv;
 226 }
 227
 228 const Quaternion Quaternion::operator/(const Quaternion& q) const
 229 {
 230   Quaternion p(q);
 231   p.Invert();
 232   return *this * p;
 233 }
 234
 235 const Quaternion Quaternion::operator*(float scale) const
 236 {
 237   return Quaternion(mVector * scale);
 238 }
 239
 240 const Quaternion Quaternion::operator/(float scale) const
 241 {
 242   return Quaternion(mVector / scale);
 243 }
 244
 245 Quaternion Quaternion::operator-() const
 246 {
 247   return Quaternion(-mVector.w, -mVector.x, -mVector.y, -mVector.z);
 248 }
 249
 250 const Quaternion& Quaternion::operator+=(const Quaternion& q)
 251 {
 252   mVector += q.mVector;
 253   return *this;
 254 }
 255
 256 const Quaternion& Quaternion::operator-=(const Quaternion& q)
 257 {
 258   mVector -= q.mVector;
 259   return *this;
 260 }
 261
 262 const Quaternion& Quaternion::operator*=(const Quaternion& q)
 263 {
 264   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 12);
 265
 266   float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
 267
 268   mVector.w = mVector.w * q.mVector.w - mVector.Dot(q.mVector);
 269   mVector.x = y * q.mVector.z - z * q.mVector.y + w * q.mVector.x + x * q.mVector.w;
 270   mVector.y = z * q.mVector.x - x * q.mVector.z + w * q.mVector.y + y * q.mVector.w;
 271   mVector.z = x * q.mVector.y - y * q.mVector.x + w * q.mVector.z + z * q.mVector.w;
 272   return *this;
 273 }
 274
 275 const Quaternion& Quaternion::operator*=(float scale)
 276 {
 277   mVector *= scale;
 278   return *this;
 279 }
 280
 281 const Quaternion& Quaternion::operator/=(float scale)
 282 {
 283   mVector /= scale;
 284   return *this;
 285 }
 286
 287 bool Quaternion::operator==(const Quaternion& rhs) const
 288 {
 289   return ((fabsf(mVector.x - rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
 290            fabsf(mVector.y - rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
 291            fabsf(mVector.z - rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
 292            fabsf(mVector.w - rhs.mVector.w) < Math::MACHINE_EPSILON_1) ||
 293           // Or equal to negation of rhs
 294           (fabsf(mVector.x + rhs.mVector.x) < Math::MACHINE_EPSILON_1 &&
 295            fabsf(mVector.y + rhs.mVector.y) < Math::MACHINE_EPSILON_1 &&
 296            fabsf(mVector.z + rhs.mVector.z) < Math::MACHINE_EPSILON_1 &&
 297            fabsf(mVector.w + rhs.mVector.w) < Math::MACHINE_EPSILON_1));
 298 }
 299
 300 bool Quaternion::operator!=(const Quaternion& rhs) const
 301 {
 302   return !operator==(rhs);
 303 }
 304
 305 float Quaternion::Length() const
 306 {
 307   return static_cast<float>(sqrt(mVector.w * mVector.w + mVector.Dot(mVector)));
 308 }
 309
 310 float Quaternion::LengthSquared() const
 311 {
 312   return static_cast<float>(mVector.w * mVector.w + mVector.Dot(mVector));
 313 }
 314
 315 void Quaternion::Normalize()
 316 {
 317   *this /= Length();
 318 }
 319
 320 Quaternion Quaternion::Normalized() const
 321 {
 322   return *this / Length();
 323 }
 324
 325 void Quaternion::Conjugate()
 326 {
 327   mVector.x = -mVector.x;
 328   mVector.y = -mVector.y;
 329   mVector.z = -mVector.z;
 330 }
 331
 332 void Quaternion::Invert()
 333 {
 334   Conjugate();
 335   *this /= LengthSquared();
 336 }
 337
 338 Quaternion Quaternion::Log() const
 339 {
 340   float      a    = acosf(mVector.w);
 341   float      sina = sinf(a);
 342   Quaternion ret;
 343
 344   ret.mVector.w = 0;
 345   if(fabsf(sina) >= Math::MACHINE_EPSILON_1)
 346   {
 347     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 4);
 348
 349     float angleBySinAngle = a * (1.0f / sina);
 350     ret.mVector.x         = mVector.x * angleBySinAngle;
 351     ret.mVector.y         = mVector.y * angleBySinAngle;
 352     ret.mVector.z         = mVector.z * angleBySinAngle;
 353   }
 354   else
 355   {
 356     ret.mVector.x = ret.mVector.y = ret.mVector.z = 0;
 357   }
 358   return ret;
 359 }
 360
 361 Quaternion Quaternion::Exp() const
 362 {
 363   DALI_ASSERT_ALWAYS(EqualsZero(mVector.w) && "Cannot perform Exponent");
 364
 365   float      a    = mVector.Length();
 366   float      sina = sinf(a);
 367   Quaternion ret;
 368
 369   ret.mVector.w = cosf(a);
 370
 371   if(a >= Math::MACHINE_EPSILON_1)
 372   {
 373     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 4);
 374
 375     float sinAOverA = sina * (1.0f / a);
 376     ret.mVector.x   = mVector.x * sinAOverA;
 377     ret.mVector.y   = mVector.y * sinAOverA;
 378     ret.mVector.z   = mVector.z * sinAOverA;
 379   }
 380   else
 381   {
 382     ret.mVector.x = ret.mVector.y = ret.mVector.z = 0.0f;
 383   }
 384   return ret;
 385 }
 386
 387 float Quaternion::Dot(const Quaternion& q1, const Quaternion& q2)
 388 {
 389   return q1.mVector.Dot4(q2.mVector);
 390 }
 391
 392 Quaternion Quaternion::Lerp(const Quaternion& q1, const Quaternion& q2, float t)
 393 {
 394   return (q1 * (1.0f - t) + q2 * t).Normalized();
 395 }
 396
 397 Quaternion Quaternion::Slerp(const Quaternion& q1, const Quaternion& q2, float progress)
 398 {
 399   Quaternion q3;
 400   float      cosTheta = Quaternion::Dot(q1, q2);
 401
 402   /**
 403    * If cos(theta) < 0, q1 and q2 are more than 90 degrees apart,
 404    * so invert one to reduce spinning.
 405    */
 406   if(cosTheta < 0.0f)
 407   {
 408     cosTheta = -cosTheta;
 409     q3       = -q2;
 410   }
 411   else
 412   {
 413     q3 = q2;
 414   }
 415
 416   if(fabsf(cosTheta) < 0.95f)
 417   {
 418     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 5);
 419
 420     // Normal SLERP
 421     float sine    = sqrtf(1.0f - cosTheta * cosTheta);
 422     float angle   = atan2f(sine, cosTheta);
 423     float invSine = 1.0f / sine;
 424     float coeff0  = sinf((1.0f - progress) * angle) * invSine;
 425     float coeff1  = sinf(progress * angle) * invSine;
 426
 427     return q1 * coeff0 + q3 * coeff1;
 428   }
 429   else
 430   {
 431     // If the angle is small, use linear interpolation
 432     Quaternion result = q1 * (1.0f - progress) + q3 * progress;
 433
 434     return result.Normalized();
 435   }
 436 }
 437
 438 Quaternion Quaternion::SlerpNoInvert(const Quaternion& q1, const Quaternion& q2, float t)
 439 {
 440   float cosTheta = Quaternion::Dot(q1, q2);
 441
 442   if(cosTheta > -0.95f && cosTheta < 0.95f)
 443   {
 444     MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 2);
 445
 446     float theta = acosf(cosTheta);
 447     return (q1 * sinf(theta * (1.0f - t)) + q2 * sinf(theta * t)) / sinf(theta);
 448   }
 449   else
 450   {
 451     return Lerp(q1, q2, t);
 452   }
 453 }
 454
 455 Quaternion Quaternion::Squad(const Quaternion& start, const Quaternion& end, const Quaternion& ctrl1, const Quaternion& ctrl2, float t)
 456 {
 457   MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 2);
 458
 459   Quaternion c = SlerpNoInvert(start, end, t);
 460   Quaternion d = SlerpNoInvert(ctrl1, ctrl2, 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 = acosf((2 * X * X) - 1); // float arc cosine
 476
 477   return theta;
 478 }
 479
 480 Vector4 Quaternion::Rotate(const Vector4& vector) const
 481 {
 482   Quaternion V(0.0f, vector.x, vector.y, vector.z);
 483   Quaternion conjugate(*this);
 484   conjugate.Conjugate();
 485   return (*this * V * conjugate).mVector;
 486 }
 487
 488 Vector3 Quaternion::Rotate(const Vector3& vector) const
 489 {
 490   Quaternion V(0.0f, vector.x, vector.y, vector.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   Radian  angleRadians;
 546
 547   quaternion.ToAxisAngle(axis, angleRadians);
 548   Degree degrees(angleRadians);
 549
 550   return o << "[ Axis: [" << axis.x << ", " << axis.y << ", " << axis.z << "], Angle: " << degrees.degree << " degrees ]";
 551 }
 552
 553 } // namespace Dali