Permalink
Browse files

Fixed bug in cpVect, and added groove joint

  • Loading branch information...
1 parent b721933 commit 815b3e09761ca23fd533ffe7128e499e149e5e29 @josephg committed Jan 14, 2012
Showing with 298 additions and 16 deletions.
  1. +2 −1 Makefile
  2. +138 −5 cp.js
  3. +1 −1 cp.min.js
  4. +1 −1 demo/Joints.js
  5. +18 −6 demo/demo.js
  6. +136 −0 lib/constraints/cpGrooveJoint.js
  7. +2 −2 lib/cpVect.js
View
@@ -22,7 +22,8 @@ constraints = \
cpConstraint.js \
cpPinJoint.js \
cpSlideJoint.js \
- cpPivotJoint.js
+ cpPivotJoint.js \
+ cpGrooveJoint.js
demos = demo.js \
ball.js \
View
143 cp.js
@@ -364,12 +364,12 @@ var vpvrperp = exports.v.pvrperp = function(v)
/// Returns the vector projection of v1 onto v2.
var vproject = exports.v.project = function(v1, v2)
{
- return vmult(v2, vdot(v1, v2)/v2.lengthsq());
+ return vmult(v2, vdot(v1, v2)/vlengthsq(v2));
};
Vect.prototype.project = function(v2)
{
- this.mult(vdot(this, v2) / v2.lengthsq());
+ this.mult(vdot(this, v2) / vlengthsq(v2));
return this;
};
@@ -4976,7 +4976,6 @@ var PinJoint = exports.PinJoint = function(a, b, anchr1, anchr2)
};
PinJoint.prototype = Object.create(Constraint.prototype);
-PinJoint.prototype.type = "pin joint";
PinJoint.prototype.preStep = function(dt)
{
@@ -5069,7 +5068,6 @@ var SlideJoint = exports.SlideJoint = function(a, b, anchr1, anchr2, min, max)
};
SlideJoint.prototype = Object.create(Constraint.prototype);
-SlideJoint.prototype.type = "slide joint";
SlideJoint.prototype.preStep = function(dt)
{
@@ -5187,7 +5185,6 @@ var PivotJoint = exports.PivotJoint = function(a, b, anchr1, anchr2)
};
PivotJoint.prototype = Object.create(Constraint.prototype);
-PivotJoint.prototype.type = "pivot joint";
PivotJoint.prototype.preStep = function(dt)
{
@@ -5238,4 +5235,140 @@ PivotJoint.prototype.getImpulse = function()
return vlength(this.jAcc);
};
+/* Copyright (c) 2007 Scott Lembcke
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+var GrooveJoint = exports.GrooveJoint = function(a, b, groove_a, groove_b, anchr2)
+{
+ Constraint.call(this, a, b);
+
+ this.grv_a = groove_a;
+ this.grv_b = groove_b;
+ this.grv_n = vperp(vnormalize(vsub(groove_b, groove_a)));
+ this.anchr2 = anchr2;
+
+ this.grv_tn = null;
+ this.clamp = 0;
+ this.r1 = this.r2 = null;
+
+ this.k1 = new Vect(0,0);
+ this.k2 = new Vect(0,0);
+
+ this.jAcc = vzero;
+ this.jMaxLen = 0;
+ this.bias = null;
+};
+
+GrooveJoint.prototype = Object.create(Constraint.prototype);
+
+GrooveJoint.prototype.preStep = function(dt)
+{
+ var a = this.a;
+ var b = this.b;
+
+ // calculate endpoints in worldspace
+ var ta = a.local2World(this.grv_a);
+ var tb = a.local2World(this.grv_b);
+
+ // calculate axis
+ var n = vrotate(this.grv_n, a.rot);
+ var d = vdot(ta, n);
+
+ this.grv_tn = n;
+ this.r2 = vrotate(this.anchr2, b.rot);
+
+ // calculate tangential distance along the axis of r2
+ var td = vcross(vadd(b.p, this.r2), n);
+ // calculate clamping factor and r2
+ if(td <= vcross(ta, n)){
+ this.clamp = 1;
+ this.r1 = vsub(ta, a.p);
+ } else if(td >= vcross(tb, n)){
+ this.clamp = -1;
+ this.r1 = vsub(tb, a.p);
+ } else {
+ this.clamp = 0;
+ this.r1 = vsub(vadd(vmult(vperp(n), -td), vmult(n, d)), a.p);
+ }
+
+ // Calculate mass tensor
+ k_tensor(a, b, this.r1, this.r2, this.k1, this.k2);
+
+ // compute max impulse
+ this.jMaxLen = this.maxForce * dt;
+
+ // calculate bias velocity
+ var delta = vsub(vadd(b.p, this.r2), vadd(a.p, this.r1));
+ this.bias = vclamp(vmult(delta, -bias_coef(this.errorBias, dt)/dt), this.maxBias);
+};
+
+GrooveJoint.prototype.applyCachedImpulse = function(dt_coef)
+{
+ apply_impulses(this.a, this.b, this.r1, this.r2, this.jAcc.x * dt_coef, this.jAcc.y * dt_coef);
+};
+
+GrooveJoint.prototype.grooveConstrain = function(j){
+ var n = this.grv_tn;
+ var jClamp = (this.clamp*vcross(j, n) > 0) ? j : vproject(j, n);
+ return vclamp(jClamp, this.jMaxLen);
+};
+
+GrooveJoint.prototype.applyImpulse = function()
+{
+ var a = this.a;
+ var b = this.b;
+
+ var r1 = this.r1;
+ var r2 = this.r2;
+
+ // compute impulse
+ var vr = relative_velocity(a, b, r1, r2);
+
+ var j = mult_k(vsub(this.bias, vr), this.k1, this.k2);
+ var jOld = this.jAcc;
+ this.jAcc = this.grooveConstrain(vadd(jOld, j));
+
+ // apply impulse
+ apply_impulses(a, b, this.r1, this.r2, this.jAcc.x - jOld.x, this.jAcc.y - jOld.y);
+};
+
+GrooveJoint.prototype.getImpulse = function()
+{
+ return vlength(this.jAcc);
+};
+
+GrooveJoint.prototype.setGrooveA = function(value)
+{
+ this.grv_a = value;
+ this.grv_n = vperp(vnormalize(vsub(this.grv_b, value)));
+
+ this.activateBodies();
+};
+
+GrooveJoint.prototype.setGrooveB = function(value)
+{
+ this.grv_b = value;
+ this.grv_n = vperp(vnormalize(vsub(value, this.grv_a)));
+
+ this.activateBodies();
+};
+
})();
View

Large diffs are not rendered by default.

Oops, something went wrong.
View
@@ -163,7 +163,7 @@ var Joints = function() {
boxOffset = v(480, 0);
body1 = addBall(posA, boxOffset);
body2 = addBall(posB, boxOffset);
- //space.addConstraint(cpGrooveJointNew(body1, body2, v(30,30), v(30,-30), v(-30,0)));
+ space.addConstraint(new cp.GrooveJoint(body1, body2, v(30,30), v(30,-30), v(-30,0)));
// Damped Springs
boxOffset = v(0, 120);
View
@@ -329,17 +329,17 @@ cp.CircleShape.prototype.draw = function(ctx, scale, point2canvas) {
// Draw methods for constraints
cp.PinJoint.prototype.draw = function(ctx, scale, point2canvas) {
- var a = v.add(this.a.p, v.rotate(this.anchr1, this.a.rot));
- var b = v.add(this.b.p, v.rotate(this.anchr2, this.b.rot));
+ var a = this.a.local2World(this.anchr1);
+ var b = this.b.local2World(this.anchr2);
ctx.lineWidth = 2;
ctx.strokeStyle = "grey";
drawLine(ctx, point2canvas, a, b);
};
cp.SlideJoint.prototype.draw = function(ctx, scale, point2canvas) {
- var a = v.add(this.a.p, v.rotate(this.anchr1, this.a.rot));
- var b = v.add(this.b.p, v.rotate(this.anchr2, this.b.rot));
+ var a = this.a.local2World(this.anchr1);
+ var b = this.b.local2World(this.anchr2);
var midpoint = v.add(a, v.clamp(v.sub(b, a), this.min));
ctx.lineWidth = 2;
@@ -350,14 +350,26 @@ cp.SlideJoint.prototype.draw = function(ctx, scale, point2canvas) {
};
cp.PivotJoint.prototype.draw = function(ctx, scale, point2canvas) {
- var a = v.add(this.a.p, v.rotate(this.anchr1, this.a.rot));
- var b = v.add(this.b.p, v.rotate(this.anchr2, this.b.rot));
+ var a = this.a.local2World(this.anchr1);
+ var b = this.b.local2World(this.anchr2);
ctx.strokeStyle = "grey";
ctx.fillStyle = "grey";
drawCircle(ctx, scale, point2canvas, a, 2);
drawCircle(ctx, scale, point2canvas, b, 2);
};
+cp.GrooveJoint.prototype.draw = function(ctx, scale, point2canvas) {
+ var a = this.a.local2World(this.grv_a);
+ var b = this.a.local2World(this.grv_b);
+ var c = this.b.local2World(this.anchr2);
+
+ ctx.strokeStyle = "grey";
+ drawLine(ctx, point2canvas, a, b);
+ drawCircle(ctx, scale, point2canvas, c, 3);
+};
+
+
+
var randColor = function() {
return Math.floor(Math.random() * 256);
};
@@ -0,0 +1,136 @@
+/* Copyright (c) 2007 Scott Lembcke
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+var GrooveJoint = exports.GrooveJoint = function(a, b, groove_a, groove_b, anchr2)
+{
+ Constraint.call(this, a, b);
+
+ this.grv_a = groove_a;
+ this.grv_b = groove_b;
+ this.grv_n = vperp(vnormalize(vsub(groove_b, groove_a)));
+ this.anchr2 = anchr2;
+
+ this.grv_tn = null;
+ this.clamp = 0;
+ this.r1 = this.r2 = null;
+
+ this.k1 = new Vect(0,0);
+ this.k2 = new Vect(0,0);
+
+ this.jAcc = vzero;
+ this.jMaxLen = 0;
+ this.bias = null;
+};
+
+GrooveJoint.prototype = Object.create(Constraint.prototype);
+
+GrooveJoint.prototype.preStep = function(dt)
+{
+ var a = this.a;
+ var b = this.b;
+
+ // calculate endpoints in worldspace
+ var ta = a.local2World(this.grv_a);
+ var tb = a.local2World(this.grv_b);
+
+ // calculate axis
+ var n = vrotate(this.grv_n, a.rot);
+ var d = vdot(ta, n);
+
+ this.grv_tn = n;
+ this.r2 = vrotate(this.anchr2, b.rot);
+
+ // calculate tangential distance along the axis of r2
+ var td = vcross(vadd(b.p, this.r2), n);
+ // calculate clamping factor and r2
+ if(td <= vcross(ta, n)){
+ this.clamp = 1;
+ this.r1 = vsub(ta, a.p);
+ } else if(td >= vcross(tb, n)){
+ this.clamp = -1;
+ this.r1 = vsub(tb, a.p);
+ } else {
+ this.clamp = 0;
+ this.r1 = vsub(vadd(vmult(vperp(n), -td), vmult(n, d)), a.p);
+ }
+
+ // Calculate mass tensor
+ k_tensor(a, b, this.r1, this.r2, this.k1, this.k2);
+
+ // compute max impulse
+ this.jMaxLen = this.maxForce * dt;
+
+ // calculate bias velocity
+ var delta = vsub(vadd(b.p, this.r2), vadd(a.p, this.r1));
+ this.bias = vclamp(vmult(delta, -bias_coef(this.errorBias, dt)/dt), this.maxBias);
+};
+
+GrooveJoint.prototype.applyCachedImpulse = function(dt_coef)
+{
+ apply_impulses(this.a, this.b, this.r1, this.r2, this.jAcc.x * dt_coef, this.jAcc.y * dt_coef);
+};
+
+GrooveJoint.prototype.grooveConstrain = function(j){
+ var n = this.grv_tn;
+ var jClamp = (this.clamp*vcross(j, n) > 0) ? j : vproject(j, n);
+ return vclamp(jClamp, this.jMaxLen);
+};
+
+GrooveJoint.prototype.applyImpulse = function()
+{
+ var a = this.a;
+ var b = this.b;
+
+ var r1 = this.r1;
+ var r2 = this.r2;
+
+ // compute impulse
+ var vr = relative_velocity(a, b, r1, r2);
+
+ var j = mult_k(vsub(this.bias, vr), this.k1, this.k2);
+ var jOld = this.jAcc;
+ this.jAcc = this.grooveConstrain(vadd(jOld, j));
+
+ // apply impulse
+ apply_impulses(a, b, this.r1, this.r2, this.jAcc.x - jOld.x, this.jAcc.y - jOld.y);
+};
+
+GrooveJoint.prototype.getImpulse = function()
+{
+ return vlength(this.jAcc);
+};
+
+GrooveJoint.prototype.setGrooveA = function(value)
+{
+ this.grv_a = value;
+ this.grv_n = vperp(vnormalize(vsub(this.grv_b, value)));
+
+ this.activateBodies();
+};
+
+GrooveJoint.prototype.setGrooveB = function(value)
+{
+ this.grv_b = value;
+ this.grv_n = vperp(vnormalize(vsub(value, this.grv_a)));
+
+ this.activateBodies();
+};
+
Oops, something went wrong.

0 comments on commit 815b3e0

Please sign in to comment.