Rotational submatrix
R = t2r(T) is the orthonormal rotation matrix component of homogeneous transformation matrix T. Works for T in SE(2) or SE(3)
r2t, tr2rt, rt2tr
© 1990-2014 Peter Corke.