/* http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29
http://en.wikipedia.org/wiki/Rotation_matrix
*/
void
RMtx4RotationZ( RMtx4* out, rmReal radian )
{
rmReal s = rmSin( radian );
rmReal c = rmCos( radian );
RMtx4Identity( out );
SET_ELEMENT( out, 0, 0, c );
SET_ELEMENT( out, 0, 1, -s );
SET_ELEMENT( out, 1, 0, s );
SET_ELEMENT( out, 1, 1, c );
}
void
RQuatRotationAxis( RQuat* out, const struct RVec3* axis, rmReal radian )
{
rmReal s = rmSin( radian / 2.0f );
rmReal x, y, z, w;
x = s * RVec3GetX( axis );
y = s * RVec3GetY( axis );
z = s * RVec3GetZ( axis );
w = rmCos( radian / 2.0f );
RQuatSetElements( out, x, y, z, w );
}
/* Quaternion Algebra and Calculus
http://www.geometrictools.com/Documentation/Quaternions.pdf
*/
void
RQuatSlerp( RQuat* out, const RQuat* q1, const RQuat* q2, rmReal t )
{
rmReal s1, s2;
rmReal it = 1.0f - t;
rmReal cosine = RQuatDot( q1, q2 );
RQuat qn1, qn2, qResult;
RQuatCopy( &qn1, q1 );
RQuatCopy( &qn2, q2 );
if ( cosine < 0.0f )
{
cosine *= -1.0f;
RQuatScale( &qn1, &qn1, -1.0f );
}
if ( (1.0f - cosine) > RMATH_TOLERANCE )
{
rmReal theta = rmAcos( cosine );
rmReal sin_theta = rmSin( theta );
s1 = rmSin( it * theta ) / sin_theta;
s2 = rmSin( t * theta ) / sin_theta;
}
else
{
s1 = it;
s2 = t;
}
RQuatScale( &qn1, &qn1, s1 );
RQuatScale( &qn2, &qn2, s2 );
RQuatAdd( &qResult, &qn1, &qn2 );
RQuatCopy( out, &qResult );
}
/* http://en.wikipedia.org/wiki/Rotation_matrix
*/
void
RMtx4RotationAxis( RMtx4* out, const RVec3* axis, rmReal radian )
{
rmReal s = rmSin( radian );
rmReal c = rmCos( radian );
rmReal C = 1.0f - c;
rmReal x = RVec3GetX( axis );
rmReal y = RVec3GetY( axis );
rmReal z = RVec3GetZ( axis );
RMtx4Identity( out );
SET_ELEMENT( out, 0, 0, x*x*C + c );
SET_ELEMENT( out, 0, 1, x*y*C - z*s );
SET_ELEMENT( out, 0, 2, z*x*C + y*s );
SET_ELEMENT( out, 1, 0, x*y*C + z*s );
SET_ELEMENT( out, 1, 1, y*y*C + c );
SET_ELEMENT( out, 1, 2, y*z*C - x*s );
SET_ELEMENT( out, 2, 0, z*x*C - y*s );
SET_ELEMENT( out, 2, 1, y*z*C + x*s );
SET_ELEMENT( out, 2, 2, z*z*C + c );
}