Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- this.solveConstraints = function(){
- for (var manifoldID in contactManifolds) {
- var manifold = contactManifolds[manifoldID];
- for (var contactID in manifold.contacts) {
- var contact = manifold.contacts[contactID];
- contact.appliedPushImpulse = 0;
- }
- }
- for (var uid in physique.bodies) {
- var body = physique.bodies[uid];
- body.deltaV.multiplyScalar(0);
- body.deltaW.multiplyScalar(0);
- }
- // Penetration Solver
- for (var manifoldID in contactManifolds) {
- var manifold = contactManifolds[manifoldID];
- for (var contactID in manifold.contacts) {
- var contact = manifold.contacts[contactID],
- row = contact.rows[0];
- for (var iteration=0; iteration<1; ++iteration) {
- var delta = contact.depth - contact.appliedPushImpulse,
- deltaV1Dotn = contact.normal.dot( contact.bodyA.velocity ) + contact.tangent.dot( contact.bodyA.angularVelocity ),
- deltaV2Dotn = contact.normal.clone().negate().dot( contact.bodyB.velocity ) + contact.tangent2.dot( contact.bodyB.angularVelocity );
- delta -= deltaV1Dotn * contact.JDiagABInv;
- delta -= deltaV2Dotn * contact.JDiagABInv;
- var sum = contact.appliedPushImpulse + delta;
- if (sum < 0) {
- delta = -contact.appliedPushImpulse;
- contact.appliedPushImpulse = 0;
- break;
- } else {
- contact.appliedPushImpulse = sum;
- }
- contact.bodyA.velocity.add( contact.normal.clone().multiplyScalar(contact.bodyA.invMass * delta) );
- // var v = contact.tangent.clone().multiplyScalar(-contact.bodyA.invInertiaTensor * delta),
- // e = (new THREE.Euler()).setFromVector3(v),
- // q = (new THREE.Quaternion()).setFromEuler(e);
- // contact.bodyA.angularVelocity.applyQuaternion(q);
- contact.bodyA.angularVelocity.add( contact.tangent.clone().multiplyScalar(contact.bodyA.invInertiaTensor * delta) );
- contact.bodyB.velocity.add( contact.normal.clone().negate().multiplyScalar(contact.bodyB.invMass * delta) );
- // v = contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor * delta);
- // e = (new THREE.Euler()).setFromVector3(v);
- // q = (new THREE.Quaternion()).setFromEuler(e);
- // contact.bodyB.angularVelocity.applyQuaternion(q);
- contact.bodyB.angularVelocity.add( contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor * delta) );
- }
- // Warmstart
- contact.appliedImpulse *= 0.85;
- contact.bodyA.applyImpulse( contact.normal.clone().multiplyScalar(contact.bodyA.invMass), contact.tangent.clone().multiplyScalar(-contact.bodyA.invInertiaTensor), contact.appliedImpulse );
- contact.bodyB.applyImpulse( contact.normal.clone().multiplyScalar(contact.bodyB.invMass), contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor), contact.appliedImpulse );
- // Baumgarte Stabilization (with restitution for later..)
- var baumgarte = -0.2,
- restitution = 0.00;
- var JDotV = 0;
- JDotV += row.J[0] * contact.bodyA.velocity.x;
- JDotV += row.J[1] * contact.bodyA.velocity.y;
- JDotV += row.J[2] * contact.bodyA.velocity.z;
- JDotV += row.J[3] * contact.bodyA.angularVelocity.x;
- JDotV += row.J[4] * contact.bodyA.angularVelocity.y;
- JDotV += row.J[5] * contact.bodyA.angularVelocity.z;
- JDotV += row.J[6] * contact.bodyB.velocity.x;
- JDotV += row.J[7] * contact.bodyB.velocity.y;
- JDotV += row.J[8] * contact.bodyB.velocity.z;
- JDotV += row.J[9] * contact.bodyB.angularVelocity.x;
- JDotV += row.J[10] * contact.bodyB.angularVelocity.y;
- JDotV += row.J[11] * contact.bodyB.angularVelocity.z;
- var idt = 1/dt;
- var C = JDotV,
- JDotVTerm = 0,
- Fext = new THREE.Vector3(0, -9.8, 0),
- Text = new THREE.Vector3(0, 0, 0);
- JDotVTerm += row.J[0] * (contact.bodyA.velocity.x / idt + contact.bodyA.invMass * Fext.x);
- JDotVTerm += row.J[1] * (contact.bodyA.velocity.y / idt + contact.bodyA.invMass * Fext.y);
- JDotVTerm += row.J[2] * (contact.bodyA.velocity.z / idt + contact.bodyA.invMass * Fext.z);
- JDotVTerm += row.J[3] * (contact.bodyA.angularVelocity.x / idt + contact.bodyA.invMass * Text.x);
- JDotVTerm += row.J[4] * (contact.bodyA.angularVelocity.y / idt + contact.bodyA.invMass * Text.y);
- JDotVTerm += row.J[5] * (contact.bodyA.angularVelocity.z / idt + contact.bodyA.invMass * Text.z);
- JDotVTerm += row.J[6] * (contact.bodyB.velocity.x / idt + contact.bodyB.invMass * Fext.x);
- JDotVTerm += row.J[7] * (contact.bodyB.velocity.y / idt + contact.bodyB.invMass * Fext.y);
- JDotVTerm += row.J[8] * (contact.bodyB.velocity.z / idt + contact.bodyB.invMass * Fext.z);
- JDotVTerm += row.J[9] * (contact.bodyB.angularVelocity.x / idt + contact.bodyB.invMass * Text.x);
- JDotVTerm += row.J[10] * (contact.bodyB.angularVelocity.y / idt + contact.bodyB.invMass * Text.y);
- JDotVTerm += row.J[11] * (contact.bodyB.angularVelocity.z / idt + contact.bodyB.invMass * Text.z);
- contact.rhs = baumgarte*C/idt - restitution*JDotVTerm;
- }
- }
- var collisionResIterations = 0;
- for (var manifoldID in contactManifolds) {
- var manifold = contactManifolds[manifoldID];
- for (var contactID in manifold.contacts) {
- var contact = manifold.contacts[contactID];
- for (var iteration=0; iteration<collisionResIterations; ++iteration) {
- var row = contact.rows[0], // contact constraint
- JdotV = 0,
- delta = 0;
- // Erin Catto GDC-2005
- // a = B*lambda
- var d = row.D,
- JDotA = 0,
- a = [];
- for (var ji=0; ji<12; ++ji) {
- a[ji] = row.B[ji] * contact.appliedImpulse;
- JDotA += row.J[ji] * a[ji];
- }
- delta = (contact.rhs - JDotA) / d;
- var sum = contact.appliedImpulse + delta;
- if (sum < 0) {
- delta = -contact.appliedImpulse;
- sum = 0;
- contact.appliedImpulse = 0;
- } else {
- contact.appliedImpulse = sum;
- }
- // if (Math.abs(delta) < 0.0001) break; // Breaking impulse
- contact.bodyA.applyImpulse(contact.normal.clone().multiplyScalar(contact.bodyA.invMass), contact.tangent.clone().multiplyScalar(contact.bodyA.invInertiaTensor), delta);
- contact.bodyB.applyImpulse(contact.normal.clone().multiplyScalar(-contact.bodyB.invMass), contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor), delta);
- }
- }
- }
- };
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement