TEST E22

@name PN Guidance → Vector Thruster (safe zero-range)
@inputs N:number TargetPos:vector Mass:number
@persist LastTime LastPos LastVel
@outputs OutForce:vector

interval(1)

# 0) Default to zero force every tick
OutForce = vec(0,0,0)

# 1) Get time and Δt
Time = clk()
if (LastTime == 0) {
    # first tick: init persists
    LastTime = Time
    LastPos  = entity():pos()
    LastVel  = vec(0,0,0)
}
Dt = Time - LastTime
LastTime = Time

# 2) Only proceed if Δt positive
if (Dt > 0) {
    # 3) Compute LOS
    Pos   = entity():pos()
    R     = TargetPos - Pos
    Range = R:length()

    # 4) Only proceed if range positive
    if (Range > 0) {
        U    = R / Range

        # 5) Compute your own velocity and relative velocity
        Vel      = (Pos - LastPos) / Dt
        Vrel     = Vel - LastVel

        # 6) PN guidance law
        Vc       = -Vrel:dot(U)
        UDot     = (Vrel - U*(Vrel:dot(U))) / Range
        ACmd     = UDot * (N * Vc)

        # 7) Output force = m·a
        OutForce = ACmd * Mass

        # 8) Persist for next tick
        LastVel = Vel
    }
    LastPos = Pos
}