Solving Ballistic Trajectories

ballistic.h

/*
  C/C++ reimplementation of the code.
  Read: https://www.forrestthewoods.com/blog/solving_ballistic_trajectories/
  Source: https://github.com/forrestthewoods/lib_fts/blob/master/code/fts_ballistic_trajectory.cs
  Dependencies: https://github.com/Microsoft/DirectXTK/wiki/SimpleMath
  
  HPlass (hermann.plass@gmail.com) - 2020
*/
bool solve_ballistic_arc_lateral(const VEC3 proj_pos, const float lateral_speed, const VEC3 target_pos, const float max_height, VEC3& fire_velocity, float& gravity)
{
  // Handling these cases is up to your project's coding standards
  assert(proj_pos != target_pos && lateral_speed > 0 && max_height > proj_pos.y && "fts.solve_ballistic_arc called with invalid data");

  fire_velocity = VEC3::Zero;
  gravity = NAN; //force trash here for some reason? I suppose?

  VEC3 diff = target_pos - proj_pos;
  VEC3 diffXZ = VEC3(diff.x, 0.0f, diff.z);
  float lateralDist = diffXZ.Length();

  if (lateralDist == 0) {
    return false;
  }

  diffXZ.Normalize();
  float time = lateralDist / lateral_speed;

  fire_velocity = diffXZ * lateral_speed;

  // System of equations. Hit max_height at t=.5*time. Hit target at t=time.
  //
  // peak = y0 + vertical_speed*halfTime + .5*gravity*halfTime^2
  // end = y0 + vertical_speed*time + .5*gravity*time^s
  // Wolfram Alpha: solve b = a + .5*v*t + .5*g*(.5*t)^2, c = a + vt + .5*g*t^2 for g, v
  float a = proj_pos.y;       // initial
  float b = max_height;       // peak
  float c = target_pos.y;     // final

  gravity = -4 * (a - 2 * b + c) / (time * time);
  fire_velocity.y = -(3 * a - 4 * b + c) / time;

  return true;
}