# Commits

committed cce7e07

Simplify CollisionAlgorithmProvider interface, and repair all currently implemented Shapes.

# ferox-physics/src/main/java/com/ferox/physics/collision/ClosestPair.java

`+package com.ferox.physics.collision;`
`+`
`+import com.ferox.math.Const;`
`+import com.ferox.math.Vector3;`
`+`
`+/**`
`+ * ClosestPair is a data-storage class that contains the closest pair of points`
`+ * between two Collidables, A and B. It can differentiate between separated`
`+ * objects and intersecting objects. It is used by a {@link CollisionAlgorithm} to`
`+ * compute accurate collision information between pairs of objects.`
`+ * `
`+ * @author Michael Ludwig`
`+ */`
`+public class ClosestPair {`
`+    private final Vector3 contactNormalFromA;`
`+    private final Vector3 closestPointOnA;`
`+    private final Vector3 closestPointOnB;`
`+`
`+    private final double distance;`
`+`
`+    /**`
`+     * Create a new ClosestPair. <tt>pointOnA</tt> represents the point on the`
`+     * first object's surface. The point on the second object's surface is`
`+     * reconstructed from <tt>pointOnA</tt>, the <tt>contactNormal</tt>, and the`
`+     * distance along the normal. It is assumed that the contact normal has`
`+     * already been normalized.`
`+     * `
`+     * @param pointOnA The closest point on the A's surface`
`+     * @param contactNormal The normal from pointOnA to the point on B's`
`+     *            surface, normalized`
`+     * @param distance The distance along contactNormal to get to the point on`
`+     *            B's surface, negative for an intersection situation`
`+     * @throws NullPointerException if pointOnA or contactNormal are null`
`+     */`
`+    public ClosestPair(Vector3 pointOnA, Vector3 contactNormal, double distance) {`
`+        if (pointOnA == null || contactNormal == null)`
`+            throw new NullPointerException("Input cannot be null");`
`+        this.distance = distance;`
`+`
`+        contactNormalFromA = contactNormal;`
`+        closestPointOnA = pointOnA;`
`+        closestPointOnB = new Vector3(contactNormal).scale(distance).add(pointOnA);`
`+    }`
`+`
`+    /**`
`+     * Return the normalized contact normal. The normal points in the direction`
`+     * of object A to object B. In the case of intersections, the normal's`
`+     * direction remains consistent with the direction it would be pointing were`
`+     * the two objects to slide apart. This avoids a sudden negation of the`
`+     * contact normal between two objects that approach each other and then`
`+     * intersect; instead, the distance becomes negated.`
`+     * `
`+     * @return The contact normal`
`+     */`
`+    public @Const Vector3 getContactNormal() {`
`+        return contactNormalFromA;`
`+    }`
`+`
`+    /**`
`+     * Return the world-space closest point to object B that is on the surface`
`+     * of object A.`
`+     * `
`+     * @return The closest point in this pair on the surface of A`
`+     */`
`+    public @Const Vector3 getClosestPointOnA() {`
`+        return closestPointOnA;`
`+    }`
`+`
`+    /**`
`+     * Return the world-space closest point to object A that is on the surface`
`+     * of object B.`
`+     * `
`+     * @return The closest point in this pair on the surface of B`
`+     */`
`+    public @Const Vector3 getClosestPointOnB() {`
`+        return closestPointOnB;`
`+    }`
`+`
`+    /**`
`+     * Return the distance between the two points of this closest pair. If the`
`+     * returned value is positive, the two objects are separated. If the`
`+     * distance is negative, the two objects are intersecting. A value of 0`
`+     * implies that the objects are exactly touching each other.`
`+     * `
`+     * @return The contact distance`
`+     */`
`+    public double getDistance() {`
`+        return distance;`
`+    }`
`+`
`+    /**`
`+     * Convenience function to return whether or not this pair represents the`
`+     * closest point pair between two intersecting convex hulls. This returns`
`+     * true if and only if {@link #getDistance()} is less than or equal to 0.`
`+     * `
`+     * @return True if the two involved objects are intersecting`
`+     */`
`+    public boolean isIntersecting() {`
`+        return distance <= .00001;`
`+    }`
`+    `
`+    @Override`
`+    public String toString() {`
`+        return "Pair(normal: " + contactNormalFromA + ", a: " + closestPointOnA + ", b: " + closestPointOnB + ", dist: " + distance + ")";`
`+    }`
`+}`

# ferox-physics/src/main/java/com/ferox/physics/collision/CollisionAlgorithm.java

` `
` import com.ferox.math.Const;`
` import com.ferox.math.Matrix4;`
`-import com.ferox.physics.collision.algorithm.ClosestPair;`
` `
` /**`
`  * <p>`
`  * responsible for computing two vectors within world space. Each vector`
`  * represents the closest point on one {@link Collidable} to the other.`
`  * Implementations must handle cases where the two objects are intersecting each`
`- * other, too.`
`- * </p>`
`- * <p>`
`- * CollisionAlgorithms must be thread-safe because it is likely that code`
`- * relying on the algorithms will reuse the same instance. This should not be`
`- * difficult as each invocation of`
`- * {@link #getClosestPair(Collidable, Collidable)} should be independent.`
`- * </p>`
`+ * other as well.`
`  * `
`  * @author Michael Ludwig`
`  */`

