TransformProof

close all; clear all; clc;
% Donald Hume
% Program to calculate local rotation matrices given global
% coordinates. Shell1-4 represent global marker positions on
% rigid shell.

%% Thigh shell calcs
% Global coords
tShell1 = [382.636, 197.612, 708.68];
tShell2 = [451.709, 212.126, 705.496];
tShell3 = [379.714, 208.419, 600.203];
tShell4 = [445.368, 219.232, 601.864];

% Origin
tShellO = (tShell1 + tShell2 + tShell3 + tShell4)/4;

% k
Vk = tShell1-tShell3;
vMag = sqrt(Vk(1)^2 + Vk(2)^2 + Vk(3)^2);
Vk = Vk / vMag;

% temp j
vector1 = tShell2-tShell1;

% i
Vi = cross(vector1,Vk);
vMag = sqrt(Vi(1)^2 + Vi(2)^2 + Vi(3)^2);
Vi = Vi/vMag;

% actual j
Vj = cross(Vk,Vi);
vMag = sqrt(Vj(1)^2 + Vj(2)^2 + Vj(3)^2);
Vj = Vj/vMag;

% Assemble [R] for thigh shell
tShell = [Vi(1) Vj(1) Vk(1); Vi(2) Vj(2) Vk(2); Vi(3) Vj(3) Vk(3)];

%% Shank shell calcs
% Global coords
sShell1 = [347.358, 203.705, 366.3];
sShell2 = [407.198, 230.431, 349.611];
sShell3 = [325.109, 211.878, 290.957];
sShell4 = [383.857, 234.366, 278.143];
sShellO = (sShell1 + sShell2 + sShell3 + sShell4)/4;

% k
Vk = sShell1-sShell3;
vMag = sqrt(Vk(1)^2 + Vk(2)^2 + Vk(3)^2);
Vk = Vk / vMag;

% temp j
vector1 = sShell2-sShell1;

% i
Vi = cross(vector1,Vk);
vMag = sqrt(Vi(1)^2 + Vi(2)^2 + Vi(3)^2);
Vi = Vi/vMag;

% actual j
Vj = cross(Vk,Vi);
vMag = sqrt(Vj(1)^2 + Vj(2)^2 + Vj(3)^2);
Vj = Vj/vMag;

% Assemble [R] for shank shell
sShell = [Vi(1) Vj(1) Vk(1); Vi(2) Vj(2) Vk(2); Vi(3) Vj(3) Vk(3)];

%% Foot Shell
% Global coords
fShell1 = [346.167, 228.193, 95.0572];
fShell2 = [369.64, 270.515, 109.185];
fShell3 = [387.272,210.507,71.9638];
fShell4 = [407.879,254.698,88.0399];
fShellO = (fShell1 + fShell2 + fShell3 + fShell4)/4;

% k
Vk = fShell1-fShell3;
vMag = sqrt(Vk(1)^2 + Vk(2)^2 + Vk(3)^2);
Vk = Vk / vMag;

% temp j
vector1 = fShell4-fShell3;

% i
Vi = cross(vector1,Vk);
vMag = sqrt(Vi(1)^2 + Vi(2)^2 + Vi(3)^2);
Vi = Vi/vMag;

% actual j
Vj = cross(Vk,Vi);
vMag = sqrt(Vj(1)^2 + Vj(2)^2 + Vj(3)^2);
Vj = Vj/vMag;

% Construct [R] for foot shell
fShell = [Vi(1) Vj(1) Vk(1); Vi(2) Vj(2) Vk(2); Vi(3) Vj(3) Vk(3)];

%% Calculate Position of JC
% Knee
TtoK = [-30.0134, -12.1481, -180.546]';
tShell*TtoK + tShellO';
% Ankle
StoA = [-20.3624,12.5553,-194.15]';
sShell*StoA + sShellO';
% Hip
TtoH = [-50.2285,0.478356,219.834]';
tShell*TtoH + tShellO';