1. 程式人生 > >Calculate Rotation Matrix to align Vector A to Vector B in 3d?

Calculate Rotation Matrix to align Vector A to Vector B in 3d?

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