# ferox-physics/src/main/java/com/ferox/physics/collision/CollisionAlgorithmProvider.java

` package com.ferox.physics.collision;`
` `
`-import com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm;`
`-import com.ferox.physics.collision.algorithm.SphereSphereCollisionAlgorithm;`
`-import com.ferox.physics.collision.shape.ConvexShape;`
` `
` /**`
`  * CollisionAlgorithmProviders are responsible for providing CollisionAlgorithm`
`  * instances that can be used to compute accurate intersections between two`
`- * {@link Shape shapes} of the requested types. As with the other components of`
`- * the collision subsystem, CollisionAlgorithmProviders should be thread-safe.`
`+ * {@link Shape shapes} of the requested types.`
`  * `
`  * @author Michael Ludwig`
`  */`
`      * <p>`
`      * Return a CollisionAlgorithm that can compute intersections between the`
`      * two shape types. If there is no registered algorithm supporting both`
`-     * types, then null should be returned. When choosing an algorithm to`
`-     * return, the providers should attempt to select an algorithm that best`
`-     * matches the requested types. Additionally, algorithms registered later`
`-     * should be preferred over older algorithms in the event of a tie.`
`-     * </p>`
`-     * <p>`
`-     * As an example, the {@link GjkEpaCollisionAlgorithm} supports any two`
`-     * {@link ConvexShape convex shapes}. Theoretically, it could then be used`
`-     * to compute the intersection between two spheres. However, a dedicated`
`-     * sphere-sphere algorithm (such as the`
`-     * {@link SphereSphereCollisionAlgorithm}), should be selected over the`
`-     * gjk-epa algorithm.`
`-     * </p>`
`+     * types, then null should be returned.`
`      * `
`      * @param <A> The Shape type of the first class`
`      * @param <B> The Shape type of the second class`
`      * @throws NullPointerException if shapeA or shapeB are null`
`      */`
`     public <A extends Shape, B extends Shape> CollisionAlgorithm<A, B> getAlgorithm(Class<A> shapeA, Class<B> shapeB);`
`-`
`-    /**`
`-     * Register a new CollisionAlgorithm with the CollisionAlgorithmProvider.`
`-     * Implementations may choose to register a default set of algorithms as`
`-     * well.`
`-     * `
`-     * @param algorithm An algorithm to register`
`-     * @throws NullPointerException if algorithm is null`
`-     */`
`-    public void register(CollisionAlgorithm<?, ?> algorithm);`
`-`
`-    /**`
`-     * Unregister any CollisionAlgorithm whose class equals the given type,`
`-     * <tt>algorithmType</tt>.`
`-     * `
`-     * @param algorithmType The algorithm type to remove`
`-     * @throws NullPointerException if algorithmType is null`
`-     */`
`-    public void unregister(Class<? extends CollisionAlgorithm<?, ?>> algorithmType);`
` }`

# ferox-physics/src/main/java/com/ferox/physics/collision/CollisionCallback.java

` package com.ferox.physics.collision;`
` `
`-import com.ferox.physics.collision.algorithm.ClosestPair;`
` `
` /**`
`  * <p>`

# ferox-physics/src/main/java/com/ferox/physics/collision/DefaultCollisionAlgorithmProvider.java

`         return depth;`
`     }`
` `
`-    @Override`
`     public void register(CollisionAlgorithm<?, ?> algorithm) {`
`         if (algorithm == null)`
`             throw new NullPointerException("CollisionAlgorithm cannot be null");`
`         }`
`     }`
` `
`-    @Override`
`     public void unregister(Class<? extends CollisionAlgorithm<?, ?>> algorithmType) {`
`         if (algorithmType == null)`
`             throw new NullPointerException("CollisionAlgorithm type cannot be null");`

# ferox-physics/src/main/java/com/ferox/physics/collision/Shape.java

`     /**`
`      * @return Return the current margin for this shape, defaults to .05`
`      */`
`-    public float getMargin();`
`+    public double getMargin();`
` }`

# ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/ClosestPair.java

