Exemplo n.º 1
0
 public void Set(b2ContactFeature cf)
 {
     indexA = cf.indexA;
     indexB = cf.indexB;
     typeA = cf.typeA;
     typeB = cf.typeB;
 }
Exemplo n.º 2
0
 public void Set(b2ContactFeature cf)
 {
     indexA = cf.indexA;
     indexB = cf.indexB;
     typeA  = cf.typeA;
     typeB  = cf.typeB;
 }
        /// Compute the point states given two manifolds. The states pertain to the transition from manifold1
        /// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
        public static void b2GetPointStates(b2PointState[] state1, b2PointState[] state2,
                              ref b2Manifold manifold1, ref b2Manifold manifold2)
        {
            for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
            {
                state1[i] = b2PointState.b2_nullState;
                state2[i] = b2PointState.b2_nullState;
            }

            // Detect persists and removes.
            for (int i = 0; i < manifold1.pointCount; ++i)
            {
                b2ContactFeature id = manifold1.points[i].id;

                state1[i] = b2PointState.b2_removeState;

                for (int j = 0; j < manifold2.pointCount; ++j)
                {
                    if (manifold2.points[j].id.Equals(id))
                    {
                        state1[i] = b2PointState.b2_persistState;
                        break;
                    }
                }
            }

            // Detect persists and adds.
            for (int i = 0; i < manifold2.pointCount; ++i)
            {
                b2ContactFeature id = manifold2.points[i].id;

                state2[i] = b2PointState.b2_addState;

                for (int j = 0; j < manifold1.pointCount; ++j)
                {
                    if (manifold1.points[j].id.Equals(id))
                    {
                        state2[i] = b2PointState.b2_persistState;
                        break;
                    }
                }
            }
        }
        /// Compute the collision manifold between an edge and a circle.
        public static void b2CollideEdgeAndCircle(ref b2Manifold manifold,
                                        b2EdgeShape edgeA, ref b2Transform xfA,
                                        b2CircleShape circleB, ref b2Transform xfB)
        {
            manifold.pointCount = 0;

            // Compute circle in frame of edge
            b2Vec2 Q = b2Math.b2MulT(xfA, b2Math.b2Mul(xfB, circleB.Position));

            b2Vec2 A = edgeA.Vertex1, B = edgeA.Vertex2;
            b2Vec2 e = B - A;
            b2Vec2 diff;

            // Barycentric coordinates
            diff = B - Q;
            float u = b2Math.b2Dot(ref e, ref diff); // B - Q);
            diff = Q - A;
            float v = b2Math.b2Dot(ref e, ref diff); // Q - A);

            float radius = edgeA.Radius + circleB.Radius;

            b2ContactFeature cf = b2ContactFeature.Zero;
            cf.indexB = 0;
            cf.typeB = b2ContactFeatureType.e_vertex;

            // Region A
            if (v <= 0.0f)
            {
                b2Vec2 P = A;
                b2Vec2 d = Q - P;
                float dd = d.LengthSquared; //  b2Math.b2Dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to A?
                if (edgeA.HasVertex0)
                {
                    b2Vec2 A1 = edgeA.Vertex0;
                    b2Vec2 B1 = A;
                    b2Vec2 e1 = B1 - A1;
                    diff = B1 - Q;
                    float u1 = b2Math.b2Dot(ref e1, ref diff);

                    // Is the circle in Region AB of the previous edge?
                    if (u1 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 0;
                cf.typeA = b2ContactFeatureType.e_vertex;
                manifold.pointCount = 1;
                manifold.type = b2ManifoldType.e_circles;
                manifold.localNormal.SetZero();
                manifold.localPoint = P;
                manifold.points[0].id.key = 0;
                manifold.points[0].id.Set(cf);
                manifold.points[0].localPoint = circleB.Position;
                return;
            }

            // Region B
            if (u <= 0.0f)
            {
                b2Vec2 P = B;
                b2Vec2 d = Q - P;
                float dd = d.LengthSquared; //  b2Math.b2Dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to B?
                if (edgeA.HasVertex3)
                {
                    b2Vec2 B2 = edgeA.Vertex3;
                    b2Vec2 A2 = B;
                    b2Vec2 e2 = B2 - A2;
                    diff = Q - A2;
                    float v2 = b2Math.b2Dot(ref e2, ref diff);

                    // Is the circle in Region AB of the next edge?
                    if (v2 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 1;
                cf.typeA = b2ContactFeatureType.e_vertex;
                manifold.pointCount = 1;
                manifold.type = b2ManifoldType.e_circles;
                manifold.localNormal.SetZero();
                manifold.localPoint = P;
                manifold.points[0].id.key = 0;
                manifold.points[0].id.Set(cf);
                manifold.points[0].localPoint = circleB.Position;
                return;
            }

            // Region AB
            float den = e.Length; // b2Math.b2Dot(e, e);
            System.Diagnostics.Debug.Assert(den > 0.0f);
            b2Vec2 xP = (1.0f / den) * (u * A + v * B);
            b2Vec2 xd = Q - xP;
            float xdd = xd.LengthSquared; //  b2Math.b2Dot(xd, xd);
            if (xdd > radius * radius)
            {
                return;
            }

            b2Vec2 n = b2Vec2.Zero; // new b2Vec2(-e.y, e.x); 
            n.m_x = -e.y;
            n.m_y = e.x;
            diff = Q - A;
            if (b2Math.b2Dot(ref n, ref diff) < 0.0f)
            {
                // n.Set(-n.x, -n.y);
                n.Set(-n.m_x, -n.m_y);
            }
            n.Normalize();

            cf.indexA = 0;
            cf.typeA = b2ContactFeatureType.e_face;
            manifold.pointCount = 1;
            manifold.type = b2ManifoldType.e_faceA;
            manifold.localNormal = n;
            manifold.localPoint = A;
            manifold.points[0].id.key = 0;
            manifold.points[0].id.Set(cf);
            manifold.points[0].localPoint = circleB.Position;
        }
        /// Compute the collision manifold between two polygons.
        // Find edge normal of max separation on A - return if separating axis is found
        // Find edge normal of max separation on B - return if separation axis is found
        // Choose reference edge as min(minA, minB)
        // Find incident edge
        // Clip
        // The normal points from 1 to 2
        public static void b2CollidePolygons(ref b2Manifold manifold,
                                b2PolygonShape polyA, ref b2Transform xfA,
                                b2PolygonShape polyB, ref b2Transform xfB)
        {
            manifold.pointCount = 0;
            float totalRadius = polyA.Radius + polyB.Radius;

            int edgeA = 0;
            float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
            if (separationA > totalRadius)
                return;

            int edgeB = 0;
            float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
            if (separationB > totalRadius)
                return;

            b2PolygonShape poly1;	// reference polygon
            b2PolygonShape poly2;	// incident polygon
            b2Transform xf1, xf2;
            int edge1;		// reference edge
            byte flip;
            const float k_relativeTol = 0.98f;
            const float k_absoluteTol = 0.001f;

            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1 = polyB;
                poly2 = polyA;
                xf1 = xfB;
                xf2 = xfA;
                edge1 = edgeB;
                manifold.type = b2ManifoldType.e_faceB;
                flip = 1;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1 = xfA;
                xf2 = xfB;
                edge1 = edgeA;
                manifold.type = b2ManifoldType.e_faceA;
                flip = 0;
            }

            b2ClipVertex[] incidentEdge = new b2ClipVertex[2];
            b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);

            int count1 = poly1.VertexCount;
            b2Vec2[] vertices1 = poly1.Vertices;

            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            b2Vec2 v11 = vertices1[iv1];
            b2Vec2 v12 = vertices1[iv2];

            b2Vec2 localTangent = v12 - v11;
            localTangent.Normalize();

            b2Vec2 localNormal = localTangent.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
            b2Vec2 planePoint = 0.5f * (v11 + v12);

            b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent);
            b2Vec2 normal = tangent.UnitCross(); //  b2Math.b2Cross(tangent, 1.0f);

            v11 = b2Math.b2Mul(xf1, v11);
            v12 = b2Math.b2Mul(xf1, v12);

            // Face offset.
            float frontOffset = b2Math.b2Dot(ref normal, ref v11);

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -b2Math.b2Dot(ref tangent, ref v11) + totalRadius;
            float sideOffset2 = b2Math.b2Dot(ref tangent, ref v12) + totalRadius;

            // Clip incident edge against extruded edge1 side edges.
            b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
            b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
            int np;

            // Clip to box side 1
            np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1);

            if (np < 2)
                return;

            // Clip to negative box side 1
            np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)iv2);

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            manifold.localNormal = localNormal;
            manifold.localPoint = planePoint;

            int pointCount = 0;
            for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
            {
                float separation = b2Math.b2Dot(ref normal, ref clipPoints2[i].v) - frontOffset;

                if (separation <= totalRadius)
                {
                    b2ManifoldPoint cp = manifold.points[pointCount];
                    cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v);
                    cp.id = clipPoints2[i].id;
                    if (flip != 0)
                    {
                        // Swap features
                        b2ContactFeature cf = cp.id;
                        cp.id.indexA = cf.indexB;
                        cp.id.indexB = cf.indexA;
                        cp.id.typeA = cf.typeB;
                        cp.id.typeB = cf.typeA;
                    }
                    manifold.points[pointCount] = cp;
                    ++pointCount;
                }
            }

            manifold.pointCount = pointCount;
        }
