|
7017
|
1 ## Copyright (C) 1998, 1999, 2000, 2005, 2007 |
|
|
2 ## Auburn University. All rights reserved. |
|
3452
|
3 ## |
|
|
4 ## This file is part of Octave. |
|
|
5 ## |
|
|
6 ## Octave is free software; you can redistribute it and/or modify it |
|
|
7 ## under the terms of the GNU General Public License as published by |
|
7016
|
8 ## the Free Software Foundation; either version 3 of the License, or (at |
|
|
9 ## your option) any later version. |
|
3452
|
10 ## |
|
|
11 ## Octave is distributed in the hope that it will be useful, but |
|
|
12 ## WITHOUT ANY WARRANTY; without even the implied warranty of |
|
|
13 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
|
14 ## General Public License for more details. |
|
|
15 ## |
|
|
16 ## You should have received a copy of the GNU General Public License |
|
7016
|
17 ## along with Octave; see the file COPYING. If not, see |
|
|
18 ## <http://www.gnu.org/licenses/>. |
|
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 |
|
7125
|
41 if (nargin != 3 ) |
|
|
42 print_usage (); |
|
|
43 endif |
|
|
44 |
|
3452
|
45 degrees = pi / 180; |
|
|
46 d180 = 180 * degrees; |
|
|
47 |
|
|
48 ## construct coordinate transformation to view frame |
|
|
49 |
|
|
50 cm = qtransvmat(qv); |
|
3224
|
51 |
|
3452
|
52 p1 = [-1, -1, 1]; |
|
|
53 p2 = [-1, -1, -1]; |
|
|
54 p3 = [ 1, -1, -1]; |
|
|
55 p4 = [ 1, -1, 1]; |
|
|
56 p5 = [-1, 1, 1]; |
|
|
57 p6 = [ 1, 1, 1]; |
|
|
58 p7 = [ 1, 1, -1]; |
|
|
59 p8 = [-1, 1, -1]; |
|
|
60 |
|
|
61 ## outline positive quadrant |
|
|
62 |
|
|
63 box1 = cm * [p4; p6; p5; p6; p7]'; |
|
|
64 |
|
|
65 ## outline rest of the box |
|
|
66 |
|
|
67 box2 = cm * [p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]'; |
|
3224
|
68 |
|
3452
|
69 ## compute inertial to body rotation eigenaxis |
|
|
70 ## qb = qbf*qf => qbf = qb/qf |
|
|
71 ## |
|
|
72 ## need to use inverse quaternion to rotate axes |
|
|
73 |
|
|
74 qbf = qinv (qmult (qb, qinv (qf))); |
|
|
75 |
|
|
76 [eaxv, th_eig] = quaternion (qbf); |
|
|
77 |
|
|
78 ## draw 1/3 circle in x-y plane around a unit z axis |
|
|
79 |
|
|
80 th = (0:-12:-120) * degrees * sign (th_eig); |
|
|
81 lth = length (th); |
|
3224
|
82 |
|
3452
|
83 cpts = [0, 0, 0.1*cos(th); |
|
|
84 0, 0, 0.1*sin(th); |
|
|
85 0, 1, 1*ones(1,lth)]; |
|
|
86 |
|
|
87 ## rotate the 1/3 circle around eigenaxis of inertial to body rotation |
|
|
88 ## qez = qe/qz = rotation to get from z axis to eigenaxis. |
|
|
89 ## This rotates the 1/3 circle from x-y plane to the plane normal to |
|
|
90 ## eigenaxis |
|
3224
|
91 |
|
3452
|
92 qez = qmult (qbf, qinv (quaternion (0, 0, 1, 0))); |
|
|
93 eig_xm = qtransvmat (qez); |
|
|
94 cpts = cm*eig_xm * cpts; |
|
|
95 |
|
|
96 ## transform inertial and body quaternions to view coordinates (rotate |
|
|
97 ## by azimuth, elevation) |
|
|
98 |
|
|
99 qfm = qtransvmat (qf); |
|
|
100 qbm = qtransvmat (qf); |
|
3224
|
101 |
|
3452
|
102 qf = qmult (qv, qf); |
|
|
103 qb = qmult (qv, qb); |
|
|
104 |
|
|
105 ## get coordinate axes in inertial and reference frame |
|
|
106 |
|
|
107 jnk = qtransvmat (qf); |
|
|
108 ifv = jnk(:,1); |
|
|
109 jfv = jnk(:,2); |
|
|
110 kfv = jnk(:,3); |
|
|
111 |
|
|
112 jnk = qtransvmat (qb); |
|
|
113 ibv = jnk(:,1); |
|
|
114 jbv = jnk(:,2); |
|
|
115 kbv = jnk(:,3); |
|
3224
|
116 |
|
6447
|
117 axis ([-2, 2, -2, 2], "square"); |
|
3452
|
118 |
|
|
119 [vv, theta] = quaternion (qb); |
|
|
120 |
|
|
121 xlabel (sprintf ("rotate about eigenaxis %5.2f deg", th_eig/degrees)); |
|
|
122 |
|
|
123 plot ([ibv(1), 0], [ibv(3), 0], "-@11;x (body);", |
|
|
124 [0, jbv(1)], [0, jbv(3)], "-@21;y (body);", |
|
|
125 [0, kbv(1)], [0, kbv(3)], "-@32;z (body);", |
|
|
126 [ifv(1), 0], [ifv(3), 0], "-@13;x (inertial);", |
|
|
127 [0, jfv(1)], [0, jfv(3)], "-@23;y (inertial);", |
|
|
128 [0, kfv(1)], [0, kfv(3)], "-@34;z (inertial);", |
|
|
129 cpts(1,:), cpts(3,:), ".-6 ;eigenaxis;", |
|
|
130 box2(1,:), box2(3,:), "-4;;", |
|
|
131 box1(1,:), box1(3,:), "-5;;"); |
|
|
132 |
|
3224
|
133 endfunction |
