Source code for simobject

from math import sin, cos
import pylygon
from pose import Pose

[docs]class SimObject: """The base class for all objects that can be drawn in the simulator. Every SimObject has a pose, an envelope and a color. :param pose: The position of the object. :type pose: :class:`~pose.Pose` :param color: The internal color of the object (`0xAARRGGBB` or `0xRRGGBB`). The default color is black. :type color: int """ def __init__(self, pose, color = 0): """Create an object at *pose* with *color* """ self.set_color(color) self.set_pose(pose)
[docs] def get_color(self): """Get the internal color of the object""" return self.__color
[docs] def set_color(self, color): """Set the internal color of the object""" self.__color = color
[docs] def get_pose(self): """Get the pose of the object in world coordinates""" return self.__pose
[docs] def set_pose(self,pose): """Set the pose of the object in world coordinates""" self.__world_envelope = None self.__pose = pose
[docs] def draw(self, renderer): """Draws the object using *renderer* (see :class:`~renderer.Renderer`). The object doesn't have to use only one color. It doesn't even have to use its internal color while drawing. """ raise NotImplementedError("SimObject.draw")
[docs] def get_envelope(self): """Get the envelope of the object in object's local coordinates. The envelope is a list of *xy* pairs, describing the shape of the bounding polygon. """ raise NotImplementedError("SimObject.get_envelope")
[docs] def get_world_envelope(self, recalculate=False): """Get the envelope of the object in world coordinates. Used for checking collision. The envelope is cached, and will be recalculated if *recalculate* is `True`. """ if self.__world_envelope is None or recalculate: x,y,t = self.get_pose() self.__world_envelope = [(x+p[0]*cos(t)-p[1]*sin(t), y+p[0]*sin(t)+p[1]*cos(t)) for p in self.get_envelope()] return self.__world_envelope
[docs] def get_bounding_rect(self): """Get the smallest rectangle that contains the object as a tuple (x, y, width, height).""" xmin, ymin, xmax, ymax = self.get_bounds() return (xmin,ymin,xmax-xmin,ymax-ymin)
[docs] def has_collision(self, other): """Check if the object has collided with *other*. Return True or False""" self_poly = pylygon.Polygon(self.get_world_envelope()) other_poly = pylygon.Polygon(other.get_world_envelope()) # TODO: use distance() for performance #print("Dist:", self_poly.distance(other_poly)) collision = self_poly.collidepoly(other_poly) if isinstance(collision, bool): if not collision: return False # Test code - print out collisions #print("Collision between {} and {}".format(self, other)) # end of test code return True
[docs] def get_contact_points(self, other): """Get a list of contact points with other object. Returns a list of (x, y)""" self_poly = pylygon.Polygon(self.get_world_envelope()) other_poly = pylygon.Polygon(other.get_world_envelope()) return self_poly.intersection_points(other_poly)
[docs] def get_bounds(self): """Get the smallest rectangle that contains the object as a tuple (xmin, ymin, xmax, ymax)""" xs, ys = zip(*self.get_world_envelope()) return (min(xs), min(ys), max(xs), max(ys))
[docs]class Polygon(SimObject): """The polygon is a simobject that gets the envelope supplied at construction. It draws itself as a filled polygon. :param pose: The position of the polygon. :type pose: :class:`~pose.Pose` :param shape: The list of points making up the polygon. :type shape: list((int,int)) :param color: The color of the polygon (`0xAARRGGBB` or `0xRRGGBB`). :type color: int """ def __init__(self, pose, shape, color): SimObject.__init__(self,pose, color) self.__shape = shape def get_envelope(self): return self.__shape
[docs] def draw(self,r): """Draw the envelope (shape) filling it with the internal color.""" r.set_pose(self.get_pose()) r.set_brush(self.get_color()) r.draw_polygon(self.get_envelope())
[docs]class Path(SimObject): """The path is a simobject that draws itself as a polyline. The line starts at `start`, and can be continued by adding points using :meth:`~simobject.Path.add_point`. :param start: The starting point of the polyline in world coordinates. :type start: :class:`~pose.Pose` :param color: The color of the line (`0xAARRGGBB` or `0xRRGGBB`). :type color: int The path is used by the simulator to track the history of robot motion""" def __init__(self, start, color): SimObject.__init__(self, Pose(), color) self.reset(start)
[docs] def reset(self,start): """Set the start point to start.x and start.y and remove all other points""" self.points = [(start.x,start.y)]
[docs] def add_point(self,pose): """Append a point at *pose* to the path. The orientation of the pose is ignored""" self.points.append((pose.x,pose.y))
[docs] def draw(self,r): """Draw a polyline with modes at all added points, using the internal color""" r.set_pose(self.get_pose()) # Reset everything r.set_pen(self.get_color()) for i in range(1,len(self.points)): x1,y1 = self.points[i-1] x2,y2 = self.points[i] r.draw_line(x1,y1,x2,y2)