ADD several test to debug collision funtions.

Issue #3
Issue #4