示例#1
0
        /// Implement Shape.
        public override Shape Clone()
        {
            var clone = new PolygonShape();
            clone.ShapeType = ShapeType;
            clone._radius = _radius;
            clone._vertexCount = _vertexCount;
            clone._centroid = _centroid;
            clone._vertices = _vertices;
            clone._normals = _normals;

            return clone;
        }
示例#2
0
        // Find the max separation between poly1 and poly2 using edge normals from poly1.
        static float FindMaxSeparation( out int edgeIndex,
								        PolygonShape poly1, ref XForm xf1,
								        PolygonShape poly2, ref XForm xf2)
        {
            edgeIndex = -1;
            int count1 = poly1._vertexCount;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            Vector2 d = MathUtils.Multiply(ref xf2, poly2._centroid) - MathUtils.Multiply(ref xf1, poly1._centroid);
            Vector2 dLocal1 = MathUtils.MultiplyT(ref xf1.R, d);

            // Find edge normal on poly1 that has the largest projection onto d.
            int edge = 0;
            float maxDot = -Settings.b2_FLT_MAX;
            for (int i = 0; i < count1; ++i)
            {
                float dot = Vector2.Dot(poly1._normals[i], dLocal1);
                if (dot > maxDot)
                {
                    maxDot = dot;
                    edge = i;
                }
            }

            // Get the separation for the edge normal.
            float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

            // Check the separation for the previous edge normal.
            int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
            float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);

            // Check the separation for the next edge normal.
            int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
            float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);

            // Find the best edge and the search direction.
            int bestEdge;
            float bestSeparation;
            int increment;
            if (sPrev > s && sPrev > sNext)
            {
                increment = -1;
                bestEdge = prevEdge;
                bestSeparation = sPrev;
            }
            else if (sNext > s)
            {
                increment = 1;
                bestEdge = nextEdge;
                bestSeparation = sNext;
            }
            else
            {
                edgeIndex = edge;
                return s;
            }

            // Perform a local search for the best edge normal.
            for ( ; ; )
            {
                if (increment == -1)
                    edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
                else
                    edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;

                s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

                if (s > bestSeparation)
                {
                    bestEdge = edge;
                    bestSeparation = s;
                }
                else
                {
                    break;
                }
            }

            edgeIndex = bestEdge;
            return bestSeparation;
        }
示例#3
0
        static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
							         PolygonShape poly1, ref XForm xf1, int edge1,
							         PolygonShape poly2, ref XForm xf2)
        {
            c = new FixedArray2<ClipVertex>();

            int count1 = poly1._vertexCount;
            int count2 = poly2._vertexCount;

            Debug.Assert(0 <= edge1 && edge1 < count1);

            // Get the normal of the reference edge in poly2's frame.
            Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]));

            // Find the incident edge on poly2.
            int index = 0;
            float minDot = Settings.b2_FLT_MAX;
            for (int i = 0; i < count2; ++i)
            {
                float dot = Vector2.Dot(normal1, poly2._normals[i]);
                if (dot < minDot)
                {
                    minDot = dot;
                    index = i;
                }
            }

            // Build the clip vertices for the incident edge.
            int i1 = index;
            int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

            var cv0 = c[0];

            cv0.v = MathUtils.Multiply(ref xf2, poly2._vertices[i1]);
            cv0.id.Features.ReferenceEdge = (byte)edge1;
            cv0.id.Features.IncidentEdge = (byte)i1;
            cv0.id.Features.IncidentVertex = 0;

            c[0] = cv0;

            var cv1 = c[1];
            cv1.v = MathUtils.Multiply(ref xf2, poly2._vertices[i2]);
            cv1.id.Features.ReferenceEdge = (byte)edge1;
            cv1.id.Features.IncidentEdge = (byte)i2;
            cv1.id.Features.IncidentVertex = 1;

            c[1] = cv1;
        }