`-package com.ferox.physics.collision.algorithm;`
`-`
`-import com.ferox.math.Const;`
`-import com.ferox.math.Vector3;`
`-import com.ferox.physics.collision.CollisionAlgorithm;`
`-`
`-/**`
`- * ClosestPair is a data-storage class that contains the closest pair of points`
`- * between two Collidables, A and B. It can differentiate between separated`
`- * objects and intersecting objects. It is used by a {@link CollisionAlgorithm} to`
`- * compute accurate collision information between pairs of objects.`
`- * `
`- * @author Michael Ludwig`
`- */`
`-public class ClosestPair {`
`-    private final Vector3 contactNormalFromA;`
`-    private final Vector3 closestPointOnA;`
`-    private final Vector3 closestPointOnB;`
`-`
`-    private final double distance;`
`-`
`-    /**`
`-     * Create a new ClosestPair. <tt>pointOnA</tt> represents the point on the`
`-     * first object's surface. The point on the second object's surface is`
`-     * reconstructed from <tt>pointOnA</tt>, the <tt>contactNormal</tt>, and the`
`-     * distance along the normal. It is assumed that the contact normal has`
`-     * already been normalized.`
`-     * `
`-     * @param pointOnA The closest point on the A's surface`
`-     * @param contactNormal The normal from pointOnA to the point on B's`
`-     *            surface, normalized`
`-     * @param distance The distance along contactNormal to get to the point on`
`-     *            B's surface, negative for an intersection situation`
`-     * @throws NullPointerException if pointOnA or contactNormal are null`
`-     */`
`-    public ClosestPair(Vector3 pointOnA, Vector3 contactNormal, double distance) {`
`-        if (pointOnA == null || contactNormal == null)`
`-            throw new NullPointerException("Input cannot be null");`
`-        this.distance = distance;`
`-`
`-        contactNormalFromA = contactNormal;`
`-        closestPointOnA = pointOnA;`
`-        closestPointOnB = new Vector3(contactNormal).scale(distance).add(pointOnA);`
`-    }`
`-`
`-    /**`
`-     * Return the normalized contact normal. The normal points in the direction`
`-     * of object A to object B. In the case of intersections, the normal's`
`-     * direction remains consistent with the direction it would be pointing were`
`-     * the two objects to slide apart. This avoids a sudden negation of the`
`-     * contact normal between two objects that approach each other and then`
`-     * intersect; instead, the distance becomes negated.`
`-     * `
`-     * @return The contact normal`
`-     */`
`-    public @Const Vector3 getContactNormal() {`
`-        return contactNormalFromA;`
`-    }`
`-`
`-    /**`
`-     * Return the world-space closest point to object B that is on the surface`
`-     * of object A.`
`-     * `
`-     * @return The closest point in this pair on the surface of A`
`-     */`
`-    public @Const Vector3 getClosestPointOnA() {`
`-        return closestPointOnA;`
`-    }`
`-`
`-    /**`
`-     * Return the world-space closest point to object A that is on the surface`
`-     * of object B.`
`-     * `
`-     * @return The closest point in this pair on the surface of B`
`-     */`
`-    public @Const Vector3 getClosestPointOnB() {`
`-        return closestPointOnB;`
`-    }`
`-`
`-    /**`
`-     * Return the distance between the two points of this closest pair. If the`
`-     * returned value is positive, the two objects are separated. If the`
`-     * distance is negative, the two objects are intersecting. A value of 0`
`-     * implies that the objects are exactly touching each other.`
`-     * `
`-     * @return The contact distance`
`-     */`
`-    public double getDistance() {`
`-        return distance;`
`-    }`
`-`
`-    /**`
`-     * Convenience function to return whether or not this pair represents the`
`-     * closest point pair between two intersecting convex hulls. This returns`
`-     * true if and only if {@link #getDistance()} is less than or equal to 0.`
`-     * `
`-     * @return True if the two involved objects are intersecting`
`-     */`
`-    public boolean isIntersecting() {`
`-        return distance <= .00001;`
`-    }`
`-    `
`-    @Override`
`-    public String toString() {`
`-        return "Pair(normal: " + contactNormalFromA + ", a: " + closestPointOnA + ", b: " + closestPointOnB + ", dist: " + distance + ")";`
`-    }`
`-}`

# ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/GjkEpaCollisionAlgorithm.java

` import com.ferox.math.Const;`
` import com.ferox.math.Matrix4;`
` import com.ferox.math.Vector3;`
`+import com.ferox.physics.collision.ClosestPair;`
` import com.ferox.physics.collision.CollisionAlgorithm;`
` import com.ferox.physics.collision.shape.ConvexShape;`
` `

# ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/MinkowskiDifference.java

` import com.ferox.math.Matrix4;`
` import com.ferox.math.Vector3;`
` import com.ferox.math.Vector4;`
`+import com.ferox.physics.collision.ClosestPair;`
` import com.ferox.physics.collision.Collidable;`
` import com.ferox.physics.collision.shape.ConvexShape;`
` `

# ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/SphereSphereCollisionAlgorithm.java

` import com.ferox.math.Const;`
` import com.ferox.math.Matrix4;`
` import com.ferox.math.Vector3;`
`+import com.ferox.physics.collision.ClosestPair;`
` import com.ferox.physics.collision.CollisionAlgorithm;`
` import com.ferox.physics.collision.shape.Sphere;`
` `

# ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/SwappingCollisionAlgorithm.java

` import com.ferox.math.Const;`
` import com.ferox.math.Matrix4;`
` import com.ferox.math.Vector3;`
`+import com.ferox.physics.collision.ClosestPair;`
` import com.ferox.physics.collision.CollisionAlgorithm;`
` import com.ferox.physics.collision.Shape;`
` `

# ferox-physics/src/main/java/com/ferox/physics/collision/shape/AxisSweptShape.java

