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