Clean up casts and Math.* functions.
Clean up body/BodyIndexPair.java
Clean up collision/BroadPhasePair.java
Clean up collision/broadphase/BodyPair.java
Changes related to class clean up.
Clean up body/CollisionBody.java
Clean up collision/CollisionDetection.java
Clean up narrowphase/NarrowPhaseAlgorithm.java
Clean up shape classes.
Clean up mathematics/Quaternion.java
Removed invalid assertions.
Fixed assertion.
Fixed margin usage. This should be consistent across all classes.
Sphere and capsule shape constructors now have an explicit margin parameter.