/Cocos2d-x/Cocos2d-x/libs/cocos2dx/kazmath/src/quaternion.c
C | 586 lines | 331 code | 101 blank | 154 comment | 29 complexity | f5d006a8e2ce4f94400f7307ba0edcd4 MD5 | raw file
- /*
- Copyright (c) 2008, Luke Benstead.
- All rights reserved.
- Redistribution and use in source and binary forms, with or without modification,
- are permitted provided that the following conditions are met:
- * Redistributions of source code must retain the above copyright notice,
- this list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice,
- this list of conditions and the following disclaimer in the documentation
- and/or other materials provided with the distribution.
- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
- ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
- ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
- ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
- #include <assert.h>
- #include <memory.h>
- #include "kazmath/utility.h"
- #include "kazmath/mat3.h"
- #include "kazmath/vec3.h"
- #include "kazmath/quaternion.h"
- #ifndef NULL
- #define NULL ((void *)0)
- #endif
- ///< Returns pOut, sets pOut to the conjugate of pIn
- kmQuaternion* const kmQuaternionConjugate(kmQuaternion* pOut, const kmQuaternion* pIn)
- {
- pOut->x = -pIn->x;
- pOut->y = -pIn->y;
- pOut->z = -pIn->z;
- pOut->w = pIn->w;
- return pOut;
- }
- ///< Returns the dot product of the 2 quaternions
- const kmScalar kmQuaternionDot(const kmQuaternion* q1, const kmQuaternion* q2)
- {
- // A dot B = B dot A = AtBt + AxBx + AyBy + AzBz
- return (q1->w * q2->w +
- q1->x * q2->x +
- q1->y * q2->y +
- q1->z * q2->z);
- }
- ///< Returns the exponential of the quaternion
- kmQuaternion* kmQuaternionExp(kmQuaternion* pOut, const kmQuaternion* pIn)
- {
- assert(0);
- return pOut;
- }
- ///< Makes the passed quaternion an identity quaternion
- kmQuaternion* kmQuaternionIdentity(kmQuaternion* pOut)
- {
- pOut->x = 0.0;
- pOut->y = 0.0;
- pOut->z = 0.0;
- pOut->w = 1.0;
- return pOut;
- }
- ///< Returns the inverse of the passed Quaternion
- kmQuaternion* kmQuaternionInverse(kmQuaternion* pOut,
- const kmQuaternion* pIn)
- {
- kmScalar l = kmQuaternionLength(pIn);
- kmQuaternion tmp;
- if (fabs(l) > kmEpsilon)
- {
- pOut->x = 0.0;
- pOut->y = 0.0;
- pOut->z = 0.0;
- pOut->w = 0.0;
- return pOut;
- }
- ///Get the conjugute and divide by the length
- kmQuaternionScale(pOut,
- kmQuaternionConjugate(&tmp, pIn), 1.0f / l);
- return pOut;
- }
- ///< Returns true if the quaternion is an identity quaternion
- int kmQuaternionIsIdentity(const kmQuaternion* pIn)
- {
- return (pIn->x == 0.0 && pIn->y == 0.0 && pIn->z == 0.0 &&
- pIn->w == 1.0);
- }
- ///< Returns the length of the quaternion
- kmScalar kmQuaternionLength(const kmQuaternion* pIn)
- {
- return sqrtf(kmQuaternionLengthSq(pIn));
- }
- ///< Returns the length of the quaternion squared (prevents a sqrt)
- kmScalar kmQuaternionLengthSq(const kmQuaternion* pIn)
- {
- return pIn->x * pIn->x + pIn->y * pIn->y +
- pIn->z * pIn->z + pIn->w * pIn->w;
- }
- ///< Returns the natural logarithm
- kmQuaternion* kmQuaternionLn(kmQuaternion* pOut,
- const kmQuaternion* pIn)
- {
- /*
- A unit quaternion, is defined by:
- Q == (cos(theta), sin(theta) * v) where |v| = 1
- The natural logarithm of Q is, ln(Q) = (0, theta * v)
- */
- assert(0);
- return pOut;
- }
- ///< Multiplies 2 quaternions together
- extern
- kmQuaternion* kmQuaternionMultiply(kmQuaternion* pOut,
- const kmQuaternion* q1,
- const kmQuaternion* q2)
- {
- pOut->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
- pOut->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
- pOut->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
- pOut->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
- return pOut;
- }
- ///< Normalizes a quaternion
- kmQuaternion* kmQuaternionNormalize(kmQuaternion* pOut,
- const kmQuaternion* pIn)
- {
- kmScalar length = kmQuaternionLength(pIn);
- assert(fabs(length) > kmEpsilon);
- kmQuaternionScale(pOut, pIn, 1.0f / length);
- return pOut;
- }
- ///< Rotates a quaternion around an axis
- kmQuaternion* kmQuaternionRotationAxis(kmQuaternion* pOut,
- const kmVec3* pV,
- kmScalar angle)
- {
- kmScalar rad = angle * 0.5f;
- kmScalar scale = sinf(rad);
- pOut->w = cosf(rad);
- pOut->x = pV->x * scale;
- pOut->y = pV->y * scale;
- pOut->z = pV->z * scale;
- return pOut;
- }
- ///< Creates a quaternion from a rotation matrix
- kmQuaternion* kmQuaternionRotationMatrix(kmQuaternion* pOut,
- const kmMat3* pIn)
- {
- /*
- Note: The OpenGL matrices are transposed from the description below
- taken from the Matrix and Quaternion FAQ
- if ( mat[0] > mat[5] && mat[0] > mat[10] ) { // Column 0:
- S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
- X = 0.25 * S;
- Y = (mat[4] + mat[1] ) / S;
- Z = (mat[2] + mat[8] ) / S;
- W = (mat[9] - mat[6] ) / S;
- } else if ( mat[5] > mat[10] ) { // Column 1:
- S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
- X = (mat[4] + mat[1] ) / S;
- Y = 0.25 * S;
- Z = (mat[9] + mat[6] ) / S;
- W = (mat[2] - mat[8] ) / S;
- } else { // Column 2:
- S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
- X = (mat[2] + mat[8] ) / S;
- Y = (mat[9] + mat[6] ) / S;
- Z = 0.25 * S;
- W = (mat[4] - mat[1] ) / S;
- }
- */
- float x, y, z, w;
- float *pMatrix = NULL;
- float m4x4[16] = {0};
- float scale = 0.0f;
- float diagonal = 0.0f;
- if(!pIn) {
- return NULL;
- }
- /* 0 3 6
- 1 4 7
- 2 5 8
- 0 1 2 3
- 4 5 6 7
- 8 9 10 11
- 12 13 14 15*/
- m4x4[0] = pIn->mat[0];
- m4x4[1] = pIn->mat[3];
- m4x4[2] = pIn->mat[6];
- m4x4[4] = pIn->mat[1];
- m4x4[5] = pIn->mat[4];
- m4x4[6] = pIn->mat[7];
- m4x4[8] = pIn->mat[2];
- m4x4[9] = pIn->mat[5];
- m4x4[10] = pIn->mat[8];
- m4x4[15] = 1;
- pMatrix = &m4x4[0];
- diagonal = pMatrix[0] + pMatrix[5] + pMatrix[10] + 1;
- if(diagonal > kmEpsilon) {
- // Calculate the scale of the diagonal
- scale = (float)sqrt(diagonal ) * 2;
- // Calculate the x, y, x and w of the quaternion through the respective equation
- x = ( pMatrix[9] - pMatrix[6] ) / scale;
- y = ( pMatrix[2] - pMatrix[8] ) / scale;
- z = ( pMatrix[4] - pMatrix[1] ) / scale;
- w = 0.25f * scale;
- }
- else
- {
- // If the first element of the diagonal is the greatest value
- if ( pMatrix[0] > pMatrix[5] && pMatrix[0] > pMatrix[10] )
- {
- // Find the scale according to the first element, and double that value
- scale = (float)sqrt( 1.0f + pMatrix[0] - pMatrix[5] - pMatrix[10] ) * 2.0f;
- // Calculate the x, y, x and w of the quaternion through the respective equation
- x = 0.25f * scale;
- y = (pMatrix[4] + pMatrix[1] ) / scale;
- z = (pMatrix[2] + pMatrix[8] ) / scale;
- w = (pMatrix[9] - pMatrix[6] ) / scale;
- }
- // Else if the second element of the diagonal is the greatest value
- else if (pMatrix[5] > pMatrix[10])
- {
- // Find the scale according to the second element, and double that value
- scale = (float)sqrt( 1.0f + pMatrix[5] - pMatrix[0] - pMatrix[10] ) * 2.0f;
- // Calculate the x, y, x and w of the quaternion through the respective equation
- x = (pMatrix[4] + pMatrix[1] ) / scale;
- y = 0.25f * scale;
- z = (pMatrix[9] + pMatrix[6] ) / scale;
- w = (pMatrix[2] - pMatrix[8] ) / scale;
- }
- // Else the third element of the diagonal is the greatest value
- else
- {
- // Find the scale according to the third element, and double that value
- scale = (float)sqrt( 1.0f + pMatrix[10] - pMatrix[0] - pMatrix[5] ) * 2.0f;
- // Calculate the x, y, x and w of the quaternion through the respective equation
- x = (pMatrix[2] + pMatrix[8] ) / scale;
- y = (pMatrix[9] + pMatrix[6] ) / scale;
- z = 0.25f * scale;
- w = (pMatrix[4] - pMatrix[1] ) / scale;
- }
- }
- pOut->x = x;
- pOut->y = y;
- pOut->z = z;
- pOut->w = w;
- return pOut;
- #if 0
- kmScalar T = pIn->mat[0] + pIn->mat[5] + pIn->mat[10];
- if (T > kmEpsilon) {
- //If the trace is greater than zero we always use this calculation:
- /* S = sqrt(T) * 2;
- X = ( mat[9] - mat[6] ) / S;
- Y = ( mat[2] - mat[8] ) / S;
- Z = ( mat[4] - mat[1] ) / S;
- W = 0.25 * S;*/
- /* kmScalar s = sqrtf(T) * 2;
- pOut->x = (pIn->mat[9] - pIn->mat[6]) / s;
- pOut->y = (pIn->mat[8] - pIn->mat[2]) / s;
- pOut->z = (pIn->mat[1] - pIn->mat[4]) / s;
- pOut->w = 0.25f * s;
- kmQuaternionNormalize(pOut, pOut);
- return pOut;
- }
- //Otherwise the calculation depends on which major diagonal element has the greatest value.
- if (pIn->mat[0] > pIn->mat[5] && pIn->mat[0] > pIn->mat[10]) {
- kmScalar s = sqrtf(1 + pIn->mat[0] - pIn->mat[5] - pIn->mat[10]) * 2;
- pOut->x = 0.25f * s;
- pOut->y = (pIn->mat[1] + pIn->mat[4]) / s;
- pOut->z = (pIn->mat[8] + pIn->mat[2]) / s;
- pOut->w = (pIn->mat[9] - pIn->mat[6]) / s;
- }
- else if (pIn->mat[5] > pIn->mat[10]) {
- kmScalar s = sqrtf(1 + pIn->mat[5] - pIn->mat[0] - pIn->mat[10]) * 2;
- pOut->x = (pIn->mat[1] + pIn->mat[4]) / s;
- pOut->y = 0.25f * s;
- pOut->z = (pIn->mat[9] + pIn->mat[6]) / s;
- pOut->w = (pIn->mat[8] - pIn->mat[2]) / s;
- }
- else {
- kmScalar s = sqrt(1.0f + pIn->mat[10] - pIn->mat[0] - pIn->mat[5]) * 2.0f;
- pOut->x = (pIn->mat[8] + pIn->mat[2] ) / s;
- pOut->y = (pIn->mat[6] + pIn->mat[9] ) / s;
- pOut->z = 0.25f * s;
- pOut->w = (pIn->mat[1] - pIn->mat[4] ) / s;
- }
- kmQuaternionNormalize(pOut, pOut);
- return pOut;*/
- #endif // 0
- }
- ///< Create a quaternion from yaw, pitch and roll
- kmQuaternion* kmQuaternionRotationYawPitchRoll(kmQuaternion* pOut,
- kmScalar yaw,
- kmScalar pitch,
- kmScalar roll)
- {
- kmScalar ex, ey, ez; // temp half euler angles
- kmScalar cr, cp, cy, sr, sp, sy, cpcy, spsy; // temp vars in roll,pitch yaw
- ex = kmDegreesToRadians(pitch) / 2.0f; // convert to rads and half them
- ey = kmDegreesToRadians(yaw) / 2.0f;
- ez = kmDegreesToRadians(roll) / 2.0f;
- cr = cosf(ex);
- cp = cosf(ey);
- cy = cosf(ez);
- sr = sinf(ex);
- sp = sinf(ey);
- sy = sinf(ez);
- cpcy = cp * cy;
- spsy = sp * sy;
- pOut->w = cr * cpcy + sr * spsy;
- pOut->x = sr * cpcy - cr * spsy;
- pOut->y = cr * sp * cy + sr * cp * sy;
- pOut->z = cr * cp * sy - sr * sp * cy;
- kmQuaternionNormalize(pOut, pOut);
- return pOut;
- }
- ///< Interpolate between 2 quaternions
- kmQuaternion* kmQuaternionSlerp(kmQuaternion* pOut,
- const kmQuaternion* q1,
- const kmQuaternion* q2,
- kmScalar t)
- {
- /*float CosTheta = Q0.DotProd(Q1);
- float Theta = acosf(CosTheta);
- float SinTheta = sqrtf(1.0f-CosTheta*CosTheta);
- float Sin_T_Theta = sinf(T*Theta)/SinTheta;
- float Sin_OneMinusT_Theta = sinf((1.0f-T)*Theta)/SinTheta;
- Quaternion Result = Q0*Sin_OneMinusT_Theta;
- Result += (Q1*Sin_T_Theta);
- return Result;*/
- if (q1->x == q2->x &&
- q1->y == q2->y &&
- q1->z == q2->z &&
- q1->w == q2->w) {
- pOut->x = q1->x;
- pOut->y = q1->y;
- pOut->z = q1->z;
- pOut->w = q1->w;
- return pOut;
- }
- {
- kmScalar ct = kmQuaternionDot(q1, q2);
- kmScalar theta = acosf(ct);
- kmScalar st = sqrtf(1.0 - kmSQR(ct));
- kmScalar stt = sinf(t * theta) / st;
- kmScalar somt = sinf((1.0 - t) * theta) / st;
- kmQuaternion temp, temp2;
- kmQuaternionScale(&temp, q1, somt);
- kmQuaternionScale(&temp2, q2, stt);
- kmQuaternionAdd(pOut, &temp, &temp2);
- }
- return pOut;
- }
- ///< Get the axis and angle of rotation from a quaternion
- void kmQuaternionToAxisAngle(const kmQuaternion* pIn,
- kmVec3* pAxis,
- kmScalar* pAngle)
- {
- kmScalar tempAngle; // temp angle
- kmScalar scale; // temp vars
- tempAngle = acosf(pIn->w);
- scale = sqrtf(kmSQR(pIn->x) + kmSQR(pIn->y) + kmSQR(pIn->z));
- if (((scale > -kmEpsilon) && scale < kmEpsilon)
- || (scale < 2*kmPI + kmEpsilon && scale > 2*kmPI - kmEpsilon)) // angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0
- {
- *pAngle = 0.0f;
- pAxis->x = 0.0f;
- pAxis->y = 0.0f;
- pAxis->z = 1.0f;
- }
- else
- {
- *pAngle = tempAngle * 2.0f; // angle in radians
- pAxis->x = pIn->x / scale;
- pAxis->y = pIn->y / scale;
- pAxis->z = pIn->z / scale;
- kmVec3Normalize(pAxis, pAxis);
- }
- }
- kmQuaternion* kmQuaternionScale(kmQuaternion* pOut,
- const kmQuaternion* pIn,
- kmScalar s)
- {
- pOut->x = pIn->x * s;
- pOut->y = pIn->y * s;
- pOut->z = pIn->z * s;
- pOut->w = pIn->w * s;
- return pOut;
- }
- kmQuaternion* kmQuaternionAssign(kmQuaternion* pOut, const kmQuaternion* pIn)
- {
- memcpy(pOut, pIn, sizeof(float) * 4);
- return pOut;
- }
- kmQuaternion* kmQuaternionAdd(kmQuaternion* pOut, const kmQuaternion* pQ1, const kmQuaternion* pQ2)
- {
- pOut->x = pQ1->x + pQ2->x;
- pOut->y = pQ1->y + pQ2->y;
- pOut->z = pQ1->z + pQ2->z;
- pOut->w = pQ1->w + pQ2->w;
- return pOut;
- }
- /** Adapted from the OGRE engine!
- Gets the shortest arc quaternion to rotate this vector to the destination
- vector.
- @remarks
- If you call this with a dest vector that is close to the inverse
- of this vector, we will rotate 180 degrees around the 'fallbackAxis'
- (if specified, or a generated axis if not) since in this case
- ANY axis of rotation is valid.
- */
- kmQuaternion* kmQuaternionRotationBetweenVec3(kmQuaternion* pOut, const kmVec3* vec1, const kmVec3* vec2, const kmVec3* fallback) {
- kmVec3 v1, v2;
- kmScalar a;
- kmVec3Assign(&v1, vec1);
- kmVec3Assign(&v2, vec2);
- kmVec3Normalize(&v1, &v1);
- kmVec3Normalize(&v2, &v2);
- a = kmVec3Dot(&v1, &v2);
- if (a >= 1.0) {
- kmQuaternionIdentity(pOut);
- return pOut;
- }
- if (a < (1e-6f - 1.0f)) {
- if (fabs(kmVec3LengthSq(fallback)) < kmEpsilon) {
- kmQuaternionRotationAxis(pOut, fallback, kmPI);
- } else {
- kmVec3 axis;
- kmVec3 X;
- X.x = 1.0;
- X.y = 0.0;
- X.z = 0.0;
- kmVec3Cross(&axis, &X, vec1);
- //If axis is zero
- if (fabs(kmVec3LengthSq(&axis)) < kmEpsilon) {
- kmVec3 Y;
- Y.x = 0.0;
- Y.y = 1.0;
- Y.z = 0.0;
- kmVec3Cross(&axis, &Y, vec1);
- }
- kmVec3Normalize(&axis, &axis);
- kmQuaternionRotationAxis(pOut, &axis, kmPI);
- }
- } else {
- kmScalar s = sqrtf((1+a) * 2);
- kmScalar invs = 1 / s;
- kmVec3 c;
- kmVec3Cross(&c, &v1, &v2);
- pOut->x = c.x * invs;
- pOut->y = c.y * invs;
- pOut->z = c.z * invs;
- pOut->w = s * 0.5f;
- kmQuaternionNormalize(pOut, pOut);
- }
- return pOut;
- }
- kmVec3* kmQuaternionMultiplyVec3(kmVec3* pOut, const kmQuaternion* q, const kmVec3* v) {
- kmVec3 uv, uuv, qvec;
- qvec.x = q->x;
- qvec.y = q->y;
- qvec.z = q->z;
- kmVec3Cross(&uv, &qvec, v);
- kmVec3Cross(&uuv, &qvec, &uv);
- kmVec3Scale(&uv, &uv, (2.0f * q->w));
- kmVec3Scale(&uuv, &uuv, 2.0f);
- kmVec3Add(pOut, v, &uv);
- kmVec3Add(pOut, pOut, &uuv);
- return pOut;
- }