| double GetTimeToTargetRWithMinimalInitialSpeed(double k, double vInfinity, |
| double rX, double rY) { |
| // 1. Start by getting coefficients for the function f(t) = a4*t∧4 + a3*t∧3 |
| // + 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∧4 = a0*u∧4 + a1*u∧3 + |
| // a3*u + a4 = 0 for u where u = 1 / t, but the latter is more well-behaved, |
| // being a strictly concave function for u > 0 for any set of valid inputs, |
| // so solve e(u)=0 for u instead by converging from an upper bound towards |
| double kVInfinity = k * vInfinity, rr = rX * rX + rY * rY; // the root and |
| double a0 = −rr, a1 = a0 * k, a3 = k * kVInfinity * rY; // return 1/u. |
| double a4 = kVInfinity * kVInfinity; |
| double maxInvRelError = 1.0E6; // Use an achievable inverse error bound. |
| double de, e, uDelta = 0; |
| // 2. Set u to an upper bound by solving e(u) with a3 = a1 = 0, clamped by |
| // the result of a Newton method’s iteration at u = 0 if positive. |
| double u = std::sqrt(kVInfinity / std::sqrt(rr)); |
| if (rY < 0) u = std::min(u, −vInfinity / rY); |
| // 3. Let u monotonically converge to e(u)’s positive root using a modified |
| // Newton’s method that speeds up convergence for double roots, but is likely |
| // to overshoot eventually. Here, “e” = e(u) and “de” = de(u)/du. |
| for (int it = 0; it < 10; ++it, uDelta = e / de, u −= 1.9 * uDelta) { |
| de = a0 * u; e = de + a1; de = de + e; e = e * u; |
| de = de * u + e; e = e * u + a3; de = de * u + e; e = e * u + a4; |
| if (!(e < 0 && de < 0)) break; // Overshot the root. |
| } |
| u += 0.9 * uDelta; // Trace back to the unmodified Newton method’s output. |
| // 4. Continue to converge monotonically from the overestimated u to e(u)’s |
| // only positive root using Newton’s method. |
| for (int it = 0; uDelta * maxInvRelError > u && it < 10; ++it) { |
| de = a0 * u; e = de + a1; de = de + e; e = e * u; |
| de = de * u + e; e = e * u + a3; de = de * u + e; e = e * u + a4; |
| uDelta = e / de; u −= uDelta; |
| } |
| // 5. Return the solved time t to hit [rX, rY], or 0 if no solution exists. |
| return u > 0 1 / u : 0; |
| } |
