/**< Get the axis and angle of rotation from a quaternion*/ void kmQuaternionToAxisAngle(const kmQuaternion* pIn, kmVec3* pAxis, kmScalar* pAngle) { kmScalar scale; /* temp vars*/ kmQuaternion tmp; if(pIn->w > 1.0) { kmQuaternionNormalize(&tmp, pIn); } else { kmQuaternionAssign(&tmp, pIn); } *pAngle = 2.0 * acosf(tmp.w); scale = sqrtf(1.0 - kmSQR(tmp.w)); if (scale < kmEpsilon) { /* angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0*/ pAxis->x = 0.0f; pAxis->y = 0.0f; pAxis->z = 1.0f; } else { pAxis->x = tmp.x / scale; pAxis->y = tmp.y / scale; pAxis->z = tmp.z / scale; kmVec3Normalize(pAxis, pAxis); } }
///< 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; }
void MotionRecognizer::normalize(vector<kmVecPair>& data, vector<kmVec3>& norm) { kmQuaternion q1, q2, q3; kmVec3 n; vector<kmVecPair>::iterator iter; for (iter = data.begin(); iter != data.end(); iter++) { q1.x = ((kmVecPair)*iter).accel.x; q1.y = ((kmVecPair)*iter).accel.y; q1.z = ((kmVecPair)*iter).accel.z; q1.w = 0; if (q1.x * q1.x + q1.y * q1.y + q1.z * q1.z < ACCELERATION_THRESHOLD) continue; q2.x = ((kmVecPair)*iter).rot.x; q2.y = ((kmVecPair)*iter).rot.y; q2.z = ((kmVecPair)*iter).rot.z; q2.w = ((kmVecPair)*iter).rot.w; // For every acclerations, rotate them by rotation quaternion. // Then, axes of acclerations are adjusted to global earth axes. kmQuaternionMultiply(&q3, &q2, &q1); kmQuaternionConjugate(&q1, &q2); kmQuaternionMultiply(&q2, &q3, &q1); // Normalize vector's size equal to size of codebook vector (=SQRT6) kmQuaternionNormalize(&q1, &q2); n.x = q1.x * SQRT6; n.y = q1.y * SQRT6; n.z = q1.z * SQRT6; norm.push_back(n); } }
uint8_t lite3d_scene_node_update(lite3d_scene_node *node) { uint8_t updated = LITE3D_FALSE; SDL_assert(node); if (node->recalc) { kmMat4 transMat; kmMat4 scaleMat; kmQuaternionNormalize(&node->rotation, &node->rotation); kmMat4RotationQuaternion(&node->localView, &node->rotation); kmMat4Translation(&transMat, node->isCamera ? -node->position.x : node->position.x, node->isCamera ? -node->position.y : node->position.y, node->isCamera ? -node->position.z : node->position.z); if (node->scale.x != 1.0f || node->scale.y != 1.0f || node->scale.z != 1.0f) { kmMat4Scaling(&scaleMat, node->scale.x, node->scale.y, node->scale.z); kmMat4Multiply(&transMat, &transMat, &scaleMat); } if (node->rotationCentered) kmMat4Multiply(&node->localView, &transMat, &node->localView); else kmMat4Multiply(&node->localView, &node->localView, &transMat); if (node->baseNode) { kmMat4Multiply(&node->worldView, &node->baseNode->worldView, &node->localView); } else { node->worldView = node->localView; } kmMat3NormalMatrix(&node->normalModel, &node->worldView); node->recalc = LITE3D_FALSE; node->invalidated = LITE3D_TRUE; updated = LITE3D_TRUE; } return updated; }
kmQuaternion* kmQuaternionLookRotation(kmQuaternion* pOut, const kmVec3* direction, const kmVec3* upDirection) { kmMat4 lookAt; kmMat3 rot; kmMat4LookAt(&lookAt, &KM_VEC3_ZERO, direction, upDirection); kmMat4ExtractRotationMat3(&lookAt, &rot); kmQuaternionRotationMatrix(pOut, &rot); kmQuaternionNormalize(pOut, pOut); return pOut; }
/**< Interpolate between 2 quaternions*/ kmQuaternion* kmQuaternionSlerp(kmQuaternion* pOut, const kmQuaternion* q1, const kmQuaternion* q2, kmScalar t) { kmQuaternion tmp; kmQuaternion t1, t2; kmScalar theta_0; kmScalar theta; kmScalar dot = kmQuaternionDot(q1, q2); const double DOT_THRESHOLD = 0.9995; if (dot > DOT_THRESHOLD) { kmQuaternion diff; kmQuaternionSubtract(&diff, q2, q1); kmQuaternionScale(&diff, &diff, t); kmQuaternionAdd(pOut, q1, &diff); kmQuaternionNormalize(pOut, pOut); return pOut; } dot = kmClamp(dot, -1, 1); theta_0 = acos(dot); theta = theta_0 * t; kmQuaternionScale(&tmp, q1, dot); kmQuaternionSubtract(&tmp, q2, &tmp); kmQuaternionNormalize(&tmp, &tmp); kmQuaternionScale(&t1, q1, cos(theta)); kmQuaternionScale(&t2, &tmp, sin(theta)); kmQuaternionAdd(pOut, &t1, &t2); return pOut; }
kmScalar kmQuatToAxisAngle(struct kmVec3* pOut, const kmQuaternion* pIn) { kmQuaternion q; kmQuaternionNormalize(&q, pIn); if (pOut) { pOut->x = q.x; pOut->y = q.y; pOut->z = q.z; kmVec3Normalize(pOut, pOut); } return 2.0f * acos(q.w);; }
/**< Rotates a quaternion around an axis*/ kmQuaternion* kmQuaternionRotationAxisAngle(kmQuaternion* pOut, const kmVec3* pV, kmScalar angle) { kmScalar rad = angle * 0.5f; kmScalar scale = sinf(rad); pOut->x = pV->x * scale; pOut->y = pV->y * scale; pOut->z = pV->z * scale; pOut->w = cosf(rad); kmQuaternionNormalize(pOut, pOut); return pOut; }
/* * Returns a Quaternion representing the angle between two vectors */ kmQuaternion* kmQuaternionBetweenVec3(kmQuaternion* pOut, const kmVec3* u, const kmVec3* v) { kmVec3 w; kmScalar len; kmQuaternion q; if(kmVec3AreEqual(u, v)) { kmQuaternionIdentity(pOut); return pOut; } len = sqrtf(kmVec3LengthSq(u) * kmVec3LengthSq(v)); kmVec3Cross(&w, u, v); kmQuaternionFill(&q, w.x, w.y, w.z, kmVec3Dot(u, v) + len); return kmQuaternionNormalize(pOut, &q); }
kmQuaternion* kmQuaternionExtractRotationAroundAxis(const kmQuaternion* pIn, const kmVec3* axis, kmQuaternion* pOut) { /** Given a quaternion, and an axis. This extracts the rotation around the axis into pOut as another quaternion. Uses the swing-twist decomposition. http://stackoverflow.com/questions/3684269/component-of-a-quaternion-rotation-around-an-axis/22401169?noredirect=1#comment34098058_22401169 */ kmVec3 ra; kmScalar d; kmVec3Fill(&ra, pIn->x, pIn->y, pIn->z); d = kmVec3Dot(&ra, axis); kmQuaternionFill(pOut, axis->x * d, axis->y * d, axis->z * d, pIn->w); kmQuaternionNormalize(pOut, pOut); return pOut; }
kmQuaternion* kmQuaternionLookRotation(kmQuaternion* pOut, const kmVec3* direction, const kmVec3* up) { kmMat3 tmp; kmMat3LookAt(&tmp, &KM_VEC3_ZERO, direction, up); return kmQuaternionNormalize(pOut, kmQuaternionRotationMatrix(pOut, &tmp)); /* if(!direction->x && !direction->y && !direction->z) { return kmQuaternionIdentity(pOut); } kmVec3 right; kmVec3Cross(&right, up, direction); pOut->w = sqrtf(1.0f + right.x + up->y + direction->z) * 0.5f; float w4_recip = 1.0f / (4.0f * pOut->w); pOut->x = (up->z - direction->y) * w4_recip; pOut->y = (direction->x - right.z) * w4_recip; pOut->z = (right.y - up->x) * w4_recip; return kmQuaternionNormalize(pOut, pOut);*/ }
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) { kmQuaternionRotationAxisAngle(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); kmQuaternionRotationAxisAngle(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; }
///< 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; }