Skip to content

Instantly share code, notes, and snippets.

@peteristhegreat

peteristhegreat/fcn_RotationFromTwoVectors.m

Created August 20, 2015 22:09
Show Gist options
  • Save peteristhegreat/3b76d5169d7b9fc1e333 to your computer and use it in GitHub Desktop.
Save peteristhegreat/3b76d5169d7b9fc1e333 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. peteristhegreat created this gist Aug 20, 2015.
    34 changes: 34 additions & 0 deletions fcn_RotationFromTwoVectors.m
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,34 @@
    function R=fcn_RotationFromTwoVectors(A, B)
    % http://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
    % R*v1=v2
    % v1 and v2 should be column vectors and 3x1

    %% Method 1
    % % 1. rotation vector
    % w=cross(v1,v2);
    % w=w/norm(w);
    % w_hat=fcn_GetSkew(w);
    % % 2. rotation angle
    % cos_tht=v1'*v2/norm(v1)/norm(v2);
    % tht=acos(cos_tht);
    % % 3. rotation matrix, using Rodrigues' formula
    % R=eye(size(v1,1))+w_hat*sin(tht)+w_hat^2*(1-cos(tht));
    %
    % function x_skew=fcn_GetSkew(x)
    % x_skew=[0 -x(3) x(2);
    % x(3) 0 -x(1);
    % -x(2) x(1) 0];

    %% Method 2
    % g = [ dot(A,B) -norm(cross(A,B)) 0;
    % norm(cross(A,B)) dot(A,B) 0;
    % 0 0 1];
    %
    % f = [ A (B-dot(A,B)*A)/norm(B-dot(A,B)*A) cross(B,A) ];
    %
    % R = f*g/f;

    %% Method 3
    v = cross(A,B);
    ssc = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];
    R = eye(3) + ssc + ssc^2*(1-dot(A,B))/(norm(v))^2;