| 42 | using namespace std; |
| 43 | |
| 44 | location::GpsInfo LinearExtrapolation(location::GpsInfo const & gpsInfo1, location::GpsInfo const & gpsInfo2, |
| 45 | uint64_t timeAfterPoint2Ms) |
| 46 | { |
| 47 | if (gpsInfo2.m_timestamp <= gpsInfo1.m_timestamp) |
| 48 | { |
| 49 | ASSERT(false, ("Incorrect gps data")); |
| 50 | return gpsInfo2; |
| 51 | } |
| 52 | |
| 53 | auto const timeBetweenPointsMs = static_cast<uint64_t>((gpsInfo2.m_timestamp - gpsInfo1.m_timestamp) * 1000); |
| 54 | if (timeBetweenPointsMs == 0) |
| 55 | return gpsInfo2; |
| 56 | |
| 57 | location::GpsInfo result = gpsInfo2; |
| 58 | LinearExtrapolator const e(timeBetweenPointsMs, timeAfterPoint2Ms); |
| 59 | |
| 60 | result.m_timestamp += static_cast<double>(timeAfterPoint2Ms) / 1000.0; |
| 61 | result.m_longitude = math::Clamp(e.Extrapolate(gpsInfo1.m_longitude, gpsInfo2.m_longitude), -180.0, 180.0); |
| 62 | result.m_latitude = math::Clamp(e.Extrapolate(gpsInfo1.m_latitude, gpsInfo2.m_latitude), -90.0, 90.0); |
| 63 | result.m_altitude = e.Extrapolate(gpsInfo1.m_altitude, gpsInfo2.m_altitude); |
| 64 | |
| 65 | // @TODO(bykoianko) Now |result.m_bearing| == |gpsInfo2.m_bearing|. |
| 66 | // In case of |gpsInfo1.HasBearing() && gpsInfo2.HasBearing() == true| |
| 67 | // consider finding an average value between |gpsInfo1.m_bearing| and |gpsInfo2.m_bearing| |
| 68 | // taking into account that they are periodic. It's important to implement it |
| 69 | // because current implementation leads to changing course by steps. It doesn't |
| 70 | // look nice when the road changes its direction. |
| 71 | |
| 72 | if (gpsInfo1.HasSpeed() && gpsInfo2.HasSpeed()) |
| 73 | result.m_speed = e.Extrapolate(gpsInfo1.m_speed, gpsInfo2.m_speed); |
| 74 | |
| 75 | return result; |
| 76 | } |
| 77 | |
| 78 | bool AreCoordsGoodForExtrapolation(location::GpsInfo const & info1, location::GpsInfo const & info2) |
| 79 | { |