Fix some bugs for contact detection

(Still enough bugs left to keep me busy next week :p)