示例#4
0
        // Find the separation between poly1 and poly2 for a give edge normal on poly1.
        static float EdgeSeparation(PolygonShape poly1, ref XForm xf1, int edge1,
							        PolygonShape poly2, ref XForm xf2)
        {
            int count1 = poly1._vertexCount;
            int count2 = poly2._vertexCount;

            Debug.Assert(0 <= edge1 && edge1 < count1);

            // Convert normal from poly1's frame into poly2's frame.
            Vector2 normal1World = MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]);
            Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, normal1World);

            // Find support vertex on poly2 for -normal.
            int index = 0;
            float minDot = Settings.b2_FLT_MAX;

            for (int i = 0; i < count2; ++i)
            {
                float dot = Vector2.Dot(poly2._vertices[i], normal1);
                if (dot < minDot)
                {
                    minDot = dot;
                    index = i;
                }
            }

            Vector2 v1 = MathUtils.Multiply(ref xf1, poly1._vertices[edge1]);
            Vector2 v2 = MathUtils.Multiply(ref xf2, poly2._vertices[index]);
            float separation = Vector2.Dot(v2 - v1, normal1World);
            return separation;
        }
示例#5
0
        /// Compute the collision manifold between two polygons.
        public static void CollidePolygons(ref Manifold manifold,
                               PolygonShape polyA, ref XForm xfA,
                               PolygonShape polyB, ref XForm xfB)
        {
            manifold._pointCount = 0;
            float totalRadius = polyA._radius + polyB._radius;

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

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

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

            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1 = polyB;
                poly2 = polyA;
                xf1 = xfB;
                xf2 = xfA;
                edge1 = edgeB;
                manifold._type = ManifoldType.FaceB;
                flip = 1;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1 = xfA;
                xf2 = xfB;
                edge1 = edgeA;
                manifold._type = ManifoldType.FaceA;
                flip = 0;
            }

            FixedArray2<ClipVertex> incidentEdge;
            FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);

            int count1 = poly1._vertexCount;

            Vector2 v11 = poly1._vertices[edge1];
            Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1+1] : poly1._vertices[0];

            Vector2 dv = v12 - v11;

            Vector2 localNormal = MathUtils.Cross(dv, 1.0f);
            localNormal.Normalize();
            Vector2 planePoint = 0.5f * (v11 + v12);

            Vector2 sideNormal = MathUtils.Multiply(ref xf1.R, v12 - v11);
            sideNormal.Normalize();
            Vector2 frontNormal = MathUtils.Cross(sideNormal, 1.0f);

            v11 = MathUtils.Multiply(ref xf1, v11);
            v12 = MathUtils.Multiply(ref xf1, v12);

            float frontOffset = Vector2.Dot(frontNormal, v11);
            float sideOffset1 = -Vector2.Dot(sideNormal, v11);
            float sideOffset2 = Vector2.Dot(sideNormal, v12);

            // Clip incident edge against extruded edge1 side edges.
            FixedArray2<ClipVertex> clipPoints1;
            FixedArray2<ClipVertex> clipPoints2;
            int np;

            // Clip to box side 1
            np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -sideNormal, sideOffset1);

            if (np < 2)
                return;

            // Clip to negative box side 1
            np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, sideNormal, sideOffset2);

            if (np < 2)
            {
                return;
            }

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

            int pointCount = 0;
            for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
            {
                float separation = Vector2.Dot(frontNormal, clipPoints2[i].v) - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold._points[pointCount];
                    cp.LocalPoint = MathUtils.MultiplyT(ref xf2, clipPoints2[i].v);
                    cp.Id = clipPoints2[i].id;
                    cp.Id.Features.Flip = flip;
                    manifold._points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold._pointCount = pointCount;
        }
