close

%length of base of rotation, analogous to the hand
base = 15; %cm
%distance between phalanges
dbp = .5; %cm
%phalange length
phal = 2.4; %cm

%in cm
x = [0    0     0     0     base  base  base  base];
y = [0    1.75  0     1.75  0     1.75  0     1.75];
z = [0    0      .33   .33   .33   .33  0     0   ];

%base for movement
p(1) = patch([x(3) x(4) x(6) x(5)],[y(3) y(4) y(6) y(5)],[z(3) z(4) z(6) z(5)],'r');    %top face
p(2) = patch([x(1) x(3) x(4) x(2)],[y(1) y(3) y(4) y(2)],[z(1) z(3) z(4) z(2)],'r');    %proximal face
p(3) = patch([x(1) x(3) x(5) x(7)],[y(1) y(3) y(5) y(7)],[z(1) z(3) z(5) z(7)],'r');    %lateral face
p(4) = patch([x(2) x(4) x(6) x(8)],[y(2) y(4) y(6) y(8)],[z(2) z(4) z(6) z(8)],'r');    %medial face
p(5) = patch([x(5) x(7) x(8) x(6)],[y(5) y(7) y(8) y(6)],[z(5) z(7) z(8) z(6)],'r');    %distal face
p(6) = patch([x(1) x(2) x(8) x(7)],[y(1) y(2) y(8) y(7)],[z(1) z(2) z(8) z(7)],'r');    %bottom face

%y and z vectors are the same, but are repeated for fun
%xs is shifted in the positive x direction
xs = [0    0     0     0     phal  phal  phal  phal] + base + dbp;
y  = [0    1.75  0     1.75  0     1.75  0     1.75];
z  = [0    0      .33   .33   .33   .33  0     0   ];

%first joint
p2(1) = patch([xs(3) xs(4) xs(6) xs(5)],[y(3) y(4) y(6) y(5)],[z(3) z(4) z(6) z(5)],'r');    %top face
p2(2) = patch([xs(1) xs(2) xs(4) xs(3)],[y(1) y(2) y(4) y(3)],[z(1) z(2) z(4) z(3)],'r');    %proximal face
p2(3) = patch([xs(1) xs(3) xs(5) xs(7)],[y(1) y(3) y(5) y(7)],[z(1) z(3) z(5) z(7)],'r');    %lateral face
p2(4) = patch([xs(2) xs(4) xs(6) xs(8)],[y(2) y(4) y(6) y(8)],[z(2) z(4) z(6) z(8)],'r');    %medial face
p2(5) = patch([xs(5) xs(6) xs(8) xs(7)],[y(5) y(6) y(8) y(7)],[z(5) z(6) z(8) z(7)],'r');    %distal face
p2(6) = patch([xs(1) xs(2) xs(8) xs(7)],[y(1) y(2) y(8) y(7)],[z(1) z(2) z(8) z(7)],'r');    %bottom face

%y and z vectors are the same, but are repeated for fun
%xs is shifted in the positive x direction
xs2 = [0    0     0     0     phal  phal  phal  phal] + base + dbp + phal + dbp;
y  = [0    1.75  0     1.75  0     1.75  0     1.75];
z  = [0    0      .33   .33   .33   .33  0     0   ];

%second joint
p3(1) = patch([xs2(3) xs2(4) xs2(6) xs2(5)],[y(3) y(4) y(6) y(5)],[z(3) z(4) z(6) z(5)],'r');    %top face
p3(2) = patch([xs2(1) xs2(2) xs2(4) xs2(3)],[y(1) y(2) y(4) y(3)],[z(1) z(2) z(4) z(3)],'r');    %proximal face
p3(3) = patch([xs2(1) xs2(3) xs2(5) xs2(7)],[y(1) y(3) y(5) y(7)],[z(1) z(3) z(5) z(7)],'r');    %lateral face
p3(4) = patch([xs2(2) xs2(4) xs2(6) xs2(8)],[y(2) y(4) y(6) y(8)],[z(2) z(4) z(6) z(8)],'r');    %medial face
p3(5) = patch([xs2(5) xs2(6) xs2(8) xs2(7)],[y(5) y(6) y(8) y(7)],[z(5) z(6) z(8) z(7)],'r');    %distal face
p3(6) = patch([xs2(1) xs2(2) xs2(8) xs2(7)],[y(1) y(2) y(8) y(7)],[z(1) z(2) z(8) z(7)],'r');    %bottom face

