|
3452
|
1 ## Copyright (C) 1998 Auburn University. All rights reserved. |
|
|
2 ## |
|
|
3 ## This file is part of Octave. |
|
|
4 ## |
|
|
5 ## Octave is free software; you can redistribute it and/or modify it |
|
|
6 ## under the terms of the GNU General Public License as published by |
|
|
7 ## the Free Software Foundation; either version 2, or (at your option) |
|
|
8 ## any later version. |
|
|
9 ## |
|
|
10 ## Octave is distributed in the hope that it will be useful, but |
|
|
11 ## WITHOUT ANY WARRANTY; without even the implied warranty of |
|
|
12 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
|
13 ## General Public License for more details. |
|
|
14 ## |
|
|
15 ## You should have received a copy of the GNU General Public License |
|
|
16 ## along with Octave; see the file COPYING. If not, write to the Free |
|
5307
|
17 ## Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA |
|
|
18 ## 02110-1301, USA. |
|
3224
|
19 |
|
3452
|
20 ## -*- texinfo -*- |
|
|
21 ## @deftypefn {Function File} {} qcoordinate_plot (@var{qf}, @var{qb}, @var{qv}) |
|
|
22 ## Plot in the current figure a set of coordinate axes as viewed from |
|
|
23 ## the orientation specified by quaternion @var{qv}. Inertial axes are |
|
|
24 ## also plotted: |
|
|
25 ## |
|
|
26 ## @table @var |
|
|
27 ## @item qf |
|
|
28 ## Quaternion from reference (x,y,z) to inertial. |
|
|
29 ## @item qb |
|
|
30 ## Quaternion from reference to body. |
|
|
31 ## @item qv |
|
|
32 ## Quaternion from reference to view angle. |
|
|
33 ## @end table |
|
|
34 ## @end deftypefn |
|
|
35 |
|
|
36 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
37 ## Adapted-By: jwe |
|
|
38 |
|
|
39 function qcoordinate_plot (qf, qb, qv) |
|
3224
|
40 |
|
3452
|
41 degrees = pi / 180; |
|
|
42 d180 = 180 * degrees; |
|
|
43 |
|
|
44 ## construct coordinate transformation to view frame |
|
|
45 |
|
|
46 cm = qtransvmat(qv); |
|
3224
|
47 |
|
3452
|
48 p1 = [-1, -1, 1]; |
|
|
49 p2 = [-1, -1, -1]; |
|
|
50 p3 = [ 1, -1, -1]; |
|
|
51 p4 = [ 1, -1, 1]; |
|
|
52 p5 = [-1, 1, 1]; |
|
|
53 p6 = [ 1, 1, 1]; |
|
|
54 p7 = [ 1, 1, -1]; |
|
|
55 p8 = [-1, 1, -1]; |
|
|
56 |
|
|
57 ## outline positive quadrant |
|
|
58 |
|
|
59 box1 = cm * [p4; p6; p5; p6; p7]'; |
|
|
60 |
|
|
61 ## outline rest of the box |
|
|
62 |
|
|
63 box2 = cm * [p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]'; |
|
3224
|
64 |
|
3452
|
65 ## compute inertial to body rotation eigenaxis |
|
|
66 ## qb = qbf*qf => qbf = qb/qf |
|
|
67 ## |
|
|
68 ## need to use inverse quaternion to rotate axes |
|
|
69 |
|
|
70 qbf = qinv (qmult (qb, qinv (qf))); |
|
|
71 |
|
|
72 [eaxv, th_eig] = quaternion (qbf); |
|
|
73 |
|
|
74 ## draw 1/3 circle in x-y plane around a unit z axis |
|
|
75 |
|
|
76 th = (0:-12:-120) * degrees * sign (th_eig); |
|
|
77 lth = length (th); |
|
3224
|
78 |
|
3452
|
79 cpts = [0, 0, 0.1*cos(th); |
|
|
80 0, 0, 0.1*sin(th); |
|
|
81 0, 1, 1*ones(1,lth)]; |
|
|
82 |
|
|
83 ## rotate the 1/3 circle around eigenaxis of inertial to body rotation |
|
|
84 ## qez = qe/qz = rotation to get from z axis to eigenaxis. |
|
|
85 ## This rotates the 1/3 circle from x-y plane to the plane normal to |
|
|
86 ## eigenaxis |
|
3224
|
87 |
|
3452
|
88 qez = qmult (qbf, qinv (quaternion (0, 0, 1, 0))); |
|
|
89 eig_xm = qtransvmat (qez); |
|
|
90 cpts = cm*eig_xm * cpts; |
|
|
91 |
|
|
92 ## transform inertial and body quaternions to view coordinates (rotate |
|
|
93 ## by azimuth, elevation) |
|
|
94 |
|
|
95 qfm = qtransvmat (qf); |
|
|
96 qbm = qtransvmat (qf); |
|
3224
|
97 |
|
3452
|
98 qf = qmult (qv, qf); |
|
|
99 qb = qmult (qv, qb); |
|
|
100 |
|
|
101 ## get coordinate axes in inertial and reference frame |
|
|
102 |
|
|
103 jnk = qtransvmat (qf); |
|
|
104 ifv = jnk(:,1); |
|
|
105 jfv = jnk(:,2); |
|
|
106 kfv = jnk(:,3); |
|
|
107 |
|
|
108 jnk = qtransvmat (qb); |
|
|
109 ibv = jnk(:,1); |
|
|
110 jbv = jnk(:,2); |
|
|
111 kbv = jnk(:,3); |
|
3224
|
112 |
|
5215
|
113 __gnuplot_set__ size square |
|
3452
|
114 axis ([-2, 2, -2, 2]); |
|
|
115 |
|
|
116 [vv, theta] = quaternion (qb); |
|
|
117 |
|
|
118 xlabel (sprintf ("rotate about eigenaxis %5.2f deg", th_eig/degrees)); |
|
|
119 |
|
|
120 plot ([ibv(1), 0], [ibv(3), 0], "-@11;x (body);", |
|
|
121 [0, jbv(1)], [0, jbv(3)], "-@21;y (body);", |
|
|
122 [0, kbv(1)], [0, kbv(3)], "-@32;z (body);", |
|
|
123 [ifv(1), 0], [ifv(3), 0], "-@13;x (inertial);", |
|
|
124 [0, jfv(1)], [0, jfv(3)], "-@23;y (inertial);", |
|
|
125 [0, kfv(1)], [0, kfv(3)], "-@34;z (inertial);", |
|
|
126 cpts(1,:), cpts(3,:), ".-6 ;eigenaxis;", |
|
|
127 box2(1,:), box2(3,:), "-4;;", |
|
|
128 box1(1,:), box1(3,:), "-5;;"); |
|
|
129 |
|
3224
|
130 endfunction |
