Contact Resolution part 2

Fixed contact resolution in IndigoWorld class. Works for small numbers
of objects (definitely works for 2). The bounding volume hierarchy is
broken though. Will be fixed in next commit.