Fixed collision position algorithm bugs