| 15 | {} |
| 16 | |
| 17 | void NearestEdgeFinder::AddInformationSource(IRoadGraph::FullRoadInfo const & roadInfo) |
| 18 | { |
| 19 | if (!roadInfo.m_featureId.IsValid()) |
| 20 | return; |
| 21 | |
| 22 | Candidate res; |
| 23 | |
| 24 | auto const & junctions = roadInfo.m_roadInfo.m_junctions; |
| 25 | size_t const count = junctions.size(); |
| 26 | ASSERT_GREATER(count, 1, ()); |
| 27 | for (size_t i = 1; i < count; ++i) |
| 28 | { |
| 29 | m2::ParametrizedSegment<m2::PointD> segment(junctions[i - 1].GetPoint(), junctions[i].GetPoint()); |
| 30 | |
| 31 | m2::PointD const closestPoint = segment.ClosestPointTo(m_point); |
| 32 | double const squaredDist = m_point.SquaredLength(closestPoint); |
| 33 | |
| 34 | if (squaredDist < res.m_squaredDist) |
| 35 | { |
| 36 | res.m_segId = static_cast<uint32_t>(i - 1); |
| 37 | res.m_squaredDist = squaredDist; |
| 38 | } |
| 39 | } |
| 40 | |
| 41 | if (res.m_segId == Candidate::kInvalidSegmentId) |
| 42 | return; |
| 43 | |
| 44 | // Closest point to |this->m_point| found. It has index |res.m_segId + 1| in |junctions|. |
| 45 | size_t const idx = res.m_segId + 1; |
| 46 | geometry::PointWithAltitude const & segStart = junctions[idx - 1]; |
| 47 | geometry::PointWithAltitude const & segEnd = junctions[idx]; |
| 48 | geometry::Altitude const startAlt = segStart.GetAltitude(); |
| 49 | geometry::Altitude const endAlt = segEnd.GetAltitude(); |
| 50 | m2::ParametrizedSegment<m2::PointD> segment(junctions[idx - 1].GetPoint(), junctions[idx].GetPoint()); |
| 51 | m2::PointD const closestPoint = segment.ClosestPointTo(m_point); |
| 52 | |
| 53 | double const segLenM = mercator::DistanceOnEarth(segStart.GetPoint(), segEnd.GetPoint()); |
| 54 | geometry::Altitude projPointAlt = geometry::kDefaultAltitudeMeters; |
| 55 | if (segLenM == 0.0) |
| 56 | { |
| 57 | projPointAlt = startAlt; |
| 58 | } |
| 59 | else |
| 60 | { |
| 61 | double const distFromStartM = mercator::DistanceOnEarth(segStart.GetPoint(), closestPoint); |
| 62 | ASSERT_LESS_OR_EQUAL(distFromStartM, segLenM, (roadInfo.m_featureId)); |
| 63 | projPointAlt = startAlt + static_cast<geometry::Altitude>((endAlt - startAlt) * distFromStartM / segLenM); |
| 64 | } |
| 65 | |
| 66 | res.m_fid = roadInfo.m_featureId; |
| 67 | res.m_segStart = segStart; |
| 68 | res.m_segEnd = segEnd; |
| 69 | res.m_bidirectional = roadInfo.m_roadInfo.m_bidirectional; |
| 70 | |
| 71 | ASSERT_NOT_EQUAL(res.m_segStart.GetAltitude(), geometry::kInvalidAltitude, ()); |
| 72 | ASSERT_NOT_EQUAL(res.m_segEnd.GetAltitude(), geometry::kInvalidAltitude, ()); |
| 73 | res.m_projPoint = geometry::PointWithAltitude(closestPoint, projPointAlt); |
| 74 | |