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