1 /*
2 * Copyright (C) 2023 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16 #ifndef API_BASE_MATH_QUATERNION_UTIL_H
17 #define API_BASE_MATH_QUATERNION_UTIL_H
18
19 #include <cmath>
20
21 #include <base/math/mathf.h>
22 #include <base/math/quaternion.h>
23 #include <base/math/vector_util.h>
24 #include <base/namespace.h>
25
BASE_BEGIN_NAMESPACE()26 BASE_BEGIN_NAMESPACE()
27 namespace Math {
28 /** \addtogroup group_math_quaternionutil
29 * @{
30 */
31 /** Create quaternion from vector3 (takes radians) */
32 static inline Quat FromEulerRad(const Vec3& euler)
33 {
34 const float roll = euler.x;
35 const float pitch = euler.y;
36 const float yaw = euler.z;
37 const float cy = Math::cos(yaw * 0.5f);
38 const float sy = Math::sin(yaw * 0.5f);
39 const float cp = Math::cos(pitch * 0.5f);
40 const float sp = Math::sin(pitch * 0.5f);
41 const float cr = Math::cos(roll * 0.5f);
42 const float sr = Math::sin(roll * 0.5f);
43
44 return Quat(cy * cp * sr - sy * sp * cr, sy * cp * sr + cy * sp * cr, sy * cp * cr - cy * sp * sr,
45 cy * cp * cr + sy * sp * sr);
46 }
47
48 /** Get squared length */
49 static inline constexpr float LengthSquared(const Quat& quat)
50 {
51 return quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w;
52 }
53
54 /** Get length */
55 static inline float Length(const Quat& quat)
56 {
57 return Math::sqrt(quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w);
58 }
59
60 /** Get dot product of two quaternions */
61 static inline constexpr float Dot(const Quat& q1, const Quat& q2)
62 {
63 return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
64 }
65
66 /** Creates a rotation which rotates angle degrees around axis, takes in angle as radians */
67 static inline Quat AngleAxis(const float& angle, const Vec3& axis)
68 {
69 const float sinHalfAngle = Math::sin(angle * 0.5f);
70 const Vec3 axisMultiplied = axis * sinHalfAngle;
71 return Quat(axisMultiplied.x, axisMultiplied.y, axisMultiplied.z, Math::cos(angle * 0.5f));
72 }
73
74 /** Inverse */
75 static inline constexpr Quat Inverse(const Quat& rotation)
76 {
77 const float lengthSq = LengthSquared(rotation);
78 if (lengthSq != 0.0f) {
79 const float inverse = 1.0f / lengthSq;
80 return Quat(rotation.x * -inverse, rotation.y * -inverse, rotation.z * -inverse, rotation.w * inverse);
81 }
82 return rotation;
83 }
84
85 /** Creates quaternion from three separate euler angles (degrees) */
86 static inline Quat Euler(const float& x, const float& y, const float& z)
87 {
88 return FromEulerRad(Vec3(x, y, z) * Math::DEG2RAD);
89 }
90
91 /** Creates quaternion from euler angles (degrees) */
92 static inline Quat Euler(const Vec3& euler)
93 {
94 return FromEulerRad(euler * Math::DEG2RAD);
95 }
96
97 /** Look rotation from forward and up vectors */
98 static inline Quat LookRotation(Vec3 forward, Vec3 up)
99 {
100 forward = Math::Normalize(forward);
101 const Vec3 right = Math::Normalize(Math::Cross(up, forward));
102 up = Math::Cross(forward, right);
103 const float m00 = right.x;
104 const float m01 = right.y;
105 const float m02 = right.z;
106 const float m10 = up.x;
107 const float m11 = up.y;
108 const float m12 = up.z;
109 const float m20 = forward.x;
110 const float m21 = forward.y;
111 const float m22 = forward.z;
112
113 // discard values less than epsilon
114 const auto discardEpsilon = [](const float value) { return value < Math::EPSILON ? 0.f : value; };
115
116 // based on https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
117 // Because quaternions cannot represent a reflection component, the matrix must be special orthogonal. For a special
118 // orthogonal matrix, 1 + trace is always positive. The case switch is not needed, just do sqrt() on the 4 trace
119 // variants and you are done:
120 const float w = Math::sqrt(discardEpsilon(1.f + m00 + m11 + m22)) * 0.5f;
121 float x = Math::sqrt(discardEpsilon(1.f + m00 - m11 - m22)) * 0.5f;
122 float y = Math::sqrt(discardEpsilon(1.f - m00 + m11 - m22)) * 0.5f;
123 float z = Math::sqrt(discardEpsilon(1.f - m00 - m11 + m22)) * 0.5f;
124
125 // Recover signs
126 x = std::copysign(x, m12 - m21);
127 y = std::copysign(y, m20 - m02);
128 z = std::copysign(z, m01 - m10);
129
130 return { x, y, z, w };
131 }
132
133 /** Normalize single (degree) angle */
134 static inline constexpr float NormalizeAngle(float angle)
135 {
136 while (angle > 360.0f) {
137 angle -= 360.0f;
138 }
139 while (angle < 0.0f) {
140 angle += 360.0f;
141 }
142 return angle;
143 }
144
145 /** Normalize vector3 angles (degree) */
146 static inline constexpr Vec3 NormalizeAngles(Vec3 angles)
147 {
148 angles.x = NormalizeAngle(angles.x);
149 angles.y = NormalizeAngle(angles.y);
150 angles.z = NormalizeAngle(angles.z);
151 return angles;
152 }
153
154 /** Convert quaternion to euler values (radians) */
155 static inline Vec3 ToEulerRad(const Quat& q)
156 {
157 // roll (x-axis rotation)
158 const float sinrCosp = +2.0f * (q.w * q.x + q.y * q.z);
159 const float cosrCosp = +1.0f - 2.0f * (q.x * q.x + q.y * q.y);
160 const float roll = atan2(sinrCosp, cosrCosp);
161
162 // pitch (y-axis rotation)
163 const float sinp = +2.0f * (q.w * q.y - q.z * q.x);
164 // use 90 degrees if out of range
165 const float pitch = (fabs(sinp) >= 1.f) ? copysign(Math::PI / 2.0f, sinp) : asin(sinp);
166
167 // yaw (z-axis rotation)
168 const float sinyCosp = +2.0f * (q.w * q.z + q.x * q.y);
169 const float cosyCosp = +1.0f - 2.0f * (q.y * q.y + q.z * q.z);
170 const float yaw = atan2(sinyCosp, cosyCosp);
171
172 Vec3 eulerRadRotations(roll, pitch, yaw);
173 return eulerRadRotations;
174 }
175
176 /** Conjugates quaternion */
177 static inline Quat Conjugate(const Quat& q)
178 {
179 return Quat(-q.x, -q.y, -q.z, q.w);
180 }
181
182 /** Normalizes quaternion */
183 static inline Quat Normalize(const Quat& q)
184 {
185 const float len = Length(q);
186 if (len <= 0.0f) {
187 return Quat(0.0f, 0.0f, 0.0f, 1.0f);
188 }
189 const float oneOverLen = 1.0f / len;
190 return Quat(q.x * oneOverLen, q.y * oneOverLen, q.z * oneOverLen, q.w * oneOverLen);
191 }
192
193 /** Multiply vector3 by quaternion(rotation) */
194 inline Vec3 operator*(const Quat& q, const Vec3& v)
195 {
196 const Vec3 QuatVector(q.x, q.y, q.z);
197 const Vec3 uv(Cross(QuatVector, v));
198 const Vec3 uuv(Cross(QuatVector, uv));
199
200 return v + ((uv * q.w) + uuv) * 2.0f;
201 }
202
203 /** Multiply vector3 by quaternion(rotation) */
204 inline Vec3 operator*(const Vec3& v, const Quat& q)
205 {
206 return Inverse(q) * v;
207 }
208
209 /** Multiply quaternion with scalar */
210 inline constexpr Quat operator*(const float scalar, const Quat& quat)
211 {
212 return Quat(quat.x * scalar, quat.y * scalar, quat.z * scalar, quat.w * scalar);
213 }
214
215 /** Multiply quaternion with scalar */
216 inline constexpr Quat operator*(const Quat& quat, const float scalar)
217 {
218 return Quat(quat.x * scalar, quat.y * scalar, quat.z * scalar, quat.w * scalar);
219 }
220
221 /** Adds two quaternions */
222 inline constexpr Quat operator+(const Quat& q, const Quat& p)
223 {
224 return Quat(q.x + p.x, q.y + p.y, q.z + p.z, q.w + p.w);
225 }
226
227 /** Spherically interpolate between x and y by a */
228 inline constexpr Quat Slerp(Quat const& x, Quat const& y, float a)
229 {
230 Quat z = y;
231 float cosTheta = Dot(x, z);
232 if (cosTheta < 0.0f) {
233 z.x = -z.x;
234 z.y = -z.y;
235 z.z = -z.z;
236 z.w = -z.w;
237 cosTheta = -cosTheta;
238 }
239
240 if (cosTheta > 1.0f - EPSILON) {
241 return Quat(lerp(x.x, z.x, a), lerp(x.y, z.y, a), lerp(x.z, z.z, a), lerp(x.w, z.w, a));
242 }
243
244 // A Fast and Accurate Estimate for SLERP
245 // sin(a * angle) / sin(angle) and sin((1-a) * angle) / sin(angle) are approximated with Chebyshev Polynomials
246 constexpr float mu = 1.85298109240830f;
247 constexpr float u[8] = // 1 / [i(2i + 1)] for i >= 1
248 { 1.f / (1 * 3), 1.f / (2 * 5), 1.f / (3 * 7), 1.f / (4 * 9), 1.f / (5 * 11), 1.f / (6 * 13), 1.f / (7 * 15),
249 mu / (8 * 17) };
250 constexpr float v[8] = // i / (2i + 1) for i >= 1
251 { 1.f / 3, 2.f / 5, 3.f / 7, 4.f / 9, 5.f / 11, 6.f / 13, 7.f / 15, mu * 8.f / 17 };
252
253 const float xm1 = cosTheta - 1; // in [-1, 0]
254 const float d = 1 - a;
255 const float sqrA = a * a;
256 const float sqrD = d * d;
257 // bA[] stores a-related values, bD[] stores (1 - a)-related values
258 float bA[8] {};
259 float bD[8] {};
260 for (int i = 7; i >= 0; --i) {
261 bA[i] = (u[i] * sqrA - v[i]) * xm1;
262 bD[i] = (u[i] * sqrD - v[i]) * xm1;
263 }
264 const float coeff1 =
265 a *
266 (1 + bA[0] * (1 + bA[1] * (1 + bA[2] * (1 + bA[3] * (1 + bA[4] * (1 + bA[5] * (1 + bA[6] * (1 + bA[7]))))))));
267 const float coeff2 =
268 d *
269 (1 + bD[0] * (1 + bD[1] * (1 + bD[2] * (1 + bD[3] * (1 + bD[4] * (1 + bD[5] * (1 + bD[6] * (1 + bD[7]))))))));
270 return (coeff2 * x) + (coeff1 * z);
271 }
272 /** @} */
273 } // namespace Math
274 BASE_END_NAMESPACE()
275
276 #endif // API_BASE_MATH_QUATERNION_UTIL_H
277