def get_mid_point_index(rt: PointXyzList, indexFrom: int,
indexTo: int) -> Optional[int]:
if indexFrom < 0 or indexFrom < 0 or indexTo >= len(
rt) or indexFrom > indexTo:
raise Exception("Wrong indices given to get_mid_point_index: " +
str(len(rt)) + " " + str(indexFrom) + " " +
str(indexTo))
if indexTo - indexFrom < 2:
return None
if indexTo - indexFrom == 2:
return int((indexFrom + indexTo) / 2)
m = None
i = indexFrom + 1
minDiff: float = 1000000
pFrom = rt[indexFrom]
pTo = rt[indexTo]
while i < indexTo:
pI = rt[i]
diff = abs(
pointutils.distance(pFrom, pI) - pointutils.distance(pI, pTo))
if minDiff > diff:
m = i
minDiff = diff
i += 1
return m
def remove_close_points(rt: PointXyzList, dist_m: float) -> None:
if len(rt) <= 2:
return
j = 0
i = 1
while i < len(rt):
if pointutils.distance(rt[j], rt[i]) < dist_m:
i += 1
continue
m = None
if i - j > 2:
m = routeutils.get_mid_point_index(rt, j, i)
for k in range(j + 1, i):
if k != m:
rt[k] = None # type: ignore
j = i
i += 1
for k in range(j + 1, i - 1):
rt[k] = None # type: ignore
utils.remove_all_none_from_list(rt)
def advance_candidates(point: PointXyz, candidates: List[RouteCandidateInfo],
routes: List[PointXyzList], tolerance: float) -> None:
for candidate_id, candidate in enumerate(candidates):
if candidate.route_id >= len(routes):
raise Exception('Candidate route id is out of range')
rt = routes[candidate.route_id]
if candidate.point_id >= len(rt):
raise Exception('Candidate point is out of range')
pt_id = candidate.point_id
checked_point: Optional[PointXyz] = None
checked_point_id: int = 0
while pt_id < len(rt) and pointutils.distance(point,
rt[pt_id]) <= tolerance:
checked_point = rt[pt_id]
checked_point_id = pt_id
pt_id += 1
if checked_point is None:
candidates[candidate_id] = None # type: ignore
else:
candidates[candidate_id] = RouteCandidateInfo(
checked_point, checked_point_id, candidate.route_id)
utils.remove_all_none_from_list(candidates)
def get_length(rt: PointXyzList) -> float:
if len(rt) < 2:
return 0
sm: float = 0
for i in range(1, len(rt)):
sm += pointutils.distance(rt[i - 1], rt[i])
return sm
def get(self, p: PointXyz, dist: float) -> List[RouteCandidateInfo]:
res = self.__grid.get(PointXy(p.x, p.y), dist)
return [i for i in res if pointutils.distance(i.point, p) <= dist]
def test_distance(self):
self.assertEqual(10, pointutils.distance(PointXyz(0, 0, 0), PointXyz(10, 0, 0)))
self.assertEqual(10, pointutils.distance(PointXyz(0, 0, 0), PointXyz(0, 10, 0)))
self.assertEqual(10, pointutils.distance(PointXyz(0, 0, 0), PointXyz(0, 0, 10)))
self.assertEqual(math.sqrt(300), pointutils.distance(PointXyz(0, 0, 0), PointXyz(10, 10, 10)))