[EDIT: I added a comment indicating where you should start reading the code. Everything before is library-style code that doesn't add much to understanding how I solve the problem.]
Well, here's some code that seems to do the job. I wrote the whole thing today and I apologize for its many shortcomings, but it should serve the purpose of illustrating how your problem can be solved.
The program is rather long, because it contains a set of functions to handle polynomials, a set of functions to handle vectors and a few extra things to be able to handle "vectors" of polynomials (not really vectors, because polynomials don't form a field, so instead of a vector space we are dealing with a module; but that probably only bothers people who know entirely too much math). By using Vector<Polynomial> I can describe the equation that gives me the time to interception symbolically, which should make the code more understandable.
Enjoy!
#include <iostream>
#include <vector>
#include <initializer_list>
#include <cmath>
#include <cassert>
#include <limits>
struct Polynomial {
std::vector<float> coefs;
float operator[](size_t i) const {
return i < coefs.size() ? coefs[i] : 0.0f;
}
template <typename It>
Polynomial(It begin, It end) : coefs(begin, end) {
}
explicit Polynomial(size_t size) : coefs(size) {
}
Polynomial(std::initializer_list<float> const &list) : coefs(list) {
}
float eval(float x) const {
float result = 0.0f;
float x_pow = 1.0f;
for (float c : coefs) {
result += c * x_pow;
x_pow *= x;
}
return result;
}
float eval_derivative(float x) const {
float result = 0.0f;
float x_pow = 1.0f;
for (size_t i = 1; i < coefs.size(); ++i) {
result += i * coefs[i] * x_pow;
x_pow *= x;
}
return result;
}
float newton_raphson(float x) const {
for (int i = 0; i < 10; ++i)
x -= eval(x) / eval_derivative(x);
return x;
}
bool find_root(float &x) const {
for (int i = 0; i < 10; ++i) {
float initial_guess = i;
x = newton_raphson(initial_guess);
if (std::abs(eval(x)) < 1.0e-4f) // I hate having a magic constant here, but I want to keep things simple
return true;
}
return false;
}
Polynomial divide_by_x_minus(float r) const {
assert(coefs.size() > 0);
Polynomial result(++coefs.begin(), coefs.end());
float carry = 0.0f;
for (size_t i = coefs.size() - 1; i > 0; --i) {
result.coefs[i - 1] = coefs[i] + carry;
carry = result.coefs[i - 1] * r;
}
return result;
}
};
Polynomial operator*(Polynomial const &p, float scalar) {
Polynomial result = p;
for (size_t i = 0; i < p.coefs.size(); ++i)
result.coefs[i] = p.coefs[i] * scalar;
return result;
}
Polynomial operator*(float scalar, Polynomial const &p) {
return p * scalar;
}
Polynomial operator+(Polynomial const &p1, Polynomial const &p2) {
size_t max_size = std::max(p1.coefs.size(), p2.coefs.size());
Polynomial result(max_size);
for (size_t i = 0; i < max_size; ++i)
result.coefs[i] = p1[i] + p2[i];
return result;
}
Polynomial operator+(Polynomial const &p, float c) {
return p + Polynomial({c});
}
Polynomial operator-(Polynomial const &p1, Polynomial const &p2) {
Polynomial result(std::max(p1.coefs.size(), p2.coefs.size()));
for (size_t i = 0; i < result.coefs.size(); ++i) {
float a = (i < p1.coefs.size()) ? p1.coefs[i] : 0.0f;
float b = (i < p2.coefs.size()) ? p2.coefs[i] : 0.0f;
result.coefs[i] = a - b;
}
return result;
}
Polynomial operator-(Polynomial const &p, float c) {
return p + Polynomial({-c});
}
Polynomial operator*(Polynomial const &p1, Polynomial const &p2) {
size_t size = p1.coefs.size() + p2.coefs.size() - 1;
Polynomial result(size);
for (size_t i = 0; i < size; ++i) {
float s = 0.0f;
for (size_t j = std::max(0, int(i) - int(p2.coefs.size()) + 1); j <= i && j < p1.coefs.size(); ++j)
s += p1.coefs[j] * p2.coefs[i - j];
result.coefs[i] = s;
}
return result;
}
float smallest_positive_root(Polynomial p) {
float root;
float result = std::numeric_limits<float>::infinity();
while (p.find_root(root)) {
if (root > 0.0f && root < result)
result = root;
p = p.divide_by_x_minus(root);
}
return result;
}
template <typename S>
struct Vector {
S x, y, z;
Vector(S x, S y, S z) : x(x), y(y), z(z) {
}
};
template <typename S>
Vector<S> operator*(S scale, Vector<S> const &v) {
return Vector<S>(scale * v.x, scale * v.y, scale * v.z);
}
template <typename S>
Vector<S> operator*(Vector<S> const &v, S scale) {
return scale * v;
}
template <typename S>
Vector<S> operator+(Vector<S> const &v1, Vector<S> const &v2) {
return Vector<S>(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
}
template <typename S>
Vector<S> operator-(Vector<S> const &v1, Vector<S> const &v2) {
return Vector<S>(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}
template <typename S>
S dot_product(Vector<S> const &v1, Vector<S> const &v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
template <typename S>
S length_squared(Vector<S> const &v) {
return dot_product(v, v);
}
Vector<Polynomial> operator*(Vector<float> const &v, Polynomial scalar) {
return Vector<Polynomial>(v.x * scalar, v.y * scalar, v.z * scalar);
}
Vector<Polynomial> operator*(Polynomial scalar, Vector<float> const &v) {
return v * scalar;
}
Vector<Polynomial> operator+(Vector<float> const &v1, Vector<Polynomial> const &v2) {
return Vector<Polynomial>(Polynomial({v1.x}) + v2.x,
Polynomial({v1.y}) + v2.y,
Polynomial({v1.z}) + v2.z);
}
Vector<Polynomial> operator+(Vector<Polynomial> const &v1, Vector<float> const &v2) {
return v2 + v1;
}
// --- START READING HERE ---
// This code assumes we are using coordinates where the shooter is at the origin and has velocity 0
float time_to_interception(Vector<float> target_position,
Vector<float> target_velocity,
Vector<float> gravity,
float initial_bullet_speed) {
Polynomial t({0.0f, 1.0f});
Polynomial equation = length_squared(0.5f * gravity * t * t - (target_position + target_velocity * t))
- initial_bullet_speed * initial_bullet_speed * t * t;
return smallest_positive_root(equation);
}
Vector<float> initial_velocity_to_reach_point_with_gravity(Vector<float> point, float t, Vector<float> gravity) {
return point * (1.0f / t) - 0.5f * gravity * t;
}
std::ostream &operator<<(std::ostream &os, Vector<float> const &v) {
return os << '(' << v.x << ',' << v.y << ',' << v.z << ')';
}
int main() {
Vector<float> y0(3.0f, 4.0f, 0.0f);
Vector<float> yv(1.0f, 0.0f, 0.0f);
Vector<float> g(0.0f, 0.0f, -9.80665f);
float s0 = 20.0f;
std::cout << "Initial target position: " << y0 << '\n';
std::cout << "Target velocity: " << yv << '\n';
std::cout << "Gravity: " << g << '\n';
std::cout << "Initial bullet speed: " << s0 << '\n';
std::cout << '\n';
float t = time_to_interception(y0, yv, g, s0);
Vector<float> interception_position = y0 + yv * t;
Vector<float> v0 = initial_velocity_to_reach_point_with_gravity(interception_position, t, g);
std::cout << "Time to interception: " << t << '\n';
std::cout << "Target will be at (" << interception_position.x << ',' << interception_position.y << ',' << interception_position.z << ")\n";
std::cout << "We should shoot the bullet with the initial velocity (" << v0.x << ',' << v0.y << ',' << v0.z << ")\n";
std::cout << "The length of that initial velocity is " << std::sqrt(length_squared(v0)) << " (sanity check) \n";
}