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 *****************************************************************************/ 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 *****************************************************************************/ 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 *****************************************************************************/ 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 *****************************************************************************/ 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 *****************************************************************************/ 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 *****************************************************************************/ 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 *****************************************************************************/ 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
