Advertisement
Guest User

Bad Physics!

a guest
Mar 7th, 2015
419
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.         this.solveConstraints = function(){
  2.  
  3.             for (var manifoldID in contactManifolds) {
  4.  
  5.                 var manifold = contactManifolds[manifoldID];
  6.                 for (var contactID in manifold.contacts) {
  7.  
  8.                     var contact = manifold.contacts[contactID];
  9.                     contact.appliedPushImpulse = 0;
  10.                 }
  11.             }
  12.  
  13.             for (var uid in physique.bodies) {
  14.                 var body = physique.bodies[uid];
  15.                 body.deltaV.multiplyScalar(0);
  16.                 body.deltaW.multiplyScalar(0);
  17.             }
  18.  
  19.  
  20.             // Penetration Solver
  21.             for (var manifoldID in contactManifolds) {
  22.  
  23.                 var manifold = contactManifolds[manifoldID];
  24.                 for (var contactID in manifold.contacts) {
  25.  
  26.                     var contact = manifold.contacts[contactID],
  27.                         row = contact.rows[0];
  28.  
  29.  
  30.  
  31.                     for (var iteration=0; iteration<1; ++iteration) {
  32.                         var delta = contact.depth - contact.appliedPushImpulse,
  33.                             deltaV1Dotn = contact.normal.dot( contact.bodyA.velocity ) + contact.tangent.dot( contact.bodyA.angularVelocity ),
  34.                             deltaV2Dotn = contact.normal.clone().negate().dot( contact.bodyB.velocity ) + contact.tangent2.dot( contact.bodyB.angularVelocity );
  35.  
  36.                         delta -= deltaV1Dotn * contact.JDiagABInv;
  37.                         delta -= deltaV2Dotn * contact.JDiagABInv;
  38.  
  39.                         var sum = contact.appliedPushImpulse + delta;
  40.                         if (sum < 0) {
  41.                             delta = -contact.appliedPushImpulse;
  42.                             contact.appliedPushImpulse = 0;
  43.                             break;
  44.                         } else {
  45.                             contact.appliedPushImpulse = sum;
  46.                         }
  47.  
  48.                         contact.bodyA.velocity.add( contact.normal.clone().multiplyScalar(contact.bodyA.invMass * delta) );
  49.  
  50.  
  51.                         // var v = contact.tangent.clone().multiplyScalar(-contact.bodyA.invInertiaTensor * delta),
  52.                         //  e = (new THREE.Euler()).setFromVector3(v),
  53.                         //  q = (new THREE.Quaternion()).setFromEuler(e);
  54.                         // contact.bodyA.angularVelocity.applyQuaternion(q);
  55.                         contact.bodyA.angularVelocity.add( contact.tangent.clone().multiplyScalar(contact.bodyA.invInertiaTensor * delta) );
  56.  
  57.                         contact.bodyB.velocity.add( contact.normal.clone().negate().multiplyScalar(contact.bodyB.invMass * delta) );
  58.                         // v = contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor * delta);
  59.                         // e = (new THREE.Euler()).setFromVector3(v);
  60.                         // q = (new THREE.Quaternion()).setFromEuler(e);
  61.                         // contact.bodyB.angularVelocity.applyQuaternion(q);
  62.                         contact.bodyB.angularVelocity.add( contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor * delta) );
  63.  
  64.                     }
  65.  
  66.  
  67.  
  68.                     // Warmstart
  69.                     contact.appliedImpulse *= 0.85;
  70.  
  71.                     contact.bodyA.applyImpulse( contact.normal.clone().multiplyScalar(contact.bodyA.invMass), contact.tangent.clone().multiplyScalar(-contact.bodyA.invInertiaTensor), contact.appliedImpulse );
  72.                     contact.bodyB.applyImpulse( contact.normal.clone().multiplyScalar(contact.bodyB.invMass), contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor), contact.appliedImpulse );
  73.  
  74.  
  75.                     // Baumgarte Stabilization (with restitution for later..)
  76.                     var baumgarte = -0.2,
  77.                         restitution = 0.00;
  78.  
  79.  
  80.                     var JDotV = 0;
  81.                     JDotV += row.J[0] * contact.bodyA.velocity.x;
  82.                     JDotV += row.J[1] * contact.bodyA.velocity.y;
  83.                     JDotV += row.J[2] * contact.bodyA.velocity.z;
  84.                     JDotV += row.J[3] * contact.bodyA.angularVelocity.x;
  85.                     JDotV += row.J[4] * contact.bodyA.angularVelocity.y;
  86.                     JDotV += row.J[5] * contact.bodyA.angularVelocity.z;
  87.  
  88.                     JDotV += row.J[6]  * contact.bodyB.velocity.x;
  89.                     JDotV += row.J[7]  * contact.bodyB.velocity.y;
  90.                     JDotV += row.J[8]  * contact.bodyB.velocity.z;
  91.                     JDotV += row.J[9]  * contact.bodyB.angularVelocity.x;
  92.                     JDotV += row.J[10] * contact.bodyB.angularVelocity.y;
  93.                     JDotV += row.J[11] * contact.bodyB.angularVelocity.z;
  94.                     var idt = 1/dt;
  95.                     var C = JDotV,
  96.                         JDotVTerm = 0,
  97.                         Fext = new THREE.Vector3(0, -9.8, 0),
  98.                         Text = new THREE.Vector3(0, 0, 0);
  99.                     JDotVTerm += row.J[0] * (contact.bodyA.velocity.x / idt + contact.bodyA.invMass * Fext.x);
  100.                     JDotVTerm += row.J[1] * (contact.bodyA.velocity.y / idt + contact.bodyA.invMass * Fext.y);
  101.                     JDotVTerm += row.J[2] * (contact.bodyA.velocity.z / idt + contact.bodyA.invMass * Fext.z);
  102.                     JDotVTerm += row.J[3] * (contact.bodyA.angularVelocity.x / idt + contact.bodyA.invMass * Text.x);
  103.                     JDotVTerm += row.J[4] * (contact.bodyA.angularVelocity.y / idt + contact.bodyA.invMass * Text.y);
  104.                     JDotVTerm += row.J[5] * (contact.bodyA.angularVelocity.z / idt + contact.bodyA.invMass * Text.z);
  105.  
  106.                     JDotVTerm += row.J[6]  * (contact.bodyB.velocity.x / idt + contact.bodyB.invMass * Fext.x);
  107.                     JDotVTerm += row.J[7]  * (contact.bodyB.velocity.y / idt + contact.bodyB.invMass * Fext.y);
  108.                     JDotVTerm += row.J[8]  * (contact.bodyB.velocity.z / idt + contact.bodyB.invMass * Fext.z);
  109.                     JDotVTerm += row.J[9]  * (contact.bodyB.angularVelocity.x / idt + contact.bodyB.invMass * Text.x);
  110.                     JDotVTerm += row.J[10] * (contact.bodyB.angularVelocity.y / idt + contact.bodyB.invMass * Text.y);
  111.                     JDotVTerm += row.J[11] * (contact.bodyB.angularVelocity.z / idt + contact.bodyB.invMass * Text.z);
  112.  
  113.  
  114.                     contact.rhs = baumgarte*C/idt - restitution*JDotVTerm;
  115.  
  116.                 }
  117.             }
  118.  
  119.  
  120.             var collisionResIterations = 0;
  121.             for (var manifoldID in contactManifolds) {
  122.  
  123.                 var manifold = contactManifolds[manifoldID];
  124.                 for (var contactID in manifold.contacts) {
  125.  
  126.                     var contact = manifold.contacts[contactID];
  127.                     for (var iteration=0; iteration<collisionResIterations; ++iteration) {
  128.  
  129.                         var row = contact.rows[0], // contact constraint
  130.                             JdotV = 0,
  131.                             delta = 0;
  132.  
  133.  
  134.                         // Erin Catto GDC-2005
  135.                         // a = B*lambda
  136.                         var d = row.D,
  137.                             JDotA = 0,
  138.                             a = [];
  139.  
  140.                         for (var ji=0; ji<12; ++ji) {
  141.                             a[ji] = row.B[ji] * contact.appliedImpulse;
  142.                             JDotA += row.J[ji] * a[ji];
  143.                         }
  144.                         delta = (contact.rhs - JDotA) / d;
  145.  
  146.  
  147.  
  148.                         var sum = contact.appliedImpulse + delta;
  149.                         if (sum < 0) {
  150.                             delta = -contact.appliedImpulse;
  151.                             sum = 0;
  152.                             contact.appliedImpulse = 0;
  153.                         } else {
  154.                             contact.appliedImpulse = sum;
  155.                         }
  156.  
  157.                         // if (Math.abs(delta) < 0.0001) break; // Breaking impulse
  158.  
  159.                         contact.bodyA.applyImpulse(contact.normal.clone().multiplyScalar(contact.bodyA.invMass), contact.tangent.clone().multiplyScalar(contact.bodyA.invInertiaTensor), delta);
  160.                         contact.bodyB.applyImpulse(contact.normal.clone().multiplyScalar(-contact.bodyB.invMass), contact.tangent2.clone().multiplyScalar(contact.bodyB.invInertiaTensor), delta);
  161.  
  162.  
  163.  
  164.                     }
  165.                 }
  166.             }
  167.  
  168.         };
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement