function RotateCamera

sqaure_point =   [0 0 0;
                  1 0 0;
                  1 1 0;
                  0 1 0;
                  0 0 1;
                  1 0 1;
                  1 1 1;
                  0 1 1];
square_edge = [1 2;
               2 3;
               3 4;
               4 1;
               5 6;
               6 7;
               7 8;
               8 5;
               1 5;
               2 6;
               3 7;
               4 8];

K = [200 0 100;
     0 200 100;
     0 0 1];

center_of_mass = mean(sqaure_point);
radius = 5;

theta = 0:0.02:2*pi;

h = figure(1);
clf;
h1 = figure(2);
clf;
for i = 1 : length(theta)
    camera_offset = [radius*cos(theta(i)); radius*sin(theta(i)); 0];
    camera_center = camera_offset + center_of_mass';
    
    rz = [-cos(theta(i)); -sin(theta(i)); 0];
    ry = [0 0 -1]';
    rx = [-sin(theta(i)); cos(theta(i)); 0];
    R = [rx'; ry'; rz'];
    C = camera_center;
    P = K * R * [ eye(3) -C];
    
    proj = [];
    for j = 1 : size(sqaure_point,1)
        u = P * [sqaure_point(j,:)';1];
        proj(j,:) = u'/u(3);
    end    
    
    figure(2);
    clf;
    % Plot camera trajectory
    plot3(radius*cos(theta)+center_of_mass(1), radius*sin(theta)+center_of_mass(2), zeros(size(theta))+center_of_mass(3), 'k:');
    R_w = eye(3);
    % Plot world coordinate
    DrawAxis(center_of_mass, R_w(1,:), R_w(2,:), R_w(3,:));
    
    % Plot 3D camera coordinate
    DrawAxis(C, R(1,:), R(2,:), R(3,:));
    
    for j = 1 : size(square_edge,1)
        hold on
        plot3([sqaure_point(square_edge(j,1),1) sqaure_point(square_edge(j,2),1)], [sqaure_point(square_edge(j,1),2) sqaure_point(square_edge(j,2),2)], [sqaure_point(square_edge(j,1),3) sqaure_point(square_edge(j,2),3)], '-', 'Color', [0.5 0 0.5]);
    end
    
    for j = 1 : size(square_edge,1)
        hold on
        plot3([sqaure_point(square_edge(j,1),1) sqaure_point(square_edge(j,2),1)]+1, [sqaure_point(square_edge(j,1),2) sqaure_point(square_edge(j,2),2)]+1, [sqaure_point(square_edge(j,1),3) sqaure_point(square_edge(j,2),3)]+1, '-', 'Color', [0 0.5 0.5]);
    end
    
    axis equal
    grid on
    
    
    figure(1);
    clf;
    for j = 1 : size(square_edge,1)
        hold on
        plot([proj(square_edge(j,1),1) proj(square_edge(j,2),1)], [proj(square_edge(j,1),2) proj(square_edge(j,2),2)], '-', 'Color', [0.5 0 0.5]);
    end
    
    proj = [];
    for j = 1 : size(sqaure_point,1)
        u = P * [sqaure_point(j,:)'+1;1];
        proj(j,:) = u'/u(3);
    end  
    for j = 1 : size(square_edge,1)
        hold on
        plot([proj(square_edge(j,1),1) proj(square_edge(j,2),1)], [proj(square_edge(j,1),2) proj(square_edge(j,2),2)], '-', 'Color', [0 0.5 0.5]);
    end

    axis([0 200 0 200]);
    axis off
    drawnow
%     print(h, sprintf('im%07d.png', i), '-dpng');
%     print(h1, sprintf('ca%07d.png', i), '-dpng');
    pause(0.1);
end

function DrawAxis(c, rx, ry, rz)

hold on
plot3([c(1) c(1)+rx(1)], [c(2) c(2)+rx(2)], [c(3) c(3)+rx(3)], 'r-', 'LineWidth', 2);
hold on
plot3([c(1) c(1)+ry(1)], [c(2) c(2)+ry(2)], [c(3) c(3)+ry(3)], 'g-', 'LineWidth', 2);
hold on
plot3([c(1) c(1)+rz(1)], [c(2) c(2)+rz(2)], [c(3) c(3)+rz(3)], 'b-', 'LineWidth', 2);
axis equal