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