Skip to content

Instantly share code, notes, and snippets.

@peteristhegreat
Created August 20, 2015 22:09
Show Gist options
  • Star 9 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save peteristhegreat/3b76d5169d7b9fc1e333 to your computer and use it in GitHub Desktop.
Save peteristhegreat/3b76d5169d7b9fc1e333 to your computer and use it in GitHub Desktop.
Given two vectors, create a rotation matrix to rotate from A to B, in matlab
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;
@mpatel8
Copy link

mpatel8 commented Mar 19, 2018

Thanks ! just what I was looking for. One minor improvement would be to offer an option to return normalised matrix.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment