Projectile Motion Trajectory Prediction: Use the Newton's Kinematic formula to find the the projectile distance covered and the time taken

Physics.cpp

struct QueryResult
  {
    QueryResult(): m_HitPos(0.0f), m_ValidHit(false) {}

    Vec3 m_HitPos;
    bool m_ValidHit;
  };

  QueryResult Raycast( const Vec3& p0, const Vec3& p1 );

struct Plane
  {
    Plane() : normal( 0.0f ), d( 0.0f ) {}
    Plane( float a, float b, float c, float pd ) : normal( a, b, c ), d( pd ) {}
    Plane( Vec3 pnormal, float pd ) : normal( pnormal ), d( pd ) {}

    Vec3 normal;
    float d;
  };

  void ResetPlanes();
  void AddPlane( const Plane& plane );
  void PrintPlanes();

ProjectileMotion.cpp

struct TrajectoryResult
{
  Vec3 m_EndPoint;
  float m_Time;
  bool  m_ValidHit;
};

Vec3 getNextPositionInTrajectory(const Vec3& position, const Vec3& velocity, const Vec3& up_vector, float gravity_accel, float time) {
    return Vec3(
        position.x + velocity.x * time + 0.5f * (up_vector.x * gravity_accel * time * time),
        position.y + velocity.y * time + 0.5f * (up_vector.y * gravity_accel * time * time),
        position.z + velocity.z * time + 0.5f * (up_vector.z * gravity_accel * time * time)
    );
}

Vec3 getNextVelocityInTrajectory(const Vec3& velocity, const Vec3& up_vector, float gravity_accel, float time) {
    return Vec3(
        velocity.x + up_vector.x * gravity_accel * time,
        velocity.y + up_vector.y * gravity_accel * time,
        velocity.z + up_vector.z * gravity_accel * time
    );
}

float solveQuadraticEquationForTime(float a, float b, float c) {
    float r1 = (-b + Sqrtf(b * b - (4.0f * a * c))) / (2 * a);
    float r2 = (-b - Sqrtf(b * b - (4.0f * a * c))) / (2 * a);

    // Debug("Quadartic Sol: %f %f", r1, r2);
    if (r1 > 0 && r2 > 0) {
        return Minf(r1, r2);
    }
    else {
        return Maxf(r1, r2);
    }
}

float getEuclideanDistance(const Vec3& v1, const Vec3& v2) {
    return Sqrtf((v2.x - v1.x) * (v2.x - v1.x) + (v2.y - v1.y) * (v2.y - v1.y) + (v2.z - v1.z) * (v2.z - v1.z));
}

TrajectoryResult PredictTrajectory(const Vec3& start_position, const Vec3& start_velocity, const Vec3& up_vector, float gravity_accel, float raycast_time_step, float max_time)
{
    float delta = raycast_time_step;

    TrajectoryResult result;
    result.m_ValidHit = false;
    result.m_Time = max_time;

    Physics::QueryResult rayResult;

    float elapsedTime = delta;
    Vec3 lastPosition;
    Vec3 position = start_position;
    Vec3 lastVelocity;
    Vec3 velocity = start_velocity;

    while (elapsedTime < max_time) {
        lastPosition = position;
        lastVelocity = velocity;

        position = getNextPositionInTrajectory(position, velocity, up_vector, gravity_accel, delta);
        velocity = getNextVelocityInTrajectory(velocity, up_vector, gravity_accel, delta);

        // Collision Detection
        rayResult = Physics::Raycast(lastPosition, position);
        if (rayResult.m_ValidHit) {
            position = rayResult.m_HitPos;

            if (raycast_time_step == delta) {
                elapsedTime -= raycast_time_step;
            }

            // Debug("%f", elapsedTime);
            float t = 0.0f;
            if (!IsEqualf(getEuclideanDistance(lastPosition, position), 0.0f, kZeroTolTime)) {
                t = solveQuadraticEquationForTime(0.5f * gravity_accel * up_vector.y, lastVelocity.y, lastPosition.y - position.y);
            }

            result.m_Time = elapsedTime + t;
            result.m_ValidHit = true;

            break;
        }

        // Step time correction at last iteration
        elapsedTime += delta;
        if (elapsedTime > max_time) {
            elapsedTime -= delta;
            delta = max_time - elapsedTime;
        }
    }// end of while

    result.m_EndPoint = position;

    return result;
}

Vec3.cpp

class Vec3
{
public:
  // Data Members
  float x;
  float y;
  float z;

  // Default Constructor
  inline Vec3() = default;

  // Explicit Constructors
  constexpr explicit Vec3( float f ) : x( f ), y( f ), z( f ) {}
  constexpr explicit Vec3( const float* f ) : x( f[0] ), y( f[1] ), z( f[2] ) {}
  constexpr          Vec3( float vx, float vy, float vz ) : x( vx ), y( vy ), z( vz ) {}
  constexpr          Vec3( const Vec3& v ) = default;

  // Arithmetic Operators
  inline Vec3     operator+ ( const float f ) const { return Vec3( x + f, y + f, z + f ); }
  constexpr Vec3  operator+ ( const Vec3& v ) const { return Vec3( x + v.x, y + v.y, z + v.z ); }

  inline Vec3     operator- ( const float f ) const { return Vec3( x - f, y - f, z - f ); }
  constexpr Vec3  operator- ( const Vec3& v ) const { return Vec3( x - v.x, y - v.y, z - v.z ); }

  constexpr Vec3  operator* ( const float f ) const { return Vec3( x * f, y * f, z * f ); }
  inline Vec3     operator* ( const Vec3& v ) const { return Vec3( x * v.x, y * v.y, z * v.z ); }

  inline Vec3     operator/ ( const float f ) const { float inv_f = 1.0f / f; return Vec3( x * inv_f, y * inv_f, z * inv_f ); }
  inline Vec3     operator/ ( const Vec3& v ) const { return Vec3( x / v.x, y / v.y, z / v.z ); }

  // Comparison Operators
  inline bool     operator== ( const Vec3& v ) const { return (x == v.x) && (y == v.y) && (z == v.z); }
  inline bool     operator!= ( const Vec3& v ) const { return (x != v.x) || (y != v.y) || (z != v.z); }

  // Set functions
  inline Vec3& Set( float vx, float vy, float vz ) { x = vx;   y = vy;   z = vz;   return *this; }
  inline Vec3& SetToZero() { x = 0.0f; y = 0.0f; z = 0.0f; return *this; }
  inline Vec3& SetToUnit() { x = 1.0f; y = 1.0f; z = 1.0f; return *this; }
  inline Vec3& SetToXAxis() { x = 1.0f; y = 0.0f; z = 0.0f; return *this; }
  inline Vec3& SetToYAxis() { x = 0.0f; y = 1.0f; z = 0.0f; return *this; }
  inline Vec3& SetToZAxis() { x = 0.0f; y = 0.0f; z = 1.0f; return *this; }
};