Heun’s Integrator with Adjustable Step Size

def Acceleration(Pos, Gravitators):
  {Calculate net acceration Acc}
  return Acc

def Integrate(Pos, Velocity, Duration, InitStepSize, Tolerance):

  TimeRemaining = Duration

  h = InitStepSize
  NextH = h
  P = Pos
  V = Velocity

  while TimeRemaining > 0.0:

    # Euler method

    A = Acceleration(P)
    PE = P + h * V
    VE = V + h * A

    # Heun's improvement

    P = P + h * 0.5 * (V + VE)
    V = V + h * 0.5 * (A + Acceleration(PE)

    # Adjust the delta time required by considering the
    # Long term error caused by Euler's method.
    LTE = VLength(PE - P) + Duration * VLength(VE - V)
    NextH = h * sqrt(Tolerance / LTE) # Might want to protect from overflow
    TimeRemaining -= h
    h = min(NextH, TimeRemaining)

  return P, V