@inputs Bodyparts:array Animated
@inputs Masterent:entity
@outputs Fw:vector
@persist Defaultpos:array Defaultangle:array
 
if(first()){
    #Default arrangement of props defined by array of 4*4 matrices. 
    #For brevity and simplicity the format is GLON, saved by animation recorder from the dev chassis dupe.
    #Ideally all animations should flow naturally into this organisation, but props having momentum helps somewhat.
Defaultpos = glonDecode("-0.0132767950187890.99820703740787-4.90741109848020.011314522886804-0.0582160268664611.22119307518010.999847824561730.01391379350965314.913084983826-0.103259509532150.994220203898238.7628507614136-0.85909465168583-0.1040380133967-1.62830770015720.501292174862130.026499948308922-11.4439153671260.025850314429467-0.999083097635166.60789966583250.702926063447710.042439746194633-0.840162396430970.71079295944582-0.0056350891075923-29.453918457031-0.01030628867136-0.770076448747632.0748605728149-0.036822437471223-0.63717727152095-26.2842941284180.99926866658691-0.031422032332331-15.32320022583-0.0047511016532018-0.768794058478566.8661212921143-0.0083804999713068-0.6394329550017921.5258827209470.99995357152829-0.0090118062556123-13.597520828247-0.637408443815590.0022164125718476-0.388358592987060.770484050876320.012270103047065-8.80574512481690.0080417863628379-0.99992222112927-48.596607208252-0.998336052149570.00042049871909237-1.81111645698550.0576620463071350.000342259499781698.2956066131592-0.00040006394979719-0.99999988093581-48.596714019775")
    }
    
if(!Masterent){Masterent=entity()}
Localframe = matrix(Masterent)*1 Origin = Masterent:toWorld(vec(50,0,100))
#Positioning is done with PD controllers. 
#Genericize so it's easy to copypaste
P=10 D=-0.8 Index=1
OriP=800 OriD=-0.75
runOnTick(1)

Positionarray = Defaultpos:clone()
 
