clear all 
 close all

data = csvread('Results_best_hip_lean_bruno/recorded_phua_com.csv');

time = data(:,1);
time = time/100000;

COM_X=data(:,2);
COM_Y=data(:,3);
COM_Z=data(:,4);



% figure('position', [80 80 600 600], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 8;


% plot3(COM_X, COM_Y, COM_Z, '*b');
% hold on
% plot(time, left_joint1, 'r', 'LineWidth', 0.5);
%  plot(COM_X, COM_Z, '*b');
% 
% set(gca, 'XTick', -0.5:0.05:0.5, 'YTick', -0.5:0.01:0.5, 'FontName', AxisFontName, 'FontSize', AxisFontSize)
% 
% legend('right joint', 'left joint') 
% title('Hip roll', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
%  xlabel('X Component', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
%  ylabel('Z Component', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
%  zlabel('Z Component', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
% grid on
% axis ([-0.15 0.15 0.32 0.42])
% 
% view(2)
% view([ 0 0])
% 
% export_fig paulo_best_hip_xz1 -pdf -transparent


v1=[0.5, 0, 0.39];
v2=[-0.5, 0, 0.39];

a=v1-v2;
dis=[];
max=0;

for i=1:size(COM_X)
   point=[ COM_X(i) COM_Y(i) COM_Z(i)];
      
    b=point-v2;
    
    d=norm(cross(a,b))/norm(a);
    
    dis=[dis d];
    
    if d>max 
        max=d;
    end
    
end



mean1=mean(dis);
median1=median(dis);
