// Solve the firing arc with a fixed lateral speed. Vertical speed and gravity varies.
// This enables a visually pleasing arc.
//
// proj_pos (Vector3): point projectile will fire from
// lateral_speed (float): scalar speed of projectile along XZ plane
// target_pos (Vector3): point projectile is trying to hit
// max_height (float): height above Max(proj_pos, impact_pos) for projectile to peak at
//
// fire_velocity (out Vector3): firing velocity
// gravity (out float): gravity necessary to projectile to hit precisely max_height
// impact_point (out Vector3): point where moving target will be hit
//
// return (bool): true if a valid solution was found
public static 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)
{
// Handling these cases is up to your project's coding standards
Debug.Assert(proj_pos != target && lateral_speed > 0, "fts.solve_ballistic_arc_lateral called with invalid data");
// Initialize output variables
fire_velocity = Vector3.zero;
gravity = 0f;
impact_point = Vector3.zero;
// Ground plane terms
Vector3 targetVelXZ = new Vector3(target_velocity.x, 0f, target_velocity.z);
Vector3 diffXZ = target - proj_pos;
diffXZ.y = 0;
// Derivation
// (1) Base formula: |P + V*t| = S*t
// (2) Substitute variables: |diffXZ + targetVelXZ*t| = S*t
// (3) Square both sides: Dot(diffXZ,diffXZ) + 2*Dot(diffXZ, targetVelXZ)*t + Dot(targetVelXZ, targetVelXZ)*t^2 = S^2 * t^2
// (4) Quadratic: (Dot(targetVelXZ,targetVelXZ) - S^2)t^2 + (2*Dot(diffXZ, targetVelXZ))*t + Dot(diffXZ, diffXZ) = 0
float c0 = Vector3.Dot(targetVelXZ, targetVelXZ) - lateral_speed * lateral_speed;
float c1 = 2f * Vector3.Dot(diffXZ, targetVelXZ);
float c2 = Vector3.Dot(diffXZ, diffXZ);
double t0, t1;
int n = Fts.SolveQuadric(c0, c1, c2, out t0, out t1);
// pick smallest, positive time
bool valid0 = n > 0 && t0 > 0;
bool valid1 = n > 1 && t1 > 0;
float t;
if (!valid0 && !valid1)
{
return(false);
}
else if (valid0 && valid1)
{
t = Mathf.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 = new Vector3(dir.x, 0f, dir.z).normalized *lateral_speed;
// Solve 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 = Mathf.Max(proj_pos.y, impact_point.y) + max_height_offset; // peak
float c = impact_point.y; // final
gravity = -4 * (a - 2 * b + c) / (t * t);
fire_velocity.y = -(3 * a - 4 * b + c) / t;
return(true);
}
// 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;
/* substitute 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 = Fts.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 = System.Math.Sqrt(u);
}
else
{
return(0);
}
if (IsZero(v))
{
v = 0;
}
else if (v > 0)
{
v = System.Math.Sqrt(v);
}
else
{
return(0);
}
coeffs[2] = z - u;
coeffs[1] = q < 0 ? -v : v;
coeffs[0] = 1;
num = Fts.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 += Fts.SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s0, out s1);
}
if (num == 1)
{
num += Fts.SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s1, out s2);
}
if (num == 2)
{
num += Fts.SolveQuadric(coeffs[0], coeffs[1], coeffs[2], out s2, out s3);
}
}
/* resubstitute */
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);
}
