Rotation matrix causes distance changes
6 views (last 30 days)
Show older comments
What I am doing: Rotate a point (sphn) around a custom unit vector created between two points (sph1, sph2). The points have the format [x;y;z;a;b;c] but only x,y,z should be used.
Problem: The distance between sphn and sph1/sph2 after rotation is different before/after rotation. My guess: I'm forgetting about something simple.
I'm using the rotation matrix from Wikipedia, Rotation Matrix from axis and angle. I've tried both sin(rad) and sind(deg) versions of the rotation matrix but the problem seems to persist. For simple examples (e.g. sph1 = [0;0;0];sph2=[0;0;10];sphn=[1;10;5];) the deg variant seems to be holding up well (distances match, sphn(3) doesn't change) but for more complex examples the problem persists.
My code:
function [ sph, sphalt ] = rotatetest( sphn, sph1, sph2 )
g12=sph2(1:3)-sph1(1:3);
g12n = g12/norm(g12);
ux = g12n(1);
uy = g12n(2);
uz = g12n(3);
alpha = 20;
t = alpha*pi()/180;
R1 = [ cos(t)+ux^2*(1-cos(t)), ux*uy*(1-cos(t))-uz*sin(t), ux*uz*(1-cos(t))+uy*sin(t);
uy*ux*(1-cos(t))+uz*sin(t), cos(t)+uy^2*(1-cos(t)), ux*uz*(1-cos(t))-ux*sin(t);
uz*ux*(1-cos(t))-uz*sin(t), uz*uy*(1-cos(t))+ux*sin(t), cos(t)+uz^2*(1-cos(t))];
sph = R1*sphn(1:3);
R2 = [ (cosd(alpha)+ux^2*(1-cosd(alpha))), (ux*uy*(1-cosd(alpha))-uz*sind(alpha)), (ux*uz*(1-cosd(alpha))+uy*sind(alpha));
(uy*ux*(1-cosd(alpha))+ uz*sind(alpha)), (cosd(alpha)+uy^2*(1-cosd(alpha))), (uy*uz*(1-cosd(alpha)) - ux*sind(alpha));
(uz*ux*(1-cosd(alpha))-uy*sind(alpha)), (uz*uy*(1-cosd(alpha)) + ux*sind(alpha)), (cosd(alpha) + uz^2*(1-cosd(alpha)))];
sphalt = R2*sphn(1:3);
dist1n1 = sqrt(sum( (sph1(1:3)-sphn(1:3)).^2))
dist1n2 = sqrt(sum( (sph2(1:3)-sphn(1:3)).^2))
R1dist2n1 = sqrt(sum( (sph1(1:3)-sph(1:3)).^2))
R1dist2n2 = sqrt(sum( (sph2(1:3)-sph(1:3)).^2))
R2dist2n1 = sqrt(sum( (sph1(1:3)-sphalt(1:3)).^2))
R2dist2n2 = sqrt(sum( (sph2(1:3)-sphalt(1:3)).^2))
end
0 Comments
Accepted Answer
More Answers (1)
See Also
Products
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!