Rotation matrix causes distance changes

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

Answer 1

[Matt J]

How big are the differences that you see? In any case, you could try this FEX file,

http://www.mathworks.com/matlabcentral/fileexchange/30864-3d-rotation-about-shifted-axis

and see if you get significantly different results.

[mosch]

The differences varied by distance from origin. I outlined it an answer below. I'll accept yours anyways because it looks like it'd also work (and it would have been less work for me had I found this earlier)! The FEX file seems to use a vector/line in the form of x(t)=x0+t*u . This means it doesn't necessarily pass through the origin, removing the need for moving the point prior and after rotation.

Edit: Forgot: Thank You!

Answer 2

[mosch]

Ok, I found the problem: For the rotation matrix to work correctly, the point I want to rotate needs to be moved towards the origin (because the rotation vector is treated like it moves through the origin). This is achieved by subtracting one of the two rotation vector points. Then apply the rotation. Afterwards move the point back (add the coordinates of the rotation vector point used earlier).

In code:

%Add this before the rotation matrix
sphn(1:3) = sphn(1:3)-sph1(1:3);
sphn=sphn.';

%add this after applying rotation matrix 1 (sph = ...):
sph(1:3) = sph(1:3) + sph1(1:3)

%add this after applying rotation matrix 2 (sphalt = ...):
sphalt(1:3) = sphalt(1:3) + sph1(1:3)

This should make the code functional.