def contains(self, point):
#return pygame.rect.Rect(self.left, self.top, self.width, self.height).collidepoint(point.x, point.y)
# p = point, a,b,d = corners on rectangles
top_left = Point(self.left, self.top)
top_right = Point(self.left + self.width, self.top)
bottom_left = Point(self.left, self.top + self.height)
tLeft_to_point = Vector2D.from_points(top_left, point)
tLeft_to_tRight = Vector2D.from_points(top_left, top_right)
tLeft_to_bLeft = Vector2D.from_points(top_left, bottom_left)
return (0 < tLeft_to_point.dot(tLeft_to_tRight) and tLeft_to_point.dot(tLeft_to_tRight) < tLeft_to_tRight.dot(tLeft_to_tRight)) and (0 < tLeft_to_point.dot(tLeft_to_bLeft) and tLeft_to_point.dot(tLeft_to_bLeft) < tLeft_to_bLeft.dot(tLeft_to_bLeft))
def move(self):
if self.state == State.Motion:
scan_result = self.scan_for_obstacles()
if scan_result == None:
self.visited_points.append(Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.updateHeading()
self.distance_traveled += 1
else:
# in range of an obstacle
self.circumnavigation_obstacle = scan_result
self.state = State.Circumnavigation
self.found_closest_point = False
self.circumnavigation_end_point = Point(self.position.x, self.position.y)
self.circumnavigation_closest_point = Point(self.position.x, self.position.y)
self.circumnavigation_travel = 0
self.circumnavigation_perimeter = 2 * math.pi * self.circumnavigation_obstacle.radius
self.circumnavigation_start = Point(self.position.x, self.position.y)
self.move_tangent_line()
else:
# circumnavigating
if self.circumnavigation_travel > self.circumnavigation_perimeter:
self.position.x = self.circumnavigation_start.x
self.position.y = self.circumnavigation_start.y
self.circumnavigation_travel = 0
if self.position == self.circumnavigation_end_point and not self.found_closest_point:
self.circumnavigation_end_point = self.circumnavigation_closest_point
self.found_closest_point = True
self.visited_points = []
elif self.position == self.circumnavigation_end_point and self.found_closest_point:
self.state = State.Motion
self.direction = Vector2D.from_points(self.position, self.goal)
self.direction.normalize()
self.circumnavigation_travel = 0
self.circumnavigation_perimeter = 0
self.circumnavigation_start = None
self.circumnavigation_obstacle = None
self.found_closest_point = False
self.circumnavigation_end_point = None
self.circumnavigation_closest_point = None
else:
# normal circumnavigation
self.move_tangent_line()
def contains(self, point):
#return pygame.rect.Rect(self.left, self.top, self.width, self.height).collidepoint(point.x, point.y)
# p = point, a,b,d = corners on rectangles
top_left = Point(self.left, self.top)
top_right = Point(self.left + self.width, self.top)
bottom_left = Point(self.left, self.top + self.height)
tLeft_to_point = Vector2D.from_points(top_left, point)
tLeft_to_tRight = Vector2D.from_points(top_left, top_right)
tLeft_to_bLeft = Vector2D.from_points(top_left, bottom_left)
return (0 < tLeft_to_point.dot(tLeft_to_tRight)
and tLeft_to_point.dot(tLeft_to_tRight) <
tLeft_to_tRight.dot(tLeft_to_tRight)) and (
0 < tLeft_to_point.dot(tLeft_to_bLeft)
and tLeft_to_point.dot(tLeft_to_bLeft) <
tLeft_to_bLeft.dot(tLeft_to_bLeft))
def clear_path(self):
# get vector from point to goal
heading_to_goal = Vector2D.from_points(self.point, self.goal)
#heading_to_goal.normalize()
point = Point(self.position.x + heading_to_goal.x, self.position.y + heading_to_goal.y)
# check next point if in obstacle
print "clear_path ??? {0}".format(not self.point_on_boundaries(point))
return not self.point_on_boundaries(point)
def move(self):
if self.state == State.Circumnavigation:
if (self.on_m_line()) and self.clear_path() and not (self.position.x, self.position.y) in self.jump_points:
self.state = State.Motion
self.direction = Vector2D.from_points(self.position, self.goal)
self.direction.normalize()
self.jump_points.append((self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
elif self.in_obstacle() or not self.on_obstacle_boundary() or self.hasVisited():
# go back one step, and turn 90 degrees
print "circumnavigating but not on boundary of obstacle, or inside.or repeat point"
self.position.x -= self.direction.x
self.position.y -= self.direction.y
self.visited_points.pop(len(self.visited_points)-1)
new_heading = math.radians(round(math.degrees(math.atan2(self.direction.y, self.direction.x))) + 90)
print " NEW HEADING -> {0}".format(new_heading)
self.direction = Vector2D(math.cos(new_heading), math.sin(new_heading))
else:
print "normal circumnavigate"
self.visited_points.append(Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.distance_traveled += 1
elif not self.in_obstacle() and not self.on_obstacle_boundary():
self.visited_points.append(Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.updateHeading()
self.distance_traveled += 1
self.state = State.Motion
print "not on obstacle!"
else:
# new obstacle..begin circumnavigation
new_heading = math.radians(round(math.degrees(math.atan2(self.direction.y, self.direction.x))) + 90)
print " NEW HEADING -> {0}".format(new_heading)
self.direction = Vector2D(math.cos(new_heading), math.sin(new_heading))
#self.direction.rotate(math.pi/2)
self.visited_points.append(Point(self.position.x, self.position.y))
# set 'end point' and variable for 'closest point'
self.position.x += 2 * self.direction.x
self.position.y += 2 * self.direction.y
self.distance_traveled += 1
self.state = State.Circumnavigation
def clear_path(self):
# get vector from point to goal
heading_to_goal = Vector2D.from_points(self.point, self.goal)
#heading_to_goal.normalize()
point = Point(self.position.x + heading_to_goal.x,
self.position.y + heading_to_goal.y)
# check next point if in obstacle
print "clear_path ??? {0}".format(not self.point_on_boundaries(point))
return not self.point_on_boundaries(point)
def __init__(self, obstacles, start, goal):
self.jump_points = []
slope_vector = Vector2D.from_points(start, goal)
if slope_vector.x == 0:
self.slope = None
else:
self.slope = slope_vector.y / slope_vector.x
self.point = Point(goal.x, goal.y)
super(BugTwo, self).__init__(obstacles, start, goal)
def __init__(self, obstacles, start, goal):
self.jump_points = []
slope_vector = Vector2D.from_points(start, goal)
if slope_vector.x == 0:
self.slope = None
else:
self.slope = slope_vector.y/slope_vector.x
self.point = Point(goal.x, goal.y)
super(BugTwo, self).__init__(obstacles, start, goal)
def __init__(self, obstacles, start, goal):
self.obstacles = obstacles
self.position = start
self.goal = goal
self.direction = Vector2D.from_points(start, goal)
self.direction.normalize()
self.distance_traveled = 0
self.visited_points = []
self.state = State.Motion
self.euclidean_distance = self.position.distance_to(self.goal)
def move(self):
if self.state == State.Motion:
scan_result = self.scan_for_obstacles()
if scan_result == None:
self.visited_points.append(
Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.updateHeading()
self.distance_traveled += 1
else:
# in range of an obstacle
self.circumnavigation_obstacle = scan_result
self.state = State.Circumnavigation
self.move_tangent_line()
else:
# circumnavigating
# check for on m-line
if self.on_m_line() and self.clear_path():
self.visited_points = []
self.state = State.Motion
self.direction = Vector2D.from_points(self.position, self.goal)
self.direction.normalize()
self.circumnavigation_obstacle = None
else:
# normal circumnavigation
self.move_tangent_line()
def move(self):
if self.state == State.Motion:
scan_result = self.scan_for_obstacles()
if scan_result == None:
self.visited_points.append(Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.updateHeading()
self.distance_traveled += 1
else:
# in range of an obstacle
self.circumnavigation_obstacle = scan_result
self.state = State.Circumnavigation
self.move_tangent_line()
else:
# circumnavigating
# check for on m-line
if self.on_m_line() and self.clear_path():
self.visited_points = []
self.state = State.Motion
self.direction = Vector2D.from_points(self.position, self.goal)
self.direction.normalize()
self.circumnavigation_obstacle = None
else:
# normal circumnavigation
self.move_tangent_line()
def updateHeading(self):
self.direction = Vector2D.from_points(self.position, self.goal)
self.direction.normalize()
def move(self):
if self.state == State.Circumnavigation:
if (self.on_m_line()) and self.clear_path() and not (
self.position.x, self.position.y) in self.jump_points:
self.state = State.Motion
self.direction = Vector2D.from_points(self.position, self.goal)
self.direction.normalize()
self.jump_points.append((self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
elif self.in_obstacle(
) or not self.on_obstacle_boundary() or self.hasVisited():
# go back one step, and turn 90 degrees
print "circumnavigating but not on boundary of obstacle, or inside.or repeat point"
self.position.x -= self.direction.x
self.position.y -= self.direction.y
self.visited_points.pop(len(self.visited_points) - 1)
new_heading = math.radians(
round(
math.degrees(
math.atan2(self.direction.y, self.direction.x))) +
90)
print " NEW HEADING -> {0}".format(new_heading)
self.direction = Vector2D(math.cos(new_heading),
math.sin(new_heading))
else:
print "normal circumnavigate"
self.visited_points.append(
Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.distance_traveled += 1
elif not self.in_obstacle() and not self.on_obstacle_boundary():
self.visited_points.append(Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.updateHeading()
self.distance_traveled += 1
self.state = State.Motion
print "not on obstacle!"
else:
# new obstacle..begin circumnavigation
new_heading = math.radians(
round(
math.degrees(math.atan2(self.direction.y,
self.direction.x))) + 90)
print " NEW HEADING -> {0}".format(new_heading)
self.direction = Vector2D(math.cos(new_heading),
math.sin(new_heading))
#self.direction.rotate(math.pi/2)
self.visited_points.append(Point(self.position.x, self.position.y))
# set 'end point' and variable for 'closest point'
self.position.x += 2 * self.direction.x
self.position.y += 2 * self.direction.y
self.distance_traveled += 1
self.state = State.Circumnavigation
def move(self):
if self.state == State.Motion:
scan_result = self.scan_for_obstacles()
if scan_result == None:
self.visited_points.append(
Point(self.position.x, self.position.y))
self.position.x += self.direction.x
self.position.y += self.direction.y
self.updateHeading()
self.distance_traveled += 1
else:
# in range of an obstacle
self.circumnavigation_obstacle = scan_result
self.state = State.Circumnavigation
self.found_closest_point = False
self.circumnavigation_end_point = Point(
self.position.x, self.position.y)
self.circumnavigation_closest_point = Point(
self.position.x, self.position.y)
self.circumnavigation_travel = 0
self.circumnavigation_perimeter = 2 * math.pi * self.circumnavigation_obstacle.radius
self.circumnavigation_start = Point(self.position.x,
self.position.y)
self.move_tangent_line()
else:
# circumnavigating
if self.circumnavigation_travel > self.circumnavigation_perimeter:
self.position.x = self.circumnavigation_start.x
self.position.y = self.circumnavigation_start.y
self.circumnavigation_travel = 0
if self.position == self.circumnavigation_end_point and not self.found_closest_point:
self.circumnavigation_end_point = self.circumnavigation_closest_point
self.found_closest_point = True
self.visited_points = []
elif self.position == self.circumnavigation_end_point and self.found_closest_point:
self.state = State.Motion
self.direction = Vector2D.from_points(self.position, self.goal)
self.direction.normalize()
self.circumnavigation_travel = 0
self.circumnavigation_perimeter = 0
self.circumnavigation_start = None
self.circumnavigation_obstacle = None
self.found_closest_point = False
self.circumnavigation_end_point = None
self.circumnavigation_closest_point = None
else:
# normal circumnavigation
self.move_tangent_line()