` package com.ferox.physics.collision.shape;`
` `
`-import com.ferox.math.MutableVector3f;`
`-import com.ferox.math.ReadOnlyVector3f;`
`-import com.ferox.math.Vector3f;`
`+import com.ferox.math.Const;`
`+import com.ferox.math.Vector3;`
` `
` /**`
`  * AxisSweptShape represents a class of class of convex shapes that features a`
`         X, Y, Z`
`     }`
`     `
`-    protected final Vector3f inertiaTensorPartial;`
`+    protected final Vector3 inertiaTensorPartial;`
`     protected final Axis dominantAxis;`
` `
`     /**`
`         if (dominantAxis == null)`
`             throw new NullPointerException("Axis cannot be null");`
`         this.dominantAxis = dominantAxis;`
`-        inertiaTensorPartial = new Vector3f();`
`+        inertiaTensorPartial = new Vector3();`
`     }`
`     `
`     /**`
`     }`
` `
`     @Override`
`-    public MutableVector3f getInertiaTensor(float mass, MutableVector3f result) {`
`-        return inertiaTensorPartial.scale(mass, result);`
`+    public Vector3 getInertiaTensor(double mass, Vector3 result) {`
`+        if (result == null)`
`+            result = new Vector3();`
`+        return result.scale(inertiaTensorPartial, mass);`
`     }`
` `
`     /**`
`      * @return The sign of the dominant component of v`
`      * @throws NullPointerException if v is null`
`      */`
`-    protected int sign(ReadOnlyVector3f v) {`
`+    protected int sign(@Const Vector3 v) {`
`         switch(dominantAxis) {`
`-        case X: return (v.getX() >= 0f ? 1 : -1);`
`-        case Y: return (v.getY() >= 0f ? 1 : -1);`
`-        case Z: return (v.getZ() >= 0f ? 1 : -1);`
`+        case X: return (v.x >= 0.0 ? 1 : -1);`
`+        case Y: return (v.y >= 0.0 ? 1 : -1);`
`+        case Z: return (v.z >= 0.0 ? 1 : -1);`
`         default: return 0;`
`         }`
`     }`
`      * @return The projected distance of v to the dominant axis`
`      * @throws NullPointerException if v is null`
`      */`
`-    protected float sigma(ReadOnlyVector3f v) {`
`-        float c1, c2;`
`+    protected double sigma(Vector3 v) {`
`+        double c1, c2;`
`         switch(dominantAxis) {`
`-        case X: c1 = v.getY(); c2 = v.getZ(); break;`
`-        case Y: c1 = v.getX(); c2 = v.getZ(); break;`
`-        case Z: c1 = v.getX(); c2 = v.getY(); break;`
`+        case X: c1 = v.y; c2 = v.z; break;`
`+        case Y: c1 = v.x; c2 = v.z; break;`
`+        case Z: c1 = v.x; c2 = v.y; break;`
`         default: return -1;`
`         }`
`         `
`-        return (float) Math.sqrt(c1 * c1 + c2 * c2);`
`+        return Math.sqrt(c1 * c1 + c2 * c2);`
`     }`
` }`

# ferox-physics/src/main/java/com/ferox/physics/collision/shape/Box.java

