Mercurial > hg > octave-nkf

comparison scripts/quaternion/qcoordinate_plot.m @ 3452:a892190f4977

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
[project @ 2000-01-18 06:00:57 by jwe]
author jwe
date Tue, 18 Jan 2000 06:01:04 +0000
parents 41602f25d19f
children 32c569794216
comparison
equal deleted inserted replaced
3451:a6cc6bc220b3 3452:a892190f4977
1 function qcoordinate_plot(qf,qb,qv) 1 ## Copyright (C) 1998 Auburn University. All rights reserved.
2 # function qcoordinate_plot(qf,qb,qv) 2 ##
3 # plot in the current figure a set of coordinate axes as viewed from 3 ## This file is part of Octave.
4 # the orientation specified by quaternion qv. Inertial axes are 4 ##
5 # also plotted 5 ## Octave is free software; you can redistribute it and/or modify it
6 # qf: quaternion from reference (x,y,z) to inertial 6 ## under the terms of the GNU General Public License as published by
7 # qb: quaternion from reference to body 7 ## the Free Software Foundation; either version 2, or (at your option)
8 # qv: quaternion from reference to view angle 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
17 ## Software Foundation, 59 Temple Place - Suite 330, Boston, MA
18 ## 02111-1307, USA.
9 19
10 degrees = pi/180; 20 ## -*- texinfo -*-
11 d180 = 180*degrees; 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
12 35
13 # construct coordinate transformation to view frame 36 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu>
14 cm = qtransvmat(qv); 37 ## Adapted-By: jwe
15 p1 = [-1,-1,1]; p2 = [-1,-1,-1]; p3 = [1,-1,-1]; p4 = [ 1,-1, 1];
16 p5 = [-1, 1,1]; p6 = [ 1, 1, 1]; p7 = [1, 1,-1]; p8 = [-1, 1,-1];
17 # outline positive quadrant
18 box1 = cm*[p4; p6; p5; p6; p7]';
19 # outline rest of the box
20 box2 =cm*[p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]';
21 38
22 # compute inertial to body rotation eigenaxis 39 function qcoordinate_plot (qf, qb, qv)
23 # qb = qbf*qf => qbf = qb/qf
24 #
25 # need to use inverse quaternion to rotate axes
26 qbf = qinv(qmult(qb,qinv(qf)));
27 40
28 [eaxv,th_eig] = quaternion(qbf); 41 degrees = pi / 180;
42 d180 = 180 * degrees;
29 43
30 # draw 1/3 circle in x-y plane around a unit z axis 44 ## construct coordinate transformation to view frame
31 th = (0:-12:-120)*degrees*sign(th_eig); lth = length(th);
32 cpts = [0, 0, 0.1*cos(th) ; 0, 0, 0.1*sin(th); 0, 1, 1*ones(1,lth)];
33 45
34 # rotate the 1/3 circle around eigenaxis of inertial to body rotation 46 cm = qtransvmat(qv);
35 # qez = qe/qz = rotation to get from z axis to eigenaxis.
36 # This rotates the 1/3 circle from x-y plane to the plane normal to
37 # eigenaxis
38 qez = qmult(qbf,qinv(quaternion(0,0,1,0)));
39 eig_xm = qtransvmat(qez);
40 cpts = cm*eig_xm*cpts;
41 47
42 # transform inertial and body quaternions to view coordinates (rotate 48 p1 = [-1, -1, 1];
43 # by azimuth, elevation) 49 p2 = [-1, -1, -1];
44 qfm = qtransvmat(qf); qbm = qtransvmat(qf); 50 p3 = [ 1, -1, -1];
45 qf = qmult(qv,qf); qb = qmult(qv,qb); 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];
46 56
47 # get coordinate axes in inertial and reference frame 57 ## outline positive quadrant
48 jnk = qtransvmat(qf); ifv = jnk(:,1); jfv = jnk(:,2); kfv = jnk(:,3);
49 jnk = qtransvmat(qb); ibv = jnk(:,1); jbv = jnk(:,2); kbv = jnk(:,3);
50 58
51 gset size square 59 box1 = cm * [p4; p6; p5; p6; p7]';
52 axis([-2,2,-2,2]); 60
53 [vv,theta] = quaternion(qb); 61 ## outline rest of the box
54 xlabel(sprintf("rotate about eigenaxis %5.2f deg",th_eig/degrees)); 62
55 plot( [ibv(1),0],[ibv(3),0],"-@11;x (body);", ... 63 box2 = cm * [p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]';
56 [0,jbv(1)],[0,jbv(3)],"-@21;y (body);", ... 64
57 [0,kbv(1)],[0,kbv(3)],"-@32;z (body);", ... 65 ## compute inertial to body rotation eigenaxis
58 [ifv(1),0],[ifv(3),0],"-@13;x (inertial);", ... 66 ## qb = qbf*qf => qbf = qb/qf
59 [0,jfv(1)],[0,jfv(3)],"-@23;y (inertial);", ... 67 ##
60 [0,kfv(1)],[0,kfv(3)],"-@34;z (inertial);", ... 68 ## need to use inverse quaternion to rotate axes
61 cpts(1,:), cpts(3,:),".-6 ;eigenaxis;", ... 69
62 box2(1,:),box2(3,:),"-4;;", ... 70 qbf = qinv (qmult (qb, qinv (qf)));
63 box1(1,:),box1(3,:),"-5;;"); 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);
78
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
87
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);
97
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);
112
113 gset size square
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
64 endfunction 130 endfunction