load gaitdata
load RHJC
fs=100;
t=[1/fs:1/fs:length(LASI)/100];
LASI2=filter([1/4 1/4 1/4 1/4],1,LASI);
RANKLAT2=filter([1/4 1/4 1/4 1/4],1,RANKLAT);
RANKMED2=filter([1/4 1/4 1/4 1/4],1,RANKMED);
RASI2=filter([1/4 1/4 1/4 1/4],1,RASI);
RGRT2=filter([1/4 1/4 1/4 1/4],1,RGRT);
RHEE2=filter([1/4 1/4 1/4 1/4],1,RHEE);
RKNELAT2=filter([1/4 1/4 1/4 1/4],1,RKNELAT);
RKNEMED2=filter([1/4 1/4 1/4 1/4],1,RKNEMED);
RTHI2=filter([1/4 1/4 1/4 1/4],1,RTHI);
RTOE2=filter([1/4 1/4 1/4 1/4],1,RTOE);
SACR2=filter([1/4 1/4 1/4 1/4],1,SACR);
for a=1:length(SACR2)
ANKLEC(a,1)=RKNELAT2(a,1)+0.5*(RANKMED2(a,1)-RANKLAT2(a,1));
ANKLEC(a,2)=RKNELAT2(a,2)+0.5*(RANKMED2(a,2)-RANKLAT2(a,2));
ANKLEC(a,3)=RKNELAT2(a,3)+0.5*(RANKMED2(a,3)-RANKLAT2(a,3));
KNEEC(a,1)=[RKNELAT2(a,1)+RKNEMED2(a,1)]/2;
KNEEC(a,2)=[RKNELAT2(a,1)+RKNEMED2(a,1)]/2;
KNEEC(a,3)=[RKNELAT2(a,1)+RKNEMED2(a,1)]/2;
end
for b=1:length(RTOE2)
COMfoot=RTOE2+0.5*(RHEE2-RTOE2);
COMleg=ANKLEC+0.567*(KNEEC-ANKLEC);
COMthigh=KNEEC+0.567*(RHJC-KNEEC);
footzmag(b,1)=sqrt((ANKLEC(b,1)-COMfoot(b,1))^2+(ANKLEC(b,2)-COMfoot(b,2))^2+(ANKLEC(b,3)-COMfoot(b,3))^2);
footxmag(b,1)=sqrt((RANKLAT2(b,1)-RTOE2(b,1))^2+(RANKLAT2(b,2)-RTOE2(b,2))^2+(RANKLAT2(b,3)-RTOE2(b,3))^2);
legzmag(b,1)=sqrt((KNEEC(b,1)-COMleg(b,1))^2+(KNEEC(b,2)-COMleg(b,2))^2+(KNEEC(b,3)-COMleg(b,3))^2);
legxmag(b,1)=sqrt((RKNELAT2(b,1)-KNEEC(b,1))^2+(RKNELAT2(b,2)-KNEEC(b,2))^2+(RKNELAT2(b,3)-KNEEC(b,3))^2);
thighzmag(b,1)=sqrt((RHJC(b,1)-COMthigh(b,1))^2+(RHJC(b,2)-COMthigh(b,2))^2+(RHJC(b,3)-COMthigh(b,3))^2);
thighxmag(b,1)=sqrt((LASI(b,1)-RHJC(b,1))^2+(LASI(b,2)-RHJC(b,2))^2+(LASI(b,3)-RHJC(b,3))^2);
zfoot(b,1)=[ANKLEC(b,1)-COMfoot(b,1)]/(footzmag(b,1));
zfoot(b,2)=[ANKLEC(b,2)-COMfoot(b,2)]/(footzmag(b,1));
zfoot(b,3)=[ANKLEC(b,3)-COMfoot(b,3)]/(footzmag(b,1));
xfootTEMP(b,1)=[RANKLAT2(b,1)-RTOE2(b,1)]/(footxmag(b,1));
xfootTEMP(b,2)=[RANKLAT2(b,2)-RTOE2(b,2)]/(footxmag(b,1));
xfootTEMP(b,3)=[RANKLAT2(b,3)-RTOE2(b,3)]/(footxmag(b,1));
yfoot=cross(zfoot,xfootTEMP);
xfoot=cross(yfoot,zfoot);
zleg(b,1)=[KNEEC(b,1)-COMleg(b,1)]/(legzmag(b,1));
zleg(b,2)=[KNEEC(b,2)-COMleg(b,2)]/(legzmag(b,1));
zleg(b,3)=[KNEEC(b,3)-COMleg(b,3)]/(legzmag(b,1));
xlegTEMP(b,1)=[RKNELAT2(b,1)-KNEEC(b,1)]/(legxmag(b,1));
xlegTEMP(b,2)=[RKNELAT2(b,2)-KNEEC(b,2)]/(legxmag(b,1));
xlegTEMP(b,3)=[RKNELAT2(b,3)-KNEEC(b,3)]/(legxmag(b,1));
yleg=cross(zleg,xlegTEMP);
xleg=cross(yleg,zleg);
zthigh(b,1)=[RHJC(b,1)-COMthigh(b,1)]/(thighzmag(b,1));
zthigh(b,2)=[RHJC(b,2)-COMthigh(b,2)]/(thighzmag(b,1));
zthigh(b,3)=[RHJC(b,3)-COMthigh(b,3)]/(thighzmag(b,1));
xthighTEMP(b,1)=[LASI2(b,1)-RHJC(b,1)]/(thighxmag(b,1));
xthighTEMP(b,2)=[LASI2(b,2)-RHJC(b,2)]/(thighxmag(b,1));
xthighTEMP(b,3)=[LASI2(b,3)-RHJC(b,3)]/(thighxmag(b,1));
ythigh=cross(zthigh,xthighTEMP);
xthigh=cross(ythigh,zthigh);
Tdank=[xfoot(b,1),xfoot(b,2),xfoot(b,3);yfoot(b,1),yfoot(b,2),yfoot(b,3);zfoot(b,1),zfoot(b,2),zfoot(b,3)];
Tpkne=[xleg(b,1),xleg(b,2),xleg(b,3);yleg(b,1),yleg(b,2),yleg(b,3);zleg(b,1),zleg(b,2),zleg(b,3)];
Tdkne=[xthigh(b,1),xthigh(b,2),xthigh(b,3);ythigh(b,1),ythigh(b,2),ythigh(b,3);zthigh(b,1),zthigh(b,2),zthigh(b,3)];
end