1 /*
2 Copyright (C) 2009 Sony Computer Entertainment Inc.
3 All rights reserved.
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14
15 */
16
17 #ifndef _VECTORMATH_QUAT_AOS_CPP_H
18 #define _VECTORMATH_QUAT_AOS_CPP_H
19
20 //-----------------------------------------------------------------------------
21 // Definitions
22
23 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
24 #define _VECTORMATH_INTERNAL_FUNCTIONS
25
26 #endif
27
28 namespace Vectormath
29 {
30 namespace Aos
31 {
Quat(const Quat & quat)32 inline Quat::Quat(const Quat &quat)
33 {
34 vXYZW = quat.vXYZW;
35 }
36
Quat(float _x,float _y,float _z,float _w)37 inline Quat::Quat(float _x, float _y, float _z, float _w)
38 {
39 mXYZW[0] = _x;
40 mXYZW[1] = _y;
41 mXYZW[2] = _z;
42 mXYZW[3] = _w;
43 }
44
Quat(float32x4_t fXYZW)45 inline Quat::Quat(float32x4_t fXYZW)
46 {
47 vXYZW = fXYZW;
48 }
49
Quat(const Vector3 & xyz,float _w)50 inline Quat::Quat(const Vector3 &xyz, float _w)
51 {
52 this->setXYZ(xyz);
53 this->setW(_w);
54 }
55
Quat(const Vector4 & vec)56 inline Quat::Quat(const Vector4 &vec)
57 {
58 mXYZW[0] = vec.getX();
59 mXYZW[1] = vec.getY();
60 mXYZW[2] = vec.getZ();
61 mXYZW[3] = vec.getW();
62 }
63
Quat(float scalar)64 inline Quat::Quat(float scalar)
65 {
66 vXYZW = vdupq_n_f32(scalar);
67 }
68
identity()69 inline const Quat Quat::identity()
70 {
71 return Quat(0.0f, 0.0f, 0.0f, 1.0f);
72 }
73
lerp(float t,const Quat & quat0,const Quat & quat1)74 inline const Quat lerp(float t, const Quat &quat0, const Quat &quat1)
75 {
76 return (quat0 + ((quat1 - quat0) * t));
77 }
78
slerp(float t,const Quat & unitQuat0,const Quat & unitQuat1)79 inline const Quat slerp(float t, const Quat &unitQuat0, const Quat &unitQuat1)
80 {
81 Quat start;
82 float recipSinAngle, scale0, scale1, cosAngle, angle;
83 cosAngle = dot(unitQuat0, unitQuat1);
84 if (cosAngle < 0.0f)
85 {
86 cosAngle = -cosAngle;
87 start = (-unitQuat0);
88 }
89 else
90 {
91 start = unitQuat0;
92 }
93 if (cosAngle < _VECTORMATH_SLERP_TOL)
94 {
95 angle = acosf(cosAngle);
96 recipSinAngle = (1.0f / sinf(angle));
97 scale0 = (sinf(((1.0f - t) * angle)) * recipSinAngle);
98 scale1 = (sinf((t * angle)) * recipSinAngle);
99 }
100 else
101 {
102 scale0 = (1.0f - t);
103 scale1 = t;
104 }
105 return ((start * scale0) + (unitQuat1 * scale1));
106 }
107
squad(float t,const Quat & unitQuat0,const Quat & unitQuat1,const Quat & unitQuat2,const Quat & unitQuat3)108 inline const Quat squad(float t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3)
109 {
110 Quat tmp0, tmp1;
111 tmp0 = slerp(t, unitQuat0, unitQuat3);
112 tmp1 = slerp(t, unitQuat1, unitQuat2);
113 return slerp(((2.0f * t) * (1.0f - t)), tmp0, tmp1);
114 }
115
loadXYZW(Quat & quat,const float * fptr)116 inline void loadXYZW(Quat &quat, const float *fptr)
117 {
118 quat = Quat(fptr[0], fptr[1], fptr[2], fptr[3]);
119 }
120
storeXYZW(const Quat & quat,float * fptr)121 inline void storeXYZW(const Quat &quat, float *fptr)
122 {
123 vst1q_f32(fptr, quat.getvXYZW());
124 }
125
126 inline Quat &Quat::operator=(const Quat &quat)
127 {
128 vXYZW = quat.getvXYZW();
129 return *this;
130 }
131
setXYZ(const Vector3 & vec)132 inline Quat &Quat::setXYZ(const Vector3 &vec)
133 {
134 mXYZW[0] = vec.getX();
135 mXYZW[1] = vec.getY();
136 mXYZW[2] = vec.getZ();
137 return *this;
138 }
139
getXYZ()140 inline const Vector3 Quat::getXYZ() const
141 {
142 return Vector3(mXYZW[0], mXYZW[1], mXYZW[2]);
143 }
144
getvXYZW()145 inline float32x4_t Quat::getvXYZW() const
146 {
147 return vXYZW;
148 }
149
setX(float _x)150 inline Quat &Quat::setX(float _x)
151 {
152 mXYZW[0] = _x;
153 return *this;
154 }
155
getX()156 inline float Quat::getX() const
157 {
158 return mXYZW[0];
159 }
160
setY(float _y)161 inline Quat &Quat::setY(float _y)
162 {
163 mXYZW[1] = _y;
164 return *this;
165 }
166
getY()167 inline float Quat::getY() const
168 {
169 return mXYZW[1];
170 }
171
setZ(float _z)172 inline Quat &Quat::setZ(float _z)
173 {
174 mXYZW[2] = _z;
175 return *this;
176 }
177
getZ()178 inline float Quat::getZ() const
179 {
180 return mXYZW[2];
181 }
182
setW(float _w)183 inline Quat &Quat::setW(float _w)
184 {
185 mXYZW[3] = _w;
186 return *this;
187 }
188
getW()189 inline float Quat::getW() const
190 {
191 return mXYZW[3];
192 }
193
setElem(int idx,float value)194 inline Quat &Quat::setElem(int idx, float value)
195 {
196 *(&mXYZW[0] + idx) = value;
197 return *this;
198 }
199
getElem(int idx)200 inline float Quat::getElem(int idx) const
201 {
202 return *(&mXYZW[0] + idx);
203 }
204
205 inline float &Quat::operator[](int idx)
206 {
207 return *(&mXYZW[0] + idx);
208 }
209
210 inline float Quat::operator[](int idx) const
211 {
212 return *(&mXYZW[0] + idx);
213 }
214
215 inline const Quat Quat::operator+(const Quat &quat) const
216 {
217 return Quat(vaddq_f32(vXYZW, quat.vXYZW));
218 }
219
220 inline const Quat Quat::operator-(const Quat &quat) const
221 {
222 return Quat(vsubq_f32(vXYZW, quat.vXYZW));
223 }
224
225 inline const Quat Quat::operator*(float scalar) const
226 {
227 float32x4_t v_scalar = vdupq_n_f32(scalar);
228 return Quat(vmulq_f32(vXYZW, v_scalar));
229 }
230
231 inline Quat &Quat::operator+=(const Quat &quat)
232 {
233 *this = *this + quat;
234 return *this;
235 }
236
237 inline Quat &Quat::operator-=(const Quat &quat)
238 {
239 *this = *this - quat;
240 return *this;
241 }
242
243 inline Quat &Quat::operator*=(float scalar)
244 {
245 *this = *this * scalar;
246 return *this;
247 }
248
249 inline const Quat Quat::operator/(float scalar) const
250 {
251 return Quat(
252 (mXYZW[0] / scalar),
253 (mXYZW[1] / scalar),
254 (mXYZW[2] / scalar),
255 (mXYZW[3] / scalar));
256 }
257
258 inline Quat &Quat::operator/=(float scalar)
259 {
260 *this = *this / scalar;
261 return *this;
262 }
263
264 inline const Quat Quat::operator-() const
265 {
266 return Quat(vnegq_f32(vXYZW));
267 }
268
269 inline const Quat operator*(float scalar, const Quat &quat)
270 {
271 return quat * scalar;
272 }
273
dot(const Quat & quat0,const Quat & quat1)274 inline float dot(const Quat &quat0, const Quat &quat1)
275 {
276 float result;
277 result = (quat0.getX() * quat1.getX());
278 result = (result + (quat0.getY() * quat1.getY()));
279 result = (result + (quat0.getZ() * quat1.getZ()));
280 result = (result + (quat0.getW() * quat1.getW()));
281 return result;
282 }
283
norm(const Quat & quat)284 inline float norm(const Quat &quat)
285 {
286 float result;
287 result = (quat.getX() * quat.getX());
288 result = (result + (quat.getY() * quat.getY()));
289 result = (result + (quat.getZ() * quat.getZ()));
290 result = (result + (quat.getW() * quat.getW()));
291 return result;
292 }
293
length(const Quat & quat)294 inline float length(const Quat &quat)
295 {
296 return ::sqrtf(norm(quat));
297 }
298
normalize(const Quat & quat)299 inline const Quat normalize(const Quat &quat)
300 {
301 float lenSqr, lenInv;
302 lenSqr = norm(quat);
303 lenInv = (1.0f / sqrtf(lenSqr));
304 return Quat(
305 (quat.getX() * lenInv),
306 (quat.getY() * lenInv),
307 (quat.getZ() * lenInv),
308 (quat.getW() * lenInv));
309 }
310
rotation(const Vector3 & unitVec0,const Vector3 & unitVec1)311 inline const Quat Quat::rotation(const Vector3 &unitVec0, const Vector3 &unitVec1)
312 {
313 float cosHalfAngleX2, recipCosHalfAngleX2;
314 cosHalfAngleX2 = sqrtf((2.0f * (1.0f + dot(unitVec0, unitVec1))));
315 recipCosHalfAngleX2 = (1.0f / cosHalfAngleX2);
316 return Quat((cross(unitVec0, unitVec1) * recipCosHalfAngleX2), (cosHalfAngleX2 * 0.5f));
317 }
318
rotation(float radians,const Vector3 & unitVec)319 inline const Quat Quat::rotation(float radians, const Vector3 &unitVec)
320 {
321 float s, c, angle;
322 angle = (radians * 0.5f);
323 s = sinf(angle);
324 c = cosf(angle);
325 return Quat((unitVec * s), c);
326 }
327
rotationX(float radians)328 inline const Quat Quat::rotationX(float radians)
329 {
330 float s, c, angle;
331 angle = (radians * 0.5f);
332 s = sinf(angle);
333 c = cosf(angle);
334 return Quat(s, 0.0f, 0.0f, c);
335 }
336
rotationY(float radians)337 inline const Quat Quat::rotationY(float radians)
338 {
339 float s, c, angle;
340 angle = (radians * 0.5f);
341 s = sinf(angle);
342 c = cosf(angle);
343 return Quat(0.0f, s, 0.0f, c);
344 }
345
rotationZ(float radians)346 inline const Quat Quat::rotationZ(float radians)
347 {
348 float s, c, angle;
349 angle = (radians * 0.5f);
350 s = sinf(angle);
351 c = cosf(angle);
352 return Quat(0.0f, 0.0f, s, c);
353 }
354
355 inline const Quat Quat::operator*(const Quat &quat) const
356 {
357 return Quat(
358 ((((mXYZW[3] * quat.mXYZW[0]) + (mXYZW[0] * quat.mXYZW[3])) + (mXYZW[1] * quat.mXYZW[2])) - (mXYZW[2] * quat.mXYZW[1])),
359 ((((mXYZW[3] * quat.mXYZW[1]) + (mXYZW[1] * quat.mXYZW[3])) + (mXYZW[2] * quat.mXYZW[0])) - (mXYZW[0] * quat.mXYZW[2])),
360 ((((mXYZW[3] * quat.mXYZW[2]) + (mXYZW[2] * quat.mXYZW[3])) + (mXYZW[0] * quat.mXYZW[1])) - (mXYZW[1] * quat.mXYZW[0])),
361 ((((mXYZW[3] * quat.mXYZW[3]) - (mXYZW[0] * quat.mXYZW[0])) - (mXYZW[1] * quat.mXYZW[1])) - (mXYZW[2] * quat.mXYZW[2])));
362 }
363
364 inline Quat &Quat::operator*=(const Quat &quat)
365 {
366 *this = *this * quat;
367 return *this;
368 }
369
rotate(const Quat & quat,const Vector3 & vec)370 inline const Vector3 rotate(const Quat &quat, const Vector3 &vec)
371 {
372 float tmpX, tmpY, tmpZ, tmpW;
373 tmpX = (((quat.getW() * vec.getX()) + (quat.getY() * vec.getZ())) - (quat.getZ() * vec.getY()));
374 tmpY = (((quat.getW() * vec.getY()) + (quat.getZ() * vec.getX())) - (quat.getX() * vec.getZ()));
375 tmpZ = (((quat.getW() * vec.getZ()) + (quat.getX() * vec.getY())) - (quat.getY() * vec.getX()));
376 tmpW = (((quat.getX() * vec.getX()) + (quat.getY() * vec.getY())) + (quat.getZ() * vec.getZ()));
377 return Vector3(
378 ((((tmpW * quat.getX()) + (tmpX * quat.getW())) - (tmpY * quat.getZ())) + (tmpZ * quat.getY())),
379 ((((tmpW * quat.getY()) + (tmpY * quat.getW())) - (tmpZ * quat.getX())) + (tmpX * quat.getZ())),
380 ((((tmpW * quat.getZ()) + (tmpZ * quat.getW())) - (tmpX * quat.getY())) + (tmpY * quat.getX())));
381 }
382
conj(const Quat & quat)383 inline const Quat conj(const Quat &quat)
384 {
385 return Quat(-quat.getX(), -quat.getY(), -quat.getZ(), quat.getW());
386 }
387
select(const Quat & quat0,const Quat & quat1,bool select1)388 inline const Quat select(const Quat &quat0, const Quat &quat1, bool select1)
389 {
390 return Quat(
391 (select1) ? quat1.getX() : quat0.getX(),
392 (select1) ? quat1.getY() : quat0.getY(),
393 (select1) ? quat1.getZ() : quat0.getZ(),
394 (select1) ? quat1.getW() : quat0.getW());
395 }
396
397 #ifdef _VECTORMATH_DEBUG
398
print(const Quat & quat)399 inline void print(const Quat &quat)
400 {
401 printf("( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW());
402 }
403
print(const Quat & quat,const char * name)404 inline void print(const Quat &quat, const char *name)
405 {
406 printf("%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW());
407 }
408
409 #endif
410
411 } // namespace Aos
412 } // namespace Vectormath
413
414 #endif
415