+init

1 files changed, 129 insertions, 0 deletions

diff --git a/Client/Source/Phy2D/Dynamic/World.cpp b/Client/Source/Phy2D/Dynamic/World.cpp
new file mode 100644
index 0000000..6b0174c
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/World.cpp
@@ -0,0 +1,129 @@
+#include "World.h"
+#include "Body.h"
+#include "Joint.h"
+#include "Arbiter.h"
+
+using namespace Phy2D;
+
+
+using std::vector;
+using std::map;
+using std::pair;
+
+typedef map<ArbiterKey, Arbiter>::iterator ArbIter;
+typedef pair<ArbiterKey, Arbiter> ArbPair;
+
+bool World::accumulateImpulses = true;
+bool World::warmStarting = true;
+bool World::positionCorrection = true;
+
+void World::Add(Body* body)
+{
+    bodies.push_back(body);
+}
+
+void World::Add(Joint* joint)
+{
+    joints.push_back(joint);
+}
+
+void World::Clear()
+{
+    bodies.clear();
+    joints.clear();
+    arbiters.clear();
+}
+
+void World::BroadPhase()
+{
+    // O(n^2) broad-phase
+    for (int i = 0; i < (int)bodies.size(); ++i)
+    {
+        Body* bi = bodies[i];
+
+        for (int j = i + 1; j < (int)bodies.size(); ++j)
+        {
+            Body* bj = bodies[j];
+
+            if (bi->invMass == 0.0f && bj->invMass == 0.0f)
+                continue;
+
+            Arbiter newArb(bi, bj);
+            ArbiterKey key(bi, bj);
+
+            if (newArb.numContacts > 0)
+            {
+                ArbIter iter = arbiters.find(key);
+                if (iter == arbiters.end())
+                {
+                    arbiters.insert(ArbPair(key, newArb));
+                }
+                else
+                {
+                    iter->second.Update(newArb.contacts, newArb.numContacts);
+                }
+            }
+            else
+            {
+                arbiters.erase(key);
+            }
+        }
+    }
+}
+
+void World::Step(number dt)
+{
+    number inv_dt = dt > 0.0f ? (number)1.0f / dt : (number)0.0f;
+
+    // Determine overlapping bodies and update contact points.
+    BroadPhase();
+
+    // Integrate forces.
+    for (int i = 0; i < (int)bodies.size(); ++i)
+    {
+        Body* b = bodies[i];
+
+        if (b->invMass == 0.0f)
+            continue;
+
+        b->velocity += dt * (gravity + b->invMass * b->force);
+        b->angularVelocity += dt * b->invI * b->torque;
+    }
+
+    // Perform pre-steps.
+    for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb)
+    {
+        arb->second.PreStep(inv_dt);
+    }
+
+    for (int i = 0; i < (int)joints.size(); ++i)
+    {
+        joints[i]->PreStep(inv_dt);
+    }
+
+    // Perform iterations
+    for (int i = 0; i < iterations; ++i)
+    {
+        for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb)
+        {
+            arb->second.ApplyImpulse();
+        }
+
+        for (int j = 0; j < (int)joints.size(); ++j)
+        {
+            joints[j]->ApplyImpulse();
+        }
+    }
+
+    // Integrate Velocities
+    for (int i = 0; i < (int)bodies.size(); ++i)
+    {
+        Body* b = bodies[i];
+
+        b->position += dt * b->velocity;
+        b->rotation += dt * b->angularVelocity;
+
+        b->force.Set(0.0f, 0.0f);
+        b->torque = 0.0f;
+    }
+}