% Create shape
numOfSides = 4;
B_S0 = makeShape(numOfSides);

% Basic Rotation Matrices
RZ =@(a) [cos(a) -sin(a) 0;
          sin(a) cos(a) 0;
          0 0 1];
RY =@(b) [cos(b) 0 sin(b);
          0 1 0;
          -sin(b) 0 cos(b)];

RX =@(g) [1 0 0;
          0 cos(g) -sin(g);
          0 sin(g) cos(g)];

figure('position',[0 0 1710 420*2]);

  subplot(261); hold all;
  title('Initial Orientation');
  plot_shape(B_S0,0.5,'b');
  plot_global_axis();

  subplot(262); hold all;
  title('Global R/P/Y');
  R = RZ(pi/4)*RY(pi/4)*RX(pi/4);
  plot_shape(R*B_S0,0.5,'k');
  plot_global_axis();

  subplot(263); hold all;
  title('Euler Angles z-y-x');
  R = RZ(pi/4)*RY(pi/4)*RX(pi/4);
  plot_shape(R*B_S0,0.5,'k');
  plot_global_axis();

  subplot(264); hold all;
  title('Axis/Angle');
  phi = acos(1/2*(trace(R) - 1));
  usk = 1/(2*sin(phi))*(R - R');
  u = [-usk(2,3); usk(1,3); -usk(1,2)];
  R = cos(phi)*eye(3) + (1-cos(phi))*u*u' + usk*sin(phi);
  plot_shape(R*B_S0,0.5,'k');
  plot_global_axis();

  subplot(265); hold all;
  title('expm');
  R = expm(usk*phi);
  plot_shape(R*B_S0,0.5,'k');
  plot_global_axis();

  subplot(266); hold all;
  title('quaterion');
  q0 = cos(phi/2);
  qv = sin(phi/2)*u;
  q = quaternion(cos(phi/2),qv(1),qv(2),qv(3));
  qB_S0 = quaternion(zeros(1,size(B_S0,2)),B_S0(1,:),B_S0(2,:),B_S0(3,:));
  [A,B,C,D] = parts(q*qB_S0*conj(q));
  G_S1 = [B', C', D']';
  plot_shape(G_S1,0.5,'k');
  plot_global_axis();

  subplot(267); hold all;
  title('Initial Orientation')
  plot_shape(B_S0,0.5,'b');
  plot_global_axis();


  subplot(269); hold all;
  title('eul2rotm');
  R = eul2rotm([pi/4, pi/4, pi/4]);
  plot_shape(R*B_S0,0.5,'k');
  plot_global_axis();

  subplot(2,6,10); hold all;
  title('axang2rotm');
  R = axang2rotm([u', phi]);
  plot_shape(R*B_S0,0.5,'k');
  plot_global_axis();

  subplot(2,6,12); hold all;
  title('rotatepoint (quat)');
  plot_shape(rotatepoint(q,B_S0')',0.5,'k');
  plot_global_axis();

% -----------------------------------------------------------------------------
%
% -- Helper functions
%
% Do not edit.
%
% -----------------------------------------------------------------------------


% -- This function creates a shape and the positional vectors for the vertices
function B_r = makeShape(nsides)
  a = linspace(0, 2*pi, nsides+1);         % create angles
  x = sqrt(2)/2.*[1; 1]*cos(a + pi/4);
  y = sqrt(2)/2.*[1; 1]*sin(a + pi/4);
  z = [-0.5*ones(1,size(x,2)); 0.5*ones(1,size(x,2))];

  % Create body frame: orgin is centered and unit vectors
  Bo = [0; 0; 0];
  Bi = [1; 0; 0];
  Bj = [0; 1; 0];
  Bk = [0; 0; 1];
  B = [Bo, Bi, Bj, Bk];

  % Matrix of positional vectors (concatenate body frame  and vertices)
  % each column represents a position vector to 'B_r' to either a vertex or
  % base frame
  B_r =  [B, [x(:),y(:),z(:)]'];
end


% -- This function plots global unit vectors.
function plot_global_axis()
  hold all;
  ph = plot3([0 1], [0 0], [0 0]);
  ph.LineWidth = 3;
  ph.Color = [1 0 0 0.25];
  ph = plot3([0 0], [0 1], [0 0]);
  ph.LineWidth = 3;
  ph.Color = [0 1 1 0.25];
  ph = plot3([0 0], [0 0], [0 1]);
  ph.LineWidth = 3;
  ph.Color = [0 0 1 0.25];
end

% -- This function plots the shape and Body-frame.
%     Inputs: G_r - positional vectors in global frame
%             alpha - transparency setting (number between 0-1)
%             c - color, e.g., 'r' for red or 'g' for green
function plot_shape(G_r,alpha,c)
  hold all;

  [B,x,y,z] = r2frame_and_vertices(G_r);

  % Actual shape
  sh = surf(x,y,z);
  sh.FaceColor = c;
  sh.FaceAlpha = alpha;
  sh.LineWidth = 3;
  ph = patch(x', y', z',c);
  ph.FaceAlpha = alpha;

  % B-frame
  ph = plot3([B(1,1) B(1,2)], [B(2,1) B(2,2)], [B(3,1) B(3,2)]);
  ph.LineWidth = 4;
  ph.Color = 'r';
  ph = plot3([B(1,1) B(1,3)], [B(2,1) B(2,3)], [B(3,1) B(3,3)]);
  ph.LineWidth = 4;
  ph.Color = 'g';
  ph = plot3([B(1,1) B(1,4)], [B(2,1) B(2,4)], [B(3,1) B(3,4)]);
  ph.LineWidth = 4;
  ph.Color = 'b';
  xlim([-1,1]);
  ylim([-1,1]);
  zlim([-1,1]);

  view(3);
  axis square;
  grid off;
end

function [B,x,y,z] = r2frame_and_vertices(r)
  % find number of vertices
  [~,m] = size(r);
  n = (m - 4)/2;

  % Seperate body frame and shape vertices
  B = r(:,1:4);
  x = r(1,5:end);
  y = r(2,5:end);
  z = r(3,5:end);

  % Reshape shape vertices to match original format
  x = reshape(x,[2,n]);
  y = reshape(y,[2,n]);
  z = reshape(z,[2,n]);
end
