def steer(self, sample, nearest_node):
# take nearest_node and expand in direction of sample
angle = math.atan2(sample.pose[1] - nearest_node.pose[1], sample.pose[0] - nearest_node.pose[0])
new_node = Node([sample.pose[0], sample.pose[1]])
currentDistance = dist(sample, nearest_node)
# find a point within growth of nearest_node, and closest to sample
if currentDistance <= self.growth:
pass
else:
new_node.pose[0] = nearest_node.pose[0] + self.growth * math.cos(angle)
new_node.pose[1] = nearest_node.pose[1] + self.growth * math.sin(angle)
new_node.cost = float("inf")
new_node.parent = None
return new_node
def steer(self, sample, nearest_node):
# take nearest_node and expand in direction of sample
vec = np.array((sample.pose[0] - nearest_node.pose[0],
sample.pose[1] - nearest_node.pose[1]))
unit_vec = vec / np.linalg.norm(vec)
new_node = Node([sample.pose[0], sample.pose[1]])
currentDistance = dist(sample, nearest_node)
# find a point within growth of nearest_node, and closest to sample
if currentDistance <= self.growth:
pass
else:
new_node.pose[0] = nearest_node.pose[0] + self.growth * unit_vec[0]
new_node.pose[1] = nearest_node.pose[1] + self.growth * unit_vec[1]
new_node.cost = float("inf")
new_node.parent = None
return new_node