` package com.ferox.physics.collision.shape;`
` `
`-import com.ferox.math.MutableVector3f;`
`-import com.ferox.math.ReadOnlyVector3f;`
`-import com.ferox.math.Vector3f;`
`+import com.ferox.math.Const;`
`+import com.ferox.math.Vector3;`
` `
` public class Box extends ConvexShape {`
`-    private final Vector3f localTensorPartial;`
`-    private final Vector3f halfExtents;`
`+    private final Vector3 localTensorPartial;`
`+    private final Vector3 halfExtents;`
`     `
`     public Box(float xExtent, float yExtent, float zExtent) {`
`-        localTensorPartial = new Vector3f();`
`-        halfExtents = new Vector3f();`
`+        localTensorPartial = new Vector3();`
`+        halfExtents = new Vector3();`
`         `
`         setExtents(xExtent, yExtent, zExtent);`
`     }`
`     `
`-    public ReadOnlyVector3f getHalfExtents() {`
`+    public @Const Vector3 getHalfExtents() {`
`         return halfExtents;`
`     }`
`     `
`-    public ReadOnlyVector3f getExtents() {`
`-        return halfExtents.scale(2f, null);`
`+    public Vector3 getExtents() {`
`+        return new Vector3().scale(halfExtents, 2.0);`
`     }`
`     `
`-    public void setExtents(float width, float height, float depth) {`
`+    public void setExtents(double width, double height, double depth) {`
`         if (width <= 0)`
`             throw new IllegalArgumentException("Invalid width, must be greater than 0, not: " + width);`
`         if (height <= 0)`
`         if (depth <= 0)`
`             throw new IllegalArgumentException("Invalid depth, must be greater than 0, not: " + depth);`
`         `
`-        halfExtents.set(width / 2f, height / 2f, depth / 2f);`
`-        localTensorPartial.set((height * height + depth * depth) / 12f,`
`-                               (width * width + depth * depth) / 12f,`
`-                               (width * width + height * height) / 12f);`
`+        halfExtents.set(width / 2.0, height / 2.0, depth / 2.0);`
`+        localTensorPartial.set((height * height + depth * depth) / 12.0,`
`+                               (width * width + depth * depth) / 12.0,`
`+                               (width * width + height * height) / 12.0);`
`         updateBounds();`
`     }`
`     `
`-    public float getWidth() {`
`-        return halfExtents.getX() * 2f;`
`+    public double getWidth() {`
`+        return halfExtents.x * 2.0;`
`     }`
`     `
`-    public float getHeight() {`
`-        return halfExtents.getY() * 2f;`
`+    public double getHeight() {`
`+        return halfExtents.y * 2.0;`
`     }`
`     `
`-    public float getDepth() {`
`-        return halfExtents.getZ() * 2f;`
`+    public double getDepth() {`
`+        return halfExtents.z * 2.0;`
`     }`
`     `
`     @Override`
`-    public MutableVector3f computeSupport(ReadOnlyVector3f v, MutableVector3f result) {`
`+    public Vector3 computeSupport(@Const Vector3 v, Vector3 result) {`
`         if (result == null)`
`-            result = new Vector3f();`
`+            result = new Vector3();`
`         `
`-        float x = (v.getX() < 0f ? -halfExtents.getX() : halfExtents.getX());`
`-        float y = (v.getY() < 0f ? -halfExtents.getY() : halfExtents.getY());`
`-        float z = (v.getZ() < 0f ? -halfExtents.getZ() : halfExtents.getZ());`
`+        double x = (v.x < 0f ? -halfExtents.x : halfExtents.x);`
`+        double y = (v.y < 0f ? -halfExtents.y : halfExtents.y);`
`+        double z = (v.z < 0f ? -halfExtents.z : halfExtents.z);`
`         `
`         return result.set(x, y, z);`
`     }`
` `
`     @Override`
`-    public MutableVector3f getInertiaTensor(float mass, MutableVector3f result) {`
`-        return localTensorPartial.scale(mass, result);`
`+    public Vector3 getInertiaTensor(double mass, Vector3 result) {`
`+        if (result == null)`
`+            result = new Vector3();`
`+        return result.scale(localTensorPartial, mass);`
`     }`
` }`

# ferox-physics/src/main/java/com/ferox/physics/collision/shape/Cone.java

