9 files changed, 988 insertions, 0 deletions

diff --git a/Client/Source/Phy2D/Dynamic/Arbiter.cpp b/Client/Source/Phy2D/Dynamic/Arbiter.cpp
new file mode 100644
index 0000000..11288a3
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/Arbiter.cpp
@@ -0,0 +1,189 @@
+#include "Arbiter.h"
+#include "World.h"
+#include "Body.h"
+#include "Joint.h"
+
+using namespace Phy2D;
+
+Arbiter::Arbiter(Body* b1, Body* b2)
+{
+    if (b1 < b2)
+    {
+        body1 = b1;
+        body2 = b2;
+    }
+    else
+    {
+        body1 = b2;
+        body2 = b1;
+    }
+
+    numContacts = Collide(contacts, body1, body2);
+
+    friction = SQRT(body1->friction * body2->friction);
+}
+
+void Arbiter::Update(Contact* newContacts, int numNewContacts)
+{
+    Contact mergedContacts[2];
+
+    for (int i = 0; i < numNewContacts; ++i)
+    {
+        Contact* cNew = newContacts + i;
+        int k = -1;
+        for (int j = 0; j < numContacts; ++j)
+        {
+            Contact* cOld = contacts + j;
+            if (cNew->feature.value == cOld->feature.value)
+            {
+                k = j;
+                break;
+            }
+        }
+
+        if (k > -1)
+        {
+            Contact* c = mergedContacts + i;
+            Contact* cOld = contacts + k;
+            *c = *cNew;
+            if (World::warmStarting)
+            {
+                c->Pn = cOld->Pn;
+                c->Pt = cOld->Pt;
+                c->Pnb = cOld->Pnb;
+            }
+            else
+            {
+                c->Pn = 0.0f;
+                c->Pt = 0.0f;
+                c->Pnb = 0.0f;
+            }
+        }
+        else
+        {
+            mergedContacts[i] = newContacts[i];
+        }
+    }
+
+    for (int i = 0; i < numNewContacts; ++i)
+        contacts[i] = mergedContacts[i];
+
+    numContacts = numNewContacts;
+}
+
+
+void Arbiter::PreStep(number inv_dt)
+{
+    const number k_allowedPenetration = 0.01f;
+    number k_biasFactor = World::positionCorrection ? 0.2f : 0.0f;
+
+    for (int i = 0; i < numContacts; ++i)
+    {
+        Contact* c = contacts + i;
+
+        Vec2 r1 = c->position - body1->position;
+        Vec2 r2 = c->position - body2->position;
+
+        // Precompute normal mass, tangent mass, and bias.
+        number rn1 = Dot(r1, c->normal);
+        number rn2 = Dot(r2, c->normal);
+        number kNormal = body1->invMass + body2->invMass;
+        kNormal += body1->invI * (Dot(r1, r1) - rn1 * rn1) + body2->invI * (Dot(r2, r2) - rn2 * rn2);
+        c->massNormal = (number) 1.0f / kNormal;
+
+        Vec2 tangent = Cross(c->normal, 1.0f);
+        number rt1 = Dot(r1, tangent);
+        number rt2 = Dot(r2, tangent);
+        number kTangent = body1->invMass + body2->invMass;
+        kTangent += body1->invI * (Dot(r1, r1) - rt1 * rt1) + body2->invI * (Dot(r2, r2) - rt2 * rt2);
+        c->massTangent = (number) 1.0f / kTangent;
+
+        c->bias = -k_biasFactor * inv_dt * Min(0.0f, c->separation + k_allowedPenetration);
+
+        if (World::accumulateImpulses)
+        {
+            // Apply normal + friction impulse
+            Vec2 P = c->Pn * c->normal + c->Pt * tangent;
+
+            body1->velocity -= body1->invMass * P;
+            body1->angularVelocity -= body1->invI * Cross(r1, P);
+
+            body2->velocity += body2->invMass * P;
+            body2->angularVelocity += body2->invI * Cross(r2, P);
+        }
+    }
+}
+
+void Arbiter::ApplyImpulse()
+{
+    Body* b1 = body1;
+    Body* b2 = body2;
+
+    for (int i = 0; i < numContacts; ++i)
+    {
+        Contact* c = contacts + i;
+        c->r1 = c->position - b1->position;
+        c->r2 = c->position - b2->position;
+
+        // Relative velocity at contact
+        Vec2 dv = b2->velocity + Cross(b2->angularVelocity, c->r2) - b1->velocity - Cross(b1->angularVelocity, c->r1);
+
+        // Compute normal impulse
+        number vn = Dot(dv, c->normal);
+
+        number dPn = c->massNormal * (-vn + c->bias);
+
+        if (World::accumulateImpulses)
+        {
+            // Clamp the accumulated impulse
+            number Pn0 = c->Pn;
+            c->Pn = Max(Pn0 + dPn, 0.0f);
+            dPn = c->Pn - Pn0;
+        }
+        else
+        {
+            dPn = Max(dPn, 0.0f);
+        }
+
+        // Apply contact impulse
+        Vec2 Pn = dPn * c->normal;
+
+        b1->velocity -= b1->invMass * Pn;
+        b1->angularVelocity -= b1->invI * Cross(c->r1, Pn);
+
+        b2->velocity += b2->invMass * Pn;
+        b2->angularVelocity += b2->invI * Cross(c->r2, Pn);
+
+        // Relative velocity at contact
+        dv = b2->velocity + Cross(b2->angularVelocity, c->r2) - b1->velocity - Cross(b1->angularVelocity, c->r1);
+
+        Vec2 tangent = Cross(c->normal, 1.0f);
+        number vt = Dot(dv, tangent);
+        number dPt = c->massTangent * (-vt);
+
+        if (World::accumulateImpulses)
+        {
+            // Compute friction impulse
+            number maxPt = friction * c->Pn;
+
+            // Clamp friction
+            number oldTangentImpulse = c->Pt;
+            c->Pt = Clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
+            dPt = c->Pt - oldTangentImpulse;
+        }
+        else
+        {
+            number maxPt = friction * dPn;
+            dPt = Clamp(dPt, -maxPt, maxPt);
+        }
+
+        // Apply contact impulse
+        Vec2 Pt = dPt * tangent;
+
+        b1->velocity -= b1->invMass * Pt;
+        b1->angularVelocity -= b1->invI * Cross(c->r1, Pt);
+
+        b2->velocity += b2->invMass * Pt;
+        b2->angularVelocity += b2->invI * Cross(c->r2, Pt);
+    }
+}
diff --git a/Client/Source/Phy2D/Dynamic/Arbiter.h b/Client/Source/Phy2D/Dynamic/Arbiter.h
new file mode 100644
index 0000000..b02b413
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/Arbiter.h
@@ -0,0 +1,91 @@
+#pragma once
+
+#include "../Common/Math.h"
+
+namespace Phy2D
+{
+
+    struct Body;
+
+    union FeaturePair
+    {
+        struct Edges
+        {
+            char inEdge1;
+            char outEdge1;
+            char inEdge2;
+            char outEdge2;
+        } e;
+        int value;
+    };
+
+    struct Contact
+    {
+        Contact() : Pn(0.0f), Pt(0.0f), Pnb(0.0f) {}
+
+        Vec2 position;
+        Vec2 normal;
+        Vec2 r1, r2;
+        number separation;
+        number Pn;	// accumulated normal impulse
+        number Pt;	// accumulated tangent impulse
+        number Pnb;	// accumulated normal impulse for position bias
+        number massNormal, massTangent;
+        number bias;
+        FeaturePair feature;
+    };
+
+    struct ArbiterKey
+    {
+        ArbiterKey(Body* b1, Body* b2)
+        {
+            if (b1 < b2)
+            {
+                body1 = b1; body2 = b2;
+            }
+            else
+            {
+                body1 = b2; body2 = b1;
+            }
+        }
+
+        Body* body1;
+        Body* body2;
+    };
+
+    struct Arbiter
+    {
+        enum { MAX_POINTS = 2 };
+
+        Arbiter(Body* b1, Body* b2);
+
+        void Update(Contact* contacts, int numContacts);
+
+        void PreStep(number inv_dt);
+        void ApplyImpulse();
+
+        Contact contacts[MAX_POINTS];
+        int numContacts;
+
+        Body* body1;
+        Body* body2;
+
+        // Combined friction
+        number friction;
+    };
+
+    // This is used by std::set
+    inline bool operator < (const ArbiterKey& a1, const ArbiterKey& a2)
+    {
+        if (a1.body1 < a2.body1)
+            return true;
+
+        if (a1.body1 == a2.body1 && a1.body2 < a2.body2)
+            return true;
+
+        return false;
+    }
+
+    int Collide(Contact* contacts, Body* body1, Body* body2);
+
+}
\ No newline at end of file
diff --git a/Client/Source/Phy2D/Dynamic/Body.cpp b/Client/Source/Phy2D/Dynamic/Body.cpp
new file mode 100644
index 0000000..73bafe2
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/Body.cpp
@@ -0,0 +1,47 @@
+#include "Body.h"
+
+using namespace Phy2D;
+
+Body::Body()
+{
+    position.Set(0.0f, 0.0f);
+    rotation = 0.0f;
+    velocity.Set(0.0f, 0.0f);
+    angularVelocity = 0.0f;
+    force.Set(0.0f, 0.0f);
+    torque = 0.0f;
+    friction = 0.2f;
+
+    width.Set(1.0f, 1.0f);
+    mass = NUMBER_MAX;
+    invMass = 0.0f;
+    I = NUMBER_MAX;
+    invI = 0.0f;
+}
+
+void Body::Set(const Vec2& w, number m)
+{
+    position.Set(0.0f, 0.0f);
+    rotation = 0.0f;
+    velocity.Set(0.0f, 0.0f);
+    angularVelocity = 0.0f;
+    force.Set(0.0f, 0.0f);
+    torque = 0.0f;
+    friction = 0.2f;
+
+    width = w;
+    mass = m;
+
+    if (mass < NUMBER_MAX)
+    {
+        invMass = (number)1.0f / mass;
+        I = mass * (width.x * width.x + width.y * width.y) / 12.0f;
+        invI = (number)1.0f / I;
+    }
+    else
+    {
+        invMass = 0.0f;
+        I = NUMBER_MAX;
+        invI = 0.0f;
+    }
+}
diff --git a/Client/Source/Phy2D/Dynamic/Body.h b/Client/Source/Phy2D/Dynamic/Body.h
new file mode 100644
index 0000000..6d86e07
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/Body.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "../Common/Math.h"
+
+namespace Phy2D
+{
+
+    struct Body
+    {
+        Body();
+        void Set(const Vec2& w, number m);
+
+        void AddForce(const Vec2& f)
+        {
+            force += f;
+        }
+
+        Vec2 position;
+        number rotation;
+
+        Vec2 velocity;
+        number angularVelocity;
+
+        Vec2 force;
+        number torque;
+
+        Vec2 width;
+
+        number friction;
+        number mass, invMass;
+        number I, invI;
+    };
+
+}
\ No newline at end of file
diff --git a/Client/Source/Phy2D/Dynamic/Collide.cpp b/Client/Source/Phy2D/Dynamic/Collide.cpp
new file mode 100644
index 0000000..647147a
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/Collide.cpp
@@ -0,0 +1,326 @@
+#include "Arbiter.h"
+#include "Body.h"
+#include "World.h"
+#include "Joint.h"
+
+using namespace Phy2D;
+
+// Box vertex and edge numbering:
+//
+//        ^ y
+//        |
+//        e1
+//   v2 ------ v1
+//    |        |
+// e2 |        | e4  --> x
+//    |        |
+//   v3 ------ v4
+//        e3
+
+enum Axis
+{
+    FACE_A_X,
+    FACE_A_Y,
+    FACE_B_X,
+    FACE_B_Y
+};
+
+enum EdgeNumbers
+{
+    NO_EDGE = 0,
+    EDGE1,
+    EDGE2,
+    EDGE3,
+    EDGE4
+};
+
+struct ClipVertex
+{
+    ClipVertex() { fp.value = 0; }
+    Vec2 v;
+    FeaturePair fp;
+};
+
+void Flip(FeaturePair& fp)
+{
+    Swap(fp.e.inEdge1, fp.e.inEdge2);
+    Swap(fp.e.outEdge1, fp.e.outEdge2);
+}
+
+int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
+    const Vec2& normal, number offset, char clipEdge)
+{
+    // Start with no output points
+    int numOut = 0;
+
+    // Calculate the distance of end points to the line
+    number distance0 = Dot(normal, vIn[0].v) - offset;
+    number distance1 = Dot(normal, vIn[1].v) - offset;
+
+    // If the points are behind the plane
+    if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
+    if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
+
+    // If the points are on different sides of the plane
+    if (distance0 * distance1 < 0.0f)
+    {
+        // Find intersection point of edge and plane
+        number interp = distance0 / (distance0 - distance1);
+        vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
+        if (distance0 > 0.0f)
+        {
+            vOut[numOut].fp = vIn[0].fp;
+            vOut[numOut].fp.e.inEdge1 = clipEdge;
+            vOut[numOut].fp.e.inEdge2 = NO_EDGE;
+        }
+        else
+        {
+            vOut[numOut].fp = vIn[1].fp;
+            vOut[numOut].fp.e.outEdge1 = clipEdge;
+            vOut[numOut].fp.e.outEdge2 = NO_EDGE;
+        }
+        ++numOut;
+    }
+
+    return numOut;
+}
+
+static void ComputeIncidentEdge(ClipVertex c[2], const Vec2& h, const Vec2& pos,
+    const Mat22& Rot, const Vec2& normal)
+{
+    // The normal is from the reference box. Convert it
+    // to the incident boxe's frame and flip sign.
+    Mat22 RotT = Rot.Transpose();
+    Vec2 n = -(RotT * normal);
+    Vec2 nAbs = Abs(n);
+
+    if (nAbs.x > nAbs.y)
+    {
+        if (Sign(n.x) > 0.0f)
+        {
+            c[0].v.Set(h.x, -h.y);
+            c[0].fp.e.inEdge2 = EDGE3;
+            c[0].fp.e.outEdge2 = EDGE4;
+
+            c[1].v.Set(h.x, h.y);
+            c[1].fp.e.inEdge2 = EDGE4;
+            c[1].fp.e.outEdge2 = EDGE1;
+        }
+        else
+        {
+            c[0].v.Set(-h.x, h.y);
+            c[0].fp.e.inEdge2 = EDGE1;
+            c[0].fp.e.outEdge2 = EDGE2;
+
+            c[1].v.Set(-h.x, -h.y);
+            c[1].fp.e.inEdge2 = EDGE2;
+            c[1].fp.e.outEdge2 = EDGE3;
+        }
+    }
+    else
+    {
+        if (Sign(n.y) > 0.0f)
+        {
+            c[0].v.Set(h.x, h.y);
+            c[0].fp.e.inEdge2 = EDGE4;
+            c[0].fp.e.outEdge2 = EDGE1;
+
+            c[1].v.Set(-h.x, h.y);
+            c[1].fp.e.inEdge2 = EDGE1;
+            c[1].fp.e.outEdge2 = EDGE2;
+        }
+        else
+        {
+            c[0].v.Set(-h.x, -h.y);
+            c[0].fp.e.inEdge2 = EDGE2;
+            c[0].fp.e.outEdge2 = EDGE3;
+
+            c[1].v.Set(h.x, -h.y);
+            c[1].fp.e.inEdge2 = EDGE3;
+            c[1].fp.e.outEdge2 = EDGE4;
+        }
+    }
+
+    c[0].v = pos + Rot * c[0].v;
+    c[1].v = pos + Rot * c[1].v;
+}
+
+namespace Phy2D
+{
+
+    // The normal points from A to B
+    int Collide(Contact* contacts, Body* bodyA, Body* bodyB)
+    {
+        // Setup
+        Vec2 hA = 0.5f * bodyA->width;
+        Vec2 hB = 0.5f * bodyB->width;
+
+        Vec2 posA = bodyA->position;
+        Vec2 posB = bodyB->position;
+
+        Mat22 RotA(bodyA->rotation), RotB(bodyB->rotation);
+
+        Mat22 RotAT = RotA.Transpose();
+        Mat22 RotBT = RotB.Transpose();
+
+        Vec2 dp = posB - posA;
+        Vec2 dA = RotAT * dp;
+        Vec2 dB = RotBT * dp;
+
+        Mat22 C = RotAT * RotB;
+        Mat22 absC = Abs(C);
+        Mat22 absCT = absC.Transpose();
+
+        // Box A faces
+        Vec2 faceA = Abs(dA) - hA - absC * hB;
+        if (faceA.x > 0.0f || faceA.y > 0.0f)
+            return 0;
+
+        // Box B faces
+        Vec2 faceB = Abs(dB) - absCT * hA - hB;
+        if (faceB.x > 0.0f || faceB.y > 0.0f)
+            return 0;
+
+        // Find best axis
+        Axis axis;
+        number separation;
+        Vec2 normal;
+
+        // Box A faces
+        axis = FACE_A_X;
+        separation = faceA.x;
+        normal = dA.x > 0.0f ? RotA.col1 : -RotA.col1;
+
+        const number relativeTol = 0.95f;
+        const number absoluteTol = 0.01f;
+
+        if (faceA.y > relativeTol * separation + absoluteTol * hA.y)
+        {
+            axis = FACE_A_Y;
+            separation = faceA.y;
+            normal = dA.y > 0.0f ? RotA.col2 : -RotA.col2;
+        }
+
+        // Box B faces
+        if (faceB.x > relativeTol * separation + absoluteTol * hB.x)
+        {
+            axis = FACE_B_X;
+            separation = faceB.x;
+            normal = dB.x > 0.0f ? RotB.col1 : -RotB.col1;
+        }
+
+        if (faceB.y > relativeTol * separation + absoluteTol * hB.y)
+        {
+            axis = FACE_B_Y;
+            separation = faceB.y;
+            normal = dB.y > 0.0f ? RotB.col2 : -RotB.col2;
+        }
+
+        // Setup clipping plane data based on the separating axis
+        Vec2 frontNormal, sideNormal;
+        ClipVertex incidentEdge[2];
+        number front, negSide, posSide;
+        char negEdge, posEdge;
+
+        // Compute the clipping lines and the line segment to be clipped.
+        switch (axis)
+        {
+        case FACE_A_X:
+        {
+            frontNormal = normal;
+            front = Dot(posA, frontNormal) + hA.x;
+            sideNormal = RotA.col2;
+            number side = Dot(posA, sideNormal);
+            negSide = -side + hA.y;
+            posSide = side + hA.y;
+            negEdge = EDGE3;
+            posEdge = EDGE1;
+            ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
+        }
+        break;
+
+        case FACE_A_Y:
+        {
+            frontNormal = normal;
+            front = Dot(posA, frontNormal) + hA.y;
+            sideNormal = RotA.col1;
+            number side = Dot(posA, sideNormal);
+            negSide = -side + hA.x;
+            posSide = side + hA.x;
+            negEdge = EDGE2;
+            posEdge = EDGE4;
+            ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal);
+        }
+        break;
+
+        case FACE_B_X:
+        {
+            frontNormal = -normal;
+            front = Dot(posB, frontNormal) + hB.x;
+            sideNormal = RotB.col2;
+            number side = Dot(posB, sideNormal);
+            negSide = -side + hB.y;
+            posSide = side + hB.y;
+            negEdge = EDGE3;
+            posEdge = EDGE1;
+            ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
+        }
+        break;
+
+        case FACE_B_Y:
+        {
+            frontNormal = -normal;
+            front = Dot(posB, frontNormal) + hB.y;
+            sideNormal = RotB.col1;
+            number side = Dot(posB, sideNormal);
+            negSide = -side + hB.x;
+            posSide = side + hB.x;
+            negEdge = EDGE2;
+            posEdge = EDGE4;
+            ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal);
+        }
+        break;
+        }
+
+        // clip other face with 5 box planes (1 face plane, 4 edge planes)
+
+        ClipVertex clipPoints1[2];
+        ClipVertex clipPoints2[2];
+        int np;
+
+        // Clip to box side 1
+        np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge);
+
+        if (np < 2)
+            return 0;
+
+        // Clip to negative box side 1
+        np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge);
+
+        if (np < 2)
+            return 0;
+
+        // Now clipPoints2 contains the clipping points.
+        // Due to roundoff, it is possible that clipping removes all points.
+
+        int numContacts = 0;
+        for (int i = 0; i < 2; ++i)
+        {
+            number separation = Dot(frontNormal, clipPoints2[i].v) - front;
+
+            if (separation <= 0)
+            {
+                contacts[numContacts].separation = separation;
+                contacts[numContacts].normal = normal;
+                // slide contact point onto reference face (easy to cull)
+                contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal;
+                contacts[numContacts].feature = clipPoints2[i].fp;
+                if (axis == FACE_B_X || axis == FACE_B_Y)
+                    Flip(contacts[numContacts].feature);
+                ++numContacts;
+            }
+        }
+
+        return numContacts;
+    }
+}
\ No newline at end of file
diff --git a/Client/Source/Phy2D/Dynamic/Joint.cpp b/Client/Source/Phy2D/Dynamic/Joint.cpp
new file mode 100644
index 0000000..e0d10f9
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/Joint.cpp
@@ -0,0 +1,102 @@
+#include "Joint.h"
+#include "Body.h"
+#include "World.h"
+
+using namespace Phy2D;
+
+void Joint::Set(Body* b1, Body* b2, const Vec2& anchor)
+{
+    body1 = b1;
+    body2 = b2;
+
+    Mat22 Rot1(body1->rotation);
+    Mat22 Rot2(body2->rotation);
+    Mat22 Rot1T = Rot1.Transpose();
+    Mat22 Rot2T = Rot2.Transpose();
+
+    localAnchor1 = Rot1T * (anchor - body1->position);
+    localAnchor2 = Rot2T * (anchor - body2->position);
+
+    P.Set(0.0f, 0.0f);
+
+    softness = 0.0f;
+    biasFactor = 0.2f;
+}
+
+void Joint::PreStep(number inv_dt)
+{
+    // Pre-compute anchors, mass matrix, and bias.
+    Mat22 Rot1(body1->rotation);
+    Mat22 Rot2(body2->rotation);
+
+    r1 = Rot1 * localAnchor1;
+    r2 = Rot2 * localAnchor2;
+
+    // deltaV = deltaV0 + K * impulse
+    // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+    //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+    //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+    Mat22 K1;
+    K1.col1.x = body1->invMass + body2->invMass;	K1.col2.x = 0.0f;
+    K1.col1.y = 0.0f;								K1.col2.y = body1->invMass + body2->invMass;
+
+    Mat22 K2;
+    K2.col1.x = body1->invI * r1.y * r1.y;		K2.col2.x = -body1->invI * r1.x * r1.y;
+    K2.col1.y = -body1->invI * r1.x * r1.y;		K2.col2.y = body1->invI * r1.x * r1.x;
+
+    Mat22 K3;
+    K3.col1.x = body2->invI * r2.y * r2.y;		K3.col2.x = -body2->invI * r2.x * r2.y;
+    K3.col1.y = -body2->invI * r2.x * r2.y;		K3.col2.y = body2->invI * r2.x * r2.x;
+
+    Mat22 K = K1 + K2 + K3;
+    K.col1.x += softness;
+    K.col2.y += softness;
+
+    M = K.Invert();
+
+    Vec2 p1 = body1->position + r1;
+    Vec2 p2 = body2->position + r2;
+    Vec2 dp = p2 - p1;
+
+    if (World::positionCorrection)
+    {
+        bias = -biasFactor * inv_dt * dp;
+    }
+    else
+    {
+        bias.Set(0.0f, 0.0f);
+    }
+
+    if (World::warmStarting)
+    {
+        // Apply accumulated impulse.
+        body1->velocity -= body1->invMass * P;
+        body1->angularVelocity -= body1->invI * Cross(r1, P);
+
+        body2->velocity += body2->invMass * P;
+        body2->angularVelocity += body2->invI * Cross(r2, P);
+    }
+    else
+    {
+        P.Set(0.0f, 0.0f);
+    }
+}
+
+void Joint::ApplyImpulse()
+{
+    Vec2 dv = body2->velocity + Cross(body2->angularVelocity, r2) - body1->velocity - Cross(body1->angularVelocity, r1);
+
+    Vec2 impulse;
+
+    impulse = M * (bias - dv - softness * P);
+
+    body1->velocity -= body1->invMass * impulse;
+    body1->angularVelocity -= body1->invI * Cross(r1, impulse);
+
+    body2->velocity += body2->invMass * impulse;
+    body2->angularVelocity += body2->invI * Cross(r2, impulse);
+
+    P += impulse;
+}
+
+
diff --git a/Client/Source/Phy2D/Dynamic/Joint.h b/Client/Source/Phy2D/Dynamic/Joint.h
new file mode 100644
index 0000000..34d8d8d
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/Joint.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "../Common/Math.h"
+
+namespace Phy2D
+{
+
+    struct Body;
+
+    struct Joint
+    {
+        Joint() :
+            body1(0), body2(0),
+            P(0.0f, 0.0f),
+            biasFactor(0.2f), softness(0.0f)
+        {}
+
+        void Set(Body* body1, Body* body2, const Vec2& anchor);
+
+        void PreStep(number inv_dt);
+        void ApplyImpulse();
+
+        Mat22 M;
+        Vec2 localAnchor1, localAnchor2;
+        Vec2 r1, r2;
+        Vec2 bias;
+        Vec2 P;		// accumulated impulse
+        Body* body1;
+        Body* body2;
+        number biasFactor;
+        number softness;
+    };
+
+}
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;
+    }
+}
diff --git a/Client/Source/Phy2D/Dynamic/World.h b/Client/Source/Phy2D/Dynamic/World.h
new file mode 100644
index 0000000..d5c652c
--- /dev/null
+++ b/Client/Source/Phy2D/Dynamic/World.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <vector>
+#include <map>
+#include "../Common/Math.h"
+#include "Arbiter.h"
+
+namespace Phy2D
+{
+
+    struct Body;
+    struct Joint;
+
+    struct World
+    {
+        World(Vec2 gravity, int iterations) : gravity(gravity), iterations(iterations) {}
+
+        void Add(Body* body);
+        void Add(Joint* joint);
+        void Clear();
+
+        void Step(number dt);
+
+        void BroadPhase();
+
+        std::vector<Body*> bodies;
+        std::vector<Joint*> joints;
+        std::map<ArbiterKey, Arbiter> arbiters;
+        Vec2 gravity;
+        int iterations;
+        static bool accumulateImpulses;
+        static bool warmStarting;
+        static bool positionCorrection;
+    };
+
+}