fix check segment intersection with obstacle

The previous method only worked because the step that checked collision
from a point of the line to obstacles was the same as the minimum
obstacle radius. If the obstacle radius is very small a great amount of
steps would be required.
Thus It's better to check the distance from the segment to the obstacles
directly and compare with obstacle radius
Now there is no need to have two check functions.

PathPlanning/InformedRRTStar/informed_rrt_star.py

@@ -74,10 +74,9 @@ class InformedRRTStar:
             newNode = self.get_new_node(theta, nind, nearestNode)
             d = self.line_cost(nearestNode, newNode)
 
-            isCollision = self.collision_check(newNode, self.obstacle_list)
-            isCollisionEx = self.check_collision_extend(nearestNode, theta, d)
+            isCollision = self.check_collision(nearestNode, theta, d)
 
-            if isCollision and isCollisionEx:
+            if isCollision:
                 nearInds = self.find_near_nodes(newNode)
                 newNode = self.choose_parent(newNode, nearInds)
 
@@ -110,7 +109,7 @@ class InformedRRTStar:
             dy = newNode.y - self.node_list[i].y
             d = math.sqrt(dx ** 2 + dy ** 2)
             theta = math.atan2(dy, dx)
-            if self.check_collision_extend(self.node_list[i], theta, d):
+            if self.check_collision(self.node_list[i], theta, d):
                 dList.append(self.node_list[i].cost + d)
             else:
                 dList.append(float('inf'))
@@ -194,17 +193,6 @@ class InformedRRTStar:
         minIndex = dList.index(min(dList))
         return minIndex
 
-    @staticmethod
-    def collision_check(newNode, obstacleList):
-        for (ox, oy, size) in obstacleList:
-            dx = ox - newNode.x
-            dy = oy - newNode.y
-            d = dx * dx + dy * dy
-            if d <= 1.1 * size ** 2:
-                return False  # collision
-
-        return True  # safe
-
     def get_new_node(self, theta, nind, nearestNode):
         newNode = copy.deepcopy(nearestNode)
 
@@ -234,19 +222,35 @@ class InformedRRTStar:
             if nearNode.cost > scost:
                 theta = math.atan2(newNode.y - nearNode.y,
                                    newNode.x - nearNode.x)
-                if self.check_collision_extend(nearNode, theta, d):
+                if self.check_collision(nearNode, theta, d):
                     nearNode.parent = n_node - 1
                     nearNode.cost = scost
+                    
+    @staticmethod
+    def distance_squared_point_to_segment(v, w, p):
+        # Return minimum distance between line segment vw and point p
+        if (np.array_equal(v, w)):
+            return (p-v).dot(p-v) # v == w case
+        l2 = (w-v).dot(w-v) # i.e. |w-v|^2 -  avoid a sqrt
+        # Consider the line extending the segment, parameterized as v + t (w - v).
+        # We find projection of point p onto the line. 
+        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
+        # We clamp t from [0,1] to handle points outside the segment vw.
+        t = max(0, min(1, (p - v).dot(w - v) / l2))
+        projection = v + t * (w - v) # Projection falls on the segment
+        return (p-projection).dot(p-projection)
 
-    def check_collision_extend(self, nearNode, theta, d):
+    def check_collision(self, nearNode, theta, d):
         tmpNode = copy.deepcopy(nearNode)
-
-        for i in range(int(d / self.expand_dis)):
-            tmpNode.x += self.expand_dis * math.cos(theta)
-            tmpNode.y += self.expand_dis * math.sin(theta)
-            if not self.collision_check(tmpNode, self.obstacle_list):
-                return False
-
+        endx = tmpNode.x + math.cos(theta)*d
+        endy = tmpNode.y + math.sin(theta)*d
+        for (ox, oy, size) in self.obstacle_list:
+            dd = self.distance_squared_point_to_segment(
+                np.array([tmpNode.x, tmpNode.y]),
+                np.array([endx, endy]),
+                np.array([ox, oy]))
+            if dd <= 1.1 * size**2:
+                return False  # collision
         return True
 
     def get_final_course(self, lastIndex):