Calculate Rotation Matrix to align Vector A to Vector B in 3d?
阿新 • • 發佈:2019-01-10
Octave/Matlab Implementation
The basic implementation is very simple. You could improve it by factoring out the common expressions of dot(A,B)
and cross(B,A)
. Also note that ||A×B||=||B×A||||A×B||=||B×A||.
GG = @(A,B) [ dot(A,B) -norm(cross(A,B)) 0;\ norm(cross(A,B)) dot(A,B) 0;\ 0 0 1]; FFi = @(A,B) [ A (B-dot(A,B)*A)/norm(B-dot(A,B)*A) cross(B,A) ]; UU = @(Fi,G) Fi*G*inv(Fi);
Testing:
> a=[1 0 0]'; b=[0 1 0]';
> U = UU(FFi(a,b), GG(a,b));
> norm(U) % is it length-preserving?
ans = 1
> norm(b-U*a) % does it rotate a onto b?
ans = 0
> U
U =
0 -1 0
1 0 0
0 0 1
Now with random vectors:
> vu = @(v) v/norm(v); > ru = @() vu(rand(3,1)); > a = ru() a = 0.043477 0.036412 0.998391 > b = ru() b = 0.60958 0.73540 0.29597 > U = UU(FFi(a,b), GG(a,b)); > norm(U) ans = 1 > norm(b-U*a) ans = 2.2888e-16 > U U = 0.73680 -0.32931 0.59049 -0.30976 0.61190 0.72776 -0.60098 -0.71912 0.34884