function R = HRZ(q)
    cq = cos(q); sq = sin(q);
    R = eye(4);
    R(1,1)=cq; R(1,2)=-sq;
    R(2,1)=sq; R(2,2)= cq;
end