Code covered by the BSD License

# Defining Cartesian Reference Frames based on Point Positions

### Paolo de Leva (view profile)

21 Oct 2005 (Updated )

Defining 3-D Cartesian reference frames based on the positions of at least 3 non-collinear points

frame.m
```function R = frame(a, b, axis1, c, axis2, dim)
%FRAME  Buildung a 3-D Cartesian reference frame based on three vectors
%   This function features an ARRAYLAB (MULTIMATRIX) engine.
%
%   R = FRAME(A, B, AXIS1, C, AXIS2) is equivalent to
%   R = FRAME(A, B, AXIS1, C, AXIS2, DIM), where DIM is the first
%   dimension of length 3 in arrays A, B and C.
%
%   R = FRAME(A, B, AXIS1, C, AXIS2, DIM) returns an array with N + 1
%   dimensions (*), containing M 3-by-3 orientation matrices along its
%   dimensions DIM and DIM + 1. Each matrix contains, in its columns,
%   the unit vectors of the axes of a right handed 3-D Cartesian reference
%   frame (see below). The size of R, if the length of its dimension
%   DIM + 1 is ignored, is equal to that of A, B, and C.
%       A, B, C:    Arrays with N dimensions (*) containing M 3-element
%                   vectors along dimension DIM. Each vector represents a
%                   directed 3-D distance (i.e., the position of a point
%                   relative to another). A, B, C must have the same size.
%                   C is allowed to be equal to A or B.
%   AXIS1, AXIS2:   The names (1x1 char) of two Cartesian axes (see below).
%                   Allowed names: 'x' or 'X', 'y' or 'Y', 'z' or 'Z'.
%            DIM:   The dimension of A, B, and C along which the position
%                   vectors are stored. DIM can be omitted (short syntax).
%
%   Defining the Cartesian axes:
%       If Ai, Bi, Ci are vectors identified, in A, B, and C, by the same
%       indexes, and Ri is the respective orientation matrix contained in
%       R, the three unit vectors in its columns are defined as follows:
%       U1i:   Representing axis AXIS1, aligned with V1i = Ai x Bi.
%       U2i:   Representing axis AXIS2, aligned with the projection
%              of vector Ci on the plane defined by Ai and Bi (ABi).
%       U3i:   Representing the third axis, orthogonal to U1i and U2i,
%              and forming with them a right handed reference frame.
%       Notice that U1i, U2i and U3i are not needs arranged in that order
%       inside Ri (see example 1). Ai and Bi cannot be parallel. Ci cannot
%       be orthogonal to plane ABi, but it can coincide with Ai or Bi.
%
%   Note:
%   (*) N is the number of dimensions in A, B, and C. If A, B, and C are
%       3-by-1 arrays, they are treated as 1-D arrays (N = 1 is assumed,
%       DIM = 1 is required), and R is a single 3-by-3 orientation matrix.
%
%   Examples:
%       In the following examples, the 1st, 2nd and 4th arguments passed to
%       FRAME are supposed to be 3-by-M arrays containing M 3-D distances.
%
%    1) R = FRAME(A, B, 'z', A, 'x', 1) is a 3-by-3-by-M array
%       containing M 3-by-3 matrices. Notice that, in this case, C = A.
%       Each matrix Ri contained in R (for i = 1 : M) is so constructed:
%           Column     Unit vector of    Aligned with
%           ------------------------------------------------------------
%           R(:,1,i)    Axis x (AXIS2)   A(:,i) ()
%           R(:,2,i)    Axis y           Cross prod. of column 3 by 1
%           R(:,3,i)    Axis z (AXIS1)   Cross prod. of A(:,i) by B(:,i)
%           ------------------------------------------------------------
%           () Since A(:,i) is on plane xy, it coincides with its
%               projection on that plane.
%
%    2) A simple method (implemented in the Vicon IQ software) to define
%       orientation matrices based on the positions of 3 points:
%           R = FRAME(-P1+P2, -P2+P3, 'z', -P1+P2 'x', 1)
%
%    3) Anatomical bone-embedded reference frames for the pelvis, right
%       thigh, right shank and right foot, as defined by
%       Cappozzo et al. (1995). Position and orientation in space of
%                    bones during movement: anatomical frame definition and
%                    determination. Clinical Biomechanics, 10, 171-178.
%       The directed distances are obtained from anatomical landmark
%       positions.
%           MidPSIS = (LPSIS + RPSIS) / 2;
%           MidE    = (LE + ME) / 2;
%           MidM    = (LM + MM) / 2;
%           Rpelvis = FRAME(-LASIS+MidPSIS, -LASIS+RASIS, 'y', ...
%                                            -LASIS+RASIS, 'z', 1);
%           RRthigh = FRAME(-HJC+LE,   -LE+ME,   'x', -MidE+HJC, 'y', 1);
%           RRshank = FRAME(-HF+LM,    -LM+MM,   'x', -MidM+TT,  'y', 1);
%           RRfoot  = FRAME(-HEEL+MT5, -MT5+MT1, 'x', -MT2+HEEL, 'y', 1);
%

% Version: 1.2 (old name: REFSYS)
% CODE      by:  Paolo de Leva (Univ. "Foro Italico", Rome, IT) 2013 Feb 23
% COMMENTS  by:  Code author                                    2013 Jul 22
% OUTPUT    tested by: Code author                              2005 Oct 1
% -------------------------------------------------------------------------

% Allow 5 to 6 input arguments
error( nargchk(5, 6, nargin) );

% Setting DIM if not supplied.
if nargin == 5
first3 = find(size(b)==3, 1, 'first'); % First dimension of length 3
dim = max([first3, 1]);            % If FIRST3 is empty, then DIM=1 and
end                                   % function CROSS will issue an error

% Lowercase axis names (A1 and A2)
a1 = lower(axis1);
a2 = lower(axis2);
% Checking axis names
if isequal (a1, a2)
error ('FRAME:NAaxisname', 'The two axis names must be different');
elseif ~ (isequal (a1, 'x') || isequal (a1, 'y') || isequal (a1, 'z') )
error ('FRAME:NAaxisname', ['''' a1 ''' ' ...
'is not allowed as a name for a cartesian axis'] );
elseif ~ (isequal (a2, 'x') || isequal (a2, 'y') || isequal (a2, 'z') )
error ('FRAME:NAaxisname', ['''' a2 ''' ' ...
'is not allowed as a name for a cartesian axis'] );
end

% Determining name of third axis (A3)
switch a1
case 'x'
if     a2 == 'y',   a3 =  'z';
else   a3  = 'y'; % a2 == 'z';
end
case 'y'
if     a2 == 'z',   a3 =  'x';
else   a3  = 'z'; % a2 == 'x';
end
case 'z'
if     a2 == 'x',   a3 =  'y';
else   a3  = 'x'; % a2 == 'y';
end
end

% NOTE: To make the variable names more easily understandable,
% the first three steps of the algorithm were written assuming that
% AXIS1 == 'x' and AXIS2 == 'y'.

% 1 - Normalizing input vectors for magnitude
unitA = unit(a, dim); % On plane TZ
unitB = unit(b, dim); % On plane TZ
unitC = unit(c, dim); % Not necessarily on plane TZ

% 2 - Unit vectors of the Cartesian axes
x = unit( cross(unitA, unitB, dim), dim ); % AXIS1
z = unit( cross(    x, unitC, dim), dim ); % the third axis
y =       cross(    z,     x, dim);        % AXIS2

% 3 - Selecting the verse of the third Cartesian axis
%        Step 1 is valid only if the sequence [A1 A2] is equal to
%        'xy', 'yz' or 'zx'. Otherwise, the reference frame obtained
%        above is left handed.
switch [a1 a2]
case {'xz' 'yx' 'zy'}, z = -z; % Enough to make it right handed
end

% 4 - Adding a singleton dimension to arrays X, Y, and Z

% 5 - R is built by concatenating arrays x, y, and z in the correct order,
%     based on their true names (a1, a2, a3).
%     Example:
%        If X, Y, Z are 5x3x1x9 arrays and DIM==2,
%        R is ....... a 5x3x3x9 array,
colnumbers = int8([a1 a2 a3]) - 119; % Converting axis names into indexes
columns{colnumbers(1)} = x;
columns{colnumbers(2)} = y;
columns{colnumbers(3)} = z;
R = cat(dim+1, columns{:});

%--------------------------------------------------------------------------
%   Example:
%      If A is ................. a 5x9x3   array,
%      B = ADDSINGLETON(A, 3) is a 5x9x1x3 array,

if newdim <= ndims(a)
sizA = size(a);
sizB = [sizA(1:newdim-1), 1, sizA(newdim:end)];
b = reshape(a, sizB);
else
b = a;
end
```