Added collision detection

Added collision detection, working fairly well now but still sometimes
has issues.
Also fixed a number of memory leaks so that currently everything should
be mem leak free, also created simple test scene.