Exemplo n.º 6
0
 public bool Equals(ref b2ContactFeature bcf)
 {
     return (indexA == bcf.indexA && indexB == bcf.indexB && typeA == bcf.typeA && typeB == bcf.typeB);
 }
Exemplo n.º 7
0
 public bool Equals(ref b2ContactFeature bcf)
 {
     return(indexA == bcf.indexA && indexB == bcf.indexB && typeA == bcf.typeA && typeB == bcf.typeB);
 }
Exemplo n.º 8
0
        public override bool Equals(object o)
        {
            b2ContactFeature bcf = (b2ContactFeature)o;

            return(indexA == bcf.indexA && indexB == bcf.indexB && typeA == bcf.typeA && typeB == bcf.typeB);
        }
Exemplo n.º 9
0
        public static void b2CollidePolygons(b2Manifold manifold, b2PolygonShape polyA, ref b2Transform xfA, b2PolygonShape polyB, ref b2Transform xfB)
        {
            manifold.pointCount = 0;
            float totalRadius = polyA.Radius + polyB.Radius;

            int   edgeA       = 0;
            float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);

            if (separationA > totalRadius)
            {
                return;
            }

            int   edgeB       = 0;
            float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);

            if (separationB > totalRadius)
            {
                return;
            }

            b2PolygonShape poly1; // reference polygon
            b2PolygonShape poly2; // incident polygon
            b2Transform    xf1, xf2;
            int            edge1; // reference edge
            byte           flip;
            const float    k_relativeTol = 0.98f;
            const float    k_absoluteTol = 0.001f;

            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1         = polyB;
                poly2         = polyA;
                xf1           = xfB;
                xf2           = xfA;
                edge1         = edgeB;
                manifold.type = b2ManifoldType.e_faceB;
                flip          = 1;
            }
            else
            {
                poly1         = polyA;
                poly2         = polyB;
                xf1           = xfA;
                xf2           = xfB;
                edge1         = edgeA;
                manifold.type = b2ManifoldType.e_faceA;
                flip          = 0;
            }

            b2ClipVertex[] incidentEdge = _incidentEdge;
            b2FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);

            int count1 = poly1.VertexCount;

            b2Vec2[] vertices1 = poly1.Vertices;

            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            b2Vec2 v11 = vertices1[iv1];
            b2Vec2 v12 = vertices1[iv2];

            b2Vec2 localTangent;

            localTangent.x = v12.x - v11.x;
            localTangent.y = v12.y - v11.y;

            localTangent.Normalize();

            b2Vec2 localNormal;

            localNormal.x = localTangent.y; //.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
            localNormal.y = -localTangent.x;

            b2Vec2 planePoint;

            planePoint.x = 0.5f * (v11.x + v12.x);
            planePoint.y = 0.5f * (v11.y + v12.y);

            b2Vec2 tangent;

            tangent.x = xf1.q.c * localTangent.x - xf1.q.s * localTangent.y;
            tangent.y = xf1.q.s * localTangent.x + xf1.q.c * localTangent.y;

            float normalx = tangent.y; //UnitCross(); //  b2Math.b2Cross(tangent, 1.0f);
            float normaly = -tangent.x;

            float v11x = (xf1.q.c * v11.x - xf1.q.s * v11.y) + xf1.p.x;
            float v11y = (xf1.q.s * v11.x + xf1.q.c * v11.y) + xf1.p.y;
            float v12x = (xf1.q.c * v12.x - xf1.q.s * v12.y) + xf1.p.x;
            float v12y = (xf1.q.s * v12.x + xf1.q.c * v12.y) + xf1.p.y;

            // Face offset.
            float frontOffset = normalx * v11x + normaly * v11y;

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -(tangent.x * v11x + tangent.y * v11y) + totalRadius;
            float sideOffset2 = tangent.x * v12x + tangent.y * v12y + totalRadius;

            // Clip incident edge against extruded edge1 side edges.
            b2ClipVertex[] clipPoints1 = _clipPoints1;
            int            np;

            // Clip to box side 1
            b2Vec2 t;

            t.x = -tangent.x;
            t.y = -tangent.y;
            np  = b2ClipSegmentToLine(clipPoints1, incidentEdge, ref t, sideOffset1, (byte)iv1);

            if (np < 2)
            {
                return;
            }

            b2ClipVertex[] clipPoints2 = _clipPoints2;

            // Clip to negative box side 1
            np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref tangent, sideOffset2, (byte)iv2);

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            manifold.localNormal = localNormal;
            manifold.localPoint  = planePoint;

            int pointCount = 0;

            for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
            {
                var v = clipPoints2[i].v;
                //float separation = b2Math.b2Dot(ref normal, ref v) - frontOffset;
                float separation = normalx * v.x + normaly * v.y - frontOffset;

                if (separation <= totalRadius)
                {
                    b2ManifoldPoint cp = manifold.points[pointCount];

                    //cp.localPoint = b2Math.b2MulT(ref xf2, ref v);
                    float px = v.x - xf2.p.x;
                    float py = v.y - xf2.p.y;
                    cp.localPoint.x = (xf2.q.c * px + xf2.q.s * py);
                    cp.localPoint.y = (-xf2.q.s * px + xf2.q.c * py);

                    cp.id = clipPoints2[i].id;
                    if (flip != 0)
                    {
                        // Swap features
                        b2ContactFeature cf = cp.id;
                        cp.id.indexA = cf.indexB;
                        cp.id.indexB = cf.indexA;
                        cp.id.typeA  = cf.typeB;
                        cp.id.typeB  = cf.typeA;
                    }
                    ++pointCount;
                }
            }

            manifold.pointCount = pointCount;
        }