{*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 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 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

{*End of macro section*}

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

AnthropometricData

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(AFO1,AFO2,AFO3,AFO4)
OptionalPoints(RSH1,RSH2,RSH3,RSH4)
OptionalPoints(LSH1,LSH2,LSH3,LSH4)
OptionalPoints(RMET1,RMET5,RHEEL,LMET1,LMET5,LHEEL)
OptionalPoints(RKJC,LKJC,RAJC,LAJC)
OptionalPoints(RLMAL,RMMAL,LLMAL,LMMAL)
OptionalPoints(RLEPI,RMEPI,LMEPI,LLEPI)

{*Substitutes missing markers based on clusters of 4 markers*}
SUBSTITUTE4(AFO1,AFO2,AFO3,AFO4)
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*}


AFOSeg=[AFO1,AFO1-AFO3,AFO2-AFO4,yxz]
RShinSeg=[RSH1,RSH1-RSH4,RSH2-RSH4,zxy]  
LShinSeg=[LSH1,LSH1-LSH4,LSH2-LSH4,zxy]  

SEGVIS(AFOSeg)
SEGVIS(RShinSeg)
SEGVIS(LShinSeg)

{*STATIC CALIBRATIONS*}

If $Static==1

%RLEPI=RLEPI/RShinSeg
%RMEPI=RMEPI/RShinSeg
%LLEPI=LLEPI/LShinSeg
%LMEPI=LMEPI/LShinSeg


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

PARAM (%RLEPI,%RMEPI,%LLEPI,%LMEPI,%LMMAL,%LLMAL,%RLMAL,%RMMAL)
EndIf


{*Dynamic Trials*}

{*Anatomical frame definition*}

RLEPI=%RLEPI*RShinSeg
RMEPI=%RMEPI*RShinSeg
LLEPI=%LLEPI*LShinSeg
LMEPI=%LMEPI*LShinSeg


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

OUTPUT (RLEPI,RMEPI,LLEPI,LMEPI,RMMAL,RLMAL,LMMAL,LLMAL)


{*Segment definitions*}

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

{*Knee Joint Centre for left and right leg*}

RKJC=(RLEPI+RMEPI)/2
OUTPUT(RKJC)
PARAM(RKJC)


LKJC=(LLEPI+LMEPI)/2
OUTPUT(LKJC)
PARAM(LKJC)


{*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)




{*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 System*}
RmidFOOT=(RMET1+RMET5)/2
RFootSeg=[RHEEL, RHEEL-RmidFOOT, RMET1-RMET5, yxz]
OUTPUT(RmidFOOT)
SEGVIS(RFootSeg)


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



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

{*Hierarchy*}

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*}
{* Not Normalised to body mass (NN)*}
{*Corrects for inverse sign for right side of body in frontal and transverse plane: adduction 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)


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)


OUTPUT(LAnkleForce, RAnkleForce)
OUTPUT(RAnkleMoment,LAnkleMoment)

