Added broadphase, some narrowphase

I'm trying to skip GJK and use more specific collision algorithms
instead, sofar the results are satisfying.