示例#6
0
        /// Compute the collision manifold between a polygon and a circle.
        public static void CollidePolygonAndCircle(ref Manifold manifold,
							           PolygonShape polygon, ref XForm xf1,
							           CircleShape circle, ref XForm xf2)
        {
            manifold._pointCount = 0;

            // Compute circle position in the frame of the polygon.
            Vector2 c = MathUtils.Multiply(ref xf2, circle._p);
            Vector2 cLocal = MathUtils.MultiplyT(ref xf1, c);

            // Find the min separating edge.
            int normalIndex = 0;
            float separation = -Settings.b2_FLT_MAX;
            float radius = polygon._radius + circle._radius;
            int vertexCount = polygon._vertexCount;

            for (int i = 0; i < vertexCount; ++i)
            {
                float s = Vector2.Dot(polygon._normals[i], cLocal - polygon._vertices[i]);

                if (s > radius)
                {
                    // Early out.
                    return;
                }

                if (s > separation)
                {
                    separation = s;
                    normalIndex = i;
                }
            }

            // Vertices that subtend the incident face.
            int vertIndex1 = normalIndex;
            int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
            Vector2 v1 = polygon._vertices[vertIndex1];
            Vector2 v2 = polygon._vertices[vertIndex2];

            // If the center is inside the polygon ...
            if (separation < Settings.b2_FLT_EPSILON)
            {
                manifold._pointCount = 1;
                manifold._type = ManifoldType.FaceA;
                manifold._localPlaneNormal = polygon._normals[normalIndex];
                manifold._localPoint = 0.5f * (v1 + v2);

                var p0 = manifold._points[0];

                p0.LocalPoint = circle._p;
                p0.Id.Key = 0;

                manifold._points[0] = p0;

                return;
            }

            // Compute barycentric coordinates
            float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
            float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
            if (u1 <= 0.0f)
            {
                if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
                {
                    return;
                }

                manifold._pointCount = 1;
                manifold._type = ManifoldType.FaceA;
                manifold._localPlaneNormal = cLocal - v1;
                manifold._localPlaneNormal.Normalize();
                manifold._localPoint = v1;

                var p0b = manifold._points[0];

                p0b.LocalPoint = circle._p;
                p0b.Id.Key = 0;

                manifold._points[0] = p0b;

            }
            else if (u2 <= 0.0f)
            {
                if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
                {
                    return;
                }

                manifold._pointCount = 1;
                manifold._type = ManifoldType.FaceA;
                manifold._localPlaneNormal = cLocal - v2;
                manifold._localPlaneNormal.Normalize();
                manifold._localPoint = v2;

                var p0c = manifold._points[0];

                p0c.LocalPoint = circle._p;
                p0c.Id.Key = 0;

                manifold._points[0] = p0c;
            }
            else
            {
                Vector2 faceCenter = 0.5f * (v1 + v2);
                float separation2 = Vector2.Dot(cLocal - faceCenter, polygon._normals[vertIndex1]);
                if (separation2 > radius)
                {
                    return;
                }

                manifold._pointCount = 1;
                manifold._type = ManifoldType.FaceA;
                manifold._localPlaneNormal = polygon._normals[vertIndex1];
                manifold._localPoint = faceCenter;

                var p0d = manifold._points[0];

                p0d.LocalPoint = circle._p;
                p0d.Id.Key = 0;

                manifold._points[0] = p0d;
            }
        }
