jselstad

    // This function evaluates points along a bezier curve given by a starting position and velocity, and an ending position and velocity
    // This can be thought of as interpolating between two linear trajectories
    public static Vector3 cubicBezier(Vector3 aStart, Vector3 aVel, Vector3 bEnd, Vector3 bVel, float duration, float time) {
      float alpha = time / duration; //Normalize the time
      Vector3 aEnd = aStart + (aVel * duration); //Where it would end up if it were just A's dynamics
      Vector3 bStart = bEnd - (bVel * duration); //Where it would start if it were just B's dynamics

      return
        (Mathf.Pow(1 - alpha, 3f) * aStart) +
        (3f * Mathf.Pow(1 - alpha, 2f) * alpha * aEnd) +