Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Initial public release!

  • Loading branch information...
commit 58c9f2ed12970dd6921cf64e1119ef7ef4a4274d 0 parents
@bgw bgw authored
1  README
@@ -0,0 +1 @@
+This is still a work in progress. To run the unit tests, execute "python testcase.py" located in the src folder.
242 src/geometry.py
@@ -0,0 +1,242 @@
+import node
+import math
+
+class Line(object):
+ """The name of this class can be misleading. It represents a line segment.
+ Line objects are represented by two Node object"""
+
+ def __init__(self, node_a, node_b):
+ """Constructs a Line from ``node_a`` extending to ``node_b``. As a
+ shorthand, each node in the constructor can be written as a tuple, and
+ will simply be converted to Nodes upon construction."""
+ super(Line, self).__init__()
+ if isinstance(node_a, tuple):
+ node_a = node.Node(*node_a)
+ if isinstance(node_b, tuple):
+ node_b = node.Node(*node_b)
+ self.node_a, self.node_b = node_a, node_b
+ self.midpoint = node.Node((node_a.x + node_b.x)*.5,
+ (node_a.y + node_b.y)*.5)
+
+ def get_length(self):
+ return self.node_a.dist(self.node_b)
+
+ length = property(get_length,
+ doc="The distance from ``node_a`` to ``node_b``")
+
+ def get_delta_x(self):
+ return self.node_a.x - self.node_b.x
+
+ delta_x = property(get_delta_x,
+ doc="""The horizontal distance, or the change in ``x``
+ from ``node_a`` to ``node_b``""")
+
+ def get_delta_y(self):
+ return self.node_a.y - self.node_b.y
+
+ delta_y = property(get_delta_y,
+ doc="""The vertical distance, or the change in ``y`` from
+ ``node_a`` to ``node_b``""")
+
+ def does_intersect(self, other, vertexes_count=True):
+ """Uses the algorithm defined here:
+ http://www.bryceboe.com/2006/10/23/line-segment-intersection-algorithm/
+ Along with some special handlers for edge-cases (no pun intended).
+ """
+ # the algorithm doesn't handle parallel lines, so let's check that first
+ # we'll cross-multiply the slopes, and then compare them
+ if self.delta_x * other.delta_y == self.delta_y * other.delta_x:
+ return False
+
+ # define our shorthand
+ a, b = self.node_a, self.node_b
+ c, d = other.node_a, other.node_b
+ return self.__ccw(a, c, d) != self.__ccw(b, c, d) and \
+ self.__ccw(a, b, c) != self.__ccw(a, b, d) and \
+ (vertexes_count or (a != c and a != d and b != c and b != d))
+
+ def __ccw(self, a, b, c):
+ """A utility function for ``does_intersect``."""
+ return (c.y - a.y) * (b.x - a.x) > (b.y - a.y) * (c.x - a.x)
+
+ def __eq__(self, other):
+ return (self.node_a == other.node_a and self.node_b == other.node_b) or\
+ (self.node_a == other.node_b and self.node_b == other.node_a)
+
+ def __ne__(self, other):
+ return not self.__eq__(other)
+
+ def __hash__(self):
+ return hash(self.node_a) ^ hash(self.node_b)
+
+class BasePolygon(object):
+ """This class and it's subclasses should be assumed to be immutable, for
+ performance sake"""
+ def __init__(self, *nodes):
+ super(BasePolygon, self).__init__()
+ nodes = list(nodes)
+
+ # convert shorthand tuples to nodes
+ for i in range(len(nodes)):
+ if isinstance(nodes[i], tuple):
+ nodes[i] = node.Node(*nodes[i])
+
+ # handler for if somebody completes the polygon (first node = last node)
+ if nodes[-1] == nodes[0]:
+ del nodes[-1]
+
+ assert len(nodes) >= 3
+
+ self.nodes = nodes
+
+ lines = []
+ for i in range(len(nodes)):
+ # (i + 1) % len(nodes) essentially handles the circular nature of
+ # the polygon, it is for the last in the list of nodes
+ lines.append(Line(nodes[i], nodes[(i + 1) % len(nodes)]))
+ self.lines = lines
+
+ self.perimeter = sum([l.length for l in self.lines])
+
+ def contains_node_in_area(self, node):
+ """Based on/ported from some code donated ad hoc (public domain) by
+ asarkar of #xkcd-cs on irc.foonetic.net"""
+ result = False
+ # because I'm too lazy of a porter to change all the variable names
+ # within
+ poly = list(self.nodes)
+ poly.append(poly[0])
+ p = node
+ eps = 1e-7 # a value considered "close enough" to 0
+ for i in xrange(1, len(poly)):
+ p1 = poly[i];
+ p2 = poly[i - 1];
+ if p1.x < p.x and p2.x < p.x:
+ # the segment is strictly to the left of the test point, so it
+ # can't intersect the ray cast in the positive x direction
+ continue
+ elif p == p2:
+ # the point is one of the vertices
+ return True
+ elif abs(p1.y - p.y) < eps and abs(p2.y - p.y) < eps:
+ # the segment is horizontal
+ if p.x >= min(p1.x, p2.x) and p.x <= max(p1.x, p2.x):
+ # the point is on the segment
+ return True
+ # otherwise, don't count the segment
+ elif (p1.y > p.y and p2.y <= p.y) or (p2.y > p.y and p1.y <= p.y):
+ # non-horizontal upward edges include start, exclude end;
+ # non-horizontal downward edges exclude start, include end
+ det = (p1.x - p.x) * (p2.y - p.y) - (p1.y - p.y) * (p2.x - p.x)
+ if abs(det) < eps:
+ # point is on the translated segment
+ return True
+ if p2.y < p1.y:
+ det *= -1
+ if det > 0:
+ # segment crosses if the determinant is positive
+ result = not result
+ return result
+
+ def __str__(self):
+ s = "("
+ for i in self.nodes:
+ s += str(i)
+ return s + ")"
+
+ def __repr__(self):
+ return "BasePolygon" + str(self)
+
+class Triangle(BasePolygon):
+ """A three-sided, three-vertexed polygon. Has some more features than
+ Polygon, simply because there are more assumptions that can be made about
+ triangles.
+
+ Contains some ported code donated ad hoc (public domain) by asarkar of
+ #xkcd-cs on irc.foonetic.net"""
+ def __init__(self, *nodes):
+ super(Triangle, self).__init__(*nodes)
+ assert len(self.nodes) == 3
+ # find area with modified Heron's formula. Area is positive if the node
+ # list was in ccw order, negative if it was in cw order
+ p, q, r = self.nodes
+ self.area = .5 * (-q.x * p.y + r.x * p.y + p.x * q.y - r.x * q.y - p.x *
+ r.y + q.x * r.y)
+ self.is_ccw = True
+ if self.area < 0:
+ self.area *= -1
+ self.is_ccw = False
+
+ def does_intersect_line(self, line):
+ for l in self.lines:
+ if l.does_intersect(line): return True
+ return False
+
+ def __repr__(self):
+ return "Triangle" + str(self)
+
+class Polygon(BasePolygon):
+ """A polygon is created with a list of nodes, and is internally represented
+ by list of nodes and a list of lines, in the order that they are given.
+
+ Contains some ported code donated ad hoc (public domain) by asarkar of
+ #xkcd-cs on irc.foonetic.net"""
+ def __init__(self, *nodes, **kwargs):
+ super(Polygon, self).__init__(*nodes)
+ # perform triangulation
+ if "ccw" in kwargs:
+ ccw = kwargs["ccw"]
+ ccw = True
+ copy = list(self.nodes)
+ # Convert this copy to CCW order
+ # Step One: Determine what order it is to start with
+ #angles = [math.atan2(i.delta_y, i.delta_x) for i in self.lines] # absang
+ # make angles relative to each other
+ #for i in xrange(len(angles)):
+ # a = (angles[i] - angles[(i - 1) % len(angles)]) % 360
+ # if a > 180:
+ # a -= 360
+ # angles[i] = a
+ #if sum(angles) > 0:
+ # self.is_ccw = False
+ # copy.reverse()
+ #else: self.is_ccw = True
+ self.is_ccw = ccw
+ if self.is_ccw:
+ pass
+ else:
+ copy.reverse()
+ result = []
+ while len(copy) >= 3:
+ t = Triangle(copy[0], copy[1], copy[2])
+ is_ear = t.is_ccw and \
+ self.contains_node_in_area(Line(copy[0], copy[2]).midpoint)
+
+ i = 3
+ while is_ear and i < len(copy):
+ if t.contains_node_in_area(copy[i]):
+ print "ear: ", copy[:3], ", err: ", copy[i]
+ is_ear = False
+ i += 1
+
+ if is_ear:
+ # print "found ear: ", copy[:3]
+ result.append(t)
+ copy.pop(1)
+ else:
+ p = copy.pop(0)
+ copy.append(p)
+ self.triangles = result
+ self.triangle_lines = set()
+ for t in self.triangles:
+ self.triangle_lines.update(t.lines)
+ self.triangle_lines.difference_update(self.lines)
+
+ def does_intersect_line(self, line):
+ if line in self.triangle_lines: return True
+ for t in self.triangles:
+ if t.does_intersect_line(line): return True
+ return False
+
+ def __repr__(self):
+ return "Polygon" + str(self)
163 src/mapping.py
@@ -0,0 +1,163 @@
+import geometry
+import node
+
+class Board(object):
+ """Representing a game board, this class is composed of a set of
+ ``Polygon``s which are non-moveable. These can represent PVC, blocks, or any
+ other place you wish for the robot to avoid. Knowing everything about the
+ game's design, this class can do such things as tell if a node is within the
+ line-of-sight or another node, or even utilize Dijkstra's algorithm to find
+ the shortest possible path from one node to another. Note that this board is
+ not entirely realistic. It assumes each node is simply an infinitely tiny
+ point in space, and that robots are nodes. For realistic purposes, a wrapper
+ or extended version of this class could be used."""
+
+ def __init__(self):
+ """Creates an empty board. Use the ``add`` function"""
+ super(Board, self).__init__()
+ self.polygons = set()
+ self.__precalculated = False
+
+ def add(self, poly):
+ """Adds a polygon, ``poly``, to the game board."""
+ self.polygons.add(poly)
+ self.__precalculated = False
+
+ def remove(self, poly):
+ """Removes a polygon, ``poly``, from the game board."""
+ self.polygons.remove(poly)
+ self.__precalculated = False
+
+ def get_lines(self):
+ """Creates and returns a set of every polygon's lines on this
+ ``Board``."""
+ lines = set()
+ for i in self.polygons:
+ lines.update(i.lines)
+ return lines
+
+ lines = property(get_lines,
+ doc="A set of every ``Polygon``'s lines on this ``Board``")
+
+ def get_nodes(self):
+ """Creates and returns a set of every polygon's nodes on this
+ ``Board``."""
+ nodes = set()
+ for i in self.polygons:
+ nodes.update(i.nodes)
+ return nodes
+
+ nodes = property(get_nodes,
+ doc="A set of every ``Polygon``'s nodes on this ``Board``")
+
+ def precalculate_visibility(self):
+ """Before doing a group of visibility tests, call this first, and it can
+ do some caching that may improve the performance of further is_visible
+ calls. You don't need to call this before calling ``get_shortest_path``,
+ as it will intelligently decide if it should call this or not, and if
+ so, it will do it itself."""
+ if self.__precalculated: return False
+ for i in self.nodes:
+ i.visible_siblings = set()
+ i.nonvisible_siblings = set()
+ for i in self.nodes:
+ for k in self.nodes:
+ if not k is i and k not in i.visible_siblings and \
+ k not in i.nonvisible_siblings:
+ if self.__visibility_test(i, k):
+ i.visible_siblings.add(k)
+ k.visible_siblings.add(i)
+ else:
+ i.nonvisible_siblings.add(k)
+ k.nonvisible_siblings.add(i)
+ for i in self.nodes: # cleanup
+ del i.nonvisible_siblings
+ self.__precalculated = True
+ return True
+
+ def __visibility_test(self, node_a, node_b):
+ """Performs a simple direct visibility test between two points, in an
+ unoptimized fashion"""
+ # Todo: ensure visibility checks cannot go through polygons (facepalm)
+ direct_line = geometry.Line(node_a, node_b)
+ for p in self.polygons:
+ for l in p.lines:
+ if l.does_intersect(direct_line, False):
+ return False
+ for l in p.triangle_lines:
+ if l.does_intersect(direct_line, False):
+ return False
+ if direct_line in p.triangle_lines: return False
+ return True # survived all the tests
+
+ def is_visible(self, node_a, node_b):
+ if self.__precalculated:
+ if node_a in node_b.visible_siblings:
+ return True
+ if node_a in self.nodes and node_b in self.nodes:
+ # must have been determined to be nonvisible
+ return False
+ # when all else fails...
+ return self.__visibility_test(node_a, node_b)
+
+ def get_visible_set(self, pov, *args):
+ """Returns a set of nodes that are visible to the first argument. The
+ nodes checked are the map's internal set of nodes, as well as any nodes
+ given after the first argument."""
+ visible_set = self.get_visible_set_for(pov, *args)
+ for i in self.nodes:
+ if self.is_visible(pov, i) and pov != i: visible_set.add(i)
+ return visible_set
+
+ def get_visible_set_for(self, pov, *args):
+ """Returns a set of nodes that are visible to the first argument. The
+ nodes checked are only the nodes given after the first argument."""
+ visible_set = set()
+ for i in args:
+ if self.is_visible(pov, i) and pov != i: visible_set.add(i)
+ return visible_set
+
+ def get_shortest_path(self, node_a, node_b):
+ self.precalculate_visibility()
+
+ # Handle special/common cases
+ if node_a == node_b: return [node_a]
+ if self.is_visible(node_a, node_b): return [node_b] # direct is shortest
+
+ # Format: shortest_to[node] = (goes_though, cost, is_minimum)
+ shortest_to = {node_b:None}
+ for i in self.nodes:
+ shortest_to[i] = None
+ starting_from = node_a
+ starting_from_cost = 0
+ while True:
+ # find possible paths
+ improvable = set()
+ for k in shortest_to:
+ if shortest_to[k] == None or not shortest_to[k][2]:
+ improvable.add(k)
+ improvable &= self.get_visible_set(starting_from, node_b)
+ if not improvable: # nothing else we can do
+ return None
+ # see if there is a shorter path for any of these via starting_from
+ lowest_cost = None
+ lowest_cost_node = None
+ for i in improvable:
+ cost = starting_from_cost + starting_from.dist(i)
+ if not lowest_cost or cost < lowest_cost:
+ lowest_cost = cost
+ lowest_cost_node = i
+ if not shortest_to[i] or cost < shortest_to[i][1]:
+ shortest_to[i] = (starting_from, cost, False)
+ lowest_cost_info = list(shortest_to[lowest_cost_node])
+ lowest_cost_info[2] = True
+ shortest_to[lowest_cost_node] = tuple(lowest_cost_info)
+ if lowest_cost_node == node_b: # we're done! wrap it up.
+ path = []
+ n = node_b
+ while n != node_a:
+ path[:0] = [n]
+ n = shortest_to[n][0]
+ return path
+ starting_from = lowest_cost_node
+ starting_from_cost = lowest_cost
25 src/node.py
@@ -0,0 +1,25 @@
+import math
+
+class Node(object):
+ def __init__(self, x, y):
+ super(Node, self).__init__()
+ self.x, self.y = float(x), float(y)
+ self.visible_siblings = set()
+
+ def dist(self, point):
+ return math.hypot(self.x - point.x, self.y - point.y)
+
+ def __eq__(self, other):
+ return self.x == other.x and self.y == other.y
+
+ def __ne__(self, other):
+ return not self.__eq__(other)
+
+ def __str__(self):
+ return "(" + str(self.x) + ", " + str(self.y) + ")"
+
+ def __repr__(self):
+ return "Node" + self.__str__()
+
+ def __hash__(self):
+ return hash(self.x)^hash(self.y)
88 src/testcase.py
@@ -0,0 +1,88 @@
+import geometry
+import mapping
+from node import Node
+import unittest
+
+# build the board in the example at
+# https://github.com/CBCJVM/CBCJVM/wiki/pathfinding_proposal
+# This is kind of an s-curve
+# (0, 0) ______________ ________ (5, 0)
+# | (3, 0)| E |(4, 0) |
+# | | | |
+# | (1, 1) ______| | |
+# | | (3, 1) | |
+# | | ______|(4, 2) |
+# | | |(2, 2) |
+# | | | |
+# |_(1,_3)| S |(2,_3)___(5,_3)|
+# (0, 3)
+s_left_side = geometry.Polygon((0, 0), (3, 0), (3, 1), (1, 1), (1, 3), (0, 3),
+ ccw=False)
+s_right_side = geometry.Polygon((4, 0), (5, 0), (5, 3), (2, 3), (2, 2), (4, 2),
+ ccw=False)
+s_board = mapping.Board()
+s_board.add(s_left_side); s_board.add(s_right_side)
+s_start = Node(1.5, 3)
+s_end = Node(3.5, 0)
+# del left_side; del right_side
+
+# A simple rectangular board, to test that a path cannot pass through a polygon
+# (0, 0) ________ (1, 0)
+# | |
+# | |
+# |________|
+# (0, 1) (1, 1)
+r_board = mapping.Board()
+r_board.add(geometry.Polygon((0, 0), (1, 0), (1, 1), (0, 1)))
+r_start = Node(0, 0)
+r_end = Node(1, 1)
+
+class TestContainsNodeInArea(unittest.TestCase):
+ def test_s_curve_node_in_area(self):
+ assert s_left_side.contains_node_in_area(Node(2, .5))
+ assert s_left_side.contains_node_in_area(Node(1, 2.5))
+ assert not s_left_side.contains_node_in_area(Node(3.5, .5))
+ assert not s_right_side.contains_node_in_area(Node(3.5, .5))
+
+class TestTriangulation(unittest.TestCase):
+ def test_s_curve_triangulation(self):
+ print(s_left_side.triangles)
+ print(s_right_side.triangles)
+
+class TestLineOfSight(unittest.TestCase):
+ def test_s_curve_is_visible(self):
+ # returns false every time!!!
+
+ assert not s_board.is_visible(s_start, s_end)
+ assert s_board.is_visible(Node(1.5, 2), Node(1.5, 2.5))
+ assert s_board.is_visible(Node(1.4, 2), Node(1.6, 2.5))
+ assert not s_board.is_visible(Node(3, 1), Node(5, 3))
+ assert s_board.is_visible(Node(3, 0), Node(3, 1))
+ assert s_board.is_visible(Node(2, 3), Node(1, 1))
+
+ def test_s_curve_visible_set(self):
+ assert s_board.get_visible_set(Node(1.5, 2.5)) == \
+ set([Node(2.0, 2.0), Node(2.0, 3.0), Node(1.0, 3.0),
+ Node(1.0, 1.0)])
+
+ # There are two valid answers here
+ s = s_board.get_visible_set(Node(2, 3))
+ a = set([Node(5, 3), Node(1, 3), Node(2, 2), Node(1, 1)])
+ b = set([Node(5, 3), Node(1, 3), Node(2, 2), Node(1, 1), Node(0, 3)])
+ assert s == a or s == b
+
+ def test_r_board(self):
+ assert not r_board.is_visible(r_start, r_end)
+
+class TestPathfinding(unittest.TestCase):
+ def test_s_curve_path(self):
+ s_path = [Node(2.0, 2.0), Node(3.0, 1.0), s_end]
+ assert s_board.get_shortest_path(s_start, s_end) == s_path
+ # try it backwards too
+ s_back_path = [Node(3.0, 1.0), Node(2.0, 2.0), s_start]
+ assert s_board.get_shortest_path(s_end, s_start) == s_back_path
+
+if __name__ == "__main__":
+ unittest.main()
+else:
+ raise Exception("testcase.py is meant only to be run directly.")
Please sign in to comment.
Something went wrong with that request. Please try again.