+init

1 files changed, 326 insertions, 0 deletions

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