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 "routing/checkpoint_predictor.hpp"
#include "geometry/mercator.hpp"
#include "base/stl_helpers.hpp"
#include <limits>
namespace routing
{
using namespace std;
// static
double CheckpointPredictor::CalculateDeltaMeters(m2::PointD const & from, m2::PointD const & to,
m2::PointD const & between)
{
double const directDist = MercatorBounds::DistanceOnEarth(from, to);
double const distThroughPoint = MercatorBounds::DistanceOnEarth(from, between) +
MercatorBounds::DistanceOnEarth(between, to);
return distThroughPoint - directDist;
}
size_t CheckpointPredictor::PredictPosition(vector<m2::PointD> const & points,
m2::PointD const & point) const
{
double constexpr kInvalidDistance = numeric_limits<double>::max();
double minDeltaMeters = kInvalidDistance;
size_t minDeltaIdx = 0;
// Checkpoints include start, all the intermediate points and finish.
size_t const checkpointNum = points.size() + 2 /* for start and finish points */;
for (size_t i = 0; i + 1 != checkpointNum; ++i)
{
double const delta = CalculateDeltaMeters(GetCheckpoint(points, i),
GetCheckpoint(points, i + 1), point);
if (minDeltaMeters > delta)
{
minDeltaMeters = delta;
minDeltaIdx = i;
}
}
CHECK_NOT_EQUAL(minDeltaMeters, kInvalidDistance, ());
return minDeltaIdx;
}
m2::PointD const & CheckpointPredictor::GetCheckpoint(vector<m2::PointD> const & points,
size_t index) const
{
if (index == 0)
return m_start;
if (index <= points.size())
return points[index - 1];
CHECK_EQUAL(index, points.size() + 1, ());
return m_finish;
}
} // namespace routing
|
