diff --git a/src/equations/ContactEquation.js b/src/equations/ContactEquation.js index 8aa95fdd2..ecd664ec4 100644 --- a/src/equations/ContactEquation.js +++ b/src/equations/ContactEquation.js @@ -160,84 +160,3 @@ ContactEquation.prototype.getImpactVelocityAlongNormal = function(){ return this.ni.dot(relVel); }; -// Compute C = GMG+eps in the SPOOK equation -/* -var computeC_temp1 = new Vec3(); -var computeC_temp2 = new Vec3(); -ContactEquation.prototype.computeC = function(){ - var bi = this.bi; - var bj = this.bj; - var rixn = this.rixn; - var rjxn = this.rjxn; - var invMassi = bi.invMass; - var invMassj = bj.invMass; - - var C = invMassi + invMassj + this.eps; - - var invIi = this.invIi; - var invIj = this.invIj; - - // Compute rxn * I * rxn for each body - if(bi.invInertiaWorld) bi.invInertiaWorld.vmult(rixn, this.biInvInertiaTimesRixn); - if(bj.invInertiaWorld) bj.invInertiaWorld.vmult(rjxn, this.bjInvInertiaTimesRjxn); - - C += this.biInvInertiaTimesRixn.dot(rixn); - C += this.bjInvInertiaTimesRjxn.dot(rjxn); - - return C; -}; - -var computeGWlambda_ulambda = new Vec3(); -ContactEquation.prototype.computeGWlambda = function(){ - var bi = this.bi; - var bj = this.bj; - var ulambda = computeGWlambda_ulambda; - - var GWlambda = 0.0; - - bj.vlambda.vsub(bi.vlambda, ulambda); - GWlambda += ulambda.dot(this.ni); - - // Angular - if(bi.wlambda){ - GWlambda -= bi.wlambda.dot(this.rixn); - } - if(bj.wlambda){ - GWlambda += bj.wlambda.dot(this.rjxn); - } - - return GWlambda; -}; - -var ContactEquation_addToWlambda_temp1 = new Vec3(); -var ContactEquation_addToWlambda_temp2 = new Vec3(); -ContactEquation.prototype.addToWlambda = function(deltalambda){ - var bi = this.bi, - bj = this.bj, - rixn = this.rixn, - rjxn = this.rjxn, - invMassi = bi.invMass, - invMassj = bj.invMass, - n = this.ni, - temp1 = ContactEquation_addToWlambda_temp1, - temp2 = ContactEquation_addToWlambda_temp2; - - - // Add to linear velocity - n.mult(invMassi * deltalambda, temp2); - bi.vlambda.vsub(temp2,bi.vlambda); - n.mult(invMassj * deltalambda, temp2); - bj.vlambda.vadd(temp2,bj.vlambda); - - // Add to angular velocity - if(bi.wlambda !== undefined){ - this.biInvInertiaTimesRixn.mult(deltalambda,temp1); - - bi.wlambda.vsub(temp1,bi.wlambda); - } - if(bj.wlambda !== undefined){ - this.bjInvInertiaTimesRjxn.mult(deltalambda,temp1); - bj.wlambda.vadd(temp1,bj.wlambda); - } -}; -*/ diff --git a/src/equations/FrictionEquation.js b/src/equations/FrictionEquation.js index 7580e6745..9c45fcb12 100644 --- a/src/equations/FrictionEquation.js +++ b/src/equations/FrictionEquation.js @@ -108,80 +108,3 @@ FrictionEquation.prototype.computeB = function(h){ return B; }; - -/* -// Compute C = G * Minv * G + eps -//var FEcomputeC_temp1 = new Vec3(); -//var FEcomputeC_temp2 = new Vec3(); -FrictionEquation.prototype.computeC = function(){ - var bi = this.bi, - bj = this.bj, - rixt = this.rixt, - rjxt = this.rjxt, - invMassi = bi.invMass, - invMassj = bj.invMass, - C = invMassi + invMassj + this.eps, - invIi = this.invIi, - invIj = this.invIj; - - // Compute rxt * I * rxt for each body - if(bi.invInertiaWorld) bi.invInertiaWorld.vmult(rixt,this.biInvInertiaTimesRixt); - if(bj.invInertiaWorld) bj.invInertiaWorld.vmult(rjxt,this.bjInvInertiaTimesRjxt); - C += this.biInvInertiaTimesRixt.dot(rixt); - C += this.bjInvInertiaTimesRjxt.dot(rjxt); - - return C; -}; - -var FrictionEquation_computeGWlambda_ulambda = new Vec3(); -FrictionEquation.prototype.computeGWlambda = function(){ - var bi = this.bi; - var bj = this.bj; - - var GWlambda = 0.0; - var ulambda = FrictionEquation_computeGWlambda_ulambda; - bj.vlambda.vsub(bi.vlambda,ulambda); - GWlambda += ulambda.dot(this.t); - - // Angular - if(bi.wlambda){ - GWlambda -= bi.wlambda.dot(this.rixt); - } - if(bj.wlambda){ - GWlambda += bj.wlambda.dot(this.rjxt); - } - - return GWlambda; -}; - -var FrictionEquation_addToWlambda_tmp = new Vec3(); -FrictionEquation.prototype.addToWlambda = function(deltalambda){ - var bi = this.bi, - bj = this.bj, - rixt = this.rixt, - rjxt = this.rjxt, - invMassi = bi.invMass, - invMassj = bj.invMass, - t = this.t, - tmp = FrictionEquation_addToWlambda_tmp, - wi = bi.wlambda, - wj = bj.wlambda; - - // Add to linear velocity - t.mult(invMassi * deltalambda, tmp); - bi.vlambda.vsub(tmp,bi.vlambda); - - t.mult(invMassj * deltalambda, tmp); - bj.vlambda.vadd(tmp,bj.vlambda); - - // Add to angular velocity - if(wi){ - this.biInvInertiaTimesRixt.mult(deltalambda,tmp); - wi.vsub(tmp,wi); - } - if(wj){ - this.bjInvInertiaTimesRjxt.mult(deltalambda,tmp); - wj.vadd(tmp,wj); - } -}; -*/ diff --git a/src/equations/RotationalEquation.js b/src/equations/RotationalEquation.js index 7239a91c2..058112238 100644 --- a/src/equations/RotationalEquation.js +++ b/src/equations/RotationalEquation.js @@ -86,68 +86,3 @@ RotationalEquation.prototype.computeB = function(h){ return B; }; -/* -// Compute C = GMG+eps -RotationalEquation.prototype.computeC = function(){ - var bi = this.bi; - var bj = this.bj; - var nixnj = this.nixnj; - var njxni = this.njxni; - var invMassi = bi.invMass; - var invMassj = bj.invMass; - - var C = this.eps; - - - C += bi.invInertiaWorld.vmult(njxni).dot(njxni); - C += bj.invInertiaWorld.vmult(nixnj).dot(nixnj); - - return C; -}; - -var computeGWlambda_ulambda = new Vec3(); -RotationalEquation.prototype.computeGWlambda = function(){ - var bi = this.bi; - var bj = this.bj; - var ulambda = computeGWlambda_ulambda; - - var GWlambda = 0.0; - //bj.vlambda.vsub(bi.vlambda, ulambda); - //GWlambda += ulambda.dot(this.ni); - - // Angular - if(bi.wlambda){ - GWlambda += bi.wlambda.dot(this.njxni); - } - if(bj.wlambda){ - GWlambda += bj.wlambda.dot(this.nixnj); - } - - //console.log("GWlambda:",GWlambda); - - return GWlambda; -}; - -RotationalEquation.prototype.addToWlambda = function(deltalambda){ - var bi = this.bi; - var bj = this.bj; - var nixnj = this.nixnj; - var njxni = this.njxni; - var invMassi = bi.invMass; - var invMassj = bj.invMass; - - // Add to linear velocity - //bi.vlambda.vsub(n.mult(invMassi * deltalambda),bi.vlambda); - //bj.vlambda.vadd(n.mult(invMassj * deltalambda),bj.vlambda); - - // Add to angular velocity - if(bi.wlambda){ - var I = bi.invInertiaWorld; - bi.wlambda.vsub(I.vmult(nixnj).mult(deltalambda),bi.wlambda); - } - if(bj.wlambda){ - var I = bj.invInertiaWorld; - bj.wlambda.vadd(I.vmult(nixnj).mult(deltalambda),bj.wlambda); - } -}; -*/ diff --git a/src/equations/RotationalMotorEquation.js b/src/equations/RotationalMotorEquation.js index 190091c02..6a76f77e1 100644 --- a/src/equations/RotationalMotorEquation.js +++ b/src/equations/RotationalMotorEquation.js @@ -75,70 +75,3 @@ RotationalMotorEquation.prototype.computeB = function(h){ return B; }; - -/* -// Compute C = GMG+eps -RotationalMotorEquation.prototype.computeC = function(){ - var bi = this.bi; - var bj = this.bj; - var axisA = this.axisA; - var axisB = this.axisB; - var invMassi = bi.invMass; - var invMassj = bj.invMass; - - var C = this.eps; - - C += bi.invInertiaWorld.vmult(axisA).dot(axisB); - C += bj.invInertiaWorld.vmult(axisB).dot(axisB); - - return C; -}; - -var computeGWlambda_ulambda = new Vec3(); -RotationalMotorEquation.prototype.computeGWlambda = function(){ - var bi = this.bi; - var bj = this.bj; - var ulambda = computeGWlambda_ulambda; - var axisA = this.axisA; - var axisB = this.axisB; - - var GWlambda = 0.0; - //bj.vlambda.vsub(bi.vlambda, ulambda); - //GWlambda += ulambda.dot(this.ni); - - // Angular - if(bi.wlambda){ - GWlambda += bi.wlambda.dot(axisA); - } - if(bj.wlambda){ - GWlambda += bj.wlambda.dot(axisB); - } - - //console.log("GWlambda:",GWlambda); - - return GWlambda; -}; - -RotationalMotorEquation.prototype.addToWlambda = function(deltalambda){ - var bi = this.bi; - var bj = this.bj; - var axisA = this.axisA; - var axisB = this.axisB; - var invMassi = bi.invMass; - var invMassj = bj.invMass; - - // Add to linear velocity - //bi.vlambda.vsub(n.mult(invMassi * deltalambda),bi.vlambda); - //bj.vlambda.vadd(n.mult(invMassj * deltalambda),bj.vlambda); - - // Add to angular velocity - if(bi.wlambda){ - var I = bi.invInertiaWorld; - bi.wlambda.vsub(I.vmult(axisA).mult(deltalambda),bi.wlambda); - } - if(bj.wlambda){ - var I = bj.invInertiaWorld; - bj.wlambda.vadd(I.vmult(axisB).mult(deltalambda),bj.wlambda); - } -}; -*/