| 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. |
| } |
