Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_HHT/contact_model.m
35b4c71bda0dmaster

Last Change
Raw File

contact_model.m
View Options

function [Dir_contact,Rho_contact,F_contact]= contact_model(Rot_H2T,r_HH,CMDATA)
%{
Function for defining the contact forces at each joint configuration.
--------------------------------------------------------------------------
Syntax :
[Dir_contact,Rho_contact,F_contact]= contact_model(Rot_H2T,r_HH,CMDATA)
--------------------------------------------------------------------------
Function Description :
The function receives the current joint configuration (rotation matrix and
the humeral head translation vector) and defines the contact force at each
point on the humeral head surface based on whether the point is in contact
or not. We already defined the points in the humerus body-fixed coordinate
frame through initilize_contact_model().
--------------------------------------------------------------------------
%}
h = CMDATA.h; % receive h_i s in the frame {H}
a = CMDATA.a; % receive associtaed surface area of the h_i s
for i = 1:size(h,1)-1 % associated with longtitude (theta)
for j = 2:size(h,2)-1 % associated with latitude (phi)
h_inT{i,j} = r_HH + Rot_H2T*h{i,j}; % hi in the {T} frame
u(i,j) = norm(h_inT{i,j},2) - CMDATA.r_g; % the penetration of the point hi
% in the glenoid fossa
if gt(u(i,j),0) & lt(u(i,j),0.98*CMDATA.b) % contact condition of the points
F_contact(i,j) = -a(i,j)*CMDATA.s*log(1-u(i,j)/CMDATA.b);
Dir_contact{i,j} = -h_inT{i,j}/norm(h_inT{i,j},2);
Rho_contact{i,j} = Rot_H2T*h{i,j};
elseif ge(u(i,j),0.98*CMDATA.b)
%u(i,j) = 0.98*b;
F_contact(i,j) = -a(i,j)*CMDATA.s*log(1-0.98);
Dir_contact{i,j} = -h_inT{i,j}/norm(h_inT{i,j},2);
Rho_contact{i,j} = Rot_H2T*h{i,j};
%disp(['u' num2str(i) ',' num2str(i) '=' num2str(u(i,j)) '>=0.98b']);
else % and if there is no contact there will be no contact force
F_contact(i,j) = 0;
Dir_contact{i,j} = -h_inT{i,j}/norm(h_inT{i,j},2);
Rho_contact{i,j} = Rot_H2T*h{i,j};
end
end
end
% the same calculations for the top and bottom points
% the top point
h_inT{1,1} = r_HH + Rot_H2T*h{1,1}; % hi in the {T} frame
u(1,1) = norm(h_inT{1,1},2) - CMDATA.r_g; % the penetration of the point hi
% in the glenoid fossa
if gt(u(1,1),0) & lt(u(1,1),0.98*CMDATA.b) % contact condition of the points
F_contact(1,1) = -a(1,1)*CMDATA.s*log(1-u(1,1)/CMDATA.b);
Dir_contact{1,1} = -h_inT{1,1}/norm(h_inT{1,1},2);
Rho_contact{1,1} = Rot_H2T*h{1,1};
elseif ge(u(1,1),0.98*CMDATA.b)
%u(i,j) = 0.98*b;
F_contact(1,1) = -a(1,1)*CMDATA.s*log(1-0.98);
Dir_contact{1,1} = -h_inT{1,1}/norm(h_inT{1,1},2);
Rho_contact{1,1} = Rot_H2T*h{1,1};
%disp(['u' num2str(1) ',' num2str(1) '=' num2str(u(1,1)) '>=0.98b']);
else % and if there is no contact there will be no contact force
F_contact(1,1) = 0;
Dir_contact{1,1} = -h_inT{1,1}/norm(h_inT{1,1},2);
Rho_contact{1,1} = Rot_H2T*h{1,1};
end
% the bottom point
k = size(h,1); l = size(h,2);
h_inT{k,l} = r_HH + Rot_H2T*h{k,l}; % hi in the {T} frame
u(k,l) = norm(h_inT{k,l},2) - CMDATA.r_g; % the penetration of the point hi
% in the glenoid fossa
if gt(u(k,l),0) & lt(u(k,l),0.98*CMDATA.b) % contact condition of the points
F_contact(k,l) = -a(k,l)*CMDATA.s*log(1-u(k,l)/CMDATA.b);
Dir_contact{k,l} = -h_inT{k,l}/norm(h_inT{k,l},2);
Rho_contact{k,l} = Rot_H2T*h{k,l};
elseif ge(u(k,l),0.98*CMDATA.b)
%u(i,j) = 0.98*b;
F_contact(k,l) = -a(k,l)*CMDATA.s*log(1-0.98);
Dir_contact{k,l} = -h_inT{k,l}/norm(h_inT{k,l},2);
Rho_contact{k,l} = Rot_H2T*h{k,l};
%disp(['u' num2str(k) ',' num2str(l) '=' num2str(u(k,l)) '>=0.98b']);
else % and if there is no contact there will be no contact force
F_contact(k,l) = 0;
Dir_contact{k,l} = -h_inT{k,l}/norm(h_inT{k,l},2);
Rho_contact{k,l} = Rot_H2T*h{k,l};
end
%imagesc(u);
%colorbar;
return
c4science · Help