
Body Builder code developed by Enrica Papi to model lower limb segments and calculate join kinematics and kinetics.
Marker file and parameter file are used in conjunctions to this model file if data are processed with Vicon Workstation or a vst file for analysis with Vicon Nexus.



{*Start of macro section*}
{*======================*}

macro SUBSTITUTE4(p1,p2,p3,p4)
{*Replaces any point missing from set of four fixed in a segment*}
s234 = [p3,p2-p3,p3-p4]
p1V = Average(p1/s234)*s234
s341 = [p4,p3-p4,p4-p1]
p2V = Average(p2/s341)*s341
s412 = [p1,p4-p1,p1-p2]
p3V = Average(p3/s412)*s412
s123 = [p2,p1-p2,p2-p3]
p4V = Average(p4/s123)*s123

p1 = (p1+p1V)/2 ? p1 ? p1V
p2 = (p2+p2V)/2 ? p2 ? p2V
p3 = (p3+p3V)/2 ? p3 ? p3V
p4 = (p4+p4V)/2 ? p4 ? p4V

endmacro

macro DRAWBONE(Bone,BoneLabel)
{*Outputs segment definition markers in Polygon format*}
LL = Bone#Size
DD = LL/10
WW = DD
BoneLabel#O = 0(Bone)+LL*Bone#Shift*Attitude(Bone)
BoneLabel#P = BoneLabel#O+LL*2(Bone#Scale)*2(Bone)
BoneLabel#A = BoneLabel#O+DD*1(Bone#Scale)*1(Bone)
BoneLabel#L = BoneLabel#O+WW*3(Bone#Scale)*3(Bone)
OUTPUT(BoneLabel#O,BoneLabel#P,BoneLabel#A,BoneLabel#L)
endmacro

macro DRAWBONE1(Bone,BoneLabel)
{*Outputs segment definition markers in Polygon format*}
LL = Bone#Size
DD = LL/10
WW = DD
BoneLabel#O = 0(Bone)+LL*Bone#Shift*Attitude(Bone)
BoneLabel#P = BoneLabel#O+LL*2(Bone#Scale)*2(Bone)
BoneLabel#A = BoneLabel#O-DD*1(Bone#Scale)*1(Bone)
BoneLabel#L = BoneLabel#O+WW*3(Bone#Scale)*3(Bone)
OUTPUT(BoneLabel#O,BoneLabel#P,BoneLabel#A,BoneLabel#L)
endmacro


macro POINTER(Anatomy,Segment)
{*Calculates the position of anatomical landmarks for calibration in the technical frame it belongs to*}
{*1st determine the "point" in the Global system and outputs it as point#Calib. Then converts the point into*}
{*the appropriate technical reference frame and stores it as parameter $%#point#Calib*}

Anatomy#Calib=Cal#Anatomy
OUTPUT(Anatomy#Calib)
PARAM(Anatomy#Calib)
%#Anatomy#Calib=Anatomy#Calib/Segment
PARAM(%#Anatomy#Calib)
endmacro

Macro DYNPOINTER(AnatPoint,Segment)		
AnatPoint=%#AnatPoint#Calib*Segment
OUTPUT(AnatPoint)
PARAM(AnatPoint)
EndMacro

macro SEGVIS(Segment)
{*outputs a visual representaion of the segment to be viewed in the Workspace*}
{*0(Segment) is the origin of the segment*}
ORIGIN#Segment=0(Segment)
XAXIS#Segment=0(Segment)+(1(Segment)*100)
YAXIS#Segment=0(Segment)+(2(Segment)*100)
ZAXIS#Segment=0(Segment)+(3(Segment)*100)
OUTPUT(ORIGIN#Segment,XAXIS#Segment,YAXIS#Segment,ZAXIS#Segment)
endmacro

macro ColeJCS(seg1,seg2,joint)
{*  Procedure to calculate the rotations about defined embedded axes using the joint
co-ordinate system (Grood & Suntay convention). The generalization proposed by Cole et al. is applied.

References:	Cole,G.K. et al (1993).  Application of the Joint Co-ordinate System
		to Three-dimensional Joint Attitude and Movement Representation : A
		Standardization Proposal.  Journal of Biomechanical Engineering.
		November 1993 : Vol 115 : pp 344-349

aEone,aEtwo,aEthree =unit vector describing the attitude of the 1st,2nd and 3rd axis of
the joint co-ordinate system between the proximal segment (seg1) and the distal segment
(seg2), relative to an inertial reference system. aEone and aEthree are fixed in the proximal and distal
segment respectively, aEtwo is the floating axis.

The axes of a body segment co-ordinate system are identified as an axis of Flexion, a
Longitudinal axis and a Third axis, cross product of the previous two. T
he unit vectors of these axes are Fone, Lone, Tone for the proximal segment and 
Ftwo, Ltwo, Ttwo for distal segment.

Input:	'seg1', 'seg2' describing the axes of the co-ordinate systems embedded in proximal and distal segment.
	'joint' is the joint at which the specified segments interact.

Output:	Angles of rotation about axes aEone,aEtwo,aEthree, flexion, abduction and rotation
	respectively. Counterclockwise rotations are chosen as positive*}

Fone=3(seg1)
Lone=2(seg1)
Tone=1(seg1)
Ftwo=3(seg2)
Ltwo=2(seg2)
Ttwo=1(seg2)


{*Defines e1 and e3*}
aEone=Fone
aEthree=Ltwo

{*Calculate the Vector or Cross Product between the Vectors*}
Va={2(aEthree)*3(aEone)-3(aEthree)*2(aEone),3(aEthree)*1(aEone)-1(aEthree)*3(aEone),1(aEthree)*2(aEone)-2(aEthree)*1(aEone)}
Vb=DIST({2(aEone)*3(aEthree)-3(aEone)*2(aEthree),3(aEone)*1(aEthree)-1(aEone)*3(aEthree),1(aEone)*2(aEthree)-2(aEone)*1(aEthree)},{0,0,0})
Vc={2(Va)*3(aEthree)-3(Va)*2(aEthree),3(Va)*1(aEthree)-1(Va)*3(aEthree),1(Va)*2(aEthree)-2(Va)*1(aEthree)}

{*Calculate the Scalar or Dot Product between the Vectors*}
DPone=(1(Va)*1(Ttwo))+(2(Va)*2(Ttwo))+(3(Va)*3(Ttwo))
DPtwo=(1(Vc)*1(Ftwo))+(2(Vc)*2(Ftwo))+(3(Vc)*3(Ftwo))

{*Calculates A (AA) and then e2*}
IF DPone < 0 AND DPtwo > 0 THEN AA=-1 ELSE AA=1 ENDIF
aEtwo=(Va/Vb)*AA

{*Calculate the value of r.*}
Rone={2(Fone)*3(aEtwo)-3(Fone)*2(aEtwo),3(Fone)*1(aEtwo)-1(Fone)*3(aEtwo),1(Fone)*2(aEtwo)-2(Fone)*1(aEtwo)}
Rtwo=DIST(Rone,{0,0,0})
r=Rone/Rtwo

{*Calculate the Scalar or Dot Product between the Vectors.*}
aEtwoTonedp=(1(aEtwo)*1(Tone))+(2(aEtwo)*2(Tone))+(3(aEtwo)*3(Tone))
aEtwoLonedp=(1(aEtwo)*1(Lone))+(2(aEtwo)*2(Lone))+(3(aEtwo)*3(Lone))
rLtwodp=(1(r)*1(Ltwo))+(2(r)*2(Ltwo))+(3(r)*3(Ltwo))
FoneLtwodp=(1(Fone)*1(Ltwo))+(2(Fone)*2(Ltwo))+(3(Fone)*3(Ltwo))
aEtwoTtwodp=(1(aEtwo)*1(Ttwo))+(2(aEtwo)*2(Ttwo))+(3(aEtwo)*3(Ttwo))
aEtwoFtwodp=(1(aEtwo)*1(Ftwo))+(2(aEtwo)*2(Ftwo))+(3(aEtwo)*3(Ftwo))

IF aEtwoLonedp >= 0 THEN aEtwoLonesign=1 ENDIF
IF aEtwoLonedp < 0 THEN aEtwoLonesign=-1 ENDIF
IF FoneLtwodp >= 0 THEN FoneLtwosign=1 ENDIF
IF FoneLtwodp < 0 THEN FoneLtwosign=-1 ENDIF
IF aEtwoFtwodp >= 0 THEN aEtwoFtwosign=1 ENDIF
IF aEtwoFtwodp < 0 THEN aEtwoFtwosign=-1 ENDIF

joint#Flex=(acos(aEtwoTonedp))*(aEtwoLonesign)
joint#Abd=(acos(rLtwodp))*(FoneLtwosign)
joint#Rot=(acos(aEtwoTtwodp))*(aEtwoFtwosign)
joint#angles=<joint#Flex,joint#Abd,joint#Rot>

OUTPUT(joint#angles)

joint#JCS=[joint,aEtwo,aEone,xyz]

ORIGIN#joint#jcs=0(joint#jcs)
XAXIS#joint#jcs=0(joint#jcs)+(1(joint#jcs)*100)
YAXIS#joint#jcs=0(joint#jcs)+(2(joint#jcs)*100)
ZAXIS#joint#jcs=0(joint#jcs)+(3(joint#jcs)*100)
OUTPUT(ORIGIN#joint#jcs,XAXIS#joint#jcs,YAXIS#joint#jcs,ZAXIS#joint#jcs)

ENDMACRO


{*Pelvic angles calculation following ROT sequence based on G&S convention*}
macro PELAngles(seg1,seg2,joint)

Azone=3(seg1)
Ayone=2(seg1)
Axone=1(seg1)
Pztwo=3(seg2)
Pytwo=2(seg2)
Pxtwo=1(seg2)

{*Defines e1 and e3*}
aEone=Ayone
aEthree=Pztwo

{*Calculate the Vector or Cross Product between the Vectors*}
{*Va={2(aEthree)*3(aEone)-3(aEthree)*2(aEone),3(aEthree)*1(aEone)-1(aEthree)*3(aEone),1(aEthree)*2(aEone)-2(aEthree)*1(aEone)}*}
{*Vb=DIST({2(aEone)*3(aEthree)-3(aEone)*2(aEthree),3(aEone)*1(aEthree)-1(aEone)*3(aEthree),1(aEone)*2(aEthree)-2(aEone)*1(aEthree)},{0,0,0})*}
{*Vc={2(Va)*3(aEthree)-3(Va)*2(aEthree),3(Va)*1(aEthree)-1(Va)*3(aEthree),1(Va)*2(aEthree)-2(Va)*1(aEthree)}*}

Va={2(aEone)*3(aEthree)-3(aEone)*2(aEthree),3(aEone)*1(aEthree)-1(aEone)*3(aEthree),1(aEone)*2(aEthree)-2(aEone)*1(aEthree)}
Vb=DIST({2(aEthree)*3(aEone)-3(aEthree)*2(aEone),3(aEthree)*1(aEone)-1(aEthree)*3(aEone),1(aEthree)*2(aEone)-2(aEthree)*1(aEone)},{0,0,0})
Vc={2(Va)*3(aEone)-3(Va)*2(aEone),3(Va)*1(aEone)-1(Va)*3(aEone),1(Va)*2(aEone)-2(Va)*1(aEone)}



{*Calculate the Scalar or Dot Product between the Vectors*}
DPone=(1(Va)*1(Axone))+(2(Va)*2(Axone))+(3(Va)*3(Axone))
DPtwo=(1(Vc)*1(Azone))+(2(Vc)*2(Azone))+(3(Vc)*3(Azone))

{*Calculates A (AA) and then e2*}
IF DPone < 0 AND DPtwo > 0 THEN AA=-1 ELSE AA=1 ENDIF
aEtwo=(Va/Vb)*AA

{*Calculate the value of r.*}
Rone={2(aEthree)*3(aEtwo)-3(aEthree)*2(aEtwo),3(aEthree)*1(aEtwo)-1(aEthree)*3(aEtwo),1(aEthree)*2(aEtwo)-2(aEthree)*1(aEtwo)}
Rtwo=DIST(Rone,{0,0,0})
r=Rone/Rtwo

{*Calculate the Scalar or Dot Product between the Vectors for angle calculation*}
aEtwoPxtwodp=(1(aEtwo)*1(Pxtwo))+(2(aEtwo)*2(Pxtwo))+(3(aEtwo)*3(Pxtwo))
aEtwoPytwodp=(1(aEtwo)*1(Pytwo))+(2(aEtwo)*2(Pytwo))+(3(aEtwo)*3(Pytwo))
raEonedp=(1(r)*1(aEone))+(2(r)*2(aEone))+(3(r)*3(aEone))
aEthreeaEonedp=(1(aEthree)*1(aEone))+(2(aEthree)*2(aEone))+(3(aEthree)*3(aEone))
aEtwoAxonedp=(1(aEtwo)*1(Axone))+(2(aEtwo)*2(Axone))+(3(aEtwo)*3(Axone))
aEtwoAzonedp=(1(aEtwo)*1(Azone))+(2(aEtwo)*2(Azone))+(3(aEtwo)*3(Azone))

IF aEtwoPytwodp >= 0 THEN aEtwoPytwosign=1 ENDIF
IF aEtwoPytwodp < 0 THEN aEtwoPytwosign=-1 ENDIF
IF aEthreeaEonedp >= 0 THEN aEthreeaEonesign=1 ENDIF
IF aEthreeaEonedp < 0 THEN aEthreeaEonesign=-1 ENDIF
IF aEtwoAzonedp >= 0 THEN aEtwoAzonesign=1 ENDIF
IF aEtwoAzonedp < 0 THEN aEtwoAzonesign=-1 ENDIF

joint#Flex=(acos(aEtwoPxtwodp))*(aEtwoPytwosign)
joint#Abd=(acos(raEonedp))*(aEthreeaEonesign)
joint#Rot=(acos(aEtwoAxonedp))*(aEtwoAzonesign)
joint#angles=<joint#Rot,joint#Abd,joint#Flex>

OUTPUT(joint#angles)

ENDMACRO


macro ROTangles(seg1,seg2,joint)

Pthree=3(seg1)
Ptwo=2(seg1)
Pone=1(seg1)
Dthree=3(seg2)
Dtwo=2(seg2)
Done=1(seg2)

primo= (1(Dtwo)*1(Pthree))+(2(Dtwo)*2(Pthree))+(3(Dtwo)*3(Pthree))
secondo= (1(Done)*1(Pthree))+(2(Done)*2(Pthree))+(3(Done)*3(Pthree))
terzo= (1(Dtwo)*1(Pone))+(2(Dtwo)*2(Pone))+(3(Dtwo)*3(Pone))

joint#Abd= (asin(primo))
joint#Rot= (-asin(secondo/cos(joint#Abd)))
joint#Flex= (-asin(terzo/cos(joint#Abd)))

joint#rotation=<joint#Rot,joint#Abd,joint#Flex>
OUTPUT(joint#rotation)

ENDMACRO


macro FORCEVECTOR(FP)
{*This defines the quantities of force(F), moment(M) and Centre(C) from the reaction (FP)*}
{*P_#FP is the centre of pressure and is set at the forceplate centre if load is below 10N*}

If ExistAtAll( FP )
	F_#FP = FP(1)
	M_#FP = FP(2)
	C_#FP = FP(3)
	if ( ABS ( F_#FP ) > 10 )
		P_#FP = C_#FP + { -M_#FP(2)/F_#FP(3), M_#FP(1)/F_#FP(3), -C_#FP(3) }
	else 
		P_#FP = C_#FP
	endif
	F_#FP = F_#FP + P_#FP
	OUTPUT ( P_#FP, F_#FP ) 
EndIf
endmacro


{*Macro for Dot Product*}

MACRO DotProduct (One,Two,DotProd)
	DotProd = (1(One)*1(Two)+2(One)*2(Two)+3(One)*3(Two))
ENDMACRO

{* Macro to do a cross product *}

MACRO CrossProduct (seg,FP,joint)
         Vector= FP(1)
         Point= seg(0)
	 joint#Result = {Point(2)*Vector(3)-Point(3)*Vector(2),
	Point(3)*Vector(1)-Point(1)*Vector(3), 
	Point(1)*Vector(2)-Point(2)*Vector(1)}
        OUTPUT(joint#Result)
ENDMACRO



{*End of macro section*}
{* Anthropometric Data: From DA Winter, Biomechanics and Motor Control of Human Movement *}

AnthropometricData
DefaultPelvis 0.142 0.895 0 0
DefaultFemur 0.1 0.567 0.323 0
DefaultTibia 0.0465 0.567 0.302 0
DefaultFoot 0.0145 0.5 0.475 0
EndAnthropometricData

{*Optional points are points which may not be present in every trial*}

OptionalPoints(Pointer1,Pointer2)
OptionalPoints(RASIS,LASIS,RPSIS,LPSIS,PELF)
OptionalPoints(WAIST1,WAIST2,WAIST3,WAIST4)
OptionalPoints(RTH1,RTH2,RTH3,RTH4)
OptionalPoints(LTH1,LTH2,LTH3,LTH4)
OptionalPoints(RSH1,RSH2,RSH3,RSH4)
OptionalPoints(LSH1,LSH2,LSH3,LSH4)
OptionalPoints(RMET1,RMET5,RHEEL,LMET1,LMET5,LHEEL)
OptionalPoints(RHJC,LHJC,RKJC,LKJC,RAJC,LAJC)
OptionalPoints(RLMAL,RMMAL,LLMAL,LMMAL)
OptionalPoints(RLEPI,RMEPI,LMEPI,LLEPI)

{*Substitutes missing markers based on clusters of 4 markers*}
SUBSTITUTE4(WAIST1,WAIST2,WAIST3,WAIST4)
SUBSTITUTE4(RTH1,RTH2,RTH3,RTH4)			
SUBSTITUTE4(LTH1,LTH2,LTH3,LTH4)
SUBSTITUTE4(RSH1,RSH2,RSH3,RSH4)
SUBSTITUTE4(LSH1,LSH2,LSH3,LSH4)

{*Marker cluster axis definitions*}
{*Defines technical axis systems for the segments from the marker on the clusters*}

WaistSeg=[WAIST1,WAIST1-WAIST3,WAIST3-WAIST2,zyx]
RThighSeg=[RTH1,RTH1-RTH3,RTH3-RTH2,zyx]
RShinSeg=[RSH1,RSH1-RSH4,RSH2-RSH4,zxy]  
LThighSeg=[LTH1,LTH1-LTH3,LTH3-LTH2,zyx]
LShinSeg=[LSH1,LSH1-LSH4,LSH2-LSH4,zxy]  

SEGVIS(WaistSeg)
SEGVIS(RThighSeg)
SEGVIS(LThighSeg)
SEGVIS(RShinSeg)
SEGVIS(LShinSeg)

{*STATIC CALIBRATIONS*}

If $Static==1

%RLEPI=RLEPI/RThighSeg
%RMEPI=RMEPI/RThighSeg
%LLEPI=LLEPI/LThighSeg
%LMEPI=LMEPI/LThighSeg

%RASIS=RASIS/WaistSeg
%LASIS=LASIS/WaistSeg
%RPSIS=RPSIS/WaistSeg
%LPSIS=LPSIS/WaistSeg

%RMMAL=RMMAL/RShinSeg
%LMMAL=LMMAL/LShinSeg
%RLMAL=RLMAL/RShinSeg
%LLMAL=LLMAL/LShinSeg

PARAM (%RLEPI,%RMEPI,%LLEPI,%LMEPI,%RASIS,%LASIS,%RPSIS,%LPSIS,%LMMAL,%LLMAL,%RLMAL,%RMMAL)
EndIf

{*Calculation of pelvic parameters for HJC estimation*}
If ExistAtAll(RASIS,LASIS,RPSIS,LPSIS) AND $Static==1
   $InterASISDist=DIST(LASIS, RASIS)
   SACR=(LPSIS+RPSIS)/2
   PELF=(LASIS+RASIS)/2
   $PelvicDepth=DIST(PELF,SACR)
   PARAM($InterASISDist,$PelvicDepth)
EndIf



{*Dynamic Trials*}

If $Static==0

{*Anatomical frame definition*}

RLEPI=%RLEPI*RThighSeg
RMEPI=%RMEPI*RThighSeg
LLEPI=%LLEPI*LThighSeg
LMEPI=%LMEPI*LThighSeg

RASIS=%RASIS*WaistSeg
LASIS=%LASIS*WaistSeg
RPSIS=%RPSIS*WaistSeg
LPSIS=%LPSIS*WaistSeg

RMMAL=%RMMAL*RShinSeg
RLMAL=%RLMAL*RShinSeg
LMMAL=%LMMAL*LShinSeg
LLMAL=%LLMAL*LShinSeg

OUTPUT (RLEPI,RMEPI,LLEPI,LMEPI,RPSIS,LPSIS,RASIS,LASIS,RMMAL,RLMAL,LMMAL,LLMAL)


{*Segment definitions*}

 
Global=[{0,0,0},{0,-1,0},{0,0,1},zxy]
SEGVIS(Global)

{*HJC calculated using equations from Harrington2007*}
{*Hip joint centre is a function of interAsis Distance and PelvicDepth*}

SACR=(LPSIS+RPSIS)/2
OUTPUT(SACR)
PARAM(SACR)

PELF=(LASIS+RASIS)/2
OUTPUT(PELF)
PARAM(PELF)
Pelvis=[PELF, RASIS-LASIS, SACR-PELF, zyx]

InterASISDist=$InterASISDist
OUTPUT(InterASISDist)

XHJC=(-0.24*$PelvicDepth)-9.9
YHJC=(-0.3*InterASISDist)-10.9
ZHJC=(0.33*InterASISDist)+7.3

RHJC = {XHJC,YHJC,ZHJC}*Pelvis
LHJC = {XHJC,YHJC,-ZHJC}*Pelvis


Pelvis = (LHJC+RHJC)/2 + Attitude(Pelvis)
SEGVIS(Pelvis)
OUTPUT (RHJC, LHJC)
PARAM (RHJC, LHJC)


PelvisSize = DIST(LHJC,RHJC)
PelvisScale = {1,1,1}
PelvisShift = {0,0,0}

RHipSeg=[RHJC, RASIS-LASIS, SACR-PELF, zyx]
LHipSeg=[LHJC, RASIS-LASIS, SACR-PELF, zyx]
SEGVIS(RHipSeg)
SEGVIS(LHipSeg)

{*Right Thigh Segment*}
RKJC=(RLEPI+RMEPI)/2
OUTPUT(RKJC)
PARAM(RKJC)

RFemur=[RKJC, RHJC-RKJC, RMEPI-RLEPI, yxz]
RFemur=ROT(RFemur, RFemur(2), $RFemurRot)
SEGVIS(RFemur)

{*Left Thigh Segment*}
LKJC=(LLEPI+LMEPI)/2
OUTPUT(LKJC)
PARAM(LKJC)
LFemur=[LKJC, LHJC-LKJC, LLEPI-LMEPI, yxz]
LFemur=ROT(LFemur, LFemur(2), $LFemurRot)
SEGVIS(LFemur)


LFemurSize = DIST(LKJC,LHJC)
LFemurScale = {1,1,1}
LFemurShift = {0,0,0}

RFemurSize = DIST(RKJC,RHJC)
RFemurScale = {1,1,1}
RFemurShift = {0,0,0}

{*Right Shank Segment*}
RAJC=(RMMAL+RLMAL)/2
OUTPUT(RAJC)
PARAM(RAJC)
RTibia=[RAJC, RKJC-RAJC, RMMAL-RLMAL, yxz]
SEGVIS(RTibia)

{*Left Shank Segment*}
LAJC=(LMMAL+LLMAL)/2
OUTPUT(LAJC)
PARAM(LAJC)
LTibia=[LAJC, LKJC-LAJC, LLMAL-LMMAL, yxz]
SEGVIS(LTibia)

LTibiaSize = DIST(0(LTibia),0(LFemur))
LTibiaScale = {0.93,0.93,0.93}
LTibiaShift = {0,0,-0.01}

RTibiaSize = DIST(0(RTibia),0(RFemur))
RTibiaScale = {0.93,0.93,0.93}
RTibiaShift = {0,0,-0.01}

{*Foot Segment*}
{*Considered to represent a shoe rather than a foot. The markers are put on so that they lie in a 
plane parallel to the floor in a neutral position. This can be considered as ankle joint neutral*}

{*Right Foot Segment*}
RmidFOOT=(RMET1+RMET5)/2
RFootSeg=[RHEEL, RHEEL-RmidFOOT, RMET1-RMET5, yxz]
OUTPUT(RmidFOOT)
PARAM(RmidFOOT)
SEGVIS(RFootSeg)



RFoot=[RHEEL, RHEEL-RmidFOOT, RMET5-RMET1, yxz]
RFootSize= 0.76*DIST(RHEEL,RmidFOOT)
RFootScale = {1,1,1}
RFootShift = {-0.1,-1.2,0}


{*Left Foot Segment*}
LmidFOOT=(LMET1+LMET5)/2
LFootSeg=[LHEEL, LHEEL-LmidFOOT, LMET5-LMET1, yxz]
OUTPUT(LmidFOOT)
PARAM(LmidFOOT)
SEGVIS(LFootSeg)


LFoot=[LHEEL, LHEEL-LmidFOOT, LMET1-LMET5, yxz]
LFootSize= 0.76*DIST(LHEEL,LmidFOOT)
LFootScale = {1,1,1}
LFootShift = {-0.1,-1.2,0}


LToe= [LmidFOOT, LHEEL-LmidFOOT, LMET1-LMET5, yxz]
LToeSize = 0.24*DIST(LHEEL,LmidFOOT)
LToeScale = {1,1,1}
LToeShift = {-0.3,-0.65,0}


RToe= [RmidFOOT, RHEEL-RmidFOOT, RMET5-RMET1, yxz]
RToeSize = 0.24*DIST(RHEEL,RmidFOOT)
RToeScale = {1,1,1}
RToeShift = {-0.3,-0.65,0}

{*Joint Angles*}
{*the general  direction of progression of the subject is calculated for pelvic angles calculation, 
a so called Anatomy reference frame is constructed with x axis the direction of progression*}
{*The joint names are given values to allow the creation of dummy JCS*}

Progress = (PELF[7] + PELF[6] + PELF[5] + PELF[4] + PELF[3]
	     - PELF[-7]- PELF[-6]- PELF[-5]- PELF[-4]- PELF[-3])/5
Anatomy = [0(Global),2(Global),Progress,yzx]
SEGVIS(Anatomy)

Pelvic=(LHJC+RHJC)/2
LHip=LHJC
RHip=RHJC
LKnee=LKJC
RKnee=RKJC
LAnkle=LAJC
RAnkle=RAJC



PELAngles(Anatomy, Pelvis, Pelvic)
ROTangles(Anatomy, Pelvis, Pelvic)


{*ROTangles(Anatomy, Pelvis, Pelvic)*}

ColeJCS(LHipSeg,LFemur,LHip)
SEGVIS(LHipJCS)
ColeJCS(RHipSeg,RFemur,RHip)
SEGVIS(RHipJCS)
ColeJCS(LFemur,LTibia,LKnee)
SEGVIS(LKneeJCS)
ColeJCS(RFemur,RTibia,RKnee)
SEGVIS(RKneeJCS)
ColeJCS(LTibia,LFootSeg,LAnkle)
SEGVIS(LAnkleJCS)
ColeJCS(RTibia,RFootSeg,RAnkle)
SEGVIS(RAnkleJCS)

{*corrects so that flexion, adduction and internal rotation are positive*} 
{*Order of angles is IR,add,flexion---> Rotation\Obliquity\Tilt*}

RPelvicangles=<-1(Pelvicangles),2(Pelvicangles),3(Pelvicangles)>
LPelvicangles=<1(Pelvicangles),-2(Pelvicangles),3(Pelvicangles)>

RPelvicRotation=<1(Pelvicrotation),-2(Pelvicrotation),-3(Pelvicrotation)>
LPelvicRotation=-RPelvicRotation(-3)

{*Order of angles is flexion, add, rotation*}

RHipangles=<1(RHipangles),2(RHipangles),3(RHipangles)>
LHipangles=<1(LHipangles),-2(LHipangles),-3(LHipangles)>
RKneeangles=<-1(RKneeangles),2(RKneeangles),3(RKneeangles)>
LKneeangles=<-1(LKneeangles),-2(LKneeangles),-3(LKneeangles)>
RAnkleangles=<(1(RAnkleangles)-90),2(RAnkleangles),-3(RAnkleangles)>
LAnkleangles=<(1(LAnkleangles)-90),-2(LAnkleangles),3(LAnkleangles)>

Output(RHipangles,LHipangles,LKneeangles,RKneeangles,LAnkleangles,RAnkleangles,RPelvicangles,LPelvicangles,RPelvicRotation,LPelvicRotation)

EndIF



                             
{*KINETIC CALCULATIONS*}
{*====================*}

{*Hierarchy*}

RFemur=[RFemur,Pelvis,RHJC, DefaultFemur]
LFemur=[LFemur,Pelvis,LHJC, DefaultFemur]
RTibia=[RTibia,RFemur,RKJC, DefaultTibia]
LTibia=[LTibia,LFemur,LKJC, DefaultTibia]
RFootSeg=[RFootSeg,RTibia,RAJC, DefaultFoot]
LFootSeg=[LFootSeg,LTibia,LAJC, DefaultFoot]


{*Force Vectors*}
{*=============*}
OptionalReactions(ForcePlate1, ForcePlate2, ForcePlate3, ForcePlate4)
ForceVector(ForcePlate1)
ForceVector(ForcePlate2)
ForceVector(ForcePlate3)
ForceVector(ForcePlate4)

{* Forces and Moments *}
{*====================*}

{* These moments are internal moments*}
{* Expressed in Nm*}
{*Corrects for inverse sign for right side of body in frontal and transverse plane: abduction positive, internal rotation positive*}

RAnkleForce = 1(REACTION(RFootSeg))
RAM = 2(REACTION(RFootSeg))
RAM = RAM/(1000)
RAM = {-1(RAM), -2(RAM), -3(RAM)}
DummyRA = |{0,0,0},RAM,{0,0,0}|
RAnkleMoment = 2(DummyRA)

RKneeForce = 1(REACTION(RTibia))
RKM = 2(REACTION(RTibia))
RKM = RKM/(1000)
RKM= { -1(RKM), 2(RKM), 3(RKM)}
DummyRK = |{0,0,0},RKM,{0,0,0}|
RKneeMoment= 2(DummyRK)

RHipForce = 1 (REACTION(RFemur))
RHM = 2(REACTION(RFemur))
RHM = RHM/(1000)
RHM= {-1(RHM), 2(RHM), -3(RHM)}
DummyRH = |{0,0,0},RHM,{0,0,0}|
RHipMoment =2 (DummyRH)

LAnkleForce = 1(REACTION(LFootSeg))
LAM = 2(REACTION(LFootSeg))
LAM = LAM/(1000)
LAM= {1(LAM), 2(LAM), -3(LAM)}
DummyLA = |{0,0,0},LAM,{0,0,0}|
LAnkleMoment = 2(DummyLA)

LKneeForce = 1(REACTION(LTibia))
LKM = 2(REACTION(LTibia))
LKM= LKM/(1000)
LKM={1(LKM), -2(LKM), 3(LKM)}
DummyLK = |{0,0,0},LKM,{0,0,0}|
LKneeMoment= 2(DummyLK)

LHipForce = 1(REACTION(LFemur))
LHM = 2(REACTION(LFemur))
LHM = LHM/(1000)
LHM= {1(LHM), -2(LHM), -3(LHM)}
DummyLH = |{0,0,0},LHM,{0,0,0}|
LHipMoment =2 (DummyLH)


OUTPUT(LHipForce, LKneeForce, LAnkleForce, RHipForce,  RKneeForce, RAnkleForce)
OUTPUT(RHipMoment,RKneeMoment,RAnkleMoment,LHipMoment,LKneeMoment,LAnkleMoment)

{*Draw Bones in polygon*}

DrawBone(Pelvis,PEL)
DrawBone(LFemur,LFE)
DrawBone(RFemur,RFE)
DrawBone(LTibia,LTI)
DrawBone(RTibia,RTI)
DrawBone1(LFoot,LFO)
DrawBone1(RFoot,RFO)
DrawBone1(LToe,LTO)
DrawBone1(RToe,RTO)
