Sensor test fix

Optimizations in the collision code and a fix for the sensor test.