%y and z vectors are the same, but are repeated for fun
%xs is shifted in the positive x direction
xs3 = [0    0     0     0     phal  phal  phal  phal] + base + dbp + phal + dbp + phal + dbp;
y  = [0    1.75  0     1.75  0     1.75  0     1.75];
z  = [0    0      .33   .33   .33   .33  0     0   ];

%third joint
p4(1) = patch([xs3(3) xs3(4) xs3(6) xs3(5)],[y(3) y(4) y(6) y(5)],[z(3) z(4) z(6) z(5)],'r');    %top face
p4(2) = patch([xs3(1) xs3(2) xs3(4) xs3(3)],[y(1) y(2) y(4) y(3)],[z(1) z(2) z(4) z(3)],'r');    %proximal face
p4(3) = patch([xs3(1) xs3(3) xs3(5) xs3(7)],[y(1) y(3) y(5) y(7)],[z(1) z(3) z(5) z(7)],'r');    %lateral face
p4(4) = patch([xs3(2) xs3(4) xs3(6) xs3(8)],[y(2) y(4) y(6) y(8)],[z(2) z(4) z(6) z(8)],'r');    %medial face
p4(5) = patch([xs3(5) xs3(6) xs3(8) xs3(7)],[y(5) y(6) y(8) y(7)],[z(5) z(6) z(8) z(7)],'r');    %distal face
p4(6) = patch([xs3(1) xs3(2) xs3(8) xs3(7)],[y(1) y(2) y(8) y(7)],[z(1) z(2) z(8) z(7)],'r');    %bottom face

axis('equal')
%axis([0 4 0 4 0 4])
view(3)

%saves xyz data from above in 3 vectors
hold on
savx = get(p(1),'XData');
savy = get(p(1),'YData');
savz = get(p(1),'ZData');
plot3(savx(1),(savy(1)+savy(2)/2),savz(1),'o')

pause();

for i=1:10,
    %rotates second phalange about point between both phalanges
    rotate(p2,[0 1 0],-1, [(x(5)+xs(1))/2 0 .33]);
    rotate(p3,[0 1 0],-1, [(x(5)+xs(1))/2 0 .33]);
    rotate(p4,[0 1 0],-1, [(x(5)+xs(1))/2 0 .33]);
    savx2 = get(p2(1),'XData');
    savy2 = get(p2(1),'YData');
    savz2 = get(p2(1),'ZData');
    %pauses every 10 degrees
%     if (mod(i,10)==0)
%         pause;
%     end
    %plots the second point - this second point simply keeps track of the 
    %optimal spot on the distal phalange for the nitinol attachment 
    plot3(savx2(1),(savy2(1)+savy2(2)/2),savz2(1),'o')
    %calculates the distance between the two points
    distance(i) = sqrt( (savx2(1)-savx(1))^2 + (savz2(1)-savz(1))^2 );
    pause(.05)
end


%creating coordinates for rotation based on second and third phalage
    %need to use p2(5) because that is the distal face
    savx2 = get(p2(5),'XData');
    savy2 = get(p2(5),'YData');
    savz2 = get(p2(5),'ZData'); 
    %need to use p3(2) because that is the proximal face
    savx3 = get(p3(2),'XData');
    savy3 = get(p3(2),'YData');
    savz3 = get(p3(2),'ZData');
    
    xcor = savx2(1)+0.5*(savx3(4)-savx2(1));
    zcor = savz2(1)+0.5*(savz3(4)-savz2(1));
    ycor = 0;
    
for i=1:90,
    %rotates third phalange about point between both phalanges
    
    rotate(p3,[0 1 0],-1, [xcor ycor zcor]);
    rotate(p4,[0 1 0],-1, [xcor ycor zcor]);
    
    savx3 = get(p3(1),'XData');
    savy3 = get(p3(1),'YData');
    savz3 = get(p3(1),'ZData');
    %pauses every 10 degrees
%     if (mod(i,10)==0)
%         pause;
%     end
    %plots the second point - this second point simply keeps track of the 
    %optimal spot on the distal phalange for the nitinol attachment 
    plot3(savx3(1),(savy3(1)+savy3(2)/2),savz3(1),'o')
    %calculates the distance between the two points
    distance2(i) = sqrt( (savx3(1)-savx(1))^2 + (savz3(1)-savz(1))^2 );
    pause(.05)
end

xlabel('xaxis')
ylabel('yaxis')
zlabel('zaxis')

hold on