示例#7
0
        // Find the max separation between poly1 and poly2 using edge normals from poly1.
        static float FindMaxSeparation(out int edgeIndex,
                                       PolygonShape poly1, ref XForm xf1,
                                       PolygonShape poly2, ref XForm xf2)
        {
            edgeIndex = -1;
            int count1 = poly1._vertexCount;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            Vector2 d       = MathUtils.Multiply(ref xf2, poly2._centroid) - MathUtils.Multiply(ref xf1, poly1._centroid);
            Vector2 dLocal1 = MathUtils.MultiplyT(ref xf1.R, d);

            // Find edge normal on poly1 that has the largest projection onto d.
            int   edge   = 0;
            float maxDot = -Settings.b2_FLT_MAX;

            for (int i = 0; i < count1; ++i)
            {
                float dot = Vector2.Dot(poly1._normals[i], dLocal1);
                if (dot > maxDot)
                {
                    maxDot = dot;
                    edge   = i;
                }
            }

            // Get the separation for the edge normal.
            float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

            // Check the separation for the previous edge normal.
            int   prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
            float sPrev    = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);

            // Check the separation for the next edge normal.
            int   nextEdge = edge + 1 < count1 ? edge + 1 : 0;
            float sNext    = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);

            // Find the best edge and the search direction.
            int   bestEdge;
            float bestSeparation;
            int   increment;

            if (sPrev > s && sPrev > sNext)
            {
                increment      = -1;
                bestEdge       = prevEdge;
                bestSeparation = sPrev;
            }
            else if (sNext > s)
            {
                increment      = 1;
                bestEdge       = nextEdge;
                bestSeparation = sNext;
            }
            else
            {
                edgeIndex = edge;
                return(s);
            }

            // Perform a local search for the best edge normal.
            for ( ; ;)
            {
                if (increment == -1)
                {
                    edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
                }
                else
                {
                    edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
                }

                s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

                if (s > bestSeparation)
                {
                    bestEdge       = edge;
                    bestSeparation = s;
                }
                else
                {
                    break;
                }
            }

            edgeIndex = bestEdge;
            return(bestSeparation);
        }
示例#8
0
        /// Compute the collision manifold between two polygons.
        public static void CollidePolygons(ref Manifold manifold,
                                           PolygonShape polyA, ref XForm xfA,
                                           PolygonShape polyB, ref XForm xfB)
        {
            manifold._pointCount = 0;
            float totalRadius = polyA._radius + polyB._radius;

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

            if (separationA > totalRadius)
            {
                return;
            }

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

            if (separationB > totalRadius)
            {
                return;
            }

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

            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1          = polyB;
                poly2          = polyA;
                xf1            = xfB;
                xf2            = xfA;
                edge1          = edgeB;
                manifold._type = ManifoldType.FaceB;
                flip           = 1;
            }
            else
            {
                poly1          = polyA;
                poly2          = polyB;
                xf1            = xfA;
                xf2            = xfB;
                edge1          = edgeA;
                manifold._type = ManifoldType.FaceA;
                flip           = 0;
            }

            FixedArray2 <ClipVertex> incidentEdge;

            FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);

            int count1 = poly1._vertexCount;

            Vector2 v11 = poly1._vertices[edge1];
            Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1 + 1] : poly1._vertices[0];

            Vector2 dv = v12 - v11;

            Vector2 localNormal = MathUtils.Cross(dv, 1.0f);

            localNormal.Normalize();
            Vector2 planePoint = 0.5f * (v11 + v12);

            Vector2 sideNormal = MathUtils.Multiply(ref xf1.R, v12 - v11);

            sideNormal.Normalize();
            Vector2 frontNormal = MathUtils.Cross(sideNormal, 1.0f);

            v11 = MathUtils.Multiply(ref xf1, v11);
            v12 = MathUtils.Multiply(ref xf1, v12);

            float frontOffset = Vector2.Dot(frontNormal, v11);
            float sideOffset1 = -Vector2.Dot(sideNormal, v11);
            float sideOffset2 = Vector2.Dot(sideNormal, v12);

            // Clip incident edge against extruded edge1 side edges.
            FixedArray2 <ClipVertex> clipPoints1;
            FixedArray2 <ClipVertex> clipPoints2;
            int np;

            // Clip to box side 1
            np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -sideNormal, sideOffset1);

            if (np < 2)
            {
                return;
            }

            // Clip to negative box side 1
            np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, sideNormal, sideOffset2);

            if (np < 2)
            {
                return;
            }

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

            int pointCount = 0;

            for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
            {
                float separation = Vector2.Dot(frontNormal, clipPoints2[i].v) - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold._points[pointCount];
                    cp.LocalPoint                = MathUtils.MultiplyT(ref xf2, clipPoints2[i].v);
                    cp.Id                        = clipPoints2[i].id;
                    cp.Id.Features.Flip          = flip;
                    manifold._points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold._pointCount = pointCount;
        }
