Bug fix in grid generation
Basic collision working
Improved collision test