FgMatrixC<T,3,3>
fgMatRotateAxis( // RHR rotation around an arbitrary axis
T radians,
const FgMatrixC<T,3,1> &axis) // vector describing that axis
{
FgMatrixC<T,3,3> mat;
FgTypeAttributeFloatingS<T>();
T len = axis.length();
FGASSERT(len > T(0));
FgMatrixC<T,3,1> ax = axis * (T(1) / len); // Ensure the rotation axis is normalized
T ct = (T)cos(radians);
T st = (T)sin(radians);
T vt = T(1)-ct;
mat.elem(0,0) = ax[0]*ax[0]*vt + ct;
mat.elem(0,1) = ax[0]*ax[1]*vt - ax[2]*st;
mat.elem(0,2) = ax[0]*ax[2]*vt + ax[1]*st;
mat.elem(1,0) = ax[0]*ax[1]*vt + ax[2]*st;
mat.elem(1,1) = ax[1]*ax[1]*vt + ct;
mat.elem(1,2) = ax[1]*ax[2]*vt - ax[0]*st;
mat.elem(2,0) = ax[0]*ax[2]*vt - ax[1]*st;
mat.elem(2,1) = ax[1]*ax[2]*vt + ax[0]*st;
mat.elem(2,2) = ax[2]*ax[2]*vt + ct;
return mat;
}
FgMatrixC<T,nrows,ncols>
fgNormalize(FgMatrixC<T,nrows,ncols> m)
{
return m / m.length();
}
