/**< 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);
}
}
/**< 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);
}
}
///< 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;
}
else {
kmScalar ct;
struct kmQuaternion temp;
struct kmQuaternion temp2;
kmScalar theta;
kmScalar st;
kmScalar somt;
kmScalar stt;
ct = kmQuaternionDot(q1, q2);
theta = acosf(ct);
st = sqrtf(1.0 - kmSQR(ct));
stt = sinf(t * theta) / st;
somt = sinf((1.0 - t) * theta) / st;
kmQuaternionScale(&temp, q1, somt);
kmQuaternionScale(&temp2, q2, stt);
kmQuaternionAdd(pOut, &temp, &temp2);
return pOut;
}
}
kmScalar kmVec2LengthSq(const kmVec2* pIn)
{
return kmSQR(pIn->x) + kmSQR(pIn->y);
}
kmScalar kmVec2Length(const kmVec2* pIn)
{
return sqrtf(kmSQR(pIn->x) + kmSQR(pIn->y));
}
/**
* Returns the square of the length of the vector
*/
kmScalar kmVec3LengthSq(const kmVec3* pIn)
{
return kmSQR(pIn->x) + kmSQR(pIn->y) + kmSQR(pIn->z);
}
/**
* Returns the length of the vector
*/
kmScalar kmVec3Length(const kmVec3* pIn)
{
return sqrtf(kmSQR(pIn->x) + kmSQR(pIn->y) + kmSQR(pIn->z));
}
/// Returns the length of the 4D vector squared.
kmScalar kmVec4LengthSq(const kmVec4* pIn) {
return kmSQR(pIn->x) + kmSQR(pIn->y) + kmSQR(pIn->z) + kmSQR(pIn->w);
}
/// Returns the length of a 4D vector, this uses a sqrt so if the squared length will do use
/// kmVec4LengthSq
kmScalar kmVec4Length(const kmVec4* pIn) {
return kdSqrtf(kmSQR(pIn->x) + kmSQR(pIn->y) + kmSQR(pIn->z) + kmSQR(pIn->w));
}
