function M = inv4(A)
% Inwersja 4x4 (dla macierzy jednorodnych działa dobrze)
    R = A(1:3,1:3);
    p = A(1:3,4);
    Rt = R.';             % ortonormalne (rotacja)
    M = eye(4);
    M(1:3,1:3) = Rt;
    M(1:3,4)   = -Rt*p;
end