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