Finally some collision resolution results!

It took a lot of time to research how to create this. After hours of vector math, reading documentation and brow-sweat, I was able to implement this basic functionality. It still needs more refinement though.