Mercurial > hg > octave-nkf

annotate scripts/quaternion/qcoordinate_plot.m @ 3224:1e7532b9173b

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
[project @ 1998-12-04 22:08:23 by jwe]
author jwe
date Fri, 04 Dec 1998 22:08:23 +0000
parents
children 41602f25d19f
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
3224
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
1 function qcoordinate_plot(qf,qb,qv)
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
2 # function qcoordinate_plot(qf,qb,qv)
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
3 # plot in the current figure a set of coordinate axes as viewed from
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
4 # the orientation specified by quaternion qv. Inertial axes are
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
5 # also plotted
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
6 # qf: quaternion from reference (x,y,z) to inertial
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
7 # qb: quaternion from reference to body
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
8 # qv: quaternion from reference to view angle
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
9
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
10 degrees = pi/180;
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
11 d180 = 180*degrees;
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
12
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
13 # construct coordinate transformation to view frame
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
14 cm = qtransvmat(qv);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
15 p1 = [-1,-1,1]; p2 = [-1,-1,-1]; p3 = [1,-1,-1]; p4 = [ 1,-1, 1];
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
16 p5 = [-1, 1,1]; p6 = [ 1, 1, 1]; p7 = [1, 1,-1]; p8 = [-1, 1,-1];
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
17 # outline positive quadrant
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
18 box1 = cm*[p4; p6; p5; p6; p7]';
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
19 # outline rest of the box
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
20 box2 =cm*[p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]';
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
21
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
22 # compute inertial to body rotation eigenaxis
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
23 # qb = qbf*qf => qbf = qb/qf
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
24 #
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
25 # need to use inverse quaternion to rotate axes
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
26 qbf = qinv(qmult(qb,qinv(qf)));
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
27
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
28 [eaxv,th_eig] = quaternion(qbf);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
29
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
30 # draw 1/3 circle in x-y plane around a unit z axis
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
31 th = (0:-12:-120)*degrees*sign(th_eig); lth = length(th);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
32 cpts = [0 0 0.1*cos(th) ; 0 0 0.1*sin(th); 0 1 1*ones(1,lth)];
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
33
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
34 # rotate the 1/3 circle around eigenaxis of inertial to body rotation
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
35 # qez = qe/qz = rotation to get from z axis to eigenaxis.
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
36 # This rotates the 1/3 circle from x-y plane to the plane normal to
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
37 # eigenaxis
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
38 qez = qmult(qbf,qinv(quaternion(0,0,1,0)));
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
39 eig_xm = qtransvmat(qez);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
40 cpts = cm*eig_xm*cpts;
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
41
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
42 # transform inertial and body quaternions to view coordinates (rotate
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
43 # by azimuth, elevation)
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
44 qfm = qtransvmat(qf); qbm = qtransvmat(qf);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
45 qf = qmult(qv,qf); qb = qmult(qv,qb);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
46
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
47 # get coordinate axes in inertial and reference frame
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
48 jnk = qtransvmat(qf); ifv = jnk(:,1); jfv = jnk(:,2); kfv = jnk(:,3);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
49 jnk = qtransvmat(qb); ibv = jnk(:,1); jbv = jnk(:,2); kbv = jnk(:,3);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
50
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
51 gset size square
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
52 axis([-2,2,-2,2]);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
53 [vv,theta] = quaternion(qb);
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
54 xlabel(sprintf("rotate about eigenaxis %5.2f deg",th_eig/degrees));
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
55 plot( [ibv(1),0],[ibv(3),0],"-@11;x (body);", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
56 [0,jbv(1)],[0,jbv(3)],"-@21;y (body);", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
57 [0,kbv(1)],[0,kbv(3)],"-@32;z (body);", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
58 [ifv(1),0],[ifv(3),0],"-@13;x (inertial);", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
59 [0,jfv(1)],[0,jfv(3)],"-@23;y (inertial);", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
60 [0,kfv(1)],[0,kfv(3)],"-@34;z (inertial);", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
61 cpts(1,:), cpts(3,:),".-6 ;eigenaxis;", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
62 box2(1,:),box2(3,:),"-4;;", ...
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
63 box1(1,:),box1(3,:),"-5;;");
1e7532b9173b [project @ 1998-12-04 22:08:23 by jwe]
jwe
parents:
diff changeset
64 endfunction