| /****************************************************************************** | |
| @File PVRTQuaternionX.cpp | |
| @Title PVRTQuaternionX | |
| @Version | |
| @Copyright Copyright (c) Imagination Technologies Limited. | |
| @Platform ANSI compatible | |
| @Description Set of mathematical functions for quaternions. | |
| ******************************************************************************/ | |
| #include "PVRTContext.h" | |
| #include <math.h> | |
| #include <string.h> | |
| #include "PVRTFixedPoint.h" | |
| #include "PVRTQuaternion.h" | |
| /**************************************************************************** | |
| ** Functions | |
| ****************************************************************************/ | |
| /*!*************************************************************************** | |
| @Function PVRTMatrixQuaternionIdentityX | |
| @Output qOut Identity quaternion | |
| @Description Sets the quaternion to (0, 0, 0, 1), the identity quaternion. | |
| *****************************************************************************/ | |
| void PVRTMatrixQuaternionIdentityX(PVRTQUATERNIONx &qOut) | |
| { | |
| qOut.x = PVRTF2X(0.0f); | |
| qOut.y = PVRTF2X(0.0f); | |
| qOut.z = PVRTF2X(0.0f); | |
| qOut.w = PVRTF2X(1.0f); | |
| } | |
| /*!*************************************************************************** | |
| @Function PVRTMatrixQuaternionRotationAxisX | |
| @Output qOut Rotation quaternion | |
| @Input vAxis Axis to rotate around | |
| @Input fAngle Angle to rotate | |
| @Description Create quaternion corresponding to a rotation of fAngle | |
| radians around submitted vector. | |
| *****************************************************************************/ | |
| void PVRTMatrixQuaternionRotationAxisX( | |
| PVRTQUATERNIONx &qOut, | |
| const PVRTVECTOR3x &vAxis, | |
| const int fAngle) | |
| { | |
| int fSin, fCos; | |
| fSin = PVRTXSIN(fAngle>>1); | |
| fCos = PVRTXCOS(fAngle>>1); | |
| /* Create quaternion */ | |
| qOut.x = PVRTXMUL(vAxis.x, fSin); | |
| qOut.y = PVRTXMUL(vAxis.y, fSin); | |
| qOut.z = PVRTXMUL(vAxis.z, fSin); | |
| qOut.w = fCos; | |
| /* Normalise it */ | |
| PVRTMatrixQuaternionNormalizeX(qOut); | |
| } | |
| /*!*************************************************************************** | |
| @Function PVRTMatrixQuaternionToAxisAngleX | |
| @Input qIn Quaternion to transform | |
| @Output vAxis Axis of rotation | |
| @Output fAngle Angle of rotation | |
| @Description Convert a quaternion to an axis and angle. Expects a unit | |
| quaternion. | |
| *****************************************************************************/ | |
| void PVRTMatrixQuaternionToAxisAngleX( | |
| const PVRTQUATERNIONx &qIn, | |
| PVRTVECTOR3x &vAxis, | |
| int &fAngle) | |
| { | |
| int fCosAngle, fSinAngle; | |
| int temp; | |
| /* Compute some values */ | |
| fCosAngle = qIn.w; | |
| temp = PVRTF2X(1.0f) - PVRTXMUL(fCosAngle, fCosAngle); | |
| fAngle = PVRTXMUL(PVRTXACOS(fCosAngle), PVRTF2X(2.0f)); | |
| fSinAngle = PVRTF2X(((float)sqrt(PVRTX2F(temp)))); | |
| /* This is to avoid a division by zero */ | |
| if (PVRTABS(fSinAngle)<PVRTF2X(0.0005f)) | |
| { | |
| fSinAngle = PVRTF2X(1.0f); | |
| } | |
| /* Get axis vector */ | |
| vAxis.x = PVRTXDIV(qIn.x, fSinAngle); | |
| vAxis.y = PVRTXDIV(qIn.y, fSinAngle); | |
| vAxis.z = PVRTXDIV(qIn.z, fSinAngle); | |
| } | |
| /*!*************************************************************************** | |
| @Function PVRTMatrixQuaternionSlerpX | |
| @Output qOut Result of the interpolation | |
| @Input qA First quaternion to interpolate from | |
| @Input qB Second quaternion to interpolate from | |
| @Input t Coefficient of interpolation | |
| @Description Perform a Spherical Linear intERPolation between quaternion A | |
| and quaternion B at time t. t must be between 0.0f and 1.0f | |
| Requires input quaternions to be normalized | |
| *****************************************************************************/ | |
| void PVRTMatrixQuaternionSlerpX( | |
| PVRTQUATERNIONx &qOut, | |
| const PVRTQUATERNIONx &qA, | |
| const PVRTQUATERNIONx &qB, | |
| const int t) | |
| { | |
| int fCosine, fAngle, A, B; | |
| /* Parameter checking */ | |
| if (t<PVRTF2X(0.0f) || t>PVRTF2X(1.0f)) | |
| { | |
| _RPT0(_CRT_WARN, "PVRTMatrixQuaternionSlerp : Bad parameters\n"); | |
| qOut.x = PVRTF2X(0.0f); | |
| qOut.y = PVRTF2X(0.0f); | |
| qOut.z = PVRTF2X(0.0f); | |
| qOut.w = PVRTF2X(1.0f); | |
| return; | |
| } | |
| /* Find sine of Angle between Quaternion A and B (dot product between quaternion A and B) */ | |
| fCosine = PVRTXMUL(qA.w, qB.w) + | |
| PVRTXMUL(qA.x, qB.x) + PVRTXMUL(qA.y, qB.y) + PVRTXMUL(qA.z, qB.z); | |
| if(fCosine < PVRTF2X(0.0f)) | |
| { | |
| PVRTQUATERNIONx qi; | |
| /* | |
| <http://www.magic-software.com/Documentation/Quaternions.pdf> | |
| "It is important to note that the quaternions q and -q represent | |
| the same rotation... while either quaternion will do, the | |
| interpolation methods require choosing one over the other. | |
| "Although q1 and -q1 represent the same rotation, the values of | |
| Slerp(t; q0, q1) and Slerp(t; q0,-q1) are not the same. It is | |
| customary to choose the sign... on q1 so that... the angle | |
| between q0 and q1 is acute. This choice avoids extra | |
| spinning caused by the interpolated rotations." | |
| */ | |
| qi.x = -qB.x; | |
| qi.y = -qB.y; | |
| qi.z = -qB.z; | |
| qi.w = -qB.w; | |
| PVRTMatrixQuaternionSlerpX(qOut, qA, qi, t); | |
| return; | |
| } | |
| fCosine = PVRT_MIN(fCosine, PVRTF2X(1.0f)); | |
| fAngle = PVRTXACOS(fCosine); | |
| /* Avoid a division by zero */ | |
| if (fAngle==PVRTF2X(0.0f)) | |
| { | |
| qOut = qA; | |
| return; | |
| } | |
| /* Precompute some values */ | |
| A = PVRTXDIV(PVRTXSIN(PVRTXMUL((PVRTF2X(1.0f)-t), fAngle)), PVRTXSIN(fAngle)); | |
| B = PVRTXDIV(PVRTXSIN(PVRTXMUL(t, fAngle)), PVRTXSIN(fAngle)); | |
| /* Compute resulting quaternion */ | |
| qOut.x = PVRTXMUL(A, qA.x) + PVRTXMUL(B, qB.x); | |
| qOut.y = PVRTXMUL(A, qA.y) + PVRTXMUL(B, qB.y); | |
| qOut.z = PVRTXMUL(A, qA.z) + PVRTXMUL(B, qB.z); | |
| qOut.w = PVRTXMUL(A, qA.w) + PVRTXMUL(B, qB.w); | |
| /* Normalise result */ | |
| PVRTMatrixQuaternionNormalizeX(qOut); | |
| } | |
| /*!*************************************************************************** | |
| @Function PVRTMatrixQuaternionNormalizeX | |
| @Modified quat Vector to normalize | |
| @Description Normalize quaternion. | |
| Original quaternion is scaled down prior to be normalized in | |
| order to avoid overflow issues. | |
| *****************************************************************************/ | |
| void PVRTMatrixQuaternionNormalizeX(PVRTQUATERNIONx &quat) | |
| { | |
| PVRTQUATERNIONx qTemp; | |
| int f, n; | |
| /* Scale vector by uniform value */ | |
| n = PVRTABS(quat.w) + PVRTABS(quat.x) + PVRTABS(quat.y) + PVRTABS(quat.z); | |
| qTemp.w = PVRTXDIV(quat.w, n); | |
| qTemp.x = PVRTXDIV(quat.x, n); | |
| qTemp.y = PVRTXDIV(quat.y, n); | |
| qTemp.z = PVRTXDIV(quat.z, n); | |
| /* Compute quaternion magnitude */ | |
| f = PVRTXMUL(qTemp.w, qTemp.w) + PVRTXMUL(qTemp.x, qTemp.x) + PVRTXMUL(qTemp.y, qTemp.y) + PVRTXMUL(qTemp.z, qTemp.z); | |
| f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f)))); | |
| /* Multiply vector components by f */ | |
| quat.x = PVRTXMUL(qTemp.x, f); | |
| quat.y = PVRTXMUL(qTemp.y, f); | |
| quat.z = PVRTXMUL(qTemp.z, f); | |
| quat.w = PVRTXMUL(qTemp.w, f); | |
| } | |
| /*!*************************************************************************** | |
| @Function PVRTMatrixRotationQuaternionX | |
| @Output mOut Resulting rotation matrix | |
| @Input quat Quaternion to transform | |
| @Description Create rotation matrix from submitted quaternion. | |
| Assuming the quaternion is of the form [X Y Z W]: | |
| | 2 2 | | |
| | 1 - 2Y - 2Z 2XY - 2ZW 2XZ + 2YW 0 | | |
| | | | |
| | 2 2 | | |
| M = | 2XY + 2ZW 1 - 2X - 2Z 2YZ - 2XW 0 | | |
| | | | |
| | 2 2 | | |
| | 2XZ - 2YW 2YZ + 2XW 1 - 2X - 2Y 0 | | |
| | | | |
| | 0 0 0 1 | | |
| *****************************************************************************/ | |
| void PVRTMatrixRotationQuaternionX( | |
| PVRTMATRIXx &mOut, | |
| const PVRTQUATERNIONx &quat) | |
| { | |
| const PVRTQUATERNIONx *pQ; | |
| #if defined(BUILD_DX11) | |
| PVRTQUATERNIONx qInv; | |
| qInv.x = -quat.x; | |
| qInv.y = -quat.y; | |
| qInv.z = -quat.z; | |
| qInv.w = quat.w; | |
| pQ = &qInv; | |
| #else | |
| pQ = &quat; | |
| #endif | |
| /* Fill matrix members */ | |
| mOut.f[0] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->y, pQ->y)<<1) - (PVRTXMUL(pQ->z, pQ->z)<<1); | |
| mOut.f[1] = (PVRTXMUL(pQ->x, pQ->y)<<1) - (PVRTXMUL(pQ->z, pQ->w)<<1); | |
| mOut.f[2] = (PVRTXMUL(pQ->x, pQ->z)<<1) + (PVRTXMUL(pQ->y, pQ->w)<<1); | |
| mOut.f[3] = PVRTF2X(0.0f); | |
| mOut.f[4] = (PVRTXMUL(pQ->x, pQ->y)<<1) + (PVRTXMUL(pQ->z, pQ->w)<<1); | |
| mOut.f[5] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->x, pQ->x)<<1) - (PVRTXMUL(pQ->z, pQ->z)<<1); | |
| mOut.f[6] = (PVRTXMUL(pQ->y, pQ->z)<<1) - (PVRTXMUL(pQ->x, pQ->w)<<1); | |
| mOut.f[7] = PVRTF2X(0.0f); | |
| mOut.f[8] = (PVRTXMUL(pQ->x, pQ->z)<<1) - (PVRTXMUL(pQ->y, pQ->w)<<1); | |
| mOut.f[9] = (PVRTXMUL(pQ->y, pQ->z)<<1) + (PVRTXMUL(pQ->x, pQ->w)<<1); | |
| mOut.f[10] = PVRTF2X(1.0f) - (PVRTXMUL(pQ->x, pQ->x)<<1) - (PVRTXMUL(pQ->y, pQ->y)<<1); | |
| mOut.f[11] = PVRTF2X(0.0f); | |
| mOut.f[12] = PVRTF2X(0.0f); | |
| mOut.f[13] = PVRTF2X(0.0f); | |
| mOut.f[14] = PVRTF2X(0.0f); | |
| mOut.f[15] = PVRTF2X(1.0f); | |
| } | |
| /*!*************************************************************************** | |
| @Function PVRTMatrixQuaternionMultiplyX | |
| @Output qOut Resulting quaternion | |
| @Input qA First quaternion to multiply | |
| @Input qB Second quaternion to multiply | |
| @Description Multiply quaternion A with quaternion B and return the | |
| result in qOut. | |
| Input quaternions must be normalized. | |
| *****************************************************************************/ | |
| void PVRTMatrixQuaternionMultiplyX( | |
| PVRTQUATERNIONx &qOut, | |
| const PVRTQUATERNIONx &qA, | |
| const PVRTQUATERNIONx &qB) | |
| { | |
| PVRTVECTOR3x CrossProduct; | |
| /* Compute scalar component */ | |
| qOut.w = PVRTXMUL(qA.w, qB.w) - | |
| (PVRTXMUL(qA.x, qB.x) + PVRTXMUL(qA.y, qB.y) + PVRTXMUL(qA.z, qB.z)); | |
| /* Compute cross product */ | |
| CrossProduct.x = PVRTXMUL(qA.y, qB.z) - PVRTXMUL(qA.z, qB.y); | |
| CrossProduct.y = PVRTXMUL(qA.z, qB.x) - PVRTXMUL(qA.x, qB.z); | |
| CrossProduct.z = PVRTXMUL(qA.x, qB.y) - PVRTXMUL(qA.y, qB.x); | |
| /* Compute result vector */ | |
| qOut.x = PVRTXMUL(qA.w, qB.x) + PVRTXMUL(qB.w, qA.x) + CrossProduct.x; | |
| qOut.y = PVRTXMUL(qA.w, qB.y) + PVRTXMUL(qB.w, qA.y) + CrossProduct.y; | |
| qOut.z = PVRTXMUL(qA.w, qB.z) + PVRTXMUL(qB.w, qA.z) + CrossProduct.z; | |
| /* Normalize resulting quaternion */ | |
| PVRTMatrixQuaternionNormalizeX(qOut); | |
| } | |
| /***************************************************************************** | |
| End of file (PVRTQuaternionX.cpp) | |
| *****************************************************************************/ | |