Transforming and plotting coordinate systems in MATLAB

I have just started a new project which involves lots of coordinate transformations. So I created my basic functions which would let me define, translate, rotate, plot coordinate systems. Here is the code snippet:

function MessingWithCoordinateSystems

clear all;clc; close all;

% Define absolute coordinate system (ABS).

csys_ABS = create_csys_object([0;0;0], eye(3));

% Define a new csys by rotating ABS around y axis 90 degrees, and then

% rotating around x axis by 45 degrees, then translating by 5 units in x

% direction.

Rot_ABS_to_NewCSYS = eulseq('21',[90,45]); %Rotation matrix that will rotate WCS to new csys

csys_New = rotate_csys(csys_ABS, Rot_ABS_to_NewCSYS); %Rotate WCS to obtain new csys

csys_New = translate_csys(csys_New, csys_New, [0;0;2]); %Translate new csys

% Define vector from ABS origin to NewCSYS origin.

R_ABS_to_NewCSYS_in_ABS = csys_New.Origin-csys_ABS.Origin;

% Plot the results.

h=figure(1);

plot_csys(h,csys_ABS);

plot_csys(h,csys_New);

plot_vector(h,R_ABS_to_NewCSYS_in_ABS,csys_ABS);

end

function OUT = create_csys_object(ORIGIN, ORIENTATION)

% Creates a structure OUT with the following properties:

%  .Origin: 3x1 vector representing origin of coordinate system. Copied

%           from ORIGIN.

%  .Orientation: 3x3 matrix whose columns represent directions of x,y,z

%                axes with respect to absolute frame. Copied from

%                ORIENTATION.

    if(size(ORIGIN) ~= [3,1])

        error('ORIGIN size must 3x1');

    end

    if(size(ORIENTATION) ~= [3,3])

        error('ORIENTATION size must 3x3');

    end

    OUT.Origin = ORIGIN;

    OUT.Orientation = ORIENTATION;

end

function OUT = translate_csys(CSYS, CSYS_BASE, TRANSLATION)

% Translates coordinate system CSYS by the amount given by TRANSLATION

% TRANSLATION vector is resolved in CSYS_BASE.

%

% CSYS is the coordinate frame to be translated. It is a structure with

% following properties:

%  .Origin: 3x1 vector representing origin of coordinate system.

%  .Orientation: 3x3 matrix whose columns represent directions of x,y,z

%                axes with respect to absolute frame

% 

% CSYS_BASE is the coordinate frame that TRANSLATION vector is resolved

% in. It is a structure with following properties:

%  .Origin: 3x1 vector representing origin of coordinate system.

%  .Orientation: 3x3 matrix whose columns represent directions of x,y,z

%                axes with respect to absolute frame

% 

% TRANSLATION is a 3x1 vector containing displacements in x,y,z axes of

% coordinate frame CSYS_BASE.

    OUT.Origin = CSYS.Origin+CSYS_BASE.Orientation*TRANSLATION;

    OUT.Orientation = CSYS.Orientation;

end

function OUT = rotate_csys(CSYS,ROTMAT)

% Rotates a given coordinate system

%

% CSYS is a structure with following properties:

%  .Origin: 3x1 vector representing origin of coordinate system.

%  .Orientation: 3x3 matrix whose columns represent directions of x,y,z

%                axes with respect to absolute frame.

%

% ROTMAT is a 3x3 rotation matrix. 

    OUT.Orientation = CSYS.Orientation*ROTMAT;

    OUT.Origin = CSYS.Origin;

end

function plot_csys(HANDLE, CSYS)

% Plots the coordinate system given by CSYS to figure with handle 

% HANDLE

%

% HANDLE is handle to the figure.

% CSYS is the coordinate system to be drawn. It is a structure with

% following properties:

%  .Origin: 3x1 vector representing origin of coordinate system.

%  .Orientation: 3x3 matrix whose columns represent directions of x,y,z

%                axes.

    figure(HANDLE);

    hold on;

    colors = 'rgb';

    arrowTail = CSYS.Origin; 

    for i=1:3

        arrowHead = CSYS.Origin+CSYS.Orientation(:,i);

        line([arrowTail(1),arrowHead(1)],[arrowTail(2),arrowHead(2)],...

            [arrowTail(3),arrowHead(3)],'Color',colors(i));

    end

    hold off;

    xlabel('X');

    ylabel('Y');

    zlabel('Z');

    grid on;

    axis equal;

end

function plot_vector(HANDLE, VECTOR, CSYS)

