Homec4science
Diffusion MSM (master) Browse

msm/TOOLBOX_Force_Estimation/ESTIMATION_TOOL_glenoid_cone_make.m
74ce03b0ec87master

Last Change
Raw File

ESTIMATION_TOOL_glenoid_cone_make.m
View Options

function Nout = ESTIMATION_TOOL_glenoid_cone_make(DYDATA, BLDATA)
%{
Function for building the glenoid stability cone constraint
--------------------------------------------------------------------------
Syntax :
Nout = ESTIMATION_TOOL_glenoid_cone_make(DYDATA, BLDATA)
--------------------------------------------------------------------------
File description :
This function builds the glenoid joint stability cone to keep the joint
reaction force inside the glenoid. The Cone data is all statically
defined in the thorax reference system and stored in the DYDATA
structutre. This function simply rotates the constraints into the correct
position.
--------------------------------------------------------------------------
%}
% Initialise the output
Nout = zeros(DYDATA.NbConstraints,3);
% update the cone rotation matrix according to the glenoid orientations
% from the subject specific toolbox
Original_Cone_Rb = DYDATA.Cone_Rb;
Inc = BLDATA.Glenoid_Orientations(1);Dec = BLDATA.Glenoid_Orientations(2); % inclination and declination(version)
Inc0 = BLDATA.Glenoid_Orientations_Unchanged(1);Dec0 = BLDATA.Glenoid_Orientations_Unchanged(2); % initial values for inclination and declination(version)
% DYDATA.Cone_Rb = [cosd(Inc-Inc0) 0 sind(Inc-Inc0);0 1 0;-sind(Inc-Inc0) 0 cosd(Inc-Inc0)]...
% *[cosd(Dec-Dec0) sin(Dec-Dec0) 0;-sin(Dec-Dec0) cosd(Dec-Dec0) 0;0 0 1]*Original_Cone_Rb;
New_Cone_Rb = Original_Cone_Rb*[cosd(Inc-Inc0) 0 sind(Inc-Inc0);0 1 0;-sind(Inc-Inc0) 0 cosd(Inc-Inc0)]...
*[cosd(Dec-Dec0) -sind(Dec-Dec0) 0;sind(Dec-Dec0) cosd(Dec-Dec0) 0;0 0 1];
% Get initial and current scapula rotation matrices
Rs0 = BLDATA.Initial_Matrices_L2A.Rs;
Rs = BLDATA.Current_Matrices_L2A.Rs;
% Put the Cone Center in Cone Frame
C = DYDATA.Cone_Rb'*DYDATA.ConeCentre;
% Angle Around the Ellipse (number of cutting planes)
theta = linspace(0, 2*pi, DYDATA.NbConstraints);
% Points Around the Ellipse where the normal vectors are to be computed
P = [zeros(1,DYDATA.NbConstraints)+C(1);
DYDATA.ConeDimensions(1)*cos(theta)+C(2);
DYDATA.ConeDimensions(2)*sin(theta)+C(3)];
% for i = 1:DYDATA.NbConstraints
% P_inT = DYDATA.Cone_Rb*P(:,i)*1e3 + BLDATA.Initial_Points.GH;
% hold on
% plot3(P_inT(1), P_inT(2), P_inT(3),'marker', 'o','markerfacecolor', 'green','markeredgecolor', 'green')
% end
%
%
% Build the normal vectors
for i = 1:size(Nout,1)
C1 = C(1);
y = P(2,i);
z = P(3,i);
dxdy = -(C1^2*y)/(DYDATA.ConeDimensions(1)^2*((C1^2*y^2)/DYDATA.ConeDimensions(1)^2 + (C1^2*z^2)/DYDATA.ConeDimensions(2)^2)^(1/2)); % the minus is due to the position of C_x that is defined as ((C1^2*y^2)/DYDATA.ConeDimensions(1)^2 + (C1^2*z^2)/DYDATA.ConeDimensions(2)^2)^(1/2) however we could have just put C1 here and it's fine. For the details I have the derivations and one can see them.
dxdz = -(C1^2*z)/(DYDATA.ConeDimensions(2)^2*((C1^2*y^2)/DYDATA.ConeDimensions(1)^2 + (C1^2*z^2)/DYDATA.ConeDimensions(2)^2)^(1/2));
% Compute the Normal Vector in Absolute reference frame
Nout(i,:) =(Rs*Rs0'*DYDATA.Cone_Rb*[-1; dxdy; dxdz]/norm([-1; dxdy; dxdz]))';
end
return;
c4science · Help