Code that worked in the late-nighter in which we plugged the collision checking into dviz. There are way too many debug statements in here. We need to get it all cleaned up.