function V = transform(V, angles, t)
centroid=mean(V);
t_zero=zeros(3,1);
T = SE3(eye(3),t)*SE3(eye(3),centroid)*SE3(rotmatz(angles(3)), t_zero)*SE3(rotmaty(angles(2)), t_zero)*SE3(rotmatx(angles(1)), t_zero)*SE3(eye(3), -centroid);
Vhom = [V ones(size(V,1), 1)]’;
Vhom = T*Vhom;
V = Vhom(1:3,:)’;
end
function T=SE3(R,t)
T=eye(4);
T(1:3,1:3) = R;
T(1:3,4)=t;
end
function R = rotmatx(a)
R=[1 0 0;
0 cos(a) -sin(a);
0 sin(a) cos(a)];
end
function R = rotmaty(a)
R=[cos(a) 0 sin(a);
0 1 0;
-sin(a) 0 cos(a)];
end
function R = rotmatz(a)
R=[cos(a) -sin(a) 0;
sin(a) cos(a) 0;
0 0 1];
end