2 * Copyright (c) 2020 Samsung Electronics Co., Ltd.
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
8 * http://www.apache.org/licenses/LICENSE-2.0
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.
19 #include <dali/public-api/math/quaternion.h>
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>
34 using Internal::PerformanceMonitor;
36 const Quaternion Quaternion::IDENTITY;
41 Quaternion::Quaternion()
42 : mVector(0.0f, 0.0f, 0.0f, 1.0f)
46 Quaternion::Quaternion(float cosThetaBy2, float iBySineTheta, float jBySineTheta, float kBySineTheta)
47 : mVector(iBySineTheta, jBySineTheta, kBySineTheta, cosThetaBy2)
51 Quaternion::Quaternion(const Vector4& vector)
56 Quaternion::Quaternion(Radian angle, const Vector3& axis)
58 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 4);
60 Vector3 tmpAxis = axis;
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;
71 Quaternion::Quaternion(Radian pitch, Radian yaw, Radian roll)
73 SetEuler(pitch, yaw, roll);
76 Quaternion::Quaternion(const Matrix& matrix)
78 Vector3 xAxis(matrix.GetXAxis());
79 Vector3 yAxis(matrix.GetYAxis());
80 Vector3 zAxis(matrix.GetZAxis());
82 SetFromAxes(xAxis, yAxis, zAxis);
85 Quaternion::Quaternion(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
87 SetFromAxes(xAxis, yAxis, zAxis);
90 Quaternion::Quaternion(const Vector3& v0, const Vector3& v1)
92 float dot = v0.Dot(v1);
93 if(dot > 1.0f - Math::MACHINE_EPSILON_1)
96 mVector.x = mVector.y = mVector.z = 0.0f;
99 else if(dot < -1.0f + Math::MACHINE_EPSILON_1)
101 //180 degree rotation across the Z axis
102 mVector.x = mVector.y = mVector.w = 0.0f;
107 Vector3 w = v0.Cross(v1);
108 mVector.w = 1.0f + dot;
116 Quaternion::~Quaternion() = default;
118 bool Quaternion::IsIdentity() const
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));
128 bool Quaternion::ToAxisAngle(Vector3& axis, Radian& angle) const
130 angle = acosf(mVector.w);
131 bool converted = false;
132 // pre-compute to save time
133 const float sine = sinf(angle.radian);
135 // If sine(angle) is zero, conversion is not possible
137 if(!EqualsZero(sine))
139 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 3);
141 float sinf_theta_inv = 1.0f / sine;
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;
152 const Vector4& Quaternion::AsVector() const
157 void Quaternion::SetEuler(Radian pitch, Radian yaw, Radian roll)
159 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 19);
161 const float halfX = 0.5f * pitch.radian;
162 const float halfY = 0.5f * yaw.radian;
163 const float halfZ = 0.5f * roll.radian;
165 float cosX2 = cosf(halfX);
166 float cosY2 = cosf(halfY);
167 float cosZ2 = cosf(halfZ);
169 float sinX2 = sinf(halfX);
170 float sinY2 = sinf(halfY);
171 float sinZ2 = sinf(halfZ);
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;
179 Vector4 Quaternion::EulerAngles() const
181 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 13);
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;
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);
195 const Quaternion Quaternion::operator+(const Quaternion& other) const
197 return Quaternion(mVector + other.mVector);
200 const Quaternion Quaternion::operator-(const Quaternion& other) const
202 return Quaternion(mVector - other.mVector);
205 const Quaternion Quaternion::operator*(const Quaternion& other) const
207 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 12);
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);
215 Vector3 Quaternion::operator*(const Vector3& other) const
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);
223 return other + uv + uuv;
226 const Quaternion Quaternion::operator/(const Quaternion& q) const
233 const Quaternion Quaternion::operator*(float scale) const
235 return Quaternion(mVector * scale);
238 const Quaternion Quaternion::operator/(float scale) const
240 return Quaternion(mVector / scale);
243 Quaternion Quaternion::operator-() const
245 return Quaternion(-mVector.w, -mVector.x, -mVector.y, -mVector.z);
248 const Quaternion& Quaternion::operator+=(const Quaternion& q)
250 mVector += q.mVector;
254 const Quaternion& Quaternion::operator-=(const Quaternion& q)
256 mVector -= q.mVector;
260 const Quaternion& Quaternion::operator*=(const Quaternion& q)
262 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 12);
264 float x = mVector.x, y = mVector.y, z = mVector.z, w = mVector.w;
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;
273 const Quaternion& Quaternion::operator*=(float scale)
279 const Quaternion& Quaternion::operator/=(float scale)
285 bool Quaternion::operator==(const Quaternion& rhs) const
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));
298 bool Quaternion::operator!=(const Quaternion& rhs) const
300 return !operator==(rhs);
303 float Quaternion::Length() const
305 return static_cast<float>(sqrt(mVector.w * mVector.w + mVector.Dot(mVector)));
308 float Quaternion::LengthSquared() const
310 return static_cast<float>(mVector.w * mVector.w + mVector.Dot(mVector));
313 void Quaternion::Normalize()
318 Quaternion Quaternion::Normalized() const
320 return *this / Length();
323 void Quaternion::Conjugate()
325 mVector.x = -mVector.x;
326 mVector.y = -mVector.y;
327 mVector.z = -mVector.z;
330 void Quaternion::Invert()
333 *this /= LengthSquared();
336 Quaternion Quaternion::Log() const
338 float a = acosf(mVector.w);
339 float sina = sinf(a);
343 if(fabsf(sina) >= Math::MACHINE_EPSILON_1)
345 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 4);
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;
354 ret.mVector.x = ret.mVector.y = ret.mVector.z = 0;
359 Quaternion Quaternion::Exp() const
361 DALI_ASSERT_ALWAYS(EqualsZero(mVector.w) && "Cannot perform Exponent");
363 float a = mVector.Length();
364 float sina = sinf(a);
367 ret.mVector.w = cosf(a);
369 if(a >= Math::MACHINE_EPSILON_1)
371 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 4);
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;
380 ret.mVector.x = ret.mVector.y = ret.mVector.z = 0.0f;
385 float Quaternion::Dot(const Quaternion& q1, const Quaternion& q2)
387 return q1.mVector.Dot4(q2.mVector);
390 Quaternion Quaternion::Lerp(const Quaternion& q1, const Quaternion& q2, float t)
392 return (q1 * (1.0f - t) + q2 * t).Normalized();
395 Quaternion Quaternion::Slerp(const Quaternion& q1, const Quaternion& q2, float progress)
398 float cosTheta = Quaternion::Dot(q1, q2);
401 * If cos(theta) < 0, q1 and q2 are more than 90 degrees apart,
402 * so invert one to reduce spinning.
406 cosTheta = -cosTheta;
414 if(fabsf(cosTheta) < 0.95f)
416 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 5);
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;
425 return q1 * coeff0 + q3 * coeff1;
429 // If the angle is small, use linear interpolation
430 Quaternion result = q1 * (1.0f - progress) + q3 * progress;
432 return result.Normalized();
436 Quaternion Quaternion::SlerpNoInvert(const Quaternion& q1, const Quaternion& q2, float t)
438 float cosTheta = Quaternion::Dot(q1, q2);
440 if(cosTheta > -0.95f && cosTheta < 0.95f)
442 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 2);
444 float theta = acosf(cosTheta);
445 return (q1 * sinf(theta * (1.0f - t)) + q2 * sinf(theta * t)) / sinf(theta);
449 return Lerp(q1, q2, t);
453 Quaternion Quaternion::Squad(const Quaternion& start, const Quaternion& end, const Quaternion& ctrl1, const Quaternion& ctrl2, float t)
455 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 2);
457 Quaternion c = SlerpNoInvert(start, end, t);
458 Quaternion d = SlerpNoInvert(ctrl1, ctrl2, t);
459 return SlerpNoInvert(c, d, 2 * t * (1 - t));
462 float Quaternion::AngleBetween(const Quaternion& q1, const Quaternion& q2)
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
478 Vector4 Quaternion::Rotate(const Vector4& vector) const
480 Quaternion V(0.0f, vector.x, vector.y, vector.z);
481 Quaternion conjugate(*this);
482 conjugate.Conjugate();
483 return (*this * V * conjugate).mVector;
486 Vector3 Quaternion::Rotate(const Vector3& vector) const
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);
494 void Quaternion::SetFromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
496 MATH_INCREASE_BY(PerformanceMonitor::FLOAT_POINT_MULTIPLY, 4);
498 float t = xAxis.x + yAxis.y + zAxis.z;
499 if(t > 0.0f) // w is largest
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;
508 else if(zAxis.z > xAxis.x && zAxis.z > yAxis.y) // z is largest
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;
517 else if(yAxis.y > xAxis.x) // y is largest
519 float root = sqrtf(yAxis.y - zAxis.z - xAxis.x + 1.0f);
520 float one_over_4w = 0.5f / root;
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;
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;
540 std::ostream& operator<<(std::ostream& o, const Quaternion& quaternion)
545 quaternion.ToAxisAngle(axis, angleRadians);
546 Degree degrees(angleRadians);
548 return o << "[ Axis: [" << axis.x << ", " << axis.y << ", " << axis.z << "], Angle: " << degrees.degree << " degrees ]";