if(tickClk()&Bodyparts){
    
    Current=Bodyparts:entity(Index) Matrix = Positionarray:shiftMatrix()
    Fw = Localframe*Matrix:column(1):normalized() Up = Localframe*Matrix:column(2):normalized() Goal = Matrix:column(3) 
    Goal = Origin+Localframe*Goal
    Mass = Current:mass() Inertia = Current:inertia()
    Goalangle=Defaultangle:angle(Index)
    
    Force = P*(Goal-Current:pos()) + D*Current:vel()
    Force *= Mass
    Current:applyForce(Force)
    
    Current:applyOffsetForce(Fw*OriP,Current:pos() + Inertia:x()*Current:forward())
    Current:applyOffsetForce(-Fw*OriP,Current:pos() - Inertia:x()*Current:forward())
    Current:applyOffsetForce(Up*OriP,Current:pos() + Inertia:z()*Current:up())    
    Current:applyOffsetForce(-Up*OriP,Current:pos() - Inertia:z()*Current:up())    
    Current:applyAngForce(OriD*Current:angVel()*shiftL(ang(Inertia)))
    
    Index++
    
    Current=Bodyparts:entity(Index) Matrix = Positionarray:shiftMatrix()
    Fw = Localframe*Matrix:column(1) Up = Localframe*Matrix:column(2) Goal = Matrix:column(3) 
    Goal = Origin+Localframe*Goal
    Mass = Current:mass() Inertia = Current:inertia()
    Goalangle=Defaultangle:angle(Index)
    
    Force = P*(Goal-Current:pos()) + D*Current:vel()
    Force *= Mass
    Current:applyForce(Force)
    
    Current:applyOffsetForce(Fw*OriP,Current:pos() + Inertia:x()*Current:forward())
    Current:applyOffsetForce(-Fw*OriP,Current:pos() - Inertia:x()*Current:forward())
    Current:applyOffsetForce(Up*OriP,Current:pos() + Inertia:z()*Current:up())    
    Current:applyOffsetForce(-Up*OriP,Current:pos() - Inertia:z()*Current:up())    
    Current:applyAngForce(OriD*Current:angVel()*shiftL(ang(Inertia)))
    
    Index++
    
    Current=Bodyparts:entity(Index) Matrix = Positionarray:shiftMatrix()
    Fw = Localframe*Matrix:column(1) Up = Localframe*Matrix:column(2) Goal = Matrix:column(3) 
    Goal = Origin+Localframe*Goal
    Mass = Current:mass() Inertia = Current:inertia()
    Goalangle=Defaultangle:angle(Index)
    
    Force = P*(Goal-Current:pos()) + D*Current:vel()
    Force *= Mass
    Current:applyForce(Force)
    
    Current:applyOffsetForce(Fw*OriP,Current:pos() + Inertia:x()*Current:forward())
    Current:applyOffsetForce(-Fw*OriP,Current:pos() - Inertia:x()*Current:forward())
    Current:applyOffsetForce(Up*OriP,Current:pos() + Inertia:z()*Current:up())    
    Current:applyOffsetForce(-Up*OriP,Current:pos() - Inertia:z()*Current:up())    
    Current:applyAngForce(OriD*Current:angVel()*shiftL(ang(Inertia)))
    
    Index++
    
    Current=Bodyparts:entity(Index) Matrix = Positionarray:shiftMatrix()
    Fw = Localframe*Matrix:column(1) Up = Localframe*Matrix:column(2) Goal = Matrix:column(3) 
    Goal = Origin+Localframe*Goal
    Mass = Current:mass() Inertia = Current:inertia()
    Goalangle=Defaultangle:angle(Index)
    
    Force = P*(Goal-Current:pos()) + D*Current:vel()
    Force *= Mass
    Current:applyForce(Force)
    
    Current:applyOffsetForce(Fw*OriP,Current:pos() + Inertia:x()*Current:forward())
    Current:applyOffsetForce(-Fw*OriP,Current:pos() - Inertia:x()*Current:forward())
    Current:applyOffsetForce(Up*OriP,Current:pos() + Inertia:z()*Current:up())    
    Current:applyOffsetForce(-Up*OriP,Current:pos() - Inertia:z()*Current:up())    
    Current:applyAngForce(OriD*Current:angVel()*shiftL(ang(Inertia)))
    
    Index++
    
    Current=Bodyparts:entity(Index) Matrix = Positionarray:shiftMatrix()
    Fw = Localframe*Matrix:column(1) Up = Localframe*Matrix:column(2) Goal = Matrix:column(3) 
    Goal = Origin+Localframe*Goal
    Mass = Current:mass() Inertia = Current:inertia()
    Goalangle=Defaultangle:angle(Index)
    
    Force = P*(Goal-Current:pos()) + D*Current:vel()
    Force *= Mass
    Current:applyForce(Force)
    
    Current:applyOffsetForce(Fw*OriP,Current:pos() + Inertia:x()*Current:forward())
    Current:applyOffsetForce(-Fw*OriP,Current:pos() - Inertia:x()*Current:forward())
    Current:applyOffsetForce(Up*OriP,Current:pos() + Inertia:z()*Current:up())    
    Current:applyOffsetForce(-Up*OriP,Current:pos() - Inertia:z()*Current:up())    
    Current:applyAngForce(OriD*Current:angVel()*shiftL(ang(Inertia)))
    
    Index++
    
    Current=Bodyparts:entity(Index) Matrix = Positionarray:shiftMatrix()
    Fw = Localframe*Matrix:column(1) Up = Localframe*Matrix:column(2) Goal = Matrix:column(3) 
    Goal = Origin+Localframe*Goal
    Mass = Current:mass() Inertia = Current:inertia()
    Goalangle=Defaultangle:angle(Index)
    
    Force = P*(Goal-Current:pos()) + D*Current:vel()
    Force *= Mass
    Current:applyForce(Force)
    
    Current:applyOffsetForce(Fw*OriP,Current:pos() + Inertia:x()*Current:forward())
    Current:applyOffsetForce(-Fw*OriP,Current:pos() - Inertia:x()*Current:forward())
    Current:applyOffsetForce(Up*OriP,Current:pos() + Inertia:z()*Current:up())    
    Current:applyOffsetForce(-Up*OriP,Current:pos() - Inertia:z()*Current:up())    
    Current:applyAngForce(OriD*Current:angVel()*shiftL(ang(Inertia)))
    
    Index++
    
    Current=Bodyparts:entity(Index) Matrix = Positionarray:shiftMatrix()
    Fw = Localframe*Matrix:column(1) Up = Localframe*Matrix:column(2) Goal = Matrix:column(3) 
    Goal = Origin+Localframe*Goal
    Mass = Current:mass() Inertia = Current:inertia()
    Goalangle=Defaultangle:angle(Index)
    
    Force = P*(Goal-Current:pos()) + D*Current:vel()
    Force *= Mass
    Current:applyForce(Force)
    
    Current:applyOffsetForce(Fw*OriP,Current:pos() + Inertia:x()*Current:forward())
    Current:applyOffsetForce(-Fw*OriP,Current:pos() - Inertia:x()*Current:forward())
    Current:applyOffsetForce(Up*OriP,Current:pos() + Inertia:z()*Current:up())    
    Current:applyOffsetForce(-Up*OriP,Current:pos() - Inertia:z()*Current:up())    
    Current:applyAngForce(OriD*Current:angVel()*shiftL(ang(Inertia)))
    
    Index++
    
}
