double GetTimeToTargetRGivenInitialSpeedS(double k, double vInfinity, double rX, |

* * double rY, double s, bool highArc) { |

// 1. Start by getting coefficients for the function f(t) = a4*t*∧*4 + a3*t*∧*3 |

// + a2*t*∧*2 + a1*t + a0 which is 0 at the sought time-to-target t. Solving |

// f(t) = 0 for t > 0 is equivalent to solving e(u) = f(1/u)*u*∧*3 = a0*u*∧*3 + |

// a1*u*∧*2 + a2*u + a3 + a4/u for u where u = 1 / t, but the latter is more |

// well-behaved, being a strictly convex function for u > 0 for any set of |

// inputs iff a solution exists, so solve for e(u) = 0 instead by converging |

// from a high or low bound towards the closest root and return 1/u. |

double kRX = k * rX, kRY = k * rY, kRXSq = kRX * kRX, sS = s * s; |

double twoKVInfinityRY = vInfinity * (kRY + kRY), kVInfinity = k * vInfinity; |

double a0 = rX * rX + rY * rY, a1 = (k + k) * a0; |

double a2 = kRXSq + kRY * kRY + twoKVInfinityRY − sS; |

double a3 = twoKVInfinityRY * k, a4 = kVInfinity * kVInfinity; |

double maxInvRelError = 1.0E6; // Use an achievable inverse error bound. |

double maxV0YSq = sS − kRXSq;// maxV0YSq is the max squared “V0.y” that leaves |

double e, de, u, uDelta = 0; // enough “V0.x” to reach rX horizontally. |

// 2. Set u to a lower/upper bound for the high/low arc, respectively. |

if (highArc) { // Get smallest u vertically moving rY at max possible +v0.y. |

double minusB = std::sqrt(maxV0YSq) − kRY; |

double determ = minusB * minusB − (twoKVInfinityRY + twoKVInfinityRY); |

u = (kVInfinity + kVInfinity) / (minusB + std::sqrt(determ)); |

maxInvRelError = −maxInvRelError; // Convergence over negative slopes. |

} else if (rY < 0) { // Get largest u vertically moving rY at most neg. v0.y. |

double minusB = −std::sqrt(maxV0YSq) − kRY; |

double determ = minusB * minusB − (twoKVInfinityRY + twoKVInfinityRY); |

u = (minusB − std::sqrt(determ)) / (rY + rY); |

// Clamp the above bound by the largest u that reaches rX horizontally. |

u = std::min(s / rX − k, u); |

} else u = s / std::sqrt(a0) − k; // Get the (largest) u hitting rX |

// horizontally a.s.a.p. while launching in the direction of [rX,rY]. |

// 3. Let u monotonically converge to e(u)'s closest root using a modified |

// Newton's method, almost scaling the delta as if the solution is a double |

int it = 0; // root. Note that “e” = e(u) * u*∧*2 and “de” = de(u)/du * u*∧*2. |

for (; it < 12; ++it, uDelta = e / de, u −= 1.9 * uDelta) { |

de = a0 * u; e = de + a1; de = de + e; e = e * u + a2; de = de * u + e; |

e = e * u + a3; e = (e * u + a4) * u; de = de * u * u − a4; |

if (!(u > 0 && de * maxInvRelError > 0 && e > 0)) break; // Overshot. |

} |

u += 0.9 * uDelta; // Trace back to unmodified Newton method's output. |

// 4. Continue to converge monotonically to e(u)'s closest root using |

// Newton's method from the last known conservative estimate on the convex |

// function. (Note that in practice, u will have converged enough in <12 |

for (; u > 0 && it < 12; ++it) { // iterations iff a solution does exists.) |

de = a0 * u; e = de + a1; de = de + e; e = e * u + a2; de = de * u + e; |

e = e * u + a3; e = (e * u + a4) * u; de = de * u * u − a4; |

uDelta = e / de; u −= uDelta; |

if (!(de * maxInvRelError > 0)) break; // Wrong side of the convex “dip”. |

if (uDelta * maxInvRelError < u && u > 0) return 1 / u; // 5a. Found it! |

} |

// 5b. If no solution was found, return 0. This only happens if s (minus |

// a small epsilon) is too small to have a solution, the target is at the |

return 0; // origin, or the parameters are so extreme they cause overflows. |

} |