Commits

Michael Ludwig committed eb82b00

Reduce Vector3 allocations

Comments (0)

Files changed (7)

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

                 bind(f2, 2, f3, 1);
                 bind(f3, 2, f4, 1);
                 
+                Vector3 iw = new Vector3();
+                Vector3 vw = new Vector3();
+                Horizon horizon = new Horizon();
+
                 for (int pass = 1; pass < EPA_MAX_ITERATIONS; pass++) {
+                    horizon.reset();
                     best.pass = pass;
                     
-                    Vector3 iw = new Vector3();
-                    Vector3 vw = new Vector3();
                     simplex.getShape().getSupport(iw.set(best.normal), vw);
                     
-                    Horizon horizon = new Horizon(); // FIXME cache and reset instead of alloc
                     double wdist = best.normal.dot(vw) - best.d;
                     boolean valid = true;
                     if (wdist > EPA_ACCURACY) {
                 
                 // create new reduced simplex from hull
                 Vector3 projection = new Vector3().scale(outer.normal, outer.d);
-                simplex = new Simplex(simplex.getShape());
                 simplex.setRank(outer.inputs.length);
                 for (int j = 0; j < outer.inputs.length; j++) {
                     simplex.getInput(j).set(outer.inputs[j]);
                 // FIXME these do double the number of required subtractions,
                 // also it might be worth considering creating an optimized crossLength()
                 // method that just computes the length of the cross product
-                double w1 = new Vector3().sub(outer.vertices[1], projection).cross(new Vector3().sub(outer.vertices[2], projection)).length();
-                double w2 = new Vector3().sub(outer.vertices[2], projection).cross(new Vector3().sub(outer.vertices[0], projection)).length();
-                double w3 = new Vector3().sub(outer.vertices[0], projection).cross(new Vector3().sub(outer.vertices[1], projection)).length();
+                Vector3 t = new Vector3();
+                double w1 = Util.normal(projection, outer.vertices[1], outer.vertices[2], t).length();
+                double w2 = Util.normal(projection, outer.vertices[2], outer.vertices[0], t).length();
+                double w3 = Util.normal(projection, outer.vertices[0], outer.vertices[1], t).length();
+//                double w1 = new Vector3().sub(outer.vertices[1], projection).cross(new Vector3().sub(outer.vertices[2], projection)).length();
+//                double w2 = new Vector3().sub(outer.vertices[2], projection).cross(new Vector3().sub(outer.vertices[0], projection)).length();
+//                double w3 = new Vector3().sub(outer.vertices[0], projection).cross(new Vector3().sub(outer.vertices[1], projection)).length();
                 
                 double sum = w1 + w2 + w3;
                 simplex.setWeight(0, w1 / sum);
         if (f.pass != pass) {
             int e1 = I1_MAP[e];
             if (f.normal.dot(vw) - f.d < -EPA_PLANE_EPS) {
-                Face nf = newFace(f.inputs[e1], f.vertices[e1], f.inputs[e], f.vertices[e], iw, vw, hull, false);
+                // If we need a new face, we clone iw and vw because they're 
+                // being reused in the EPA loop. The other vertices are already
+                // in the hull and won't be modified anymore so we can share references.
+                Face nf = newFace(f.inputs[e1], f.vertices[e1], 
+                                  f.inputs[e], f.vertices[e], 
+                                  new Vector3(iw), new Vector3(vw), hull, false);
                 if (nf != null) {
                     bind(nf, 0, f, e);
                     if (horizon.cf != null)
     }
     
     private static Face newFace(Simplex simplex, int i1, int i2, int i3, Bag<Face> hull) {
-        return newFace(simplex.getInput(i1), simplex.getVertex(i1), 
-                       simplex.getInput(i2), simplex.getVertex(i2),
-                       simplex.getInput(i3), simplex.getVertex(i3),
+        // the simplex will be later modified, so we clone the vertices here
+        // so that we're not inadvertently editing the hull either
+        return newFace(new Vector3(simplex.getInput(i1)), new Vector3(simplex.getVertex(i1)), 
+                       new Vector3(simplex.getInput(i2)), new Vector3(simplex.getVertex(i2)),
+                       new Vector3(simplex.getInput(i3)), new Vector3(simplex.getVertex(i3)),
                        hull, true);
     }
     
         face.inputs[1] = ib;
         face.inputs[2] = ic;
             
-        face.normal.sub(vb, va).cross(new Vector3().sub(vc, va));
+        Util.normal(va, vb, vc, face.normal);
             
         double l = face.normal.length();
         boolean valid = l > EPA_ACCURACY;
         Face cf;
         Face ff;
         int numFaces;
+        
+        public void reset() {
+            cf = null;
+            ff = null;
+            numFaces = 0;
+        }
     }
     
     private static class Face {
                 // update swapped item's index
                 hull.get(index).index = index;
             }
-            
-//            for (Face f: hull) {
-//                for (int i = 0; i < f.adjacent.length; i++) {
-//                    if (f.adjacent[i] == this) {
-//                        throw new IllegalArgumentException();
-//                    }
-//                }
-//            }
         }
     }
 }

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

         ray.set(simplex.addVertex(guess, true));
         simplex.setWeight(0, 1.0);
         
+        Vector3 scaledVertex = new Vector3();
         Vector3[] oldSupports = new Vector3[] { new Vector3(ray), new Vector3(ray),
                                                 new Vector3(ray), new Vector3(ray) };
         int lastSupportIndex = 0;
                 // compute next guess
                 ray.set(0.0, 0.0, 0.0);
                 for (int j = 0; j < simplex.getRank(); j++) {
-                    ray.add(new Vector3().scale(simplex.getVertex(j), simplex.getWeight(j)));
+                    scaledVertex.scale(simplex.getVertex(j), simplex.getWeight(j));
+                    ray.add(scaledVertex);
                 }
             } else {
                 simplex.discardLastVertex();

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

     
     private int numMargins;
     
+    // temporary variables to reduce allocation costs
+    private final Vector3 pointTemp; // used in computePointOnA/B(), getClosestPair(), and getSupport()
+    private final Vector3 inSupportTemp; // used in support()
+    
     public MinkowskiShape(ConvexShape shapeA, @Const Matrix4 transformA,
                           ConvexShape shapeB, @Const Matrix4 transformB) {
         rotationA = new Matrix3().setUpper(transformA);
         this.shapeA = shapeA;
         this.shapeB = shapeB;
         
+        pointTemp = new Vector3();
+        inSupportTemp = new Vector3();
+        
         numMargins = 1;
     }
     
         // update positions to be only a single margin away
         if (numMargins != 1) {
             // adjust a's point by moving N margins along the contact normal
-            a.add(new Vector3().scale(normal, (1 - numMargins) * shapeA.getMargin()));
+            a.add(pointTemp.scale(normal, (1 - numMargins) * shapeA.getMargin()));
             
             // compute how the contact depth changes
             double delta = scale * Math.abs(numMargins - 1) * (shapeA.getMargin() + shapeB.getMargin());
     
     private Vector3 computePointOnA(Simplex simplex) {
         Vector3 a = new Vector3();
-        Vector3 s = new Vector3();
         for (int i = 0; i < simplex.getRank(); i++) {
-            support(shapeA, rotationA, translationA, simplex.getInput(i), false, s);
-            a.add(s.scale(simplex.getWeight(i)));
+            support(shapeA, rotationA, translationA, simplex.getInput(i), false, pointTemp);
+            a.add(pointTemp.scale(simplex.getWeight(i)));
         }
         return a;
     }
     
     private Vector3 computePointOnB(Simplex simplex) {
         Vector3 b = new Vector3();
-        Vector3 s = new Vector3();
         for (int i = 0; i < simplex.getRank(); i++) {
-            support(shapeB, rotationB, translationB, simplex.getInput(i), true, s);
-            b.add(s.scale(simplex.getWeight(i)));
+            support(shapeB, rotationB, translationB, simplex.getInput(i), true, pointTemp);
+            b.add(pointTemp.scale(simplex.getWeight(i)));
         }
         return b;
     }
     
     public Vector3 getSupport(@Const Vector3 dir, Vector3 result) {
-        Vector3 a = new Vector3();
-        Vector3 b = new Vector3();
-        
-        support(shapeA, rotationA, translationA, dir, false, a);
-        support(shapeB, rotationB, translationB, dir, true, b);
-        
         if (result == null)
             result = new Vector3();
-        return result.sub(a, b);
+        support(shapeA, rotationA, translationA, dir, false, result);
+        support(shapeB, rotationB, translationB, dir, true, pointTemp);
+        return result.sub(pointTemp);
     }
     
     private void support(ConvexShape shape, @Const Matrix3 r, @Const Vector3 t,
                          @Const Vector3 d, boolean negate, Vector3 result) {
-        // FIXME optimize by replacing with cached instances
-        Vector3 transformedDir = (negate ? new Vector3().scale(d, -1.0) : new Vector3(d));
+        Vector3 transformedDir = (negate ? inSupportTemp.scale(d, -1.0) : inSupportTemp.set(d));
         transformedDir.mul(transformedDir, r);
 
         shape.computeSupport(transformedDir, result);

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

     public boolean encloseOrigin() {
         if (encloseOriginImpl()) {
             // orient the simplex
-            if (det(new Vector3().sub(vertices[0], vertices[3]),
-                    new Vector3().sub(vertices[1], vertices[3]),
-                    new Vector3().sub(vertices[2], vertices[3])) < 0.0) {
+            if (Util.tripleProduct(new Vector3().sub(vertices[0], vertices[3]),
+                                   new Vector3().sub(vertices[1], vertices[3]),
+                                   new Vector3().sub(vertices[2], vertices[3])) < 0.0) {
                 Vector3 temp = new Vector3();
                 
                 temp.set(vertices[0]);
         }
         case 3:
         {
-            Vector3 n = new Vector3().sub(vertices[1], vertices[0]).cross(new Vector3().sub(vertices[2], vertices[0]));
+            Vector3 n = Util.normal(vertices[0], vertices[1], vertices[2], null);
             if (n.lengthSquared() > 0) {
                 addVertex(n, false);
                 if (encloseOriginImpl())
         }
         case 4:
         {
-            if (Math.abs(det(new Vector3().sub(vertices[0], vertices[3]),
-                             new Vector3().sub(vertices[1], vertices[3]),
-                             new Vector3().sub(vertices[2], vertices[3]))) > 0.0) {
+            if (Math.abs(Util.tripleProduct(new Vector3().sub(vertices[0], vertices[3]),
+                                            new Vector3().sub(vertices[1], vertices[3]),
+                                            new Vector3().sub(vertices[2], vertices[3]))) > 0.0) {
                 return true;
             }
             break;
         ds[1] = new Vector3().sub(b, d);
         ds[2] = new Vector3().sub(c, d);
         
-        double vl = det(ds[0], ds[1], ds[2]);
+        double vl = Util.tripleProduct(ds[0], ds[1], ds[2]);
         boolean ng = (vl * a.dot(new Vector3().sub(b, c).cross(new Vector3().sub(a, b)))) <= 0.0;
         
         if (ng && Math.abs(vl) > 0.0) {
             if (minDist < 0.0) {
                 minDist = 0.0;
                 mask = 15;
-                weights[0] = det(c, b, d) / vl;
-                weights[1] = det(a, c, d) / vl;
-                weights[2] = det(b, a, d) / vl;
+                weights[0] = Util.tripleProduct(c, b, d) / vl;
+                weights[1] = Util.tripleProduct(a, c, d) / vl;
+                weights[2] = Util.tripleProduct(b, a, d) / vl;
                 weights[3] = 1 - weights[0] - weights[1] - weights[2];
             }
             
         }
     }
     
-    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;
-    }
-    
+    // FIXME work out a way to reduce the allocation of Projection objects
     private static class Projection {
         final double[] weights;
         final double distance;

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

+package com.ferox.physics.collision.algorithm;
+
+import com.ferox.math.Const;
+import com.ferox.math.Vector3;
+
+public class Util {
+
+    public static double tripleProduct(@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;
+    }
+    
+    public static Vector3 normal(@Const Vector3 va, @Const Vector3 vb, @Const Vector3 vc, Vector3 result) {
+        // inline subtraction of 2 vectors
+        double e1x = vb.x - va.x;
+        double e1y = vb.y - va.y;
+        double e1z = vb.z - va.z;
+        
+        double e2x = vc.x - va.x;
+        double e2y = vc.y - va.y;
+        double e2z = vc.z - va.z;
+        
+        if (result == null)
+            result = new Vector3();
+        return result.set(e1y * e2z - e2y * e1z,
+                          e1z * e2x - e2z * e1x,
+                          e1x * e2y - e2x * e1y);
+    }
+}

ferox-physics/src/main/java/com/ferox/physics/dynamics/LinearConstraintPool.java

     private int[] bodyAs = new int[0];
     private int[] bodyBs = new int[0];
     
-//    private final Vector3 direction = new Vector3();
-//    private final Vector3 torqueA = new Vector3();
-//    private final Vector3 torqueB = new Vector3();
-//    private final Vector3 linearA = new Vector3();
-//    private final Vector3 linearB = new Vector3();
-//    private final Vector3 angularA = new Vector3();
-//    private final Vector3 angularB = new Vector3();
+    private final Vector3 direction = new Vector3();
+    private final Vector3 torqueA = new Vector3();
+    private final Vector3 torqueB = new Vector3();
+    private final Vector3 linearA = new Vector3();
+    private final Vector3 linearB = new Vector3();
+    private final Vector3 angularA = new Vector3();
+    private final Vector3 angularB = new Vector3();
     
     public LinearConstraintPool(LinearConstraintPool linkedPool) {
         count = 0;
             linearDirAs[veci + 1] = direction.y * imA;
             linearDirAs[veci + 2] = direction.z * imA;
             
-//            this.torqueA.mul(bodyA.getInertiaTensorInverse(), torqueA);
-            Vector3 t = new Vector3().mul(bodyA.getInertiaTensorInverse(), torqueA);
-            t.get(angleDirAs, veci);
+            this.torqueA.mul(bodyA.getInertiaTensorInverse(), torqueA);
+            this.torqueA.get(angleDirAs, veci);
         } else {
             // assign negative id
             bodyAs[i] = -1;
             linearDirBs[veci + 1] = direction.y * imB;
             linearDirBs[veci + 2] = direction.z * imB;
             
-//            this.torqueB.mul(bodyB.getInertiaTensorInverse(), torqueB);
-            Vector3 t = new Vector3().mul(bodyB.getInertiaTensorInverse(), torqueB);
-            t.get(angleDirBs, veci);
+            this.torqueB.mul(bodyB.getInertiaTensorInverse(), torqueB);
+            this.torqueB.get(angleDirBs, veci);
         } else {
             // assign negative id
             bodyBs[i] = -1;
     }
     
     public @Const Vector3 getLinearImpulseA(int i, double impulse) {
-        return new Vector3().set(linearDirAs, i * 3).scale(impulse);
+        return linearA.set(linearDirAs, i * 3).scale(impulse);
     }
     
     public @Const Vector3 getLinearImpulseB(int i, double impulse) {
-        return new Vector3().set(linearDirBs, i * 3).scale(impulse);
+        return linearB.set(linearDirBs, i * 3).scale(impulse);
     }
     
     public @Const Vector3 getAngularImpulseA(int i, double impulse) {
-        return new Vector3().set(angleDirAs, i * 3).scale(impulse);
+        return angularA.set(angleDirAs, i * 3).scale(impulse);
     }
     
     public @Const Vector3 getAngularImpulseB(int i, double impulse) {
-        return new Vector3().set(angleDirBs, i * 3).scale(impulse);
+        return angularB.set(angleDirBs, i * 3).scale(impulse);
     }
     
     public void setAppliedImpulse(int i, double impulse) {
     }
     
     public @Const Vector3 getConstraintDirection(int i) {
-        return new Vector3().set(directions, i * 3);
+        return direction.set(directions, i * 3);
     }
     
     public @Const Vector3 getTorqueA(int i) {
-        return new Vector3().set(torqueAs, i * 3);
+        return torqueA.set(torqueAs, i * 3);
     }
     
     public @Const Vector3 getTorqueB(int i) {
-        return new Vector3().set(torqueBs, i * 3);
+        return torqueB.set(torqueBs, i * 3);
     }
     
     public double getJacobianDiagonalInverse(int i) {

ferox-physics/src/main/java/com/ferox/physics/dynamics/LinearConstraintSolver.java

         double jacobian = group.getJacobianDiagonalInverse(constraint);
         double deltaImpulse = group.getSolution(constraint);
         
-        double deltaVelADotN = 0;
-        double deltaVelBDotN = 0;
-        
         int ba = group.getBodyAIndex(constraint);
         int bb = group.getBodyBIndex(constraint);
         
             deltaLinearImpulse.get(ba, linear);
             deltaAngularImpulse.get(ba, angular);
             
-            deltaVelADotN = -jacobian * (group.getConstraintDirection(constraint).dot(linear) + group.getTorqueA(constraint).dot(angular));
+            deltaImpulse -= jacobian * (group.getConstraintDirection(constraint).dot(linear) + group.getTorqueA(constraint).dot(angular));
         }
         
         if (bb >= 0) {
             deltaLinearImpulse.get(bb, linear);
             deltaAngularImpulse.get(bb, angular);
             
-            deltaVelBDotN = jacobian * (group.getConstraintDirection(constraint).dot(linear) + group.getTorqueB(constraint).dot(angular));
+            deltaImpulse += jacobian * (group.getConstraintDirection(constraint).dot(linear) + group.getTorqueB(constraint).dot(angular));
         }
         
-        deltaImpulse = group.getSolution(constraint) + deltaVelADotN + deltaVelBDotN;
-        
-        // FIXME consolidate again
         double applied = group.getAppliedImpulse(constraint);
         double totalImpulse = applied + deltaImpulse;
-        double lower = group.getLowerImpulseLimit(constraint);
-        double upper = group.getUpperImpulseLimit(constraint);
-        if (totalImpulse < lower) {
-            deltaImpulse = lower - applied;
-            totalImpulse = lower;
-        } else if (totalImpulse > upper) {
-            deltaImpulse = upper - applied;
-            totalImpulse = upper;
+        if (totalImpulse < group.getLowerImpulseLimit(constraint)) {
+            totalImpulse = group.getLowerImpulseLimit(constraint);
+            deltaImpulse = totalImpulse - applied; // really (lower - applied)
+        } else if (totalImpulse > group.getUpperImpulseLimit(constraint)) {
+            totalImpulse = group.getUpperImpulseLimit(constraint);
+            deltaImpulse = totalImpulse - applied; // really (upper - applied)
         }
         
         if (ba >= 0) {