` package com.ferox.physics.collision.shape;`
` `
`-import com.ferox.math.MutableVector3f;`
`-import com.ferox.math.ReadOnlyVector3f;`
`-import com.ferox.math.Vector3f;`
`+import com.ferox.math.Const;`
`+import com.ferox.math.Vector3;`
` `
` public class Cone extends AxisSweptShape {`
`-    private float halfHeight;`
`-    private float baseRadius;`
`+    private double halfHeight;`
`+    private double baseRadius;`
`     `
`-    public Cone(float baseRadius, float height) {`
`+    public Cone(double baseRadius, double height) {`
`         this(baseRadius, height, Axis.Z);`
`     }`
`     `
`-    public Cone(float baseRadius, float height, Axis dominantAxis) {`
`+    public Cone(double baseRadius, double height, Axis dominantAxis) {`
`         super(dominantAxis);`
`         setBaseRadius(baseRadius);`
`         setHeight(height);`
`     }`
`     `
`-    public float getHeight() {`
`-        return 2f * halfHeight;`
`+    public double getHeight() {`
`+        return 2.0 * halfHeight;`
`     }`
`     `
`-    public float getBaseRadius() {`
`+    public double getBaseRadius() {`
`         return baseRadius;`
`     }`
`     `
`-    public void setHeight(float height) {`
`+    public void setHeight(double height) {`
`         if (height <= 0f)`
`             throw new IllegalArgumentException("Height must be greater than 0, not: " + height);`
`-        this.halfHeight = height / 2f;`
`+        this.halfHeight = height / 2.0;`
`         update();`
`     }`
`     `
`-    public void setBaseRadius(float radius) {`
`+    public void setBaseRadius(double radius) {`
`         if (radius <= 0f)`
`             throw new IllegalArgumentException("Radius must be greater than 0, not: " + radius);`
`         baseRadius = radius;`
`     }`
`     `
`     @Override`
`-    public MutableVector3f computeSupport(ReadOnlyVector3f v, MutableVector3f result) {`
`+    public Vector3 computeSupport(@Const Vector3 v, Vector3 result) {`
`         if (result == null)`
`-            result = new Vector3f();`
`+            result = new Vector3();`
`         `
`-        float sin = baseRadius / (float) Math.sqrt(baseRadius * baseRadius + 4 * halfHeight * halfHeight);`
`+        double sin = baseRadius / Math.sqrt(baseRadius * baseRadius + 4 * halfHeight * halfHeight);`
`         switch(dominantAxis) {`
`         case X:`
`-            if (v.getX() <= v.length() * sin) {`
`-                float sigma = sigma(v);`
`-                if (sigma <= 0f)`
`-                    result.set(-halfHeight, 0f, 0f);`
`+            if (v.x <= v.length() * sin) {`
`+                double sigma = sigma(v);`
`+                if (sigma <= 0.0)`
`+                    result.set(-halfHeight, 0.0, 0.0);`
`                 else`
`-                    result.set(-halfHeight, baseRadius / sigma * v.getY(), baseRadius / sigma * v.getZ());`
`-            } else`
`-                result.set(halfHeight, 0f, 0f);`
`+                    result.set(-halfHeight, baseRadius / sigma * v.y, baseRadius / sigma * v.z);`
`+            } else {`
`+                result.set(halfHeight, 0.0, 0.0);`
`+            }`
`             break;`
`         case Y:`
`-            if (v.getY() <= v.length() * sin) {`
`-                float sigma = sigma(v);`
`-                if (sigma <= 0f)`
`-                    result.set(0f, -halfHeight, 0f);`
`+            if (v.y <= v.length() * sin) {`
`+                double sigma = sigma(v);`
`+                if (sigma <= 0.0)`
`+                    result.set(0.0, -halfHeight, 0.0);`
`                 else`
`-                    result.set(baseRadius / sigma * v.getX(), -halfHeight, baseRadius / sigma * v.getZ());`
`-            } else`
`-                result.set(0f, halfHeight, 0f);`
`+                    result.set(baseRadius / sigma * v.x, -halfHeight, baseRadius / sigma * v.z);`
`+            } else {`
`+                result.set(0.0, halfHeight, 0.0);`
`+            }`
`             break;`
`         case Z:`
`-            if (v.getZ() <= v.length() * sin) {`
`-                float sigma = sigma(v);`
`-                if (sigma <= 0f)`
`-                    result.set(-0f, 0f, -halfHeight);`
`+            if (v.z <= v.length() * sin) {`
`+                double sigma = sigma(v);`
`+                if (sigma <= 0.0)`
`+                    result.set(-0.0, 0.0, -halfHeight);`
`                 else`
`-                    result.set(baseRadius / sigma * v.getX(), baseRadius / sigma * v.getY(), -halfHeight);`
`-            } else`
`-                result.set(0f, 0f, halfHeight);`
`+                    result.set(baseRadius / sigma * v.x, baseRadius / sigma * v.y, -halfHeight);`
`+            } else {`
`+                result.set(0.0, 0.0, halfHeight);`
`+            }`
`             break;`
`         }`
` `
`     }`
`     `
`     private void update() {`
`-        float m1 = 4f / 10f * halfHeight * halfHeight + 3f / 20f * baseRadius * baseRadius;`
`-        float m2 = 3f/ 10f * baseRadius * baseRadius;`
`+        double m1 = 4.0 / 10.0 * halfHeight * halfHeight + 3.0 / 20.0 * baseRadius * baseRadius;`
`+        double m2 = 3.0/ 10.0 * baseRadius * baseRadius;`
`         `
`         switch(dominantAxis) {`
`         case X:`

# ferox-physics/src/main/java/com/ferox/physics/collision/shape/ConvexShape.java

