b2dlite/arbiter.d

   1 /*
   2  * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
   3  *
   4  * Permission to use, copy, modify, distribute and sell this software
   5  * and its documentation for any purpose is hereby granted without fee,
   6  * provided that the above copyright notice appear in all copies.
   7  * Erin Catto makes no representations about the suitability
   8  * of this software for any purpose.
   9  * It is provided "as is" without express or implied warranty.
  10  */
  11 module b2dlite.arbiter;
  12
  13 import b2dlite.mathutils;
  14
  15
  16 union FeaturePair {
  17   static struct Edges {
  18     char inEdge1;
  19     char outEdge1;
  20     char inEdge2;
  21     char outEdge2;
  22   }
  23   Edges e;
  24   int value;
  25 }
  26
  27
  28 struct Contact {
  29 public:
  30   Vec2 position;
  31   Vec2 normal;
  32   Vec2 r1, r2;
  33   float separation = 0.0f;
  34   float Pn = 0.0f; // accumulated normal impulse
  35   float Pt = 0.0f; // accumulated tangent impulse
  36   float Pnb = 0.0f;  // accumulated normal impulse for position bias
  37   float massNormal, massTangent;
  38   float bias = 0.0f;
  39   FeaturePair feature;
  40 }
  41
  42
  43 class Arbiter {
  44   private import b2dlite.bbody : Body;
  45
  46 public:
  47   enum MAX_POINTS = 2;
  48
  49 public:
  50   Contact[MAX_POINTS] contacts;
  51   int numContacts;
  52
  53   Body body1;
  54   Body body2;
  55
  56   // combined friction
  57   float friction;
  58
  59 public:
  60   this (Body b1, Body b2) {
  61     import std.math : sqrt;
  62     import b2dlite.collide : Collide;
  63
  64     if (b1 < b2) {
  65       body1 = b1;
  66       body2 = b2;
  67     } else {
  68       body1 = b2;
  69       body2 = b1;
  70     }
  71     numContacts = Collide(contacts.ptr, body1, body2);
  72     friction = sqrt(body1.friction*body2.friction);
  73
  74     /*
  75     import iv.glbinds;
  76     glPointSize(4.0f);
  77     glColor3f(1.0f, 0.0f, 0.0f);
  78     glBegin(GL_POINTS);
  79     for (int i = 0; i < numContacts; ++i) glVertex2f(contacts[i].position.x, contacts[i].position.y);
  80     glEnd();
  81     glPointSize(1.0f);
  82     */
  83   }
  84
  85   void Update (Contact* newContacts, int numNewContacts) {
  86     import b2dlite.world : World;
  87
  88     Contact[2] mergedContacts;
  89     for (int i = 0; i < numNewContacts; ++i) {
  90       Contact *cNew = newContacts+i;
  91       int k = -1;
  92       for (int j = 0; j < numContacts; ++j) {
  93         Contact* cOld = contacts.ptr+j;
  94         if (cNew.feature.value == cOld.feature.value) { k = j; break; }
  95       }
  96       if (k > -1) {
  97         Contact* c = mergedContacts.ptr+i;
  98         Contact* cOld = contacts.ptr+k;
  99         *c = *cNew;
 100         if (World.warmStarting) {
 101           c.Pn = cOld.Pn;
 102           c.Pt = cOld.Pt;
 103           c.Pnb = cOld.Pnb;
 104         } else {
 105           c.Pn = 0.0f;
 106           c.Pt = 0.0f;
 107           c.Pnb = 0.0f;
 108         }
 109       } else {
 110         mergedContacts[i] = newContacts[i];
 111       }
 112     }
 113     for (int i = 0; i < numNewContacts; ++i) contacts[i] = mergedContacts[i];
 114     numContacts = numNewContacts;
 115   }
 116
 117   void PreStep (float inv_dt) {
 118     import b2dlite.world : World;
 119
 120     enum k_allowedPenetration = 0.01f;
 121     float k_biasFactor = (World.positionCorrection ? 0.2f : 0.0f);
 122     for (int i = 0; i < numContacts; ++i) {
 123       Contact *c = contacts.ptr+i;
 124       Vec2 r1 = c.position-body1.position;
 125       Vec2 r2 = c.position-body2.position;
 126
 127       // precompute normal mass, tangent mass, and bias
 128       float rn1 = Dot(r1, c.normal);
 129       float rn2 = Dot(r2, c.normal);
 130       float kNormal = body1.invMass+body2.invMass;
 131       kNormal += body1.invI*(Dot(r1, r1)-rn1*rn1)+body2.invI*(Dot(r2, r2)-rn2*rn2);
 132       c.massNormal = 1.0f/kNormal;
 133
 134       Vec2 tangent = Cross(c.normal, 1.0f);
 135       float rt1 = Dot(r1, tangent);
 136       float rt2 = Dot(r2, tangent);
 137       float kTangent = body1.invMass+body2.invMass;
 138       kTangent += body1.invI*(Dot(r1, r1)-rt1*rt1)+body2.invI*(Dot(r2, r2)-rt2*rt2);
 139       c.massTangent = 1.0f/kTangent;
 140
 141       c.bias = -k_biasFactor*inv_dt*Min(0.0f, c.separation+k_allowedPenetration);
 142
 143       if (World.accumulateImpulses) {
 144         // apply normal + friction impulse
 145         Vec2 P = c.Pn*c.normal+c.Pt*tangent;
 146
 147         body1.velocity -= body1.invMass*P;
 148         body1.angularVelocity -= body1.invI*Cross(r1, P);
 149
 150         body2.velocity += body2.invMass*P;
 151         body2.angularVelocity += body2.invI*Cross(r2, P);
 152       }
 153     }
 154   }
 155
 156   void ApplyImpulse () {
 157     import b2dlite.world : World;
 158
 159     Body b1 = body1;
 160     Body b2 = body2;
 161
 162     for (int i = 0; i < numContacts; ++i) {
 163       Contact *c = contacts.ptr+i;
 164       c.r1 = c.position-b1.position;
 165       c.r2 = c.position-b2.position;
 166
 167       // relative velocity at contact
 168       Vec2 dv = b2.velocity+Cross(b2.angularVelocity, c.r2)-b1.velocity-Cross(b1.angularVelocity, c.r1);
 169
 170       // compute normal impulse
 171       float vn = Dot(dv, c.normal);
 172
 173       float dPn = c.massNormal*(-vn+c.bias);
 174
 175       if (World.accumulateImpulses) {
 176         // clamp the accumulated impulse
 177         float Pn0 = c.Pn;
 178         c.Pn = Max(Pn0+dPn, 0.0f);
 179         dPn = c.Pn-Pn0;
 180       } else {
 181         dPn = Max(dPn, 0.0f);
 182       }
 183
 184       // apply contact impulse
 185       Vec2 Pn = dPn*c.normal;
 186
 187       b1.velocity -= b1.invMass*Pn;
 188       b1.angularVelocity -= b1.invI*Cross(c.r1, Pn);
 189
 190       b2.velocity += b2.invMass*Pn;
 191       b2.angularVelocity += b2.invI*Cross(c.r2, Pn);
 192
 193       // relative velocity at contact
 194       dv = b2.velocity+Cross(b2.angularVelocity, c.r2)-b1.velocity-Cross(b1.angularVelocity, c.r1);
 195
 196       Vec2 tangent = Cross(c.normal, 1.0f);
 197       float vt = Dot(dv, tangent);
 198       float dPt = c.massTangent*(-vt);
 199
 200       if (World.accumulateImpulses) {
 201         // compute friction impulse
 202         float maxPt = friction*c.Pn;
 203         // clamp friction
 204         float oldTangentImpulse = c.Pt;
 205         c.Pt = Clamp(oldTangentImpulse+dPt, -maxPt, maxPt);
 206         dPt = c.Pt-oldTangentImpulse;
 207       } else {
 208         float maxPt = friction*dPn;
 209         dPt = Clamp(dPt, -maxPt, maxPt);
 210       }
 211
 212       // apply contact impulse
 213       Vec2 Pt = dPt*tangent;
 214
 215       b1.velocity -= b1.invMass*Pt;
 216       b1.angularVelocity -= b1.invI*Cross(c.r1, Pt);
 217
 218       b2.velocity += b2.invMass*Pt;
 219       b2.angularVelocity += b2.invI*Cross(c.r2, Pt);
 220     }
 221   }
 222
 223   /+
 224   int opCmp() (in Arbiter a2) {
 225     //if (this.body1 < a2.body1) return -1;
 226     if (this.body1 > a2.body1) return -1;
 227     if (this.body1 == a2.body1 && this.body2 < a2.body2) return -1;
 228     return false;
 229   }
 230   +/
 231 }
 232
 233
 234 //extern int Collide (Contact *contacts, Body body1, Body body2);