Y = inverseH(H);
Y = inverseH(R,P);
Returns the inverse of a homogeneous matrix H [4,4]. The homogeneous matrix can be entered directly or by a rotation matrix R[3,3] and position vector P [3,1].
The size of the inverse Y is also [4,4].
This function uses the special nature of a homogeneous matrix. I.e the inverse can be computed directly instead of the numerical approach of the standard inverse function.
H = [cos(alpha),-sin(alpha),0,p1 ; sin(alpha),cos(alpha),0,p2 ; 0,0,0,p3 ; 0,0,0,1];
Y = inverseH(H);
or
R = [cos(alpha),-sin(alpha),0 ; sin(alpha),cos(alpha),0 ; 0,0,0];
P = [p1 ; p2 ; p3];
Y = inverseH(R,P);
H must be a homogeneous matrix of size [4,4], R must always be of size[3,3] , P must have the size [3,1] and Y must have size [4,4].