1 /******************************************************************************
2
3 @File PVRTQuaternionF.cpp
4
5 @Title PVRTQuaternionF
6
7 @Version
8
9 @Copyright Copyright (c) Imagination Technologies Limited.
10
11 @Platform ANSI compatible
12
13 @Description Set of mathematical functions for quaternions.
14
15 ******************************************************************************/
16 #include "PVRTGlobal.h"
17 #include <math.h>
18 #include <string.h>
19 #include "PVRTFixedPoint.h" // Only needed for trig function float lookups
20 #include "PVRTQuaternion.h"
21
22
23 /****************************************************************************
24 ** Functions
25 ****************************************************************************/
26
27 /*!***************************************************************************
28 @Function PVRTMatrixQuaternionIdentityF
29 @Output qOut Identity quaternion
30 @Description Sets the quaternion to (0, 0, 0, 1), the identity quaternion.
31 *****************************************************************************/
PVRTMatrixQuaternionIdentityF(PVRTQUATERNIONf & qOut)32 void PVRTMatrixQuaternionIdentityF(PVRTQUATERNIONf &qOut)
33 {
34 qOut.x = 0;
35 qOut.y = 0;
36 qOut.z = 0;
37 qOut.w = 1;
38 }
39
40 /*!***************************************************************************
41 @Function PVRTMatrixQuaternionRotationAxisF
42 @Output qOut Rotation quaternion
43 @Input vAxis Axis to rotate around
44 @Input fAngle Angle to rotate
45 @Description Create quaternion corresponding to a rotation of fAngle
46 radians around submitted vector.
47 *****************************************************************************/
PVRTMatrixQuaternionRotationAxisF(PVRTQUATERNIONf & qOut,const PVRTVECTOR3f & vAxis,const float fAngle)48 void PVRTMatrixQuaternionRotationAxisF(
49 PVRTQUATERNIONf &qOut,
50 const PVRTVECTOR3f &vAxis,
51 const float fAngle)
52 {
53 float fSin, fCos;
54
55 fSin = (float)PVRTFSIN(fAngle * 0.5f);
56 fCos = (float)PVRTFCOS(fAngle * 0.5f);
57
58 /* Create quaternion */
59 qOut.x = vAxis.x * fSin;
60 qOut.y = vAxis.y * fSin;
61 qOut.z = vAxis.z * fSin;
62 qOut.w = fCos;
63
64 /* Normalise it */
65 PVRTMatrixQuaternionNormalizeF(qOut);
66 }
67
68 /*!***************************************************************************
69 @Function PVRTMatrixQuaternionToAxisAngleF
70 @Input qIn Quaternion to transform
71 @Output vAxis Axis of rotation
72 @Output fAngle Angle of rotation
73 @Description Convert a quaternion to an axis and angle. Expects a unit
74 quaternion.
75 *****************************************************************************/
PVRTMatrixQuaternionToAxisAngleF(const PVRTQUATERNIONf & qIn,PVRTVECTOR3f & vAxis,float & fAngle)76 void PVRTMatrixQuaternionToAxisAngleF(
77 const PVRTQUATERNIONf &qIn,
78 PVRTVECTOR3f &vAxis,
79 float &fAngle)
80 {
81 float fCosAngle, fSinAngle;
82 double temp;
83
84 /* Compute some values */
85 fCosAngle = qIn.w;
86 temp = 1.0f - fCosAngle*fCosAngle;
87 fAngle = (float)PVRTFACOS(fCosAngle)*2.0f;
88 fSinAngle = (float)sqrt(temp);
89
90 /* This is to avoid a division by zero */
91 if ((float)fabs(fSinAngle)<0.0005f)
92 fSinAngle = 1.0f;
93
94 /* Get axis vector */
95 vAxis.x = qIn.x / fSinAngle;
96 vAxis.y = qIn.y / fSinAngle;
97 vAxis.z = qIn.z / fSinAngle;
98 }
99
100 /*!***************************************************************************
101 @Function PVRTMatrixQuaternionSlerpF
102 @Output qOut Result of the interpolation
103 @Input qA First quaternion to interpolate from
104 @Input qB Second quaternion to interpolate from
105 @Input t Coefficient of interpolation
106 @Description Perform a Spherical Linear intERPolation between quaternion A
107 and quaternion B at time t. t must be between 0.0f and 1.0f
108 *****************************************************************************/
PVRTMatrixQuaternionSlerpF(PVRTQUATERNIONf & qOut,const PVRTQUATERNIONf & qA,const PVRTQUATERNIONf & qB,const float t)109 void PVRTMatrixQuaternionSlerpF(
110 PVRTQUATERNIONf &qOut,
111 const PVRTQUATERNIONf &qA,
112 const PVRTQUATERNIONf &qB,
113 const float t)
114 {
115 float fCosine, fAngle, A, B;
116
117 /* Parameter checking */
118 if (t<0.0f || t>1.0f)
119 {
120 _RPT0(_CRT_WARN, "PVRTMatrixQuaternionSlerp : Bad parameters\n");
121 qOut.x = 0;
122 qOut.y = 0;
123 qOut.z = 0;
124 qOut.w = 1;
125 return;
126 }
127
128 /* Find sine of Angle between Quaternion A and B (dot product between quaternion A and B) */
129 fCosine = qA.w*qB.w + qA.x*qB.x + qA.y*qB.y + qA.z*qB.z;
130
131 if (fCosine < 0)
132 {
133 PVRTQUATERNIONf qi;
134
135 /*
136 <http://www.magic-software.com/Documentation/Quaternions.pdf>
137
138 "It is important to note that the quaternions q and -q represent
139 the same rotation... while either quaternion will do, the
140 interpolation methods require choosing one over the other.
141
142 "Although q1 and -q1 represent the same rotation, the values of
143 Slerp(t; q0, q1) and Slerp(t; q0,-q1) are not the same. It is
144 customary to choose the sign... on q1 so that... the angle
145 between q0 and q1 is acute. This choice avoids extra
146 spinning caused by the interpolated rotations."
147 */
148 qi.x = -qB.x;
149 qi.y = -qB.y;
150 qi.z = -qB.z;
151 qi.w = -qB.w;
152
153 PVRTMatrixQuaternionSlerpF(qOut, qA, qi, t);
154 return;
155 }
156
157 fCosine = PVRT_MIN(fCosine, 1.0f);
158 fAngle = (float)PVRTFACOS(fCosine);
159
160 /* Avoid a division by zero */
161 if (fAngle==0.0f)
162 {
163 qOut = qA;
164 return;
165 }
166
167 /* Precompute some values */
168 A = (float)(PVRTFSIN((1.0f-t)*fAngle) / PVRTFSIN(fAngle));
169 B = (float)(PVRTFSIN(t*fAngle) / PVRTFSIN(fAngle));
170
171 /* Compute resulting quaternion */
172 qOut.x = A * qA.x + B * qB.x;
173 qOut.y = A * qA.y + B * qB.y;
174 qOut.z = A * qA.z + B * qB.z;
175 qOut.w = A * qA.w + B * qB.w;
176
177 /* Normalise result */
178 PVRTMatrixQuaternionNormalizeF(qOut);
179 }
180
181 /*!***************************************************************************
182 @Function PVRTMatrixQuaternionNormalizeF
183 @Modified quat Vector to normalize
184 @Description Normalize quaternion.
185 *****************************************************************************/
PVRTMatrixQuaternionNormalizeF(PVRTQUATERNIONf & quat)186 void PVRTMatrixQuaternionNormalizeF(PVRTQUATERNIONf &quat)
187 {
188 float fMagnitude;
189 double temp;
190
191 /* Compute quaternion magnitude */
192 temp = quat.w*quat.w + quat.x*quat.x + quat.y*quat.y + quat.z*quat.z;
193 fMagnitude = (float)sqrt(temp);
194
195 /* Divide each quaternion component by this magnitude */
196 if (fMagnitude!=0.0f)
197 {
198 fMagnitude = 1.0f / fMagnitude;
199 quat.x *= fMagnitude;
200 quat.y *= fMagnitude;
201 quat.z *= fMagnitude;
202 quat.w *= fMagnitude;
203 }
204 }
205
206 /*!***************************************************************************
207 @Function PVRTMatrixRotationQuaternionF
208 @Output mOut Resulting rotation matrix
209 @Input quat Quaternion to transform
210 @Description Create rotation matrix from submitted quaternion.
211 Assuming the quaternion is of the form [X Y Z W]:
212
213 | 2 2 |
214 | 1 - 2Y - 2Z 2XY - 2ZW 2XZ + 2YW 0 |
215 | |
216 | 2 2 |
217 M = | 2XY + 2ZW 1 - 2X - 2Z 2YZ - 2XW 0 |
218 | |
219 | 2 2 |
220 | 2XZ - 2YW 2YZ + 2XW 1 - 2X - 2Y 0 |
221 | |
222 | 0 0 0 1 |
223 *****************************************************************************/
PVRTMatrixRotationQuaternionF(PVRTMATRIXf & mOut,const PVRTQUATERNIONf & quat)224 void PVRTMatrixRotationQuaternionF(
225 PVRTMATRIXf &mOut,
226 const PVRTQUATERNIONf &quat)
227 {
228 const PVRTQUATERNIONf *pQ;
229
230 #if defined(BUILD_DX11)
231 PVRTQUATERNIONf qInv;
232
233 qInv.x = -quat.x;
234 qInv.y = -quat.y;
235 qInv.z = -quat.z;
236 qInv.w = quat.w;
237
238 pQ = &qInv;
239 #else
240 pQ = &quat;
241 #endif
242
243 /* Fill matrix members */
244 mOut.f[0] = 1.0f - 2.0f*pQ->y*pQ->y - 2.0f*pQ->z*pQ->z;
245 mOut.f[1] = 2.0f*pQ->x*pQ->y - 2.0f*pQ->z*pQ->w;
246 mOut.f[2] = 2.0f*pQ->x*pQ->z + 2.0f*pQ->y*pQ->w;
247 mOut.f[3] = 0.0f;
248
249 mOut.f[4] = 2.0f*pQ->x*pQ->y + 2.0f*pQ->z*pQ->w;
250 mOut.f[5] = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->z*pQ->z;
251 mOut.f[6] = 2.0f*pQ->y*pQ->z - 2.0f*pQ->x*pQ->w;
252 mOut.f[7] = 0.0f;
253
254 mOut.f[8] = 2.0f*pQ->x*pQ->z - 2*pQ->y*pQ->w;
255 mOut.f[9] = 2.0f*pQ->y*pQ->z + 2.0f*pQ->x*pQ->w;
256 mOut.f[10] = 1.0f - 2.0f*pQ->x*pQ->x - 2*pQ->y*pQ->y;
257 mOut.f[11] = 0.0f;
258
259 mOut.f[12] = 0.0f;
260 mOut.f[13] = 0.0f;
261 mOut.f[14] = 0.0f;
262 mOut.f[15] = 1.0f;
263 }
264
265 /*!***************************************************************************
266 @Function PVRTMatrixQuaternionMultiplyF
267 @Output qOut Resulting quaternion
268 @Input qA First quaternion to multiply
269 @Input qB Second quaternion to multiply
270 @Description Multiply quaternion A with quaternion B and return the
271 result in qOut.
272 *****************************************************************************/
PVRTMatrixQuaternionMultiplyF(PVRTQUATERNIONf & qOut,const PVRTQUATERNIONf & qA,const PVRTQUATERNIONf & qB)273 void PVRTMatrixQuaternionMultiplyF(
274 PVRTQUATERNIONf &qOut,
275 const PVRTQUATERNIONf &qA,
276 const PVRTQUATERNIONf &qB)
277 {
278 PVRTVECTOR3f CrossProduct;
279 PVRTQUATERNIONf qRet;
280
281 /* Compute scalar component */
282 qRet.w = (qA.w*qB.w) - (qA.x*qB.x + qA.y*qB.y + qA.z*qB.z);
283
284 /* Compute cross product */
285 CrossProduct.x = qA.y*qB.z - qA.z*qB.y;
286 CrossProduct.y = qA.z*qB.x - qA.x*qB.z;
287 CrossProduct.z = qA.x*qB.y - qA.y*qB.x;
288
289 /* Compute result vector */
290 qRet.x = (qA.w * qB.x) + (qB.w * qA.x) + CrossProduct.x;
291 qRet.y = (qA.w * qB.y) + (qB.w * qA.y) + CrossProduct.y;
292 qRet.z = (qA.w * qB.z) + (qB.w * qA.z) + CrossProduct.z;
293
294 /* Normalize resulting quaternion */
295 PVRTMatrixQuaternionNormalizeF(qRet);
296
297 /* Copy result to mOut */
298 qOut = qRet;
299 }
300
301 /*****************************************************************************
302 End of file (PVRTQuaternionF.cpp)
303 *****************************************************************************/
304
305