|
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 |
|
7016
|
7 ## the Free Software Foundation; either version 3 of the License, or (at |
|
|
8 ## your option) any later version. |
|
3452
|
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 |
|
7016
|
16 ## along with Octave; see the file COPYING. If not, see |
|
|
17 ## <http://www.gnu.org/licenses/>. |
|
3224
|
18 |
|
3452
|
19 ## -*- texinfo -*- |
|
|
20 ## @deftypefn {Function File} {} qcoordinate_plot (@var{qf}, @var{qb}, @var{qv}) |
|
|
21 ## Plot in the current figure a set of coordinate axes as viewed from |
|
|
22 ## the orientation specified by quaternion @var{qv}. Inertial axes are |
|
|
23 ## also plotted: |
|
|
24 ## |
|
|
25 ## @table @var |
|
|
26 ## @item qf |
|
|
27 ## Quaternion from reference (x,y,z) to inertial. |
|
|
28 ## @item qb |
|
|
29 ## Quaternion from reference to body. |
|
|
30 ## @item qv |
|
|
31 ## Quaternion from reference to view angle. |
|
|
32 ## @end table |
|
|
33 ## @end deftypefn |
|
|
34 |
|
|
35 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
36 ## Adapted-By: jwe |
|
|
37 |
|
|
38 function qcoordinate_plot (qf, qb, qv) |
|
3224
|
39 |
|
3452
|
40 degrees = pi / 180; |
|
|
41 d180 = 180 * degrees; |
|
|
42 |
|
|
43 ## construct coordinate transformation to view frame |
|
|
44 |
|
|
45 cm = qtransvmat(qv); |
|
3224
|
46 |
|
3452
|
47 p1 = [-1, -1, 1]; |
|
|
48 p2 = [-1, -1, -1]; |
|
|
49 p3 = [ 1, -1, -1]; |
|
|
50 p4 = [ 1, -1, 1]; |
|
|
51 p5 = [-1, 1, 1]; |
|
|
52 p6 = [ 1, 1, 1]; |
|
|
53 p7 = [ 1, 1, -1]; |
|
|
54 p8 = [-1, 1, -1]; |
|
|
55 |
|
|
56 ## outline positive quadrant |
|
|
57 |
|
|
58 box1 = cm * [p4; p6; p5; p6; p7]'; |
|
|
59 |
|
|
60 ## outline rest of the box |
|
|
61 |
|
|
62 box2 = cm * [p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]'; |
|
3224
|
63 |
|
3452
|
64 ## compute inertial to body rotation eigenaxis |
|
|
65 ## qb = qbf*qf => qbf = qb/qf |
|
|
66 ## |
|
|
67 ## need to use inverse quaternion to rotate axes |
|
|
68 |
|
|
69 qbf = qinv (qmult (qb, qinv (qf))); |
|
|
70 |
|
|
71 [eaxv, th_eig] = quaternion (qbf); |
|
|
72 |
|
|
73 ## draw 1/3 circle in x-y plane around a unit z axis |
|
|
74 |
|
|
75 th = (0:-12:-120) * degrees * sign (th_eig); |
|
|
76 lth = length (th); |
|
3224
|
77 |
|
3452
|
78 cpts = [0, 0, 0.1*cos(th); |
|
|
79 0, 0, 0.1*sin(th); |
|
|
80 0, 1, 1*ones(1,lth)]; |
|
|
81 |
|
|
82 ## rotate the 1/3 circle around eigenaxis of inertial to body rotation |
|
|
83 ## qez = qe/qz = rotation to get from z axis to eigenaxis. |
|
|
84 ## This rotates the 1/3 circle from x-y plane to the plane normal to |
|
|
85 ## eigenaxis |
|
3224
|
86 |
|
3452
|
87 qez = qmult (qbf, qinv (quaternion (0, 0, 1, 0))); |
|
|
88 eig_xm = qtransvmat (qez); |
|
|
89 cpts = cm*eig_xm * cpts; |
|
|
90 |
|
|
91 ## transform inertial and body quaternions to view coordinates (rotate |
|
|
92 ## by azimuth, elevation) |
|
|
93 |
|
|
94 qfm = qtransvmat (qf); |
|
|
95 qbm = qtransvmat (qf); |
|
3224
|
96 |
|
3452
|
97 qf = qmult (qv, qf); |
|
|
98 qb = qmult (qv, qb); |
|
|
99 |
|
|
100 ## get coordinate axes in inertial and reference frame |
|
|
101 |
|
|
102 jnk = qtransvmat (qf); |
|
|
103 ifv = jnk(:,1); |
|
|
104 jfv = jnk(:,2); |
|
|
105 kfv = jnk(:,3); |
|
|
106 |
|
|
107 jnk = qtransvmat (qb); |
|
|
108 ibv = jnk(:,1); |
|
|
109 jbv = jnk(:,2); |
|
|
110 kbv = jnk(:,3); |
|
3224
|
111 |
|
6447
|
112 axis ([-2, 2, -2, 2], "square"); |
|
3452
|
113 |
|
|
114 [vv, theta] = quaternion (qb); |
|
|
115 |
|
|
116 xlabel (sprintf ("rotate about eigenaxis %5.2f deg", th_eig/degrees)); |
|
|
117 |
|
|
118 plot ([ibv(1), 0], [ibv(3), 0], "-@11;x (body);", |
|
|
119 [0, jbv(1)], [0, jbv(3)], "-@21;y (body);", |
|
|
120 [0, kbv(1)], [0, kbv(3)], "-@32;z (body);", |
|
|
121 [ifv(1), 0], [ifv(3), 0], "-@13;x (inertial);", |
|
|
122 [0, jfv(1)], [0, jfv(3)], "-@23;y (inertial);", |
|
|
123 [0, kfv(1)], [0, kfv(3)], "-@34;z (inertial);", |
|
|
124 cpts(1,:), cpts(3,:), ".-6 ;eigenaxis;", |
|
|
125 box2(1,:), box2(3,:), "-4;;", |
|
|
126 box1(1,:), box1(3,:), "-5;;"); |
|
|
127 |
|
3224
|
128 endfunction |