示例#9
0
        /// Compute the collision manifold between a polygon and a circle.
        public static void CollidePolygonAndCircle(ref Manifold manifold,
                                                   PolygonShape polygon, ref XForm xf1,
                                                   CircleShape circle, ref XForm xf2)
        {
            manifold._pointCount = 0;

            // Compute circle position in the frame of the polygon.
            Vector2 c      = MathUtils.Multiply(ref xf2, circle._p);
            Vector2 cLocal = MathUtils.MultiplyT(ref xf1, c);

            // Find the min separating edge.
            int   normalIndex = 0;
            float separation  = -Settings.b2_FLT_MAX;
            float radius      = polygon._radius + circle._radius;
            int   vertexCount = polygon._vertexCount;

            for (int i = 0; i < vertexCount; ++i)
            {
                float s = Vector2.Dot(polygon._normals[i], cLocal - polygon._vertices[i]);

                if (s > radius)
                {
                    // Early out.
                    return;
                }

                if (s > separation)
                {
                    separation  = s;
                    normalIndex = i;
                }
            }

            // Vertices that subtend the incident face.
            int     vertIndex1 = normalIndex;
            int     vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
            Vector2 v1         = polygon._vertices[vertIndex1];
            Vector2 v2         = polygon._vertices[vertIndex2];

            // If the center is inside the polygon ...
            if (separation < Settings.b2_FLT_EPSILON)
            {
                manifold._pointCount       = 1;
                manifold._type             = ManifoldType.FaceA;
                manifold._localPlaneNormal = polygon._normals[normalIndex];
                manifold._localPoint       = 0.5f * (v1 + v2);

                var p0 = manifold._points[0];

                p0.LocalPoint = circle._p;
                p0.Id.Key     = 0;

                manifold._points[0] = p0;

                return;
            }

            // Compute barycentric coordinates
            float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
            float u2 = Vector2.Dot(cLocal - v2, v1 - v2);

            if (u1 <= 0.0f)
            {
                if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
                {
                    return;
                }

                manifold._pointCount       = 1;
                manifold._type             = ManifoldType.FaceA;
                manifold._localPlaneNormal = cLocal - v1;
                manifold._localPlaneNormal.Normalize();
                manifold._localPoint = v1;

                var p0b = manifold._points[0];

                p0b.LocalPoint = circle._p;
                p0b.Id.Key     = 0;

                manifold._points[0] = p0b;
            }
            else if (u2 <= 0.0f)
            {
                if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
                {
                    return;
                }

                manifold._pointCount       = 1;
                manifold._type             = ManifoldType.FaceA;
                manifold._localPlaneNormal = cLocal - v2;
                manifold._localPlaneNormal.Normalize();
                manifold._localPoint = v2;

                var p0c = manifold._points[0];

                p0c.LocalPoint = circle._p;
                p0c.Id.Key     = 0;

                manifold._points[0] = p0c;
            }
            else
            {
                Vector2 faceCenter  = 0.5f * (v1 + v2);
                float   separation2 = Vector2.Dot(cLocal - faceCenter, polygon._normals[vertIndex1]);
                if (separation2 > radius)
                {
                    return;
                }

                manifold._pointCount       = 1;
                manifold._type             = ManifoldType.FaceA;
                manifold._localPlaneNormal = polygon._normals[vertIndex1];
                manifold._localPoint       = faceCenter;

                var p0d = manifold._points[0];

                p0d.LocalPoint = circle._p;
                p0d.Id.Key     = 0;

                manifold._points[0] = p0d;
            }
        }