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

%% Example plot
figure;
  title('Initial Orientation')
  plot_shape(B_S0,0.5,'b');
  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