-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy paththreeD_rigid_body.m
87 lines (67 loc) · 3.09 KB
/
threeD_rigid_body.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
% ----------------------------
% Let the XY plane be the horizontal plane
% ----------------------------
% Define the vertices of a rectangular plate with thickness
thickness = 0.2;
vertices = [-1 -1 -thickness/2; 1 -1 -thickness/2; 1 1 -thickness/2; -1 1 -thickness/2;
-1 -1 thickness/2; 1 -1 thickness/2; 1 1 thickness/2; -1 1 thickness/2];
% Define the faces of the plate
faces = [1 2 3 4; 5 6 7 8; 1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8];
% Rotation angle (in degrees)
theta = 180; % Complete rotation
% Number of frames for animation
numFrames = 100;
% Define the point and direction of the rotation axis
axisPoint = [0, -1.5, 0];
axisDirection = [0, 1, 0.9004]; % Arbitrary direction vector (Polar axis elevation 42 degs )
% Normalize the direction vector
axisDirection = axisDirection / norm(axisDirection);
% Rotation matrix around an arbitrary axis
R_axis = @(theta) [...
cosd(theta) + axisDirection(1)^2 * (1 - cosd(theta)), ...
axisDirection(1) * axisDirection(2) * (1 - cosd(theta)) - axisDirection(3) * sind(theta), ...
axisDirection(1) * axisDirection(3) * (1 - cosd(theta)) + axisDirection(2) * sind(theta); ...
axisDirection(2) * axisDirection(1) * (1 - cosd(theta)) + axisDirection(3) * sind(theta), ...
cosd(theta) + axisDirection(2)^2 * (1 - cosd(theta)), ...
axisDirection(2) * axisDirection(3) * (1 - cosd(theta)) - axisDirection(1) * sind(theta); ...
axisDirection(3) * axisDirection(1) * (1 - cosd(theta)) - axisDirection(2) * sind(theta), ...
axisDirection(3) * axisDirection(2) * (1 - cosd(theta)) + axisDirection(1) * sind(theta), ...
cosd(theta) + axisDirection(3)^2 * (1 - cosd(theta))];
% Create a figure
figure;
axis equal;
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
view(3);
% Enable interactive rotation
rotate3d on;
% Plot the initial plate
patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'cyan');
% Plot the rotation axis
line([axisPoint(1) axisPoint(1) + axisDirection(1) * 2], ...
[axisPoint(2) axisPoint(2) + axisDirection(2) * 2], ...
[axisPoint(3) axisPoint(3) + axisDirection(3) * 2], 'Color', 'red', 'LineWidth', 2);
% Pause to visualize the initial state
pause(1);
% Animate the rotation
for i = 1:numFrames
% Compute the rotation for the current frame
currentTheta = 90 + theta * (i / numFrames);
currentR_axis = R_axis(currentTheta);
% Apply the rotation to the vertices with respect to the axis
rotatedVertices = (currentR_axis * (vertices' - axisPoint') + axisPoint')';
% Clear the previous plot
cla;
% Plot the rotated plate
patch('Vertices', rotatedVertices, 'Faces', faces, 'FaceColor', 'cyan');
% Plot the rotation axis
line([axisPoint(1) axisPoint(1) + axisDirection(1) * 2], ...
[axisPoint(2) axisPoint(2) + axisDirection(2) * 2], ...
[axisPoint(3) axisPoint(3) + axisDirection(3) * 2], 'Color', 'red', 'LineWidth', 2);
% Update the figure and allow interactive rotation
drawnow;
% Pause to create animation effect
pause(0.05);
end