` package com.ferox.physics.collision.shape;`
` `
`+import com.ferox.math.AxisAlignedBox;`
` import com.ferox.math.Const;`
` import com.ferox.math.Vector3;`
` import com.ferox.physics.collision.Shape;`
`  * @see GjkEpaCollisionAlgorithm`
`  */`
` public abstract class ConvexShape implements Shape {`
`-    private float margin;`
`-    private final ReadOnlyAxisAlignedBox bounds;`
`+    private double margin;`
`+    private final AxisAlignedBox bounds;`
`     `
`     public ConvexShape() {`
`-        bounds = new ReadOnlyAxisAlignedBox();`
`-        margin = .05f; // avoid setter so we don't call updateBounds()`
`+        bounds = new AxisAlignedBox();`
`+        margin = .05; // avoid setter so we don't call updateBounds()`
`     }`
` `
`     /**`
`      * Compute and return the evaluation of this convex shape's support`
`      * function, on input <tt>v</tt>. The support should be stored and returned`
`      * in <tt>result</tt>. If result is null, a new vector should be created and`
`-     * returned. The support function should not include the margin.`
`+     * returned. The support function will not include the margin.`
`      * </p>`
`      * <p>`
`      * The support of a convex shape is a function <tt>Sc</tt> that maps a`
`      * vector to a point on the shape, such that <code>dot(Sc, v)</code>`
`-     * maximizes <code>dot(x, v)</code> for all <tt>x</tt> in the shape's`
`+     * maximizes <code>dot(x, v)</code> for all <tt>x</tt> on the shape's`
`      * surface.`
`      * </p>`
`      * `
`     public abstract Vector3 computeSupport(@Const Vector3 v, @Const Vector3 result);`
` `
`     @Override`
`-    public ReadOnlyAxisAlignedBox getBounds() {`
`+    public @Const AxisAlignedBox getBounds() {`
`         return bounds;`
`     }`
` `
`     @Override`
`-    public void setMargin(float margin) {`
`-        if (margin < 0f)`
`+    public void setMargin(double margin) {`
`+        if (margin < 0.0)`
`             throw new IllegalArgumentException("Margin must be at least 0, not: " + margin);`
`         this.margin = margin;`
`         updateBounds();`
`     }`
` `
`     @Override`
`-    public float getMargin() {`
`+    public double getMargin() {`
`         return margin;`
`     }`
` `
`      * affecting the bounds are changed.`
`      */`
`     protected void updateBounds() {`
`-        Vector3f d = new Vector3f();`
`-        Vector3f t = new Vector3f();`
`+        Vector3 d = new Vector3();`
`+        Vector3 t = new Vector3();`
`         `
`-        computeSupport(d.set(1f, 0f, 0f), t);`
`-        float maxX = t.getX() + 2 * margin;`
`-        computeSupport(d.set(-1f, 0f, 0f), t);`
`-        float minX = t.getX() - 2 * margin;`
`+        // FIXME why is it 2 * margin, and not just margin? are we trying to make`
`+        // the bounds have extra padding to encourage non-intersecting shapes?`
`+        // Should experiment with changing this once things start running again`
`+        computeSupport(d.set(1, 0, 0), t);`
`+        double maxX = t.x + 2 * margin;`
`+        computeSupport(d.set(-1, 0, 0), t);`
`+        double minX = t.x - 2 * margin;`
`         `
`-        computeSupport(d.set(0f, 1f, 0f), t);`
`-        float maxY = t.getY() + 2 * margin;`
`-        computeSupport(d.set(0f, -1f, 0f), t);`
`-        float minY = t.getY() - 2 * margin;`
`+        computeSupport(d.set(0, 1, 0), t);`
`+        double maxY = t.y + 2 * margin;`
`+        computeSupport(d.set(0, -1, 0), t);`
`+        double minY = t.y - 2 * margin;`
`         `
`-        computeSupport(d.set(0f, 0f, 1f), t);`
`-        float maxZ = t.getZ() + 2 * margin;`
`+        computeSupport(d.set(0, 0, 1), t);`
`+        double maxZ = t.z + 2 * margin;`
`         computeSupport(d.set(0f, 0f, -1f), t);`
`-        float minZ = t.getZ() - 2 * margin;`
`+        double minZ = t.z - 2 * margin;`
`         `
`-        bounds.getMax().set(maxX, maxY, maxZ);`
`-        bounds.getMin().set(minX, minY, minZ);`
`+        bounds.max.set(maxX, maxY, maxZ);`
`+        bounds.min.set(minX, minY, minZ);`
`     }`
` }`

# ferox-physics/src/main/java/com/ferox/physics/collision/shape/Cylinder.java

` package com.ferox.physics.collision.shape;`
` `
`-import com.ferox.math.MutableVector3f;`
`-import com.ferox.math.ReadOnlyVector3f;`
`-import com.ferox.math.Vector3f;`
`+import com.ferox.math.Const;`
`+import com.ferox.math.Vector3;`
` `
` /**`
`  * Cylinder is a convex shape implementation that represents a cylinder. It has`
`  * @author Michael Ludwig`
`  */`
` public class Cylinder extends AxisSweptShape {`
`-    private float capRadius;`
`-    private float halfHeight;`
`+    private double capRadius;`
`+    private double halfHeight;`
`     `
`     /**`
`      * `
`      * @param capRadius`
`      * @param height`
`      */`
`-    public Cylinder(float capRadius, float height) {`
`+    public Cylinder(double capRadius, double height) {`
`         this(capRadius, height, Axis.Z);`
`     }`
`     `
`-    public Cylinder(float capRadius, float height, Axis dominantAxis) {`
`+    public Cylinder(double capRadius, double height, Axis dominantAxis) {`
`         super(dominantAxis);`
`         setCapRadius(capRadius);`
`         setHeight(height);`
`     }`
`     `
`-    public float getCapRadius() {`
`+    public double getCapRadius() {`
`         return capRadius;`
`     }`
`     `
`-    public float getHeight() {`
`-        return 2f * halfHeight;`
`+    public double getHeight() {`
`+        return 2.0 * halfHeight;`
`     }`
`     `
`-    public void setCapRadius(float radius) {`
`-        if (radius <= 0f)`
`+    public void setCapRadius(double radius) {`
`+        if (radius <= 0.0)`
`             throw new IllegalArgumentException("Radius must be greater than 0, not: " + radius);`
`         capRadius = radius;`
`         update();`
`     }`
`     `
`-    public void setHeight(float height) {`
`-        if (height <= 0f)`
`+    public void setHeight(double height) {`
`+        if (height <= 0.0)`
`             throw new IllegalArgumentException("Height must be greater than 0, not: " + height);`
`-        halfHeight = height / 2f;`
`+        halfHeight = height / 2.0;`
`         update();`
`     }`
` `
`     @Override`
`-    public MutableVector3f computeSupport(ReadOnlyVector3f v, MutableVector3f result) {`
`+    public Vector3 computeSupport(@Const Vector3 v, Vector3 result) {`
`         if (result == null)`
`-            result = new Vector3f();`
`+            result = new Vector3();`
`         `
`-        float sigma = sigma(v);`
`-        if (sigma > 0f) {`
`-            float scale = capRadius / sigma;`
`+        double sigma = sigma(v);`
`+        if (sigma > 0.0) {`
`+            double scale = capRadius / sigma;`
`             switch(dominantAxis) {`
`-            case X: result.set(sign(v) * halfHeight, scale * v.getY(), scale * v.getZ()); break;`
`-            case Y: result.set(scale * v.getX(), sign(v) * halfHeight, scale * v.getZ()); break;`
`-            case Z: result.set(scale * v.getX(), scale * v.getY(), sign(v) * halfHeight); break;`
`+            case X: result.set(sign(v) * halfHeight, scale * v.y, scale * v.z); break;`
`+            case Y: result.set(scale * v.x, sign(v) * halfHeight, scale * v.z); break;`
`+            case Z: result.set(scale * v.x, scale * v.y, sign(v) * halfHeight); break;`
`             }`
`         } else {`
`             switch(dominantAxis) {`
`-            case X: result.set(sign(v) * halfHeight, 0f, 0f); break;`
`-            case Y: result.set(0f, sign(v) * halfHeight, 0f); break;`
`-            case Z: result.set(0f, 0f, sign(v) * halfHeight); break;`
`+            case X: result.set(sign(v) * halfHeight, 0, 0); break;`
`+            case Y: result.set(0, sign(v) * halfHeight, 0); break;`
`+            case Z: result.set(0, 0, sign(v) * halfHeight); break;`
`             }`
`         }`
` `
`     }`
`     `
`     private void update() {`
`-        float m1 = (3f * capRadius * capRadius + 4f * halfHeight * halfHeight) / 12f;`
`-        float m2 = capRadius * capRadius / 2f;`
`+        double m1 = (3.0 * capRadius * capRadius + 4.0 * halfHeight * halfHeight) / 12.0;`
`+        double m2 = capRadius * capRadius / 2.0;`
`         `
`         switch(dominantAxis) {`
`         case X:`

