b2dlite/world.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.world;
  12
  13 import b2dlite.arbiter;
  14 import b2dlite.mathutils;
  15
  16
  17 struct ArbiterKey {
  18   private import b2dlite.bbody : Body;
  19
  20   // pointers, actually
  21   Body body1;
  22   Body body2;
  23
  24   this (Body b1, Body b2) { if (b1 < b2) { body1 = b1; body2 = b2; } else { body1 = b2; body2 = b1; } }
  25 }
  26
  27
  28 class World {
  29 private import b2dlite.bbody, b2dlite.joint;
  30 public:
  31   Body[] bodies;
  32   Joint[] joints;
  33   Arbiter[ArbiterKey] arbiters;
  34   Vec2 gravity;
  35   int iterations;
  36
  37   static bool accumulateImpulses = true;
  38   static bool warmStarting = true;
  39   static bool positionCorrection = true;
  40
  41 public:
  42   this() (in auto ref Vec2 agravity, int aiterations) {
  43     gravity = agravity;
  44     iterations = aiterations;
  45   }
  46
  47   void Add (Body bbody) {
  48     if (bbody !is null) bodies ~= bbody;
  49   }
  50
  51   void Add (Joint joint) {
  52     if (joint !is null) joints ~= joint;
  53   }
  54
  55   void Clear () {
  56     bodies = null;
  57     joints = null;
  58     arbiters.clear();
  59   }
  60
  61   void Step (float dt) {
  62     float inv_dt = (dt > 0.0f ? 1.0f/dt : 0.0f);
  63     // determine overlapping bodies and update contact points
  64     BroadPhase();
  65     // integrate forces
  66     for (int i = 0; i < bodies.length; ++i) {
  67       Body b = bodies[i];
  68       if (b.invMass == 0.0f) continue;
  69       b.velocity += dt*(gravity+b.invMass*b.force);
  70       b.angularVelocity += dt*b.invI*b.torque;
  71     }
  72     // perform pre-steps
  73     foreach (Arbiter arb; arbiters.byValue) arb.PreStep(inv_dt);
  74     for (int i = 0; i < joints.length; ++i) joints[i].PreStep(inv_dt);
  75     // perform iterations
  76     for (int i = 0; i < iterations; ++i) {
  77       foreach (Arbiter arb; arbiters.byValue) arb.ApplyImpulse();
  78       for (int j = 0; j < joints.length; ++j) joints[j].ApplyImpulse();
  79     }
  80     // integrate velocities
  81     for (int i = 0; i < bodies.length; ++i) {
  82       Body b = bodies[i];
  83
  84       b.position += dt*b.velocity;
  85       b.rotation += dt*b.angularVelocity;
  86
  87       b.force.Set(0.0f, 0.0f);
  88       b.torque = 0.0f;
  89     }
  90   }
  91
  92   void BroadPhase () {
  93     // O(n^2) broad-phase
  94     for (int i = 0; i < bodies.length; ++i) {
  95       Body bi = bodies[i];
  96       for (int j = i+1; j < bodies.length; ++j) {
  97         Body bj = bodies[j];
  98         if (bi.invMass == 0.0f && bj.invMass == 0.0f) continue;
  99         auto newArb = new Arbiter(bi, bj);
 100         if (auto arb = ArbiterKey(bi, bj) in arbiters) {
 101           arb.Update(newArb.contacts.ptr, newArb.numContacts);
 102         } else {
 103           arbiters[ArbiterKey(bi, bj)] = newArb;
 104         }
 105         //FIXME
 106         /*
 107         bool found = false;
 108         foreach (ref kv; arbiters.byKeyValue) {
 109           if (kv.key.body1 is bi && kv.key.body2 is bj) {
 110             found = true;
 111             kv.value.Update(newArb.contacts.ptr, newArb.numContacts);
 112             break;
 113           }
 114         }
 115         if (!found) arbiters[ArbiterKey(bi, bj)] = newArb;
 116         */
 117         /*
 118         auto newArb = new Arbiter(bi, bj);
 119         auto key = new ArbiterKey(bi, bj);
 120         if (newArb.numContacts > 0) {
 121           ArbIter iter = arbiters.find(key);
 122           if (iter == arbiters.end()) {
 123             arbiters.insert(ArbPair(key, newArb));
 124           } else {
 125             iter.second.Update(newArb.contacts, newArb.numContacts);
 126           }
 127         } else {
 128           arbiters.erase(key);
 129         }
 130         */
 131       }
 132     }
 133   }
 134 }