libdddvision/PlotQuat.m at master · dddvision/libdddvision · GitHub

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
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
function []=PlotQuat(q,qdot)
% Plots a graphical representation of a body orientation and rate.
%
% q = quaternion <scalar,vector> representation of the body orientation (4-by-1)
% qdot = quaternion <scalar,vector> representation of the body rotation rate (4-by-1)
%
% Copyright 2003 David D. Diel, MIT License


%points to define the "airplane"
p(:,1)=[4;0;0]; %axis 1
p(:,2)=[0;1;0]; %axis 2.a
p(:,3)=[0;-1;0]; %axis 2.b
p(:,4)=[0;0;-0.5]; %axis 3

%rotate the points
M=tom.Rotation.quatToMatrix(q);
pp=M*p;

X=pp(1,:);
Y=pp(2,:);
Z=pp(3,:);

Tri=[[1,2,3]
     [2,3,4]
     [3,4,1]
     [4,1,2]];

%plot orientations and rates
trisurf(Tri,X,-Y,-Z,[0,1,0.5,0.5]);
hold on
plot3(0,0,0,'r.')

if nargin==2 %rates
  vc=2.5*[-1;1;1]; %center of little coordinate system

  J=2*[[-q(2), q(1),-q(4), q(3)]
       [-q(3), q(4), q(1),-q(2)]
       [-q(4),-q(3), q(2), q(1)]]; %jacobian between quaternion rate and body axis rates

  vdot=J*qdot;

  plot3(vc(1)+[0;vdot(1)],vc(2)-[0;vdot(2)],vc(3)-[0;vdot(3)],'r.-');

  %show a tiny axis system
  plot3(vc(1)+[0;1],vc(2)-[0;0],vc(3)-[0;0],'c-');
  plot3(vc(1)+[0;0],vc(2)-[0;1],vc(3)-[0;0],'c-');
  plot3(vc(1)+[0;0],vc(2)-[0;0],vc(3)-[0;1],'c-');
end

hold off
axis square
axis([-5,5,-5,5,-5,5])
set(gca,'XTickLabel',[])
set(gca,'YTickLabel',[])
set(gca,'ZTickLabel',[])

xlabel('axis 1');
ylabel('axis 2');
zlabel('axis 3');

set(gcf,'Color',[0,0,0])
set(gca,'Color',[0.1,0.1,0.1])

end