Commits

Michael Ludwig committed 670a66e

Repair low-level collision algorithms to use new math API, hopefully all of the equations were updated correctly.

  • Participants
  • Parent commits 4e16237

Comments (0)

Files changed (12)

ferox-physics/pom.xml

             <version>${project.version}</version>
         </dependency>
         <dependency>
+            <groupId>com.lhkbob.ferox</groupId>
+            <artifactId>ferox-util</artifactId>
+            <version>${project.version}</version>
+        </dependency>
+        <dependency>
             <groupId>com.lhkbob.entreri</groupId>
             <artifactId>entreri</artifactId>
             <version>${entreri.version}</version>

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

 package com.ferox.physics.collision;
 
-import com.ferox.math.ReadOnlyMatrix4f;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix4;
 import com.ferox.physics.collision.algorithm.ClosestPair;
 
 /**
      *         or null if no pair could be computed
      * @throws NullPointerException if any argument is null
      */
-    public ClosestPair getClosestPair(A shapeA, ReadOnlyMatrix4f transA, 
-                                      B shapeB, ReadOnlyMatrix4f transB);
+    public ClosestPair getClosestPair(A shapeA, @Const Matrix4 transA, 
+                                      B shapeB, @Const Matrix4 transB);
     
     /**
      * @return The Class representing the type A

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

 package com.ferox.physics.collision;
 
-import com.ferox.math.MutableVector3f;
-import com.ferox.math.bounds.ReadOnlyAxisAlignedBox;
+import com.ferox.math.AxisAlignedBox;
+import com.ferox.math.Const;
+import com.ferox.math.Vector3;
 
 /**
  * <p>
      * 
      * @return The Shape's local bounds
      */
-    public ReadOnlyAxisAlignedBox getBounds();
+    public @Const AxisAlignedBox getBounds();
 
     /**
      * Return the vector containing the inertia tensor for this shape, in its
      * @return The Shape's local inertia tensor in result if it was not null,
      *         otherwise a new vector
      */
-    public MutableVector3f getInertiaTensor(float mass, MutableVector3f result);
+    // FIXME if this isn't called that often, maybe make it create a new instance to simplify signature
+    public Vector3 getInertiaTensor(double mass, Vector3 result);
 
     /**
      * <p>
      * @param margin The new margin, must be greater than or equal to 0
      * @throws IllegalArgumentException if margin is less than 0
      */
