view scripts/quaternion/qcoordinate_plot.m @ 3295:35a6d027772c

[project @ 1999-10-19 10:13:02 by jwe]
author jwe
date Tue, 19 Oct 1999 10:13:06 +0000
parents 41602f25d19f
children a892190f4977
line wrap: on
line source

function qcoordinate_plot(qf,qb,qv)
# function qcoordinate_plot(qf,qb,qv)
# plot in the current figure a set of coordinate axes as viewed from 
# the orientation specified by quaternion qv.  Inertial axes are
# also plotted
# qf: quaternion from reference (x,y,z) to inertial
# qb: quaternion from reference to body 
# qv: quaternion from reference to view angle 

degrees = pi/180;
d180 = 180*degrees;

# construct coordinate transformation to view frame
cm = qtransvmat(qv);
p1 = [-1,-1,1]; p2 = [-1,-1,-1]; p3 = [1,-1,-1]; p4 = [ 1,-1, 1];
p5 = [-1, 1,1]; p6 = [ 1, 1, 1]; p7 = [1, 1,-1]; p8 = [-1, 1,-1];
# outline positive quadrant
box1 = cm*[p4; p6; p5; p6; p7]';
# outline rest of the box
box2 =cm*[p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]';

# compute inertial to body rotation eigenaxis
# qb = qbf*qf => qbf = qb/qf
#
# need to use inverse quaternion to rotate axes
qbf = qinv(qmult(qb,qinv(qf)));

[eaxv,th_eig] = quaternion(qbf);

# draw 1/3 circle in x-y plane around a unit z axis
th = (0:-12:-120)*degrees*sign(th_eig);    lth = length(th);
cpts = [0, 0, 0.1*cos(th) ; 0, 0, 0.1*sin(th); 0, 1, 1*ones(1,lth)];

# rotate the 1/3 circle around eigenaxis of inertial to body rotation
# qez = qe/qz = rotation to get from z axis to eigenaxis.
# This rotates the 1/3 circle from x-y plane to the plane normal to
# eigenaxis
qez = qmult(qbf,qinv(quaternion(0,0,1,0)));
eig_xm = qtransvmat(qez);
cpts = cm*eig_xm*cpts;

# transform inertial and body quaternions to view coordinates (rotate
# by azimuth, elevation)
qfm = qtransvmat(qf);                 qbm = qtransvmat(qf);
qf = qmult(qv,qf);                 qb = qmult(qv,qb);

# get coordinate axes in inertial and reference frame
jnk = qtransvmat(qf); ifv = jnk(:,1); jfv = jnk(:,2); kfv = jnk(:,3);
jnk = qtransvmat(qb); ibv = jnk(:,1); jbv = jnk(:,2); kbv = jnk(:,3);

gset size square
axis([-2,2,-2,2]);
[vv,theta] = quaternion(qb);
xlabel(sprintf("rotate about eigenaxis %5.2f deg",th_eig/degrees));
plot( [ibv(1),0],[ibv(3),0],"-@11;x (body);", ...
	[0,jbv(1)],[0,jbv(3)],"-@21;y (body);", ...
	[0,kbv(1)],[0,kbv(3)],"-@32;z (body);", ...
	[ifv(1),0],[ifv(3),0],"-@13;x (inertial);", ...
	[0,jfv(1)],[0,jfv(3)],"-@23;y (inertial);", ...
	[0,kfv(1)],[0,kfv(3)],"-@34;z (inertial);", ...
	cpts(1,:), cpts(3,:),".-6 ;eigenaxis;", ...
	box2(1,:),box2(3,:),"-4;;", ...
	box1(1,:),box1(3,:),"-5;;");
endfunction