Changed Capsule3 to Capsule and added unit tests
Implemented BVH with limited, but probably working functionality