Refactoring, added segment vs aabb test (WIP)