Velocity Prediction::PredictVelocity(CorrelationAircraft *aircraft, bool is_relative) {
Snapshot *last = _history->GetLast();
Snapshot *second_to_last = _history->GetSecondToLast();
CorrelationAircraft compare;
SphericalCoordinate pos1;
SphericalCoordinate pos2;
Velocity predicted;
if (is_relative && !second_to_last->GetAircraft()->empty()) {
// For second to last snapshot
for (int i = 0; i < second_to_last->GetAircraft()->size(); i++) {
if (second_to_last->GetAircraft()->at(i).GetTcasID().Get() == aircraft->GetTcasID().Get()) {
compare = second_to_last->GetAircraft()->at(i);
pos1 = compare.GetSphericalCoordinate();
}
}
// For last snapshot
for (int i = 0; i < last->GetAircraft()->size(); i++) {
if (last->GetAircraft()->at(i).GetTcasID().Get() == aircraft->GetTcasID().Get()) {
compare = last->GetAircraft()->at(i);
pos2 = compare.GetSphericalCoordinate();
}
}
// Vector of aircraft from second to last snapshot
Saas_Util::Vector<double, 3> vector1;
vector1.x = pos1.GetRange() * sin(pos1.GetAzimuth()) * cos(pos1.GetElevation());
vector1.y = pos1.GetRange() * cos(pos1.GetAzimuth()) * cos(pos1.GetElevation());
vector1.z = -1 * pos1.GetRange() * sin(pos1.GetElevation());
// Vector of aircraft from last snapshot
Saas_Util::Vector<double, 3> vector2;
vector1.x = pos2.GetRange() * sin(pos2.GetAzimuth()) * cos(pos1.GetElevation());
vector1.y = pos2.GetRange() * cos(pos2.GetAzimuth()) * cos(pos1.GetElevation());
vector1.z = -1 * pos2.GetRange() * sin(pos2.GetElevation());
double time_diff = difftime(compare.GetTime(), aircraft->GetTime()); // seconds
double east = (vector2.x - vector1.x) / time_diff; // x
double north = (vector2.y - vector1.y) / time_diff; // y
double down = (vector2.z - vector1.z) / time_diff; // z
// geodetic to cartesian
predicted = Velocity(east, down, north);
}
return predicted;
}
