Line data Source code
1 : /**
2 : Copyright 2013 BlackBerry Inc.
3 : Copyright (c) 2017-2022 Roman Katuntsev <sbkarr@stappler.org>
4 : Copyright (c) 2023 Stappler LLC <admin@stappler.dev>
5 :
6 : Licensed under the Apache License, Version 2.0 (the "License");
7 : you may not use this file except in compliance with the License.
8 : You may obtain a copy of the License at
9 :
10 : http://www.apache.org/licenses/LICENSE-2.0
11 :
12 : Unless required by applicable law or agreed to in writing, software
13 : distributed under the License is distributed on an "AS IS" BASIS,
14 : WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 : See the License for the specific language governing permissions and
16 : limitations under the License.
17 :
18 : Original file from GamePlay3D: http://gameplay3d.org
19 :
20 : This file was modified to fit the cocos2d-x project
21 : This file was modified for stappler project
22 : */
23 :
24 : #include "SPQuaternion.h"
25 : #include "SPMat4.h"
26 :
27 : namespace STAPPLER_VERSIONIZED stappler::geom {
28 :
29 100 : static void Quaternion_slerp(float q1x, float q1y, float q1z, float q1w, float q2x, float q2y, float q2z, float q2w, float t,
30 : float* dstx, float* dsty, float* dstz, float* dstw) {
31 : // Fast slerp implementation by kwhatmough:
32 : // It contains no division operations, no trig, no inverse trig
33 : // and no sqrt. Not only does this code tolerate small constraint
34 : // errors in the input quaternions, it actually corrects for them.
35 100 : assert(dstx && dsty && dstz && dstw);
36 100 : assert(!(t < 0.0f || t > 1.0f));
37 :
38 100 : if (t == 0.0f) {
39 25 : *dstx = q1x;
40 25 : *dsty = q1y;
41 25 : *dstz = q1z;
42 25 : *dstw = q1w;
43 25 : return;
44 75 : } else if (t == 1.0f) {
45 25 : *dstx = q2x;
46 25 : *dsty = q2y;
47 25 : *dstz = q2z;
48 25 : *dstw = q2w;
49 25 : return;
50 : }
51 :
52 50 : if (q1x == q2x && q1y == q2y && q1z == q2z && q1w == q2w) {
53 25 : *dstx = q1x;
54 25 : *dsty = q1y;
55 25 : *dstz = q1z;
56 25 : *dstw = q1w;
57 25 : return;
58 : }
59 :
60 : float halfY, alpha, beta;
61 : float u, f1, f2a, f2b;
62 : float ratio1, ratio2;
63 : float halfSecHalfTheta, versHalfTheta;
64 : float sqNotU, sqU;
65 :
66 25 : float cosTheta = q1w * q2w + q1x * q2x + q1y * q2y + q1z * q2z;
67 :
68 : // As usual in all slerp implementations, we fold theta.
69 25 : alpha = cosTheta >= 0 ? 1.0f : -1.0f;
70 25 : halfY = 1.0f + alpha * cosTheta;
71 :
72 : // Here we bisect the interval, so we need to fold t as well.
73 25 : f2b = t - 0.5f;
74 25 : u = f2b >= 0 ? f2b : -f2b;
75 25 : f2a = u - f2b;
76 25 : f2b += u;
77 25 : u += u;
78 25 : f1 = 1.0f - u;
79 :
80 : // One iteration of Newton to get 1-cos(theta / 2) to good accuracy.
81 25 : halfSecHalfTheta = 1.09f - (0.476537f - 0.0903321f * halfY) * halfY;
82 25 : halfSecHalfTheta *= 1.5f - halfY * halfSecHalfTheta * halfSecHalfTheta;
83 25 : versHalfTheta = 1.0f - halfY * halfSecHalfTheta;
84 :
85 : // Evaluate series expansions of the coefficients.
86 25 : sqNotU = f1 * f1;
87 25 : ratio2 = 0.0000440917108f * versHalfTheta;
88 25 : ratio1 = -0.00158730159f + (sqNotU - 16.0f) * ratio2;
89 25 : ratio1 = 0.0333333333f + ratio1 * (sqNotU - 9.0f) * versHalfTheta;
90 25 : ratio1 = -0.333333333f + ratio1 * (sqNotU - 4.0f) * versHalfTheta;
91 25 : ratio1 = 1.0f + ratio1 * (sqNotU - 1.0f) * versHalfTheta;
92 :
93 25 : sqU = u * u;
94 25 : ratio2 = -0.00158730159f + (sqU - 16.0f) * ratio2;
95 25 : ratio2 = 0.0333333333f + ratio2 * (sqU - 9.0f) * versHalfTheta;
96 25 : ratio2 = -0.333333333f + ratio2 * (sqU - 4.0f) * versHalfTheta;
97 25 : ratio2 = 1.0f + ratio2 * (sqU - 1.0f) * versHalfTheta;
98 :
99 : // Perform the bisection and resolve the folding done earlier.
100 25 : f1 *= ratio1 * halfSecHalfTheta;
101 25 : f2a *= ratio2;
102 25 : f2b *= ratio2;
103 25 : alpha *= f1 + f2a;
104 25 : beta = f1 + f2b;
105 :
106 : // Apply final coefficients to a and b as usual.
107 25 : const float w = alpha * q1w + beta * q2w;
108 25 : const float x = alpha * q1x + beta * q2x;
109 25 : const float y = alpha * q1y + beta * q2y;
110 25 : const float z = alpha * q1z + beta * q2z;
111 :
112 : // This final adjustment to the quaternion's length corrects for
113 : // any small constraint error in the inputs q1 and q2 But as you
114 : // can see, it comes at the cost of 9 additional multiplication
115 : // operations. If this error-correcting feature is not required,
116 : // the following code may be removed.
117 25 : f1 = 1.5f - 0.5f * (w * w + x * x + y * y + z * z);
118 25 : *dstw = w * f1;
119 25 : *dstx = x * f1;
120 25 : *dsty = y * f1;
121 25 : *dstz = z * f1;
122 : }
123 :
124 225 : static void Quaternion_slerpForSquad(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst) {
125 225 : assert(dst);
126 :
127 : // cos(omega) = q1 * q2;
128 : // slerp(q1, q2, t) = (q1*sin((1-t)*omega) + q2*sin(t*omega))/sin(omega);
129 : // q1 = +- q2, slerp(q1,q2,t) = q1.
130 : // This is a straight-forward implementation of the formula of slerp. It does not do any sign switching.
131 225 : const float c = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
132 :
133 225 : if (fabs(c) >= 1.0f) {
134 0 : dst->x = q1.x;
135 0 : dst->y = q1.y;
136 0 : dst->z = q1.z;
137 0 : dst->w = q1.w;
138 0 : return;
139 : }
140 :
141 225 : float omega = acos(c);
142 225 : float s = sqrt(1.0f - c * c);
143 225 : if (fabs(s) <= 0.00001f) {
144 0 : dst->x = q1.x;
145 0 : dst->y = q1.y;
146 0 : dst->z = q1.z;
147 0 : dst->w = q1.w;
148 0 : return;
149 : }
150 :
151 225 : float r1 = sin((1 - t) * omega) / s;
152 225 : float r2 = sin(t * omega) / s;
153 225 : dst->x = (q1.x * r1 + q2.x * r2);
154 225 : dst->y = (q1.y * r1 + q2.y * r2);
155 225 : dst->z = (q1.z * r1 + q2.z * r2);
156 225 : dst->w = (q1.w * r1 + q2.w * r2);
157 : }
158 :
159 25 : Quaternion::Quaternion(const Mat4& m) {
160 25 : m.getRotation(this);
161 25 : }
162 :
163 75 : bool Quaternion::inverse() {
164 75 : float n = x * x + y * y + z * z + w * w;
165 75 : if (n == 1.0f) {
166 50 : x = -x;
167 50 : y = -y;
168 50 : z = -z;
169 : //w = w;
170 :
171 50 : return true;
172 : }
173 :
174 : // Too close to zero.
175 25 : if (n < 0.000001f) {
176 0 : return false;
177 : }
178 :
179 25 : n = 1.0f / n;
180 25 : x = -x * n;
181 25 : y = -y * n;
182 25 : z = -z * n;
183 25 : w = w * n;
184 :
185 25 : return true;
186 : }
187 :
188 50 : Quaternion Quaternion::getInversed() const {
189 50 : Quaternion q(*this);
190 50 : q.inverse();
191 50 : return q;
192 : }
193 :
194 25 : void Quaternion::multiply(const Quaternion& q) {
195 25 : multiply(*this, q, this);
196 25 : }
197 :
198 50 : void Quaternion::multiply(const Quaternion& q1, const Quaternion& q2, Quaternion* dst) {
199 50 : assert(dst);
200 :
201 50 : float x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
202 50 : float y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
203 50 : float z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
204 50 : float w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
205 :
206 50 : dst->x = x;
207 50 : dst->y = y;
208 50 : dst->z = z;
209 50 : dst->w = w;
210 50 : }
211 :
212 75 : void Quaternion::normalize() {
213 75 : float n = x * x + y * y + z * z + w * w;
214 :
215 : // Already normalized.
216 75 : if (n == 1.0f)
217 0 : return;
218 :
219 75 : n = sqrt(n);
220 : // Too close to zero.
221 75 : if (n < 0.000001f)
222 0 : return;
223 :
224 75 : n = 1.0f / n;
225 75 : x *= n;
226 75 : y *= n;
227 75 : z *= n;
228 75 : w *= n;
229 : }
230 :
231 50 : Quaternion Quaternion::getNormalized() const {
232 50 : Quaternion q(*this);
233 50 : q.normalize();
234 50 : return q;
235 : }
236 :
237 25 : float Quaternion::toAxisAngle(Vec3* axis) const {
238 25 : assert(axis);
239 :
240 25 : Quaternion q(x, y, z, w);
241 25 : q.normalize();
242 25 : axis->x = q.x;
243 25 : axis->y = q.y;
244 25 : axis->z = q.z;
245 25 : axis->normalize();
246 :
247 25 : return (2.0f * acos(q.w));
248 : }
249 :
250 100 : void Quaternion::lerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst) {
251 100 : assert(dst);
252 100 : assert(!(t < 0.0f || t > 1.0f));
253 :
254 100 : if (t == 0.0f) {
255 25 : memcpy((void *)dst, (const void *)&q1, sizeof(float) * 4);
256 25 : return;
257 75 : } else if (t == 1.0f) {
258 25 : memcpy((void *)dst, (const void *)&q2, sizeof(float) * 4);
259 25 : return;
260 : }
261 :
262 50 : const float t1 = 1.0f - t;
263 :
264 50 : dst->x = t1 * q1.x + t * q2.x;
265 50 : dst->y = t1 * q1.y + t * q2.y;
266 50 : dst->z = t1 * q1.z + t * q2.z;
267 50 : dst->w = t1 * q1.w + t * q2.w;
268 : }
269 :
270 100 : void Quaternion::slerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst) {
271 100 : assert(dst);
272 100 : Quaternion_slerp(q1.x, q1.y, q1.z, q1.w, q2.x, q2.y, q2.z, q2.w, t, &dst->x, &dst->y, &dst->z, &dst->w);
273 100 : }
274 :
275 75 : void Quaternion::squad(const Quaternion& q1, const Quaternion& q2, const Quaternion& s1, const Quaternion& s2, float t, Quaternion* dst) {
276 75 : assert(!(t < 0.0f || t > 1.0f));
277 :
278 75 : Quaternion dstQ(0.0f, 0.0f, 0.0f, 1.0f);
279 75 : Quaternion dstS(0.0f, 0.0f, 0.0f, 1.0f);
280 :
281 75 : Quaternion_slerpForSquad(q1, q2, t, &dstQ);
282 75 : Quaternion_slerpForSquad(s1, s2, t, &dstS);
283 75 : Quaternion_slerpForSquad(dstQ, dstS, 2.0f * t * (1.0f - t), dst);
284 75 : }
285 :
286 : #ifdef __LCC__
287 :
288 : constexpr const Quaternion Quaternion::IDENTITY(0.0f, 0.0f, 0.0f, 1.0f);
289 : constexpr const Quaternion Quaternion::ZERO(0.0f, 0.0f, 0.0f, 0.0f);
290 :
291 : #endif
292 :
293 : }
|