% Plots the vector given by VECTOR resolved in coordinate system 

% CSYS to figure with handle HANDLE.

%

% HANDLE is handle to the figure.

% VECTOR is a 3x1 vector.

% CSYS is the coordinate system that the vector is resolved in. It is a

% structure with following properties:

%  .Origin: 3x1 vector representing origin of coordinate system.

%  .Orientation: 3x3 matrix whose columns represent directions of x,y,z

%                axes with respect to absolute coordinate frame.

    figure(HANDLE);

    hold on;

    arrowTail = CSYS.Origin;

    arrowHead = CSYS.Origin+VECTOR;

    line([arrowTail(1),arrowHead(1)],[arrowTail(2),arrowHead(2)],...

        [arrowTail(3),arrowHead(3)],'Color','m');

    hold off;

end

function OUT=r2d(ANGLE)

% Converts radians to degrees

    OUT = ANGLE*180/pi;

end

function OUT=d2r(ANGLE)

% Converts degrees to radians

    OUT = ANGLE*pi/180;

end

function OUT=rot(AXIS,ANGLE)

% Calculates the basic rotation matrix numerically

% 

% AXIS is either 'x', 'y', or 'z'.

% ANGLE is in degrees. 

    if (length(AXIS)>1 | length(ANGLE)>1)

        error('Input sizes must be 1x1');

    end

    ANGLE = d2r(ANGLE);

    if AXIS=='x'

        OUT=[          1,           0,           0;...

                       0,  cos(ANGLE), -sin(ANGLE);...

                       0,  sin(ANGLE),  cos(ANGLE)];

    elseif AXIS=='y'

        OUT=[ cos(ANGLE),           0,  sin(ANGLE);...

                       0,           1,           0;...

             -sin(ANGLE),           0,  cos(ANGLE)];

    elseif AXIS=='z'

        OUT=[ cos(ANGLE), -sin(ANGLE),           0;...

              sin(ANGLE),  cos(ANGLE),           0;...

                       0,           0,           1];

    else

        error('AXIS input must be x, y or z');

    end

    

end

function OUT=eulseq(SEQUENCE,ANGLES)

% Calculates rotation matrix corresponding to given Euler Sequence

% SEQUENCE and angles ANGLES.

% 

% SEQUENCE is 1xN char array composed of chars 1, 2, 3 only. For

% example '123' or '32' etc.

%

% ANGLES is 1xN array of angles in degrees. ANGLES(i) is the rotation

% about axis defined by SEQUENCE(i).

    if length(SEQUENCE) ~= length(ANGLES)

        error('SEQUENCE and ANGLES must be of same length.');

    elseif ~isempty(regexpi(SEQUENCE,'[^123]'))

        error('SEQUENCE must only contain 1, 2, and 3 characters.');

    else

        C=eye(3);

        

        str='xyz'; %used to convert 123 to xyz

        

        for k=1:length(SEQUENCE)

            C=C*rot(str(str2double(SEQUENCE(k))),ANGLES(k));

        end

        OUT=C;

    end

end

You should copy/paste all the code to a new m-file and save m-file as "MessingWithCoordinateSystems.m" and then run the code. What the main code does is as follows:

1. First an absolute coordinate system (csys) is defined. It is named csys_ABS.
2. A new csys is defined by rotating csys_ABS with '21' euler sequence by 90 and 45 degrees respectively, and later translating the rotating csys in its z direction by 2 units.
3. A vector is defined from absolute csys origin to new csys origin.
4. Everything is plotted

The output looks like this (after using the rotation tool in figure toolbar):

Figure 1. Output of code snippet showing absolute csys, transformed csys and the vector connecting their origin

My coordinate systems are structures with two properties: Origin and Orientation. In the case of absolute coordinate system, origin is zero vector and orientation is the identity matrix. All other coordinate systems and vectors are defined with respect to this absolute coordinate system. With the functions given you can do the followings:

• eulseq: You can obtain rotation matrix for any Euler angle sequence.
• rot: You can obtain basic rotation matrix around any single axis.
• d2r, r2d: Converts degrees to radians and vice versa.
• plot_vector, plot_csys: Plots csys objects and vectors on figure.
• rotate_csys: Rotates a csys by a given rotation matrix. Rotation matrix can be obtained from eulseq or rot or any other way.
• translate_csys: Translates a csys by a vector. The vector may be resolved in absolute coordinate system or the coordinate system to be translated or any other csys.
• create_csys_object: Creates csys object with required structure.