-
Notifications
You must be signed in to change notification settings - Fork 0
/
prediction.cpp
56 lines (48 loc) · 1.68 KB
/
prediction.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
#include "prediction.h"
namespace prediction
{
auto Predict(const Vector3& src, const Vector3& dst, const Vector3& dst_velocity, const float missile_speed, const float start_time) -> std::optional<Vector3>
{
//Given source position, target position, target velocity and missile speed
//if missile can collide with target, returns collision position
//otherwise returns std::nullopt
//
const long double a = dst_velocity.x * dst_velocity.x + dst_velocity.z * dst_velocity.z - missile_speed * missile_speed;
const long double b = 2 * (dst.x * dst_velocity.x - src.x * dst_velocity.x + dst.z * dst_velocity.z - src.z * dst_velocity.z - (missile_speed * missile_speed * start_time));
const long double c = dst.x * dst.x + dst.z * dst.z + src.x * src.x + src.z * src.z - 2 * (dst.x * src.x + dst.z * src.z) - (missile_speed * missile_speed * start_time * start_time);
std::optional<long double> x1, x2;
//If has solution
//
if (math::SolveQuadratic(a, b, c, x1, x2))
{
double t = 0.0;
//If only one solution
//
if (x1.has_value() xor x2.has_value())
{
//t = solution
//
t = x1.has_value() ? x1.value() : x2.value();
}
//If two solutions or infinite solutions
//
else
{
//t = 0.0 when infinite solutions
//t = min positive solution when two solutions
//
t = std::min(x1.value_or(0.0), x2.value_or(0.0));
if (t < 0)
{
t = std::max(x1.value_or(0.0), x2.value_or(0.0));
}
}
if (t >= 0)
{
std::optional<Vector3> interception{ { dst.x + dst_velocity.x * t, 0, dst.z + dst_velocity.z * t} };
return interception;
}
}
return std::nullopt;
}
}