Basic broad phase code, and some bug fixes