From 823747a3e3b29305e28cd2a9e9b8b762d267d9d8 Mon Sep 17 00:00:00 2001
From: Stefan Hedman <schteppe@gmail.com>
Date: Thu, 22 Jan 2015 13:59:31 +0100
Subject: [PATCH] optimized and cleaned up PointToPointConstraint

---
 src/constraints/PointToPointConstraint.js | 61 +++++++++++++----------
 1 file changed, 34 insertions(+), 27 deletions(-)

diff --git a/src/constraints/PointToPointConstraint.js b/src/constraints/PointToPointConstraint.js
index 43c58bb51..22ec902fc 100644
--- a/src/constraints/PointToPointConstraint.js
+++ b/src/constraints/PointToPointConstraint.js
@@ -10,7 +10,7 @@ var ContactEquation = require('../equations/ContactEquation');
  * @constructor
  * @param {Body} bodyA
  * @param {Vec3} pivotA The point relative to the center of mass of bodyA which bodyA is constrained to.
- * @param {Body} bodyB Body that will be constrained in a similar way to the same point as bodyA. We will therefore get sort of a link between bodyA and bodyB. If not specified, bodyA will be constrained to a static point.
+ * @param {Body} bodyB Body that will be constrained in a similar way to the same point as bodyA. We will therefore get a link between bodyA and bodyB. If not specified, bodyA will be constrained to a static point.
  * @param {Vec3} pivotB See pivotA.
  * @param {Number} maxForce The maximum force that should be applied to constrain the bodies.
  *
@@ -31,32 +31,39 @@ var ContactEquation = require('../equations/ContactEquation');
 function PointToPointConstraint(bodyA,pivotA,bodyB,pivotB,maxForce){
     Constraint.call(this,bodyA,bodyB);
 
+    this.pivotA = pivotA.clone();
+    this.pivotB = pivotB.clone();
+
+    var x = this.equationX = new ContactEquation(bodyA,bodyB);
+    var y = this.equationY = new ContactEquation(bodyA,bodyB);
+    var z = this.equationZ = new ContactEquation(bodyA,bodyB);
+
     // Equations to be fed to the solver
-    var eqs = this.equations = [
-        new ContactEquation(bodyA,bodyB), // Normal
-        new ContactEquation(bodyA,bodyB), // Tangent2
-        new ContactEquation(bodyA,bodyB), // Tangent2
-    ];
-
-    var normal = eqs[0];
-    var t1 = eqs[1];
-    var t2 = eqs[2];
-
-    t1.minForce = t2.minForce = normal.minForce = -maxForce;
-    t1.maxForce = t2.maxForce = normal.maxForce =  maxForce;
-
-    // Update
-    this.update = function(){
-        bodyB.position.vsub(bodyA.position,normal.ni);
-        normal.ni.normalize();
-        bodyA.quaternion.vmult(pivotA,normal.ri);
-        bodyB.quaternion.vmult(pivotB,normal.rj);
-
-        normal.ni.tangents(t1.ni,t2.ni);
-        t1.ri.copy(normal.ri);
-        t1.rj.copy(normal.rj);
-        t2.ri.copy(normal.ri);
-        t2.rj.copy(normal.rj);
-    };
+    this.equations.push(x, y, z);
+
+    // Make the equations bidirectional
+    x.minForce = y.minForce = z.minForce = -maxForce;
+    z.maxForce = y.maxForce = z.maxForce =  maxForce;
+
+    x.ni.set(1, 0, 0);
+    y.ni.set(0, 1, 0);
+    z.ni.set(0, 0, 1);
 }
 PointToPointConstraint.prototype = new Constraint();
+
+PointToPointConstraint.prototype.update = function(){
+    var bodyA = this.bodyA;
+    var bodyB = this.bodyB;
+    var x = this.equationX;
+    var y = this.equationY;
+    var z = this.equationZ;
+
+    // Rotate the pivots to world space
+    bodyA.quaternion.vmult(this.pivotA,x.ri);
+    bodyB.quaternion.vmult(this.pivotB,x.rj);
+
+    y.ri.copy(x.ri);
+    y.rj.copy(x.rj);
+    z.ri.copy(x.ri);
+    z.rj.copy(x.rj);
+};
\ No newline at end of file