Physics
Trajectory.cs
// LICENSE
//
// This software is dual-licensed to the public domain and under the following
// license: you are granted a perpetual, irrevocable license to copy, modify,
// publish, and distribute this file as you see fit.
//
// VERSION
// 0.1.0 (2016-06-01) Initial release
//
// AUTHOR
// Forrest Smith
//
// ADDITIONAL READING
// https://blog.forrestthewoods.com/solving-ballistic-trajectories-b0165523348c
//
// API
// int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target, float gravity, out Vector3 low, out Vector3 high);
// int solve_ballistic_arc(Vector3 proj_pos, float proj_speed, Vector3 target, Vector3 target_velocity, float gravity, out Vector3 s0, out Vector3 s1, out Vector3 s2, out Vector3 s3);
// bool solve_ballistic_arc_lateral(Vector3 proj_pos, float lateral_speed, Vector3 target, float max_height, out float vertical_speed, out float gravity);
// bool solve_ballistic_arc_lateral(Vector3 proj_pos, float lateral_speed, Vector3 target, Vector3 target_velocity, float max_height_offset, out Vector3 fire_velocity, out float gravity, out Vector3 impact_point);
//
// float ballistic_range(float speed, float gravity, float initial_height);
//
// bool IsZero(double d);
// int SolveQuadric(double c0, double c1, double c2, out double s0, out double s1);
// int SolveCubic(double c0, double c1, double c2, double c3, out double s0, out double s1, out double s2);
// int SolveQuartic(double c0, double c1, double c2, double c3, double c4, out double s0, out double s1, out double s2, out double s3);
using System;
using System.Diagnostics;
using System.Numerics;
namespace ACE.Server.Physics
{
public clast Trajectory
{
// SolveQuadric, SolveCubic, and SolveQuartic were ported from C as written for Graphics Gems I
// Original Author: Jochen Schwarze ([email protected])
// https://github.com/erich666/GraphicsGems/blob/240a34f2ad3fa577ef57be74920db6c4b00605e4/gems/Roots3And4.c
// Utility function used by SolveQuadratic, SolveCubic, and SolveQuartic
public static bool IsZero(double d)
{
const double eps = 1e-9;
return d > -eps && d < eps;
}
// Solve quadratic equation: c0*x^2 + c1*x + c2.
// Returns number of solutions.
public static int SolveQuadric(double c0, double c1, double c2, out double s0, out double s1)
{
s0 = double.NaN;
s1 = double.NaN;
double p, q, D;
/* normal form: x^2 + px + q = 0 */
p = c1 / (2 * c0);
q = c2 / c0;
D = p * p - q;
if (IsZero(D))
{
s0 = -p;
return 1;
}
else if (D < 0)
{
return 0;
}
else /* if (D > 0) */
{
double sqrt_D = Math.Sqrt(D);
s0 = sqrt_D - p;
s1 = -sqrt_D - p;
return 2;
}
}
// Solve cubic equation: c0*x^3 + c1*x^2 + c2*x + c3.
// Returns number of solutions.
public static int SolveCubic(double c0, double c1, double c2, double c3, out double s0, out double s1, out double s2)
{
s0 = double.NaN;
s1 = double.NaN;
s2 = double.NaN;
int num;
double sub;
double A, B, C;
double sq_A, p, q;
double cb_p, D;
/* normal form: x^3 + Ax^2 + Bx + C = 0 */
A = c1 / c0;
B = c2 / c0;
C = c3 / c0;
/* subssatute x = y - A/3 to eliminate quadric term: x^3 +px + q = 0 */
sq_A = A * A;
p = 1.0 / 3 * (-1.0 / 3 * sq_A + B);
q = 1.0 / 2 * (2.0 / 27 * A * sq_A - 1.0 / 3 * A * B + C);
/* use Cardano's formula */
cb_p = p * p * p;
D = q * q + cb_p;
if (IsZero(D))
{
if (IsZero(q)) /* one triple solution */
{
s0 = 0;
num = 1;
}
else /* one single and one double solution */
{
double u = Math.Pow(-q, 1.0 / 3.0);
s0 = 2 * u;
s1 = -u;
num = 2;
}
}
else if (D < 0) /* Casus irreducibilis: three real solutions */
{
double phi = 1.0 / 3 * Math.Acos(-q / Math.Sqrt(-cb_p));
double t = 2 * Math.Sqrt(-p);
s0 = t * Math.Cos(phi);
s1 = -t * Math.Cos(phi + Math.PI / 3);
s2 = -t * Math.Cos(phi - Math.PI / 3);
num = 3;
}
else /* one real solution */
{
double sqrt_D = Math.Sqrt(D);
double u = Math.Pow(sqrt_D - q, 1.0 / 3.0);
double v = -Math.Pow(sqrt_D + q, 1.0 / 3.0);
s0 = u + v;
num = 1;
}
/* resubssatute */
sub = 1.0 / 3 * A;
if (num > 0) s0 -= sub;
if (num > 1) s1 -= sub;
if (num > 2) s2 -= sub;
return num;
}
// Solve quartic function: c0*x^4 + c1*x^3 + c2*x^2 + c3*x + c4.
// Returns number of solutions.
public static int SolveQuartic(double c0, double c1, double c2, double c3, double c4, out double s0, out double s1, out double s2, out double s3)
{
s0 = double.NaN;
s1 = double.NaN;
s2 = double.NaN;
s3 = double.NaN;
double[] coeffs = new double[4];
double z, u, v, sub;
double A, B, C, D;
double sq_A, p, q, r;
int num;
/* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
A = c1 / c0;
B = c2 / c0;
C = c3 / c0;
D = c4 / c0;
/* subssatute x = y - A/4 to eliminate cubic term: x^4 + px^2 + qx + r = 0 */
sq_A = A * A;
p = -3.0 / 8 * sq_A + B;
q = 1.0 / 8 * sq_A * A - 1.0 / 2 * A * B + C;
r = -3.0 / 256 * sq_A * sq_A + 1.0 / 16 * sq_A * B - 1.0 / 4 * A * C + D;
if (IsZero(r))
{
/* no absolute term: y(y^3 + py + q) = 0 */
coeffs[3] = q;
coeffs[2] = p;
coeffs[1] = 0;
coeffs[0] = 1;
num = SolveCubic(coeffs[0], coeffs[1], coeffs[2], coeffs[3], out s0, out s1, out s2);
}
else
{
/* solve the resolvent cubic ... */
coeffs[3] = 1.0 / 2 * r * p - 1.0 / 8 * q * q;
coeffs[2] = -r;
coeffs[1] = -1.0 / 2 * p;
coeffs[0] = 1;
SolveCubic(coeffs[0], coeffs[1], coeffs[2], coeffs[3], out s0, out s1, out s2);
/* ... and take the one real solution ... */
z = s0;
/* ... to build two quadric equations */
u = z * z - r;
v = 2 * z - p;
if (IsZero(u))
u = 0;
else if (u > 0)
u = Math.Sqrt(u);
else
return 0;
if (IsZero(v))
v = 0;
else if (v > 0)
v = Math.Sqrt(v);
else
return 0;
coeffs[2] = z - u;
coeffs[1] = q < 0 ? -v : v;
coeffs[0] = 1;
num = SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s0, out s1);
coeffs[2] = z + u;
coeffs[1] = q < 0 ? v : -v;
coeffs[0] = 1;
if (num == 0) num += SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s0, out s1);
if (num == 1) num += SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s1, out s2);
if (num == 2) num += SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s2, out s3);
}
/* resubssatute */
sub = 1.0 / 4 * A;
if (num > 0) s0 -= sub;
if (num > 1) s1 -= sub;
if (num > 2) s2 -= sub;
if (num > 3) s3 -= sub;
return num;
}
// Calculate the maximum range that a ballistic projectile can be fired on given speed and gravity.
//
// speed (float): projectile velocity
// gravity (float): force of gravity, positive is down
// initial_height (float): distance above flat terrain
//
// return (float): maximum range
public static float ballistic_range(float speed, float gravity, float initial_height)
{
// Handling these cases is up to your project's coding standards
//Debug.astert(speed > 0 && gravity > 0 && initial_height >= 0, "fts.ballistic_range called with invalid data");
if (speed 0, "fts.solve_ballistic_arc called with invalid data");
velocityVector = Vector3.Zero;
time = float.NaN;
if (projectilePosition == targetPosition || lateralSpeed 0, "fts.solve_ballistic_arc_lateral called with invalid data");
// Initialize output variables
fire_velocity = Vector3.Zero;
time = 0.0f;
impact_point = Vector3.Zero;
if (proj_pos == target || lateral_speed 0 && t0 > 0;
bool valid1 = n > 1 && t1 > 0;
float t;
if (!valid0 && !valid1)
return false;
else if (valid0 && valid1)
t = Math.Min((float)t0, (float)t1);
else
t = valid0 ? (float)t0 : (float)t1;
// Calculate impact point
impact_point = target + (target_velocity * t);
// Calculate fire velocity along XZ plane
Vector3 dir = impact_point - proj_pos;
fire_velocity = Vector3.Normalize(new Vector3(dir.X, dir.Y, 0f)) * lateral_speed;
// Solve system of equations. Hit max_height at t=.5*time. Hit target at t=time.
//
// peak = z0 + vertical_speed*halfTime + .5*gravity*halfTime^2
// end = z0 + 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.Z; // initial
//float b = Math.Max(proj_pos.Z, impact_point.Z) + max_height_offset; // peak
float c = impact_point.Z; // final
//gravity = -4 * (a - 2 * b + c) / (t * t);
//fire_velocity.Z = -(3 * a - 4 * b + c) / t;
var g = gravity;
var b = (4 * a + 4 * c - g * t * t) / 8;
fire_velocity.Z = (2 * a - 2 * c + g * t * t) / (t * 2) * -1;
time = t;
return true;
}
}
}
