close all
clc
fw = 11.88;
fl = 17.88;

disp(['Turtle ID ',sprintf('%d',out(1).TurtleID)]);

figure(1)
hold on
rectangle('Position',[-7.5 -10 15 20], 'FaceColor','g','EdgeColor','k')
rectangle('Position',[-1 -1 2 2], 'FaceColor','g','EdgeColor','w','Curvature',[1,1],'LineWidth',3)
plot3([1 1 -1 -1 1]*fw/2, [1 -1 -1 1 1]*fl/2, [0 0 0 0 0], 'w','LineWidth',3)
plot3([-1 1]*fw/2, [0 0], [0 0], 'w','LineWidth',3)

for i=1:length(out)
    %    Position_Turtle_1(i,:) = [out(i).Turtle_Position(1) out(i).Turtle_Position(2) out(i).Turtle_Position(3)];
    Position_Turtle_2(i,:) = [out(i).Turtle_Position(4) out(i).Turtle_Position(5) out(i).Turtle_Position(6)];
    %         Position_Turtle_3(i,:) = [out(i).Turtle_Position(7) out(i).Turtle_Position(8) out(i).Turtle_Position(9)];
    %         Position_Turtle_4(i,:) = [out(i).Turtle_Position(10) out(i).Turtle_Position(11) out(i).Turtle_Position(12)];
    %         Position_Turtle_5(i,:) = [out(i).Turtle_Position(13) out(i).Turtle_Position(14) out(i).Turtle_Position(15)];
    %         Position_Turtle_6(i,:) = [out(i).Turtle_Position(16) out(i).Turtle_Position(17) out(i).Turtle_Position(18)];
    z(i,:) = 0;
    
    if ((Position_Turtle_2(i,1) ~= -100) | (Position_Turtle_2(i,2) ~= -100))
        plot3(Position_Turtle_2(i,1),Position_Turtle_2(i,2),z(i,1),'r.')
    end
end