%INVHT invert homogeneous transformation
% inverts a homogeneous transformation matrix
%
% Syntax:  Tinv = invht(T)
%
% Inputs:
%    T - transformation matrix
%
% Outputs:
%    Tinv - inverted transformation matrix

% Author: Travis Hydzik
% Last revision: 19 October 2004

function Tinv = invht(T)

    A = T(1:3, 1:3);
    b = T(1:3, 4);
    Tinv = [ A'   -A'*b
             0 0 0 1    ];