-
Notifications
You must be signed in to change notification settings - Fork 0
/
update_coordinate.m
106 lines (94 loc) · 4.95 KB
/
update_coordinate.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
% function []=draw_coordinate_system(axis_length,Rotation_matrix,origin,colorstring,cs_name)
% this function draws a coordinate system in the requested orientation given by rotation matrix R
% and at a specific origin
% example: draw_coordinate_system(10,rotd([0,0,1],45),[10,10,10]','rgb')
% uses functions rotd() and cone() and matlab function cylinder()
% copyright Nabil Simaan, 2003
%---------------------------------------------------------------------------
function []=update_coordinate(axis_length,Rotation_matrix,origin,handle)
hold on;
R=Rotation_matrix;
x_axis_unit_vec=R(:,1); %the axis directions in WCS
y_axis_unit_vec=R(:,2);
z_axis_unit_vec=R(:,3);
x_axis=axis_length*R(:,1); %end point of x_axis in wcs
y_axis=axis_length*R(:,2);
z_axis=axis_length*R(:,3);
%drawing the axis lines
% h_c_1=line([origin(1);origin(1)+x_axis(1)],[origin(2);origin(2)+x_axis(2)],[origin(3);origin(3)+x_axis(3)],'color',colorstring(1),'linewidth',2);
% h_c_2=line([origin(1);origin(1)+y_axis(1)],[origin(2);origin(2)+y_axis(2)],[origin(3);origin(3)+y_axis(3)],'color',colorstring(2),'linewidth',2);
% h_c_3=line([origin(1);origin(1)+z_axis(1)],[origin(2);origin(2)+z_axis(2)],[origin(3);origin(3)+z_axis(3)],'color',colorstring(3),'linewidth',2);
set(handle(1),'XData',[origin(1);origin(1)+x_axis(1)],'YData',[origin(2);origin(2)+x_axis(2)],'ZData',[origin(3);origin(3)+x_axis(3)]);
set(handle(2),'XData',[origin(1);origin(1)+y_axis(1)],'YData',[origin(2);origin(2)+y_axis(2)],'ZData',[origin(3);origin(3)+y_axis(3)]);
set(handle(3),'XData',[origin(1);origin(1)+z_axis(1)],'YData',[origin(2);origin(2)+z_axis(2)],'ZData',[origin(3);origin(3)+z_axis(3)]);
% % setting cone variables
% cone_height=axis_length/5;
% cone_base_radius=axis_length/10;
% numpoints=20;
% %drawing the cone for x_axis%
% Rx=rotd(y_axis_unit_vec,90)*R; %since I am specifying all my rotations about axes specified in WCS I premultiply
% %same result can be obtained by Rx=R*rotd([0;1;0],90), where I am rotating about the y axis given in the new system
% x_cone_origin=[origin(1)+x_axis(1); origin(2)+x_axis(2); origin(3)+x_axis(3)];
% [X,Y,Z]=cone(cone_height,cone_base_radius,numpoints,Rx,x_cone_origin);
% h_c_4=surf(X,Y,Z);
% set(h_c_4,'facecolor',colorstring(1));
%
% %drawing the cone for y_axis%
% Ry=rotd(x_axis_unit_vec,-90)*R;
% y_cone_origin=[origin(1)+y_axis(1); origin(2)+y_axis(2); origin(3)+y_axis(3)];
% [X,Y,Z]=cone(cone_height,cone_base_radius,numpoints,Ry,y_cone_origin);
% h_c_5=surf(X,Y,Z);
% set(h_c_5,'facecolor',colorstring(2));
%
% %drawing the cone for z_axis%
% Rz=R;
% z_cone_origin=[origin(1)+z_axis(1); origin(2)+z_axis(2); origin(3)+z_axis(3)];
% [X,Y,Z]=cone(cone_height,cone_base_radius,numpoints,R,z_cone_origin);
% h_c_6=surf(X,Y,Z);
% set(h_c_6,'facecolor',colorstring(3));
if nargin==5, %add text labels
x_label_pos= x_cone_origin + 1.8*cone_height*x_axis/axis_length; %using unit vector
y_label_pos= y_cone_origin + 1.8*cone_height*y_axis/axis_length;
z_label_pos= z_cone_origin + 1.8*cone_height*z_axis/axis_length;
text(x_label_pos(1),x_label_pos(2),x_label_pos(3),['X_',cs_name],'FontWeight','bold','color','k');
text(y_label_pos(1),y_label_pos(2),y_label_pos(3),['Y_',cs_name],'FontWeight','bold','color','k');
text(z_label_pos(1),z_label_pos(2),z_label_pos(3),['Z_',cs_name],'FontWeight','bold','color','k');
end;
%--------------------------------------------------------------------------------------------
% function cone(cone_height,cone_base,numpoints,R,cone_origin) returns the [X,Y,Z]
% such that surf([X,Y,Z]) draws a cone in the specific orientation given by rotation matrix R
% and in origin specified by cone_origin
% written by nabil simaan Nov. 25, 2003.
%---------------------------------------------------------------------------------------------
% function [X,Y,Z]=cone(cone_height,cone_base_radius,numpoints,R,cone_origin)
%
% [X,Y,Z]=cylinder([cone_base_radius,0],numpoints); %calculating a cone 1 unit in height and centered at [0,0,0] with axis in Z direction%
% Z=Z.*cone_height;
%
% Cone_base=[];
% Cone_top=[];
% for i=1:1:numpoints+1,
% Cone_base(:,i)=[X(1,i);Y(1,i);Z(1,i)];
% Cone_top(:,i)=[X(2,i);Y(2,i);Z(2,i)];
%
% Cone_base(:,i)=R*Cone_base(:,i)+cone_origin; %rotating the cone base and translating
% Cone_top(:,i)=R*Cone_top(:,i)+cone_origin; %rotating the cone top points and translating
%
% X(1,i)=Cone_base(1,i);
% X(2,i)=Cone_top(1,i);
%
% Y(1,i)=Cone_base(2,i);
% Y(2,i)=Cone_top(2,i);
%
% Z(1,i)=Cone_base(3,i);
% Z(2,i)=Cone_top(3,i);
% axis equal;
% end;
%------- function r = rotd(n,phi); -----------------
% computes the rotation matrix for rotation about axis n phi degrees
%-----------------------------------------------------------
function r = rotd(n,phi)
phi=phi*pi/180;
n = n ./ norm(n);
s = phi*[0 -n(3) n(2);n(3) 0 -n(1);-n(2) n(1) 0];
r = expm(s);