Permalink
Cannot retrieve contributors at this time
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
MatlabMain/AxelRot.m
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
180 lines (132 sloc)
3.52 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
function varargout=AxelRot(varargin) | |
%Generate roto-translation matrix for the rotation around an arbitrary line in 3D. | |
%The line need not pass through the origin. Optionally, also, apply this | |
%transformation to a list of 3D coordinates. | |
% | |
%SYNTAX 1: | |
% | |
% M=AxelRot(deg,u,x0) | |
% | |
% | |
%in: | |
% | |
% u, x0: 3D vectors specifying the line in parametric form x(t)=x0+t*u | |
% Default for x0 is [0,0,0] corresponding to pure rotation (no shift). | |
% If x0=[] is passed as input, this is also equivalent to passing | |
% x0=[0,0,0]. | |
% | |
% deg: The counter-clockwise rotation about the line in degrees. | |
% Counter-clockwise is defined using the right hand rule in reference | |
% to the direction of u. | |
% | |
% | |
%out: | |
% | |
% M: A 4x4 affine transformation matrix representing | |
% the roto-translation. Namely, M will have the form | |
% | |
% M=[R,t;0 0 0 1] | |
% | |
% where R is a 3x3 rotation and t is a 3x1 translation vector. | |
% | |
% | |
% | |
%SYNTAX 2: | |
% | |
% [R,t]=AxelRot(deg,u,x0) | |
% | |
% Same as Syntax 1 except that R and t are returned as separate arguments. | |
% | |
% | |
% | |
%SYNTAX 3: | |
% | |
% This syntax requires 4 input arguments be specified, | |
% | |
% [XYZnew, R, t] = AxelRot(XYZold, deg, u, x0) | |
% | |
% where the columns of the 3xN matrix XYZold specify a set of N point | |
% coordinates in 3D space. The output XYZnew is the transformation of the | |
% columns of XYZold by the specified rototranslation about the axis. All | |
% other input/output arguments are as before. | |
% | |
% by Matt Jacobson | |
% | |
% Copyright, Xoran Technologies, Inc. 2011 | |
if nargin>3 | |
XYZold=varargin{1}; | |
varargin(1)=[]; | |
[R,t]=AxelRot(varargin{:}); | |
XYZnew=bsxfun(@plus,R*XYZold,t); | |
varargout={XYZnew, R,t}; | |
return; | |
end | |
[deg,u]=deal(varargin{1:2}); | |
if nargin>2, x0=varargin{3}; end | |
R3x3 = nargin>2 && isequal(x0,'R'); | |
if nargin<3 || R3x3 || isempty(x0), | |
x0=[0;0;0]; | |
end | |
x0=x0(:); u=u(:)/norm(u); | |
AxisShift=x0-(x0.'*u).*u; | |
Mshift=mkaff(eye(3),-AxisShift); | |
Mroto=mkaff(R3d(deg,u)); | |
M=inv(Mshift)*Mroto*Mshift; | |
varargout(1:2)={M,[]}; | |
if R3x3 || nargout>1 | |
varargout{1}=M(1:3,1:3); | |
end | |
if nargout>1, | |
varargout{2}=M(1:3,4); | |
end | |
function R=R3d(deg,u) | |
%R3D - 3D Rotation matrix counter-clockwise about an axis. | |
% | |
%R=R3d(deg,axis) | |
% | |
% deg: The counter-clockwise rotation about the axis in degrees. | |
% axis: A 3-vector specifying the axis direction. Must be non-zero | |
R=eye(3); | |
u=u(:)/norm(u); | |
x=deg; %abbreviation | |
for ii=1:3 | |
v=R(:,ii); | |
R(:,ii)=v*cosd(x) + cross(u,v)*sind(x) + (u.'*v)*(1-cosd(x))*u; | |
%Rodrigues' formula | |
end | |
function M=mkaff(varargin) | |
% M=mkaff(R,t) | |
% M=mkaff(R) | |
% M=mkaff(t) | |
% | |
%Makes an affine transformation matrix, either in 2D or 3D. | |
%For 3D transformations, this has the form | |
% | |
% M=[R,t;[0 0 0 1]] | |
% | |
%where R is a square matrix and t is a translation vector (column or row) | |
% | |
%When multiplied with vectors [x;y;z;1] it gives [x';y';z;1] which accomplishes the | |
%the corresponding affine transformation | |
% | |
% [x';y';z']=R*[x;y;z]+t | |
% | |
if nargin==1 | |
switch numel(varargin{1}) | |
case {4,9} %Only rotation provided, 2D or 3D | |
R=varargin{1}; | |
nn=size(R,1); | |
t=zeros(nn,1); | |
case {2,3} | |
t=varargin{1}; | |
nn=length(t); | |
R=eye(nn); | |
end | |
else | |
[R,t]=deal(varargin{1:2}); | |
nn=size(R,1); | |
end | |
t=t(:); | |
M=eye(nn+1); | |
M(1:end-1,1:end-1)=R; | |
M(1:end-1,end)=t(:); | |