Skip to main content

2DGeometryEngine — wiki

There was some discussion on the Pygame list in mid-August 2006 about Vector classes and how to optimize them. Greg Ewing suggested using Numeric and aggregating the math across all points and lines. Since my own Vector class and collision detection was starting to slow my game down, I decided to reimplement the geometry engine I had been working on to use that kind of design. Below is the result. It's pretty fast. I can get at least 30 FPS, even with 40 circles and 6 polygons. Eventually, though, I switched to using Open Dynamics Engine to do my physics, but I thought this would be a neat demonstration of how you could use Numeric that way.

-- glasse

## Automatically adapted for numpy.oldnumeric Apr 05, 2011 by Xenmen

# "Geometry Engine",
# Ethan Glasser-Camp
# Hereby placed into the public domain.
# This code implements a Numeric-based class called Geometry, which keeps track
# of a bunch of objects, their position, location, and the polygons and circles
# that represent their shape. It's meant for use in a game where fast collision
# detection and response is needed. Most data is kept in Numeric arrays so that
# computations can be performed over large numbers of objects fairly quickly.
# This means that most data is kept in parallel arrays, which is a little ugly.
# I don't expect that collision detection can be done much faster than this
# (unless you want to drop into C or C++, and I don't).
# Most of the ideas in the collision detection code are taken from the
# Polycolly tutorials, which explained the SAT ideas to me.
# See:
# Some of the code, like time_to_intersect, is translated from his C++.
# Some of it was rewritten to take advantage of more Pythonic techniques.
# All of it was changed in some way to interact with Numeric well.
# In some comments, I analyze the shape of the various arrays using variables:
# P is the number of points across all polygons.
# p is the number of points in "this" polygon.
# n is the number of polygons, or the number of normals in this polygon, or
#   just a variable.
# N is the number of normals across all polygons, which = P.
# c is the number of circles.
# Each object has an object-local coordinate system. I assumed that (0, 0)
# was the upper-left corner (ie. where you'd blit in pygame), but other
# choices could have been made as well. (The center, for instance, would
# make rotation easier.)
import numpy.oldnumeric as Numeric
from numpy.oldnumeric import dot
# I keep some functions in another module, called Vector, but for illustration,
# here's what they do/look like:
    from Vector import normalize, normal
    def normal(array):
        '''Assuming array is nx2, return an array of normals to each
        new = array.copy()
        old = array
        new[:, 0] = -old[:, 1]
        new[:, 1] =  old[:, 0]
        return Numeric.reshape(new, array.shape)
    def length(array):
        '''Assuming array is nxmx...x2, return an array of the length of
        each vector.'''
        return Numeric.sqrt(Numeric.sum(array**2, -1))
    def normalize(array):
        return array/length(array)[..., Numeric.NewAxis]
class ShapeAggregate(object):
    '''This is a generic container for holding aggregated shapes.
    Each shape has an "object" field, specifying what object it came from,
    and a "label" field, specifying what part it represents in that object.
    Both of these fields are integers; it is expected that the user of the
    ShapeAggregate class keep track of what integer represents what object,
    and each object keep track of which label represents what part.'''
    def __init__(self):
        self.object = Numeric.array([], Numeric.Int16)
        self.label = Numeric.array([], Numeric.Int16)
    def add_new(self, object, label):
        '''Add an object to the ShapeAggregate.'''
        self.object = Numeric.concatenate([self.object, [object]])
        self.label = Numeric.concatenate([self.label, [label]])
    def delete_object(self, object):
        '''Delete an object from the ShapeAggregate.
        This entails deleting all shapes that have that object as their object
        field and decrementing all object numbers if they are higher
        than that object.'''
        self.label = Numeric.compress(self.object != object, self.label)
        self.object = Numeric.compress(self.object != object, self.object)
        Numeric.putmask(self.object, self.object > object, self.object - 1)
class PolygonAggregate(ShapeAggregate):
    """This is a container for holding polygons.
    We need to store points homogeneously, so we can't have point[i] be an
    array of points. Instead we store all points in self.points.
    polygon[i] has points in the range self.owned[i][0]:self.owned[i][1].
    Note that this does not include self.owned[i][1]!
    We also store the normals of each polygon in self.normals."""
    def __init__(self):
        super(PolygonAggregate, self).__init__()
        self.points = Numeric.array([], Numeric.Float32)
        self.points.shape = (0, 2)
        self.owned = Numeric.array([], Numeric.Int16)
        self.owned.shape = (0, 2)
        self.normals = Numeric.array([], Numeric.Float32)
        self.normals.shape = (0, 2)
    def add_new(self, object, label, points):
        '''Add a new polygon.'''
        ShapeAggregate.add_new(self, object, label)
        oldend = len(self.points)
        self.points = Numeric.concatenate([self.points, points])
        newend = len(self.points)
        # Be sure to get the shape right: owned was n x 2, now we want it to be
        # (n+1)x2, so we add a 1x2 array (rather than a (2,) array).
        self.owned = Numeric.concatenate([self.owned, 
                                          Numeric.array(oldend, newend)])
        segments = make_segments(Numeric.array(points))
        newnorms = normalize(normal(segments))
        self.normals = Numeric.concatenate([self.normals, newnorms])
    def delete_object(self, n):
        '''Delete object n from this aggregate.'''
        # We have to remove points from this aggregate, and that means changing
        # the numbers in self.owned.
        # We maintain a "gap" variable which specifies how many points
        # were deleted.
        gap = 0
        for i in range(len(Numeric.take(self.owned, 
                                        Numeric.nonzero(self.object == n)))):
            begin, end = self.owned[i]
            if self.object[i] == n:
                self.points = Numeric.concatenate(self.points[:begin], 
                gap += end - begin
                self.owned[i] -= gap
        self.owned = Numeric.take(self.owned, Numeric.nonzero(self.object != n))
        super(PolygonAggregate, self).delete_object(n)
class RoundAggregate(ShapeAggregate):
    '''A container for aggregating round things (circles).
    Frequently I refer to circles as "rounds", sorry. :)
    The only things we maintain for a circle is where the center is
    (in object-relative coordinates) and how big its radius is.'''
    def __init__(self):
        super(RoundAggregate, self).__init__()
        self.centers = Numeric.array([], Numeric.Float16)
        self.centers.shape = (0, 2)
        self.radius = Numeric.array([], Numeric.Float16)
    def add_new(self, object, label, radius, center = None):
        """Add a new circle.
        If the center isn't provided, it is assumed to be (r, r)."""
        super(RoundAggregate, self).add_new(object, label)
        if not center:
            center = (radius, radius)
        center = Numeric.array(center, Numeric.Float16)
        self.centers = Numeric.concatenate([self.centers, [center]])
        self.radius = Numeric.concatenate([self.radius, [radius]])
    def delete_object(self, object):
        '''Delete an object.'''
        self.centers = Numeric.take(self.centers, 
                                    Numeric.nonzero(self.object != object))
        self.radius = Numeric.take(self.radius, 
                                   Numeric.nonzero(self.object != object))
        super(RoundAggregate, self).delete_object(object)
class Geometry(object):
    '''A collection of the polygons and round things which make up an object.
    We keep track of the objects and their nicknames in objects_nicknames,
    which is a dict that maps objects to integers. To reverse the mapping,
    we use nicknames_objects. We also keep positions, velocities, masses,
    and accelerations for each object.
    Note that object n is made up of self.pos[n], self.velo[n], etc., plus
    possibly more than one circle or polygon, which may not be numbered n.'''
    def __init__(self):
        self.rounds = RoundAggregate()
        self.polys = PolygonAggregate()
        self.objects_nicknames = {}
        self.nicknames_objects = []
        self.pos = Numeric.array([], Numeric.Float32)
        self.pos.shape = (0, 2)
        self.velo = Numeric.array([], Numeric.Float32)
        self.velo.shape = (0, 2)
        self.accel = Numeric.array([], Numeric.Float32)
        self.accel.shape = (0, 2)
        self.mass = Numeric.array([], Numeric.Int8)
    def add_object(self, object, pos, velo, accel, mass):
        '''Add a new object. This creates a new entry in self.nicknames_objects
        and self.objects_nicknames, as well as in all the other arrays.'''
        n = len(self.nicknames_objects)
        self.objects_nicknames[object] = n
        self.pos = Numeric.concatenate([self.pos, [pos]])
        self.velo = Numeric.concatenate([self.velo, [velo]])
        self.accel = Numeric.concatenate([self.accel, [accel]])
        self.mass = Numeric.concatenate([self.mass, [mass]])
    def delete_object(self, object):
        '''Delete an object and all its geometric information.'''
        if object not in self.objects_nicknames: return # already deleted
        n = self.objects_nicknames[object]
        del self.nicknames_objects[n]
        del self.objects_nicknames[object]
        # Deleting object makes a hole in the nicknames.
        # Rename all nicknames above the hole to be one less.
        for key, value in self.objects_nicknames.iteritems():
            if value > n: self.objects_nicknames[key] = value - 1
        def r(array): # remove n from array
            return Numeric.concatenate([array[:n], array[n+1:]])
        self.pos = r(self.pos)
        self.velo = r(self.velo)
        self.accel = r(self.accel)
        self.mass = r(self.mass)
        # Delete all shapes belonging to this object.
    def add_polygon(self, object, label, points):
        '''Add a polygon to an object.'''
        self.polys.add_new(self.objects_nicknames[object], label, points)
    def add_round(self, object, label, radius, center=None):
        '''Add a circle to an object.'''
        self.rounds.add_new(self.objects_nicknames[object], label, radius, 
    def get_pos(self, object):
        '''Get the position of an object.'''
        return self.pos[self.objects_nicknames[object]]
    def set_pos(self, object, pos):
        '''Set the position of an object.'''
        self.pos[self.objects_nicknames[object]] = pos.astype(self.pos.dtype.char)
    def move(self, object, v):
        '''Move an object by the vector v.'''
        self.pos[self.objects_nicknames[object]] += v.astype(self.pos.dtype.char)
    #def get_fixed(self, object):
    #    '''Get whether an object is "fixed". This should probably be outside
    #    this class. I define objects of mass 0 or less to be immobile.'''
    #    return (self.mass[self.objects_nicknames[object]] &lt:= 0)
    def set_accel(self, object, accel):
        '''Set the acceleration of an object.'''
        self.accel[self.objects_nicknames[object]] = accel.astype(self.accel.dtype.char)
    def get_velo(self, object):
        return self.velo[self.objects_nicknames[object]]
    def set_velo(self, object, velo):
        self.velo[self.objects_nicknames[object]] = velo.astype(self.velo.dtype.char)
    # Functions used to update velocity and positions.
    def push_accel(self):
        '''Updates velocity with the corresponding acceleration.'''
        self.velo += self.accel
    def push_velo(self, frac = 1.0):
        '''Updates the position with the corresponding velocity.
        This function can be used to only move the objects a fraction of the 
        self.pos += (frac*self.velo).astype(self.pos.dtype.char)
    def get_shapes(self):
        '''Returns all the information needed by the display to draw everything.'''
        return (self.rounds, self.polys)
    def collide_all(self, overlap_func, bounce_func):
        '''Main collision engine.
        overlap_func is overlap_func(o1, k1, o2, k2, n, t):
        a function which is called to remove an overlap. o1 and o2 are the 
        objects which overlap. k1 is the "part" of o1 that overlaps with o2, 
        and vice versa for k2. n and t specify the direction and distance of 
        the shortest overlap.
        bounce_func is similar, except that t is the time in the future at 
        which the bounce will "happen".
        This function works by calling subsidiary functions, named 
        collide_matrix_X_Y, which will return matrices indicating time and 
        normal of collision among all Xs against all Ys. It then picks the 
        least ("soonest") collision and deals with that.'''
        # We use 10 to indicate "no collision this tick".
        tfrac = 0
        def unpack(tmat, nmat, r, c, set1, set2):
            '''Subsidiary function to pick out the objects, time, and normal 
            given all relevant info.'''
            t = tmat[r, c]
            n = nmat[r, c]
            o1 = self.nicknames_objects[set1.object[r]]
            k1 = set1.label[r]
            o2 = self.nicknames_objects[set2.object[c]]
            k2 = set2.label[c]
            return (o1, k1, o2, k2, n, t)
        def act_on(tmat, nmat, r, c, set1, set2):
            '''Function to handle an event at a given time.
            If you know tmat and nmat are the matrices where the next event
            is going to happen, and it happens at r, c of set1 and set2, this
            is what you do.'''
            t = tmat[r, c]
            if t > 0:
                bounce_func(*unpack(tmat, nmat, r, c, set1, set2))
                # To make sure this doesn't register as an overlap,
                # we push things away just a little bit.
                overlap_func(*unpack(tmat, nmat, r, c, set1, set2))
            tmat[r, c] = 10
        t = 0
        while t + tfrac == 1:
            # skip was originally intended to allow the engine to specify that
            # a collision/overlap was to be ignored. This functionality should
            # work but I haven't tested it much yet and don't really use it yet.
            skip = True
            tcolrp, ncolrp = self.collide_matrix_round_poly()
            tcolrr, ncolrr = self.collide_matrix_round_round()
            while skip:
                # We don't need to recompute matrices if we skip events.
                # Also, we mark events as "skipped" by blotting them out of the
                # matrices. So we don't want to recompute them.
                rrp, crp = min_index(tcolrp)
                rrr, crr = min_index(tcolrr)
                # Find the minimum across matrices.
                trp = tcolrp[rrp, crp]
                trr = tcolrr[rrr, crr]
                t = min(trp, trr)
                if tfrac + t >= 1:
                    # We've computed everything that happens this tick.
                if trp == trr:
                    act_on(tcolrp, ncolrp, rrp, crp, self.polys, self.rounds)
                    act_on(tcolrr, ncolrr, rrr, crr, self.rounds, self.rounds)
                if t > 0:
                    tfrac += t
                # If you wanted to use the 'skip events' functionality, you'd
                # delete the following line, and make some other way of setting
                # skip -- possibly return values from the bounce or overlap
                # handlers.
                skip = False
        # The next collision doesn't happen in this tick -- maybe next tick,
        # maybe never. We push the rest of the tick into velocity.
        self.push_velo(1 - tfrac)
    def collide_matrix_round_poly(self):
        '''Computes collision matrices for the round objects against the 
        # These are the output matrices. timematrix is n x c.
        timematrix = Numeric.zeros((len(self.polys.object),len(self.rounds.object)))\
        # normmatrix is n x c x 2, and holds the normal that generated the
        # corresponding collision time.
        normmatrix = Numeric.zeros(timematrix.shape + (2,), timematrix.dtype.char)
        # First, we calculate all the projections for circles.
        # centers is the real location of the circle's centers.
        centers = self.rounds.centers + Numeric.take(self.pos, self.rounds.object)
        r = self.rounds.radius
        # roundmatrix is the centers projected on all normals. N x c
        roundmatrix = Numeric.matrixmultiply(self.polys.normals, 
        # roundmin and roundmax are the min/max of the projection of
        # each circle on each normal. These are also N x c.
        roundmin = roundmatrix - r
        roundmax = roundmatrix + r
        # We also need the normals between the centers of the circles
        # and the corners of the polygons. Use tempind to specify
        # elements of each group.  I could do broadcasting instead,
        # but have chosen not to.
        tempind = Numeric.indices((len(self.polys.points), 
        cornernormals = normalize(Numeric.take(self.polys.points, tempind[0]) - 
                                  Numeric.take(centers, tempind[1])) # P x c x 2
        # Projections of the centers of circles on each normal.
        cornerprojs = Numeric.sum(cornernormals * centers[Numeric.NewAxis, :], -1)
        # Now, for the polygons.  We project all points on all
        # normals, and eventually take min and max for each polygon.
        # N x P.
        polymatrix = dot(self.polys.normals, Numeric.transpose(self.polys.points))
        # These are the "data" matrices. They store some important intermediate
        # results.
        # For the normals in self.polys.normals, we need to track the
        # min and max value each polygon has on each normal. Those are
        # normalmin and normalmax. The relative velocity of each
        # polygon-circle along those normals is stored in v. At first
        # they're empty.
        normalmin = Numeric.array([], Numeric.Float32) # eventually N x 1
        normalmax = Numeric.array([], Numeric.Float32) # eventually N x 1
        v = Numeric.array([], Numeric.Float32)         # eventually N x c (x 1)
        # For the corner-center normals, the dimensions are a little
        # crazier.  For each corner, there are c normals. For each of
        # those normals, we project only this polygon onto it, giving
        # us a min and max.  This gives us p x c projections for each
        # polygon, or, in total, cornermin and cornermax are P x c.
        cornermin = Numeric.array([], Numeric.Float32) # eventually P x c
        cornermax = Numeric.array([], Numeric.Float32) # eventually P x c
        # Likewise, the relative velocity along the corner normals is
        # P x c because for each 1 x c x 2 row in cornernorms, there
        # is only one polygon, so only c relative velocities (1 x c x
        # 2), and 1 x c projected relative velocities.
        vc = Numeric.array([], Numeric.Float32)        # eventually P x c (x 1)
        vc.shape = (0, len(self.rounds.object))
        v.shape = (0, len(self.rounds.object))
        cornermin.shape = (0, len(self.rounds.object))
        cornermax.shape = (0, len(self.rounds.object))
        # For each polygon we need to compute the minimum and maximum of
        # the projections for appropriate normals.
        for i in range(len(self.polys.owned)):
            begin, end = self.polys.owned[i]
            relvelo = (Numeric.take(self.velo, self.rounds.object) - 
                       self.velo[self.polys.object[i]]) # c x 2
            # This poly against its normals -- n x p
            thispoly = polymatrix[begin:end, begin:end]
            thismin = Numeric.minimum.reduce(thispoly, 1) # n x 1
            thismax = Numeric.maximum.reduce(thispoly, 1) # n x 1
            thisveloproj =[begin:end], 
            normalmin = Numeric.concatenate([normalmin, thismin])
            normalmax = Numeric.concatenate([normalmax, thismax])
            v = Numeric.concatenate([v, thisveloproj])
            # Project the p x c relevant normals along the p points.
            # thesecorners.shape = p x c x 2 . 2 x p = p x c x p
            thesecorners =[begin:end], 
            thiscmin = Numeric.minimum.reduce(thesecorners, -1) # p x c
            thiscmax = Numeric.maximum.reduce(thesecorners, -1) # p x c
            thisvc = Numeric.sum(cornernormals[begin:end]*
                                 relvelo[Numeric.NewAxis], -1) # p x c
            cornermin = Numeric.concatenate([cornermin, thiscmin])
            cornermax = Numeric.concatenate([cornermax, thiscmax])
            vc = Numeric.concatenate([vc, thisvc])
        # matrix[i][j] is the time for the poly which owns
        # self.polys.points[i] to collide with circle j along
        # self.polys.normals[i].
        matrix, flipmat = time_to_intersect(roundmin, roundmax,
                                             normalmin, normalmax, v) # N x c
        # self.polys.normals is N x 2, flipmat is N x c.
        normals = self.polys.normals[:, Numeric.NewAxis] * \
                         (1 + (-2*flipmat[..., Numeric.NewAxis]))
        cornmat, flipmat = time_to_intersect(cornerprojs - r, cornerprojs + r, 
                                             cornermin, cornermax, vc)
        # cornernormals is P x c x 2, flipmat is P x c
        cornernormals = flip_normals(cornernormals, flipmat)
        self.extract_polys(matrix, normals, cornmat, cornernormals, 
                           timematrix, normmatrix)
        # For consistency with round_round, we return the opposite normals. ????
        return timematrix, -normmatrix
    def extract_polys(self, matrix, normals, cornmat, cornernormals, timematrix, normmatrix):
        '''This function exists only to allow benchmarking to be more
        def get_rowmax(segment):
            return Numeric.maximum.reduce(segment, 0)
        def get_rownums(segment, max):
            '''Get the numbers that represent the norms that 
            give the maximum time.'''
            # We need to take the normals that belong to this polygon
            # and aggregate the results to find time-to-collision between
            # that polygon and that circle.
            # Find the normals that gave us min.
            # arange is just a column vector of indices.
            arange = Numeric.arange(segment.shape[0]) # p
            arange = Numeric.reshape(Numeric.repeat(arange, segment.shape[1]), 
            # We start with a matrix the shape of matrix[beginnorm:endnorm],
            # but with an index if that location is the right normal.
            normnums = Numeric.choose(segment == max, (len(arange), arange))
            # Pick the first normal that fits.
            return Numeric.minimum.reduce(normnums, 0)
        def get_rownorms(normals, normnums):
            '''Get the normals referred to by normnums.'''
            n, c, two = normals.shape
            cnormlist = Numeric.reshape(normals, (n*c,2))
            w = c
            arange = Numeric.arange(c)
            normrow2 = Numeric.take(cnormlist, (normnums+begin)*w+arange)
            return normrow2
        for i in range(len(self.polys.owned)):
            begin, end = self.polys.owned[i]
            matseg = matrix[begin:end]
            cornseg = cornmat[begin:end]
            # Both are 1 x c:
            newrow = get_rowmax(matseg)
            newrow2 = get_rowmax(cornseg)
            # normnums is the index of the normal for each circle.
            # normnums is 1 x c.
            normnums = get_rownums(matseg, newrow)
            normnums2 = get_rownums(cornseg, newrow2)
            # Take the normal corresponding to each element in normnums.
            normrow = get_rownorms(normals, normnums)
            normrow2 = get_rownorms(cornernormals, normnums2)
            timematrix[i, :] = Numeric.maximum(newrow, newrow2)
            # To pick the right normals, we have to do magic.
            a = Numeric.arange(len(normrow))
            takes = Numeric.choose(newrow > newrow2, (a+len(newrow), a))
            normmatrix[i, :] = Numeric.take(Numeric.concatenate([normrow, 
                                            normrow2]), takes)
    def collide_matrix_round_round(self):
        '''Computes collision matrices for the round objects against the other 
        round objects.'''
        centers = self.rounds.centers + Numeric.take(self.pos, 
        norms = normalize(centers[Numeric.NewAxis] - 
                          centers[:, Numeric.NewAxis]) # c x c x 2
        # projs1[i][j] is the projection of circle i on the normal
        # between circle i and circle j.  projs2[i][j] is the
        # projection of circle j on the same normal.
        projs1 = Numeric.sum(centers[Numeric.NewAxis] * norms, -1)
        projs2 = Numeric.sum(centers[:, Numeric.NewAxis] * norms, -1)
        r = self.rounds.radius
        v = Numeric.take(self.velo, self.rounds.object)
        relvelo = (v[Numeric.NewAxis] - v[:, Numeric.NewAxis]) #  c x c x 2
        v = Numeric.sum(norms*relvelo, -1)
        timemat, flipmat = time_to_intersect(projs1 - r[:, Numeric.NewAxis],
                                             projs1 + r[:, Numeric.NewAxis],
                                             projs2 - r[Numeric.NewAxis],
                                             projs2 + r[Numeric.NewAxis], v)
        # flipmat always all zeroes here, by construction of the normals.
        # As a result, we don't flip any normals.
        # Blot out all collisiosn where the objects are the same.
        same = self.rounds.object[Numeric.NewAxis] == \
                    self.rounds.object[:, Numeric.NewAxis]
        Numeric.putmask(timemat, same, 10)
        # Make matrix triangular. (Blot out lower half.)
        range = Numeric.arange(len(centers))
        lowhalf = range[:, Numeric.NewAxis] > range[Numeric.NewAxis]
        Numeric.putmask(timemat, lowhalf, 10)
        return timemat, norms
def min_index(array):
    '''Utility function to extract the index (r,c) of the minimum element
    in a 2d array.'''
    width = array.shape[-1]
    min = array
    for i in range(len(array.shape)):
        min = Numeric.minimum.reduce(min)
    locations = Numeric.compress((array==min).ravel(), 
    i = locations[0]
    return i/width, i%width
def make_segments(p):
    '''Constructs a list of vectors, representing the line segments of a
    polygonal object.'''
    e = []
    lp = p[-1]
    for tp in p:
        lp = tp
    return Numeric.array(e)
def overlap(geometry, geompos, obj):
    '''FIXME: this should be a utility function that computes whether obj
    overlaps anything.'''
def time_to_intersect(amin, amax, bmin, bmax, v):
    '''Computes the matrix of times when the objects represented in a
    collide with the objects represented in b, where the relative velocties
    are specified by v.
    We also return a logical matrix, where 1 means "the objects are backwards,
    so flip the normal before applying overlap handlers."'''
    # This function is mostly cribbed from the polycolly stuff.  It
    # takes advantage of broadcasting to allow amin, amax to be
    # different size from bmin, bmax.
    if len(bmin.shape) == 2:
        bmin = bmin[:, Numeric.NewAxis]
        bmax = bmax[:, Numeric.NewAxis]
    where = Numeric.where
    greater = Numeric.greater
    or_ = Numeric.logical_or
    btoadiff = amin - bmax # If there's no overlap, exactly one of these
    atobdiff = bmin - amax # will be negative.
    ta = atobdiff/v
    tb = -btoadiff/v
    tbig = where(greater(ta, tb), ta, tb)
    tsmall = where(greater(ta, tb), tb, ta)
    # t is the time to collide, if there is to be a collision
    t = where(greater(tsmall, 0), tsmall, where(greater(tbig, 0), tbig, 10))
    # If v is too small, t is wrong, so just say 10, for "no collision".
    t = where(greater(Numeric.fabs(v), 0.001), t, 10)
    # Pick the "least negative" overlap.
    overlap = where(btoadiff > atobdiff, btoadiff, atobdiff)
    fliplap = btoadiff > atobdiff
    newmat = where(or_(bmax == amin, amax == bmin), t, overlap)
    flip = where(or_(bmax == amin, amax == bmin), 0, fliplap)
    return newmat, flip
def flip_normals(normals, flip):
    """Flips the normals using a flip matrix like that returned by
    if normals.shape != flip.shape:
        flip = Numeric.reshape(flip, flip.shape + (1, ))
    return normals + normals * (-2 * flip)
if __name__=="__main__":
    class Thing:
    class OverlapException:
    class BounceException:
    import math
    o1 = Thing()
    g = Geometry()
    g.add_object(o1, (0,0), (0, 0), (0, 0), 1)
    g.add_polygon(o1, 0, [(0, 0), (60, 0), (30, 30*math.sqrt(3))])
    o2 = Thing()
    g.add_object(o2, (0,0), (0, 0), (0, 0), 1)
    g.add_round(o2, 0, 16)
    def bounce(o1, k1, o2, k2, n, t):
        print 'bounce', o1, k1, o2, k2, n, t
        raise BounceException
    def overlap(o1, k1, o2, k2, n, t):
        print 'overlap', o1, k1, o2, k2, n, t
        g.move(o2, -n*t)        
        raise OverlapException
        g.collide_all(overlap, bounce)
    except OverlapException:
    print 'o1 now at:', g.get_pos(o1)
    print 'o2 now at:', g.get_pos(o2)
    g.move(o2, Numeric.array([.01, 0]))
        g.collide_all(overlap, bounce)
    except OverlapException:
    g.move(o2, Numeric.array([25, 40]))
    g.set_velo(o2, Numeric.array([0, -1.07191]))
        g.collide_all(overlap, bounce)
    except BounceException:
    print "Everything works!"