-
Notifications
You must be signed in to change notification settings - Fork 0
/
PlotHTMArrayZ.m
33 lines (25 loc) · 905 Bytes
/
PlotHTMArrayZ.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
function h = PlotHTMArrayZ( HTMarray )
%PLOTHTMARRAY Plots the HTMs in a struct array
% PLOTHTMARRAY(HTMarray) plots the HTM in the struct array HTMarray.
%
% HTMarray is a struct array where the struct contains a field, called
% HTM, a 4x4 array representing a Homogeneous Transformation Matrix.
%
% X and y axes are plotted in blue; z axis is plotted in red.
%
% See also PLOT3DREFAXIS
% hh is an array with all HTMs, side by side
hh = [HTMarray.HTM];
% Get the position vectors, corresponding to HTM(1:3,4)
% p is a matrix where each column is the respective position vector
p = hh(1:3,4:4:size(hh,2));
% Get the x, y and z coords of the reference frame axes associated to the
% HTMs
x = hh(1:3,1:4:size(hh,2));
y = hh(1:3,2:4:size(hh,2));
z = hh(1:3,3:4:size(hh,2));
hold on
% Plot the z axis vectors
h = quiver3(p(1,:),p(2,:),p(3,:),z(1,:),z(2,:),z(3,:),0,'r');
end