-    public void setMargin(float margin);
+    public void setMargin(double margin);
     
     /**
      * @return Return the current margin for this shape, defaults to .05

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

 package com.ferox.physics.collision.algorithm;
 
-import com.ferox.math.ReadOnlyVector3f;
+import com.ferox.math.Const;
+import com.ferox.math.Vector3;
 import com.ferox.physics.collision.CollisionAlgorithm;
 
 /**
  * @author Michael Ludwig
  */
 public class ClosestPair {
-    private final ReadOnlyVector3f contactNormalFromA;
-    private final ReadOnlyVector3f closestPointOnA;
-    private final ReadOnlyVector3f closestPointOnB;
+    private final Vector3 contactNormalFromA;
+    private final Vector3 closestPointOnA;
+    private final Vector3 closestPointOnB;
 
-    private final float distance;
+    private final double distance;
 
     /**
      * Create a new ClosestPair. <tt>pointOnA</tt> represents the point on the
      *            B's surface, negative for an intersection situation
      * @throws NullPointerException if pointOnA or contactNormal are null
      */
-    public ClosestPair(ReadOnlyVector3f pointOnA, ReadOnlyVector3f contactNormal, float distance) {
+    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 = contactNormal.scaleAdd(distance, pointOnA, null);
+        closestPointOnB = new Vector3(contactNormal).scale(distance).add(pointOnA);
     }
 
     /**
      * 
      * @return The contact normal
      */
-    public ReadOnlyVector3f getContactNormal() {
+    public @Const Vector3 getContactNormal() {
         return contactNormalFromA;
     }
 
      * 
      * @return The closest point in this pair on the surface of A
      */
-    public ReadOnlyVector3f getClosestPointOnA() {
+    public @Const Vector3 getClosestPointOnA() {
         return closestPointOnA;
     }
 
      * 
      * @return The closest point in this pair on the surface of B
      */
-    public ReadOnlyVector3f getClosestPointOnB() {
+    public @Const Vector3 getClosestPointOnB() {
         return closestPointOnB;
     }
 
      * 
      * @return The contact distance
      */
-    public float getDistance() {
+    public double getDistance() {
         return distance;
     }
 
      * @return True if the two involved objects are intersecting
      */
     public boolean isIntersecting() {
-        return distance <= .00001f;
+        return distance <= .00001;
     }
     
     @Override

ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/EPA.java

 package com.ferox.physics.collision.algorithm;
 
-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;
 import com.ferox.physics.collision.algorithm.Simplex.Vertex;
 import com.ferox.util.Bag;
 
     }
     
     private static final int EPA_MAX_ITERATIONS = 255;
-    private static final float EPA_ACCURACY = .00001f;
-    private static final float EPA_PLANE_EPS = .00001f;
-    private static final float EPA_INSIDE_EPS = .00001f;
+    private static final double EPA_ACCURACY = .00001;
+    private static final double EPA_PLANE_EPS = .00001;
+    private static final double EPA_INSIDE_EPS = .00001;
     
     private static final int[] I1M3 = new int[] { 1, 2, 0 };
     private static final int[] I2M3 = new int[] { 2, 0, 1 };
     
     
     private final GJK gjk;
-    private final Vector3f tempCache;
+    private final Vector3 tempCache;
     
     
-    private Vector3f normal;
+    private Vector3 normal;
     private Bag<Face> hull;
     
     private Simplex simplex;
-    private float depth;
+    private double depth;
     private Status status;
 
     /**
             throw new NullPointerException("GJK cannot be null");
         
         this.gjk = gjk;
-        depth = 0f;
-        tempCache = new Vector3f();
+        depth = 0.0;
+        tempCache = new Vector3();
     }
 
     /**
     }
 
     /**
-     * Return the depth of penetration after {@link #evaluate(ReadOnlyVector3f)}
+     * Return the depth of penetration after {@link #evaluate(Vector3)}
      * is invoked. If the status of EPA is not VALID, the depth is invalid.
      * 
      * @return The penetration depth
      */
-    public float getDepth() {
+    public double getDepth() {
         return depth;
     }
 
     /**
      * Return the contact normal between the two intersecting convex hulls from
-     * the last invocation of {@link #evaluate(ReadOnlyVector3f)}. If the status
+     * the last invocation of {@link #evaluate(Vector3)}. If the status
      * is not VALID, the returned normal is invalid and possibly null.
      * 
      * @return The contact normal from A to B
      */
-    public ReadOnlyVector3f getNormal() {
+    public @Const Vector3 getNormal() {
         return normal;
     }
 
      * @return The Status of the evaluation
      * @throws NullPointerException if guess is null
      */
-    public Status evaluate(ReadOnlyVector3f guess) {
+    public Status evaluate(@Const Vector3 guess) {
         if (guess == null)
             throw new NullPointerException("Guess cannot be null");
         
         Simplex simplex = gjk.getSimplex();
         MinkowskiDifference function = gjk.getMinkowskiDifference();
         
-        normal = new Vector3f();
+        normal = new Vector3();
         hull = new Bag<Face>();
         status = Status.FAILED;
         
                 
                 Horizon horizon = new Horizon();
                 boolean valid;
-                float wdist;
+                double wdist;
                 for (int iter = 0; iter < EPA_MAX_ITERATIONS; iter++) {
                     // reset the horizon for the next iteration
                     horizon.cf = null; 
                     valid = true;
                     best.pass = ++pass;
                     
-                    best.normal.normalize(w.input);
-                    function.getSupport(w.input, w.vertex);
-                    wdist = best.normal.dot(w.vertex) - best.d;
+                    w.getInputVector().normalize(best.normal);
+                    function.getSupport(w.getInputVector(), w.getVertex());
+                    wdist = best.normal.dot(w.getVertex()) - best.d;
                     
                     if (wdist > EPA_ACCURACY) {
                         for (int j = 0; j < 3 && valid; j++) {
                     }
                 }
                 
-                MutableVector3f projection = outer.normal.scale((float) outer.d, tempCache);
+                Vector3 projection = tempCache.scale(outer.normal, outer.d);
                 normal.set(outer.normal);
                 depth = outer.d;
                 
-                Vector3f t1 = new Vector3f();
-                Vector3f t2 = new Vector3f();
+                // FIXME why do we have tempCache for some vector computations,
+                // but are willing to allocate two vectors here?
+                // At the very least we need to use a unified practice here
+                Vector3 t1 = new Vector3();
+                Vector3 t2 = new Vector3();
                 
-                float w1 = outer.vertices[1].vertex.sub(projection, t1).cross(outer.vertices[2].vertex.sub(projection, t2)).length();
-                float w2 = outer.vertices[2].vertex.sub(projection, t1).cross(outer.vertices[0].vertex.sub(projection, t2)).length();
-                float w3 = outer.vertices[0].vertex.sub(projection, t1).cross(outer.vertices[1].vertex.sub(projection, t2)).length();
-                
-                float sum = w1 + w2 + w3;
+                double w1 = t1.sub(outer.vertices[1].getVertex(), projection).cross(t2.sub(outer.vertices[2].getVertex(), projection)).length();
+                double w2 = t1.sub(outer.vertices[2].getVertex(), projection).cross(t2.sub(outer.vertices[0].getVertex(), projection)).length();
+                double w3 = t1.sub(outer.vertices[0].getVertex(), projection).cross(t2.sub(outer.vertices[1].getVertex(), projection)).length();
+
+                double sum = w1 + w2 + w3;
                 
                 outer.vertices[0].setWeight(w1 / sum);
                 outer.vertices[1].setWeight(w2 / sum);
         
     private Face findBest() {
         Face minf = hull.get(0);
-        float mind = minf.d * minf.d;
-        float maxp = minf.p;
+        double mind = minf.d * minf.d;
+        double maxp = minf.p;
         
         Face f;
-        float sqd;
+        double sqd;
         int ct = hull.size();
         for (int i = 1; i < ct; i++) {
             f = hull.get(i);
     private boolean expand(int pass, Vertex w, Face face, int index, Horizon horizon) {
         if (face.pass != pass) {
             int e1 = I1M3[index];
-            if (face.normal.dot(w.vertex) - face.d < -EPA_PLANE_EPS) {
+            if (face.normal.dot(w.getVertex()) - face.d < -EPA_PLANE_EPS) {
                 Face nf = new Face(face.vertices[e1], face.vertices[index], w, false);
                 if (nf.hullIndex >= 0) {
                     bind(nf, 0, face, index);
     }
     
     private class Face {
-        final MutableVector3f normal;
-        float d;
-        float p;
+        final Vector3 normal;
+        double d;
+        double p;
         
         final Vertex[] vertices;
         final Face[] adjacent;
             pass = 0;
             
             vertices = new Vertex[] { a, b, c };
-            normal = b.vertex.sub(a.vertex, null).cross(c.vertex.sub(a.vertex, tempCache));
-            float l = normal.length();
+            normal = new Vector3().sub(b.getVertex(), a.getVertex()).cross(tempCache.sub(c.getVertex(), a.getVertex()));
+            double l = normal.length();
             boolean v = l > EPA_ACCURACY;
             
-            float invL = 1f / l;
-            float d1 = a.vertex.dot(normal.cross(a.vertex.sub(b.vertex, tempCache), tempCache));
-            float d2 = b.vertex.dot(normal.cross(b.vertex.sub(c.vertex, tempCache), tempCache));
-            float d3 = c.vertex.dot(normal.cross(c.vertex.sub(a.vertex, tempCache), tempCache));
-            p = Math.min(Math.min(d1, d2), d3) * (v ? invL : 1f);
+            double invL = 1.0 / l;
+            double d1 = a.getVertex().dot(tempCache.cross(normal, tempCache.sub(a.getVertex(), b.getVertex())));
+            double d2 = b.getVertex().dot(tempCache.cross(normal, tempCache.sub(b.getVertex(), c.getVertex())));
+            double d3 = c.getVertex().dot(tempCache.cross(normal, tempCache.sub(c.getVertex(), a.getVertex())));
+
+            p = Math.min(Math.min(d1, d2), d3) * (v ? invL : 1.0);
             if (p >= -EPA_INSIDE_EPS)
-                p = 0;
+                p = 0.0;
             
             hullIndex = -1;
             if (v) {
-                d = a.vertex.dot(normal) * invL;
-                normal.scale((float) invL);
+                d = a.getVertex().dot(normal) * invL;
+                normal.scale(invL);
                 if (force || d >= -EPA_PLANE_EPS) {
                     hull.add(this);
                     hullIndex = hull.size() - 1;

ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/GJK.java

 package com.ferox.physics.collision.algorithm;
 
-import com.ferox.math.ReadOnlyVector3f;
-import com.ferox.math.Vector3f;
+import com.ferox.math.Const;
+import com.ferox.math.Vector3;
 
 /**
  * <p>
  */
 public class GJK {
     public static final int GJK_MAX_ITERATIONS = 128;
-    public static final float GJK_MIN_DISTANCE = .0001f;
-    public static final float GJK_DUPLICATE_EPS = .0001f;
-    public static final float GJK_ACCURACY = .0001f;
+    public static final double GJK_MIN_DISTANCE = .0001;
+    public static final double GJK_DUPLICATE_EPS = .0001;
+    public static final double GJK_ACCURACY = .0001;
 
     /**
      * Status represents the three states that a GJK evaluation can take. A
      * @return The status of the evaluation
      * @throws NullPointerException if guess is null
      */
-    public Status evaluate(ReadOnlyVector3f guess) {
+    public Status evaluate(@Const Vector3 guess) {
         if (guess == null)
             throw new NullPointerException("Guess cannot be null");
         
         Status status = Status.VALID;
         simplex = new Simplex();
-        Vector3f ray = new Vector3f(guess);
+        Vector3 ray = new Vector3(guess);
         if (ray.lengthSquared() < GJK_MIN_DISTANCE * GJK_MIN_DISTANCE)
-            ray.set(-1f, 0f, 0f); // for 0-vector guess, choose arbitrary vector to start
+            ray.set(-1, 0, 0); // for 0-vector guess, choose arbitrary vector to start
         
         int iterations = 0;
-        float alpha = 0f;
+        double alpha = 0f;
         
         simplex.addVertex(function, ray, true);
         simplex.getVertex(0).setWeight(1f);
         ray.set(simplex.getVertex(0).getVertex());
         // old support values are tracked so we can terminate when a duplicate is 
         // returned in subsequent iterations
-        Vector3f[] oldSupports = new Vector3f[] { new Vector3f(ray), new Vector3f(ray), 
-                                                  new Vector3f(ray), new Vector3f(ray) };
+        Vector3[] oldSupports = new Vector3[] { new Vector3(ray), new Vector3(ray), 
+                                                new Vector3(ray), new Vector3(ray) };
         
         int lastSupportIndex = 0;
-        
-        float rayLength;
-        ReadOnlyVector3f support;
+        Vector3 scaledVertex = new Vector3();
+
+        double rayLength;
+        Vector3 support;
         while(status == Status.VALID) {
             rayLength = ray.length();
             if (rayLength < GJK_MIN_DISTANCE) {
             // check for duplicates
             boolean duplicate = false;
             for (int i = 0; i < 4; i++) {
-                if (support.epsilonEquals(oldSupports[i], (float) GJK_DUPLICATE_EPS)) {
+                if (support.epsilonEquals(oldSupports[i], GJK_DUPLICATE_EPS)) {
                     duplicate = true;
                     break;
                 }
             
             // check for termination
             alpha = Math.max(ray.dot(support) / rayLength, alpha);
-            if ((rayLength - alpha) - (GJK_ACCURACY * rayLength) <= 0f) {
+            if ((rayLength - alpha) - (GJK_ACCURACY * rayLength) <= 0.0) {
                 // error threshold is small enough so we can terminate
                 simplex.removeVertex();
                 break;
             // reduce the simplex
             if (simplex.reduce()) {
                 // the simplex is valid, compute next guess
-                ray.set(0f, 0f, 0f);
+                ray.set(0.0, 0.0, 0.0);
                 for (int i = 0; i < simplex.getRank(); i++)
-                    simplex.getVertex(i).getVertex().scaleAdd(simplex.getVertex(i).getWeight(), ray, ray);
+                    ray.add(ray, scaledVertex.scale(simplex.getVertex(i).getVertex(), simplex.getVertex(i).getWeight()));
                 
                 // terminate if the simplex is full
                 if (simplex.getRank() == 4)

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

 package com.ferox.physics.collision.algorithm;
 
-import com.ferox.math.MutableVector3f;
-import com.ferox.math.ReadOnlyMatrix4f;
-import com.ferox.math.ReadOnlyVector3f;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix4;
+import com.ferox.math.Vector3;
 import com.ferox.physics.collision.CollisionAlgorithm;
 import com.ferox.physics.collision.shape.ConvexShape;
 
     private static final int MAX_EPA_CHECKS = 4;
     
     @Override
-    public ClosestPair getClosestPair(ConvexShape shapeA, ReadOnlyMatrix4f transA,
-                                      ConvexShape shapeB, ReadOnlyMatrix4f transB) {
+    public ClosestPair getClosestPair(ConvexShape shapeA, @Const Matrix4 transA,
+                                      ConvexShape shapeB, @Const Matrix4 transB) {
         // MinkowskiDifference does the error checking for GjkEpaCollisionAlgorithm
         MinkowskiDifference support = new MinkowskiDifference(shapeA, transA, shapeB, transB);
         support.setNumAppliedMargins(0);
         GJK gjk = new GJK(support);
         
-        ReadOnlyVector3f pa = transA.getCol(3).getAsVector3f();
-        ReadOnlyVector3f pb = transB.getCol(3).getAsVector3f();
+        // FIXME this code is duplicated in a number of places, particularly MinkowskiDifference,
+        // but in MD it needs to mutate the vectors, another place is in SphereSphereCollisionAlgorithm
+        Vector3 pa = new Vector3(transA.m03, transA.m13, transA.m23);
+        Vector3 pb = new Vector3(transB.m03, transB.m13, transB.m23);
         
         ClosestPair p = null;
-        MutableVector3f guess = pb.sub(pa, null);
+        Vector3 guess = new Vector3().sub(pb, pa);
         if (gjk.evaluate(guess) == GJK.Status.VALID) {
             // non-intersecting pair
            p = support.getClosestPair(gjk.getSimplex(), null);
         EPA epa = new EPA(gjk);
         for (int i = 1; i < MAX_EPA_CHECKS; i++) {
             // intersection or failure, fall back onto EPA
-            // must re-run the GJK without scaling so that the simplex is in the correct space
+            // must re-run the GJK with scaling so that the simplex is in the correct space
             support.setNumAppliedMargins(i);
 
             if (gjk.evaluate(guess) == GJK.Status.VALID) {

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

 package com.ferox.physics.collision.algorithm;
 
-import com.ferox.math.MutableVector3f;
-import com.ferox.math.ReadOnlyMatrix4f;
-import com.ferox.math.ReadOnlyVector3f;
-import com.ferox.math.Vector3f;
-import com.ferox.math.Vector4f;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix4;
+import com.ferox.math.Vector3;
+import com.ferox.math.Vector4;
 import com.ferox.physics.collision.Collidable;
 import com.ferox.physics.collision.shape.ConvexShape;
 
  * @author Michael Ludwig
  */
 public class MinkowskiDifference {
-    private static final float CONTACT_NORMAL_ACCURACY = .0001f;
+    private static final double CONTACT_NORMAL_ACCURACY = .0001;
     
     private ConvexShape shapeA;
     private ConvexShape shapeB;
     
-    private ReadOnlyMatrix4f transA; // transforms a to world
-    private ReadOnlyMatrix4f transB; // transforms b to world
+    private Matrix4 transA; // transforms a to world
+    private Matrix4 transB; // transforms b to world
     
     private int appliedMargins;
     
     // final variables to reuse during computations, should be faster than thread-local
-    private final Vector3f supportCache;
-    private final Vector3f dirCache;
-    private final Vector4f transformCache;
+    private final Vector3 supportCache;
+    private final Vector3 dirCache;
+    private final Vector4 transformCache;
 
     /**
      * Create a new MinkowskiDifference between the two Shape objects. The
      * @param transB The second shape's world transform
      * @throws NullPointerException if any arguments are null
      */
-    public MinkowskiDifference(ConvexShape shapeA, ReadOnlyMatrix4f transA,
-                               ConvexShape shapeB, ReadOnlyMatrix4f transB) {
+    public MinkowskiDifference(ConvexShape shapeA, @Const Matrix4 transA,
+                               ConvexShape shapeB, @Const Matrix4 transB) {
         if (shapeA == null || shapeB == null || transA == null || transB == null)
             throw new NullPointerException("Arguments cannot be null");
-        supportCache = new Vector3f();
-        dirCache = new Vector3f();
-        transformCache = new Vector4f();
+        supportCache = new Vector3();
+        dirCache = new Vector3();
+        transformCache = new Vector4();
         
         this.shapeA = shapeA;
         this.shapeB = shapeB;
      *         computed
      * @throws NullPointerException if simplex is null
      */
-    public ClosestPair getClosestPair(Simplex simplex, ReadOnlyVector3f zeroNormal) {
-        ReadOnlyVector3f pa = transA.getCol(3).getAsVector3f();
-        ReadOnlyVector3f pb = transB.getCol(3).getAsVector3f();
+    public ClosestPair getClosestPair(Simplex simplex, @Const Vector3 zeroNormal) {
+        Vector3 pa = new Vector3(transA.m03, transA.m13, transA.m23);
+        Vector3 pb = new Vector3(transB.m03, transB.m13, transB.m23);
         
-        Vector3f ma = getClosestPointA(simplex);
-        Vector3f mb = getClosestPointB(simplex);
+        Vector3 ma = getClosestPointA(simplex);
+        Vector3 mb = getClosestPointB(simplex);
         
         return constructPair(ma, mb, pa, pb, zeroNormal);
     }
     
-    private ClosestPair constructPair(MutableVector3f wA, MutableVector3f wB, 
-                                      ReadOnlyVector3f pA, ReadOnlyVector3f pB,
-                                      ReadOnlyVector3f zeroNormal) {
+    private ClosestPair constructPair(Vector3 wA, Vector3 wB, 
+                                      Vector3 pA, Vector3 pB,
+                                      @Const Vector3 zeroNormal) {
         boolean intersecting = (pA.distanceSquared(wB) < pA.distanceSquared(wA)) && 
                                (pB.distanceSquared(wA) < pB.distanceSquared(wB));
-        MutableVector3f normal = wB.sub(wA); // wB becomes the normal here
-        float distance = normal.length() * (intersecting ? -1f : 1f);
+        // after this, pA and pB are free vectors to be mutated
+        
+        Vector3 normal = wB.sub(wA); // wB becomes the normal here
+        double distance = normal.length() * (intersecting ? -1.0 : 1.0);
 
         if (Math.abs(distance) < CONTACT_NORMAL_ACCURACY) {
             // special case for very close contact points, where the normal might become inaccurate
             if (zeroNormal != null) {
-                zeroNormal.normalize(normal);
+                normal.normalize(zeroNormal);
                 if (intersecting)
                     normal.scale(-1f);
             } else
                 return null;
         } else {
             // normalize, and possibly flip the normal based on intersection
-            normal.scale(1f / distance);
+            normal.scale(1.0 / distance);
         }
         
         if (appliedMargins != 1) {
             int scale = Math.abs(appliedMargins - 1);
-            float distDelta = scale * shapeA.getMargin() + scale * shapeB.getMargin();
+            double distDelta = scale * (shapeA.getMargin() + shapeB.getMargin());
             if (intersecting)
-                distDelta *= -1f;
+                distDelta *= -1.0;
             
-            normal.scaleAdd(-(appliedMargins - 1) * shapeA.getMargin(), wA, wA);
+            wA.add(wA, pA.scale(normal, (1 - appliedMargins) * shapeA.getMargin()));
             if ((appliedMargins == 0 && intersecting) || (appliedMargins > 1 && !intersecting)) {
                 // moving to one margin increases distance
                 distance += distDelta;
     }
 
 
-    private Vector3f getClosestPointA(Simplex simplex) {
-        Vector3f result = new Vector3f();
+    private Vector3 getClosestPointA(Simplex simplex) {
+        Vector3 result = new Vector3();
         for (int i = 0; i < simplex.getRank(); i++) {
             // sum weighted supports from simplex
             getAffineSupport(shapeA, transA, simplex.getVertex(i).getInputVector(), supportCache);
-            supportCache.scaleAdd(simplex.getVertex(i).getWeight(), result, result);
+            result.add(supportCache.scale(simplex.getVertex(i).getWeight()), result);
         }
         return result;
     }
 
-    private Vector3f getClosestPointB(Simplex simplex) {
-        Vector3f result = new Vector3f();
+    private Vector3 getClosestPointB(Simplex simplex) {
+        Vector3 result = new Vector3();
         for (int i = 0; i < simplex.getRank(); i++) {
             // sum weighted supports from simplex
-            getAffineSupport(shapeB, transB, simplex.getVertex(i).getInputVector().scale(-1f, supportCache), supportCache);
-            supportCache.scaleAdd(simplex.getVertex(i).getWeight(), result, result);
+            getAffineSupport(shapeB, transB, supportCache.scale(simplex.getVertex(i).getInputVector(), -1.0), supportCache);
+            result.add(supportCache.scale(simplex.getVertex(i).getWeight()), result);
         }
         return result;
     }
      * @return The support, stored in result or a new vector if result was null
      * @throws NullPointerException if d is null
      */
-    public MutableVector3f getSupport(ReadOnlyVector3f d, MutableVector3f result) {
+    public Vector3 getSupport(@Const Vector3 d, Vector3 result) {
         if (result == null)
-            result = new Vector3f();
+            result = new Vector3();
         
         getAffineSupport(shapeA, transA, d, result);
-        getAffineSupport(shapeB, transB, d.scale(-1f, supportCache), supportCache);
+        getAffineSupport(shapeB, transB, supportCache.scale(d, -1.0), supportCache);
         return result.sub(supportCache);
     }
     
-    private void getAffineSupport(ConvexShape shape, ReadOnlyMatrix4f t, ReadOnlyVector3f d, MutableVector3f result) {
+    private void getAffineSupport(ConvexShape shape, @Const Matrix4 t, @Const Vector3 d, Vector3 result) {
         // first step is to transform d by the transpose of the upper 3x3
         // we do this by wrapping d in a 4-vector and setting w = 0
-        transformCache.set(d.getX(), d.getY(), d.getZ(), 0f);
-        t.mulPre(transformCache);
+        transformCache.set(d.x, d.y, d.z, 0.0);
+        transformCache.mul(transformCache, t);
         
         // second step is to compute the actual support
-        result.set(transformCache.getX(), transformCache.getY(), transformCache.getZ());
+        result.set(transformCache.x, transformCache.y, transformCache.z);
         dirCache.set(result);
         
         shape.computeSupport(result, result);
         
         if (appliedMargins > 0) {
             // apply a number of margin offsets, as if a sphere is added to the convex shape
-            dirCache.scaleAdd(appliedMargins * shape.getMargin(), result, result);
+            result.add(dirCache.scale(appliedMargins * shape.getMargin()), result);
         }
         
         // then transform that by the complete affine transform, so w = 1
-        transformCache.set(result.getX(), result.getY(), result.getZ(), 1f);
-        t.mul(transformCache);
-        result.set(transformCache.getX(), transformCache.getY(), transformCache.getZ());
+        transformCache.set(result.x, result.y, result.z, 1.0);
+        transformCache.mul(t, transformCache);
+        result.set(transformCache.x, transformCache.y, transformCache.z);
     }
 }

ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/Simplex.java

 
 import java.util.Arrays;
 
-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;
 
 /**
  * <p>
      * </p>
      */
     public static class Vertex {
-        final Vector3f input;
-        final Vector3f vertex;
-        private float weight;
+        private final Vector3 input;
+        private final Vector3 vertex;
+        private double weight;
         
         public Vertex() {
-            input = new Vector3f();
-            vertex = new Vector3f();
-            weight = 0f;
+            input = new Vector3();
+            vertex = new Vector3();
+            weight = 0;
         }
         
         /**
          * @return The input vector used to evaluate the support function
          */
-        public ReadOnlyVector3f getInputVector() {
+        public @Const Vector3 getInputVector() {
             return input;
         }
         
         /**
          * @return The vertex of the simplex
          */
-        public ReadOnlyVector3f getVertex() {
+        public @Const Vector3 getVertex() {
             return vertex;
         }
         
         /**
          * @return The current weight of the simplex
          */
-        public float getWeight() {
+        public double getWeight() {
             return weight;
         }
 
          * @param w The new weight
          * @throws IllegalArgumentException if w is less than 0
          */
-        public void setWeight(float w) {
+        public void setWeight(double w) {
             if (w < 0f)
                 throw new IllegalArgumentException("Weight must be positive, not: " + w);
             weight = w;
     private int mask;
     // for use with SimplexUtil
     private final int[] maskCache;
-    private final float[] weightCache;
+    private final double[] weightCache;
     
     /**
      * Create a Simplex that is initially empty and has a rank of 0.
         rank = 0;
         
         maskCache = new int[1];
-        weightCache = new float[4];
+        weightCache = new double[4];
     }
 
     /**
         rank = 3;
         
         maskCache = new int[1];
-        weightCache = new float[4];
+        weightCache = new double[4];
     }
     
     public void clear() {
     private boolean encloseOriginImpl(MinkowskiDifference shape) {
         switch(rank) {
         case 1: {
-            Vector3f axis = new Vector3f();
+            Vector3 axis = new Vector3();
             for (int i = 0; i < 3; i++) {
-                addVertex(shape, axis.set(0f, 0f, 0f).set(i, 1f));
+                addVertex(shape, axis.set(0.0, 0.0, 0.0).set(i, 1.0));
                 if (encloseOrigin(shape))
                     return true;
                 removeVertex();
-                addVertex(shape, axis.scale(-1f));
+                addVertex(shape, axis.scale(-1));
                 if (encloseOrigin(shape))
                     return true;
                 removeVertex();
             }
             break; }
         case 2: {
-            MutableVector3f d = vertices[1].vertex.sub(vertices[0].vertex, null);
-            Vector3f axis = new Vector3f();
+            Vector3 d = new Vector3().sub(vertices[1].vertex, vertices[0].vertex);
+            Vector3 axis = new Vector3();
             
             for (int i = 0; i < 3; i++) {
-                d.cross(axis.set(0f, 0f, 0f).set(i, 1f), axis);
+                d.cross(axis.set(0.0, 0.0, 0.0).set(i, 1.0), axis);
                 if (axis.lengthSquared() > 0) {
                     addVertex(shape, axis);
                     if (encloseOrigin(shape))
                         return true;
                     removeVertex();
-                    addVertex(shape, axis.scale(-1f));
+                    addVertex(shape, axis.scale(-1.0));
                     if (encloseOrigin(shape))
                         return true;
                     removeVertex();
             }
             break; }
         case 3: {
-            MutableVector3f n = vertices[1].vertex.sub(vertices[0].vertex, null).cross(vertices[2].vertex.sub(vertices[0].vertex, null));
-            if (n.lengthSquared() > 0) {
+            Vector3 n = new Vector3().sub(vertices[1].vertex, vertices[0].vertex).cross(new Vector3().sub(vertices[2].vertex, vertices[0].vertex));
+            if (n.lengthSquared() > 0.0) {
                 addVertex(shape, n);
                 if (encloseOrigin(shape))
                     return true;
                 removeVertex();
-                addVertex(shape, n.scale(-1f));
+                addVertex(shape, n.scale(-1.0));
                 if (encloseOrigin(shape))
                     return true;
                 removeVertex();
         case 4: {
             if (Math.abs(SimplexUtil.det(vertices[0].vertex.sub(vertices[3].vertex, null),
                                          vertices[1].vertex.sub(vertices[3].vertex, null),
-                                         vertices[2].vertex.sub(vertices[3].vertex, null))) > 0f)
+                                         vertices[2].vertex.sub(vertices[3].vertex, null))) > 0.0)
                 return true;
             break; }
         }
     private void orient() {
         if (SimplexUtil.det(vertices[0].vertex.sub(vertices[3].vertex, null),
                             vertices[1].vertex.sub(vertices[3].vertex, null),
-                            vertices[2].vertex.sub(vertices[3].vertex, null)) < 0f) {
-            Vector3f t = new Vector3f();
+                            vertices[2].vertex.sub(vertices[3].vertex, null)) < 0.0) {
+            Vector3 t = new Vector3();
             
             t.set(vertices[0].input);
             vertices[0].input.set(vertices[1].input);
             vertices[0].vertex.set(vertices[1].vertex);
             vertices[1].vertex.set(t);
             
-            float tt = vertices[0].weight;
+            double tt = vertices[0].weight;
             vertices[0].weight = vertices[1].weight;
             vertices[1].weight = tt;
         }
      * @param d The input to the support function
      * @throws NullPointerException if s or d are null
      */
-    public void addVertex(MinkowskiDifference s, ReadOnlyVector3f d) {
+    public void addVertex(MinkowskiDifference s, @Const Vector3 d) {
         addVertex(s, d, false);
     }
 
      * @param negate True if <tt>d</tt> should be negated before being passed to
      *            the support function
      */
-    public void addVertex(MinkowskiDifference s, ReadOnlyVector3f d, boolean negate) {
+    public void addVertex(MinkowskiDifference s, @Const Vector3 d, boolean negate) {
         if (rank == 4)
             throw new IllegalStateException("Cannot add a vertex to a full simplex");
         if (vertices[rank] == null)
             vertices[rank] = new Vertex();
         Vertex v = vertices[rank++];
         
-        v.weight = 0f;
+        v.weight = 0.0;
         if (negate)
-            d.scale(-1f, v.input).normalize();
+            v.input.scale(d, -1.0).normalize();
         else
-            d.normalize(v.input);
+            v.input.normalize(d);
         s.getSupport(v.input, v.vertex);
     }
     
      * Update the weights and mask to perform the actual math needed by reduce()
      */
     private boolean projectOrigin() {
-        Arrays.fill(weightCache, 0f);
+        Arrays.fill(weightCache, 0.0);
         maskCache[0] = 0;
         
-        float sqdist = 0f;
+        double sqdist = 0.0;
         switch(rank) {
         case 2:
             sqdist = simplexUtil.get().projectOrigin2(vertices[0].vertex, vertices[1].vertex, 
             break;
         }
         
-        if (sqdist >= 0f) {
+        if (sqdist >= 0.0) {
             // copy weights back into member variables
             for (int i = 0; i < rank; i++)
                 vertices[i].weight = weightCache[i];
         private static final int[] IMD3 = new int[] {1, 2, 0};
 
         // used only in projectOrigin2
-        private final Vector3f p2 = new Vector3f(); 
+        private final Vector3 p2 = new Vector3(); 
         
         // used only in projectOrigin3
-        private final ReadOnlyVector3f[] vt3 = new ReadOnlyVector3f[3];
-        private final Vector3f[] dl3 = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() };
-        private final Vector3f n3 = new Vector3f();
-        private final Vector3f p3 = new Vector3f();
+        private final Vector3[] vt3 = new Vector3[3];
+        private final Vector3[] dl3 = new Vector3[] { new Vector3(), new Vector3(), new Vector3() };
+        private final Vector3 n3 = new Vector3();
+        private final Vector3 p3 = new Vector3();
         
-        private final float[] subw3 = new float[2];
+        private final double[] subw3 = new double[2];
         private final int[] subm3 = new int[1];
         
         // used only in projectOrigin4
-        private final ReadOnlyVector3f[] vt4 = new ReadOnlyVector3f[4];
-        private final Vector3f[] dl4 = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() };
-        private final Vector3f n4 = new Vector3f();
-        private final Vector3f p4 = new Vector3f();
+        private final Vector3[] vt4 = new Vector3[4];
+        private final Vector3[] dl4 = new Vector3[] { new Vector3(), new Vector3(), new Vector3() };
+        private final Vector3 n4 = new Vector3();
+        private final Vector3 p4 = new Vector3();
         
-        private final float[] subw4 = new float[3];
+        private final double[] subw4 = new double[3];
         private final int[] subm4 = new int[1];
         
-        
-        
-        public float projectOrigin2(ReadOnlyVector3f a, ReadOnlyVector3f b, float[] weights, int[] mask) {
-            MutableVector3f d = b.sub(a, p2);
-            float l = d.lengthSquared();
+        public double projectOrigin2(@Const Vector3 a, @Const Vector3 b, double[] weights, int[] mask) {
+            Vector3 d = p2.sub(b, a);
+            double l = d.lengthSquared();
             
-            if (l > 0f) {
-                float t = -a.dot(d) / l;
-                if (t >= 1) {
-                    weights[0] = 0f; 
-                    weights[1] = 1f; 
+            if (l > 0.0) {
+                double t = -a.dot(d) / l;
+                if (t >= 1.0) {
+                    weights[0] = 0.0; 
+                    weights[1] = 1.0; 
                     mask[0] = 2; 
                     return b.lengthSquared();
                 } else if (t <= 0) {
                 } else {
                     weights[0] = 1 - t; 
                     weights[1] = t; 
-                    mask[0] = 3; 
-                    return d.scaleAdd((float) t, a).lengthSquared();
+                    mask[0] = 3;
+                    return d.scale(t).add(a).lengthSquared();
                 }
             } else
                 return -1f;
         }
         
-        public float projectOrigin3(ReadOnlyVector3f a, ReadOnlyVector3f b, ReadOnlyVector3f c, float[] weights, int[] mask) {
+        public double projectOrigin3(@Const Vector3 a, @Const Vector3 b, @Const Vector3 c, double[] weights, int[] mask) {
             vt3[0] = a; vt3[1] = b; vt3[2] = c;
-            a.sub(b, dl3[0]);
-            b.sub(c, dl3[1]);
-            c.sub(a, dl3[2]);
+            dl3[0].sub(a, b);
+            dl3[1].sub(b, c);
+            dl3[2].sub(c, a);
             
-            dl3[0].cross(dl3[1], n3);
-            float l = n3.lengthSquared();
+            n3.cross(dl3[0], dl3[1]);
+            double l = n3.lengthSquared();
             
-            if (l > 0f) {
-                float minDist = -1f;
-                subw3[0] = 0f; subw3[1] = 0f;
+            if (l > 0.0) {
+                double minDist = -1.0;
+                subw3[0] = 0.0; subw3[1] = 0.0;
                 subm3[0] = 0;
                 
                 for (int i = 0; i < 3; i++) {
-                    if (vt3[i].dot(dl3[i].cross(n3, p3)) > 0) {
+                    if (vt3[i].dot(p3.cross(dl3[i], n3)) > 0.0) {
                         int j = IMD3[i];
-                        float subd = projectOrigin2(vt3[i], vt3[j], subw3, subm3);
-                        if (minDist < 0f || subd < minDist) {
+                        double subd = projectOrigin2(vt3[i], vt3[j], subw3, subm3);
+                        if (minDist < 0.0 || subd < minDist) {
                             minDist = subd;
                             mask[0] = ((subm3[0] & 1) != 0 ? (1 << i) : 0) + 
                                       ((subm3[0] & 2) != 0 ? (1 << j) : 0);
                     }
                 }
                 
-                if (minDist < 0f) {
-                    float d = a.dot(n3);
-                    float s = (float) Math.sqrt(l);
-                    n3.scale((float) (d / l), p3);
+                if (minDist < 0.0) {
+                    double d = a.dot(n3);
+                    double s = Math.sqrt(l);
+                    p3.scale(n3, d / l);
                     
                     minDist = p3.lengthSquared();
                     mask[0] = 7;
-                    weights[0] = dl3[1].cross(b.sub(p3, n3)).length() / s; // at this point dl[1] and n are throwaway
-                    weights[1] = dl3[2].cross(c.sub(p3, n3)).length() / s; // at this point dl[2] and n are throwaway
+                    weights[0] = dl3[1].cross(n3.sub(b, p3)).length() / s; // at this point dl[1] and n are throwaway
+                    weights[1] = dl3[2].cross(n3.sub(c, p3)).length() / s; // at this point dl[2] and n are throwaway
                     weights[2] = 1 - weights[0] - weights[1];
                 }
                 return minDist;
             } else
-                return -1f;
+                return -1.0;
         }
         
-        public float projectOrigin4(ReadOnlyVector3f a, ReadOnlyVector3f b, ReadOnlyVector3f c, ReadOnlyVector3f d, float[] weights, int[] mask) {
+        public double projectOrigin4(@Const Vector3 a, @Const Vector3 b, @Const Vector3 c, @Const Vector3 d, double[] weights, int[] mask) {
             vt4[0] = a; vt4[1] = b; vt4[2] = c; vt4[3] = d;
-            a.sub(d, dl4[0]);
-            b.sub(d, dl4[1]);
-            c.sub(d, dl4[2]);
+            dl4[0].sub(a, d);
+            dl4[1].sub(b, d);
+            dl4[2].sub(c, d);
             
-            float vl = det(dl4[0], dl4[1], dl4[2]);
-            boolean ng = (vl * a.dot(b.sub(c, n4).cross(a.sub(b, p4)))) <= 0f;
+            double vl = det(dl4[0], dl4[1], dl4[2]);
+            boolean ng = (vl * a.dot(n4.sub(b, c).cross(p4.sub(a, b)))) <= 0.0;
             
-            if (ng && Math.abs(vl) > 0f) {
-                float minDist = -1f;
-                subw4[0] = 0f; subw4[1] = 0f; subw4[2] = 0f;
+            if (ng && Math.abs(vl) > 0.0) {
+                double minDist = -1.0;
+                subw4[0] = 0.0; subw4[1] = 0.0; subw4[2] = 0.0;
                 subm4[0] = 0;
                 
                 for (int i = 0; i < 3; i++) {
                     int j = IMD3[i];
-                    float s = vl * d.dot(dl4[i].cross(dl4[j], n4));
-                    if (s > 0) {
-                        float subd = projectOrigin3(vt4[i], vt4[j], d, subw4, subm4);
-                        if (minDist < 0f || subd < minDist) {
+                    double s = vl * d.dot(n4.cross(dl4[i], dl4[j]));
+                    if (s > 0.0) {
+                        double subd = projectOrigin3(vt4[i], vt4[j], d, subw4, subm4);
+                        if (minDist < 0.0 || subd < minDist) {
                             minDist = subd;
                             mask[0] = ((subm4[0] & 1) != 0 ? (1 << i) : 0) +
                                       ((subm4[0] & 2) != 0 ? (1 << j) : 0) +
                                       ((subm4[0] & 4) != 0 ? 8 : 0);
                             weights[i] = subw4[0];
                             weights[j] = subw4[1];
-                            weights[IMD3[j]] = 0f;
+                            weights[IMD3[j]] = 0.0;
                             weights[3] = subw4[2];
                         }
                     }
                 }
                 
-                if (minDist < 0f) {
-                    minDist = 0f;
+                if (minDist < 0.0) {
+                    minDist = 0.0;
                     mask[0] = 15;
                     weights[0] = det(c, b, d) / vl;
                     weights[1] = det(a, c, d) / vl;
                 }
                 return minDist;
             } else
-                return -1f;
+                return -1.0;
         }
         
-        public static float det(ReadOnlyVector3f a, ReadOnlyVector3f b, ReadOnlyVector3f c) {
-            return a.getY() * b.getZ() * c.getX() + a.getZ() * b.getX() * c.getY() -
-                   a.getX() * b.getZ() * c.getY() - a.getY() * b.getX() * c.getZ() +
-                   a.getX() * b.getY() * c.getZ() - a.getZ() * b.getY() * c.getX();
+        public static double det(@Const Vector3 a, @Const Vector3 b, @Const Vector3 c) {
+            return a.y * b.z * c.x + a.z * b.x * c.y -
+                   a.x * b.z * c.y - a.y * b.x * c.z +
+                   a.x * b.y * c.z - a.z * b.y * c.x;
         }
     }
 }

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

 package com.ferox.physics.collision.algorithm;
 
-import com.ferox.math.MutableVector3f;
-import com.ferox.math.ReadOnlyMatrix4f;
-import com.ferox.math.Vector3f;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix4;
+import com.ferox.math.Vector3;
 import com.ferox.physics.collision.CollisionAlgorithm;
 import com.ferox.physics.collision.shape.Sphere;
 
  */
 public class SphereSphereCollisionAlgorithm implements CollisionAlgorithm<Sphere, Sphere> {
     @Override
-    public ClosestPair getClosestPair(Sphere shapeA, ReadOnlyMatrix4f transA, Sphere shapeB,
-                                      ReadOnlyMatrix4f transB) {
-        Vector3f ca = new Vector3f(transA.get(0, 3), transA.get(1, 3), transA.get(2, 3));
-        Vector3f cb = new Vector3f(transB.get(0, 3), transB.get(1, 3), transB.get(2, 3));
+    public ClosestPair getClosestPair(Sphere shapeA, @Const Matrix4 transA, 
+                                      Sphere shapeB, @Const Matrix4 transB) {
+        Vector3 ca = new Vector3(transA.m03, transA.m13, transA.m23);
+        Vector3 cb = new Vector3(transB.m03, transB.m13, transB.m23);
         
-        float ra = shapeA.getRadius() + shapeA.getMargin();
-        float rb = shapeB.getRadius() + shapeB.getMargin();
-        float dist = ca.distance(cb) - ra - rb;
+        double ra = shapeA.getRadius() + shapeA.getMargin();
+        double rb = shapeB.getRadius() + shapeB.getMargin();
+        double dist = ca.distance(cb) - ra - rb;
         
         // FIXME: doesn't work if spheres are centered on each other
-        MutableVector3f normal = cb.sub(ca).normalize();
-        normal.scaleAdd(ra, ca, ca);
+        Vector3 normal = new Vector3().sub(cb, ca).normalize();
+        Vector3 pa = cb.scale(normal, ra).add(ca); // consumes cb
 
         if (normal.lengthSquared() > .000001f) {
-            return new ClosestPair(ca, normal, dist);
+            return new ClosestPair(pa, normal, dist);
         } else {
             // happens when spheres are perfectly centered on each other
             if (ra < rb) {
                 // sphere a is inside sphere b
-                normal.set(0f, 0f, -1f);
-                ca.setZ(ca.getZ() + ra);
-                return new ClosestPair(ca, normal, ra - rb);
+                normal.set(0, 0, -1);
+                pa.z = pa.z + ra;
+                return new ClosestPair(cb, normal, ra - rb);
             } else {
                 // sphere b is inside sphere a
-                normal.set(0f, 0f, 1f);
-                ca.setZ(ca.getZ() + ra);
-                return new ClosestPair(ca, normal, rb - ra);
+                normal.set(0, 0, 1);
+                pa.z = pa.z + ra;
+                return new ClosestPair(cb, normal, rb - ra);
             }
         }
     }

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

 package com.ferox.physics.collision.algorithm;
 
-import com.ferox.math.ReadOnlyMatrix4f;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix4;
+import com.ferox.math.Vector3;
 import com.ferox.physics.collision.CollisionAlgorithm;
 import com.ferox.physics.collision.Shape;
 
     }
     
     @Override
-    public ClosestPair getClosestPair(A shapeA, ReadOnlyMatrix4f transA, 
-                                      B shapeB, ReadOnlyMatrix4f transB) {
+    public ClosestPair getClosestPair(A shapeA, @Const Matrix4 transA, 
+                                      B shapeB, @Const Matrix4 transB) {
         ClosestPair original = delegate.getClosestPair(shapeB, transB, shapeA, transA);
         if (original == null)
             return null;
         
         // swap the points and contact normal
-        return new ClosestPair(original.getClosestPointOnB(), original.getContactNormal().scale(-1f, null), 
+        return new ClosestPair(original.getClosestPointOnB(), new Vector3().scale(original.getContactNormal(), -1.0),
                                original.getDistance());
     }
 

ferox-physics/src/main/java/com/ferox/physics/collision/shape/ConvexShape.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.bounds.ReadOnlyAxisAlignedBox;
+import com.ferox.math.Const;
+import com.ferox.math.Vector3;
 import com.ferox.physics.collision.Shape;
 import com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm;
 
      * @return result, or a new vector, if result was null
      * @throws NullPointerException if v is null
      */
-    public abstract MutableVector3f computeSupport(ReadOnlyVector3f v, MutableVector3f result);
+    public abstract Vector3 computeSupport(@Const Vector3 v, @Const Vector3 result);
 
     @Override
     public ReadOnlyAxisAlignedBox getBounds() {