# ferox-physics/src/main/java/com/ferox/physics/collision/shape/Sphere.java

` package com.ferox.physics.collision.shape;`
` `
`-import com.ferox.math.MutableVector3f;`
`-import com.ferox.math.ReadOnlyVector3f;`
`-import com.ferox.math.Vector3f;`
`+import com.ferox.math.Const;`
`+import com.ferox.math.Vector3;`
` `
` /**`
`  * Sphere is a ConvexShape that represents a mathematical sphere.`
`  * @author Michael Ludwig`
`  */`
` public class Sphere extends ConvexShape {`
`-    private float radius;`
`-    private float inertiaTensorPartial;`
`+    private double radius;`
`+    private double inertiaTensorPartial;`
` `
`     /**`
`      * Create a new Sphere with the initial radius, ignoring the margin.`
`      * `
`-     * @see #setRadius(float)`
`+     * @see #setRadius(double)`
`      * @param radius The initial radius`
`      * @throws IllegalArgumentException if radius is less than or equal to 0`
`      */`
`-    public Sphere(float radius) {`
`+    public Sphere(double radius) {`
`         setRadius(radius);`
`     }`
` `
`      * @param radius The new radius, must be greater than 0`
`      * @throws IllegalArgumentException if radius is less than or equal to 0`
`      */`
`-    public void setRadius(float radius) {`
`-        if (radius <= 0f)`
`+    public void setRadius(double radius) {`
`+        if (radius <= 0.0)`
`             throw new IllegalArgumentException("Radius must be greater than 0, not: " + radius);`
`         `
`         this.radius = radius;`
`-        inertiaTensorPartial = 2f * radius * radius / 5f;`
`+        inertiaTensorPartial = 2.0 * radius * radius / 5.0;`
`         updateBounds();`
`     }`
` `
`      * `
`      * @return The radius of the sphere`
`      */`
`-    public float getRadius() {`
`+    public double getRadius() {`
`         return radius;`
`     }`
`     `
`     @Override`
`-    public MutableVector3f computeSupport(ReadOnlyVector3f v, MutableVector3f result) {`
`-        return v.normalize(result).scale(radius);`
`+    public Vector3 computeSupport(@Const Vector3 v, Vector3 result) {`
`+        if (result == null)`
`+            result = new Vector3();`
`+        return result.normalize(v).scale(radius);`
`     }`
` `
`     @Override`
`-    public MutableVector3f getInertiaTensor(float mass, MutableVector3f result) {`
`+    public Vector3 getInertiaTensor(double mass, Vector3 result) {`
`         if (result == null)`
`-            result = new Vector3f();`
`-        float m = inertiaTensorPartial * mass;`
`+            result = new Vector3();`
`+        double m = inertiaTensorPartial * mass;`
`         return result.set(m, m, m);`
`     }`
` }`