/// <summary>
        /// Compute the collision manifold between two circles.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="circleA"></param>
        /// <param name="xfA"></param>
        /// <param name="circleB"></param>
        /// <param name="xfB"></param>
        public static void CollideCircles(ref Manifold manifold,
            CircleShape circleA, ref Transform xfA,
            CircleShape circleB, ref Transform xfB)
        {
            manifold._pointCount = 0;

            Vector2 pA = MathUtils.Multiply(ref xfA, circleA._p);
            Vector2 pB = MathUtils.Multiply(ref xfB, circleB._p);

            Vector2 d = pB - pA;
            float distSqr = Vector2.Dot(d, d);
            float rA = circleA._radius;
            float rB = circleB._radius;
            float radius = rA + rB;
            if (distSqr > radius * radius)
            {
                return;
            }

            manifold._type = ManifoldType.Circles;
            manifold._localPoint = circleA._p;
            manifold._localNormal = Vector2.Zero;
            manifold._pointCount = 1;

            var p0 = manifold._points[0];

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

            manifold._points[0] = p0;
        }
        /// <summary>
        /// Compute the collision manifold between a polygon and a circle.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="polygonA"></param>
        /// <param name="xfA"></param>
        /// <param name="circleB"></param>
        /// <param name="xfB"></param>
        public static void CollidePolygonAndCircle(ref Manifold manifold,
            PolygonShape polygonA, ref Transform xfA,
            CircleShape circleB, ref Transform xfB)
        {
            manifold._pointCount = 0;

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

            // Find the min separating edge.
            int normalIndex = 0;
            float separation = -Settings.b2_maxFloat;
            float radius = polygonA._radius + circleB._radius;
            int vertexCount = polygonA._vertexCount;

            for (int i = 0; i < vertexCount; ++i)
            {
                float s = Vector2.Dot(polygonA._normals[i], cLocal - polygonA._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 = polygonA._vertices[vertIndex1];
            Vector2 v2 = polygonA._vertices[vertIndex2];

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

                var p0 = manifold._points[0];

                p0.LocalPoint = circleB._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._localNormal = cLocal - v1;
                manifold._localNormal.Normalize();
                manifold._localPoint = v1;

                var p0b = manifold._points[0];

                p0b.LocalPoint = circleB._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._localNormal = cLocal - v2;
                manifold._localNormal.Normalize();
                manifold._localPoint = v2;

                var p0c = manifold._points[0];

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

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

                manifold._pointCount = 1;
                manifold._type = ManifoldType.FaceA;
                manifold._localNormal = polygonA._normals[vertIndex1];
                manifold._localPoint = faceCenter;

                var p0d = manifold._points[0];

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

                manifold._points[0] = p0d;
            }
        }
示例#3
0
 /// <summary>
 /// This is one the methods of the IContactListener interface.
 /// No implementation.
 /// </summary>
 /// <param name="contact">Box2D Contact</param>
 /// <param name="oldManifold">Box2D Manifold</param>
 public void PreSolve(Contact contact, ref Manifold oldManifold)
 {
 }
        /// <summary>
        /// Evaluate the manifold with supplied transforms. This assumes
        /// modest motion from the original state. This does not change the
        /// point count, impulses, etc. The radii must come from the shapes
        /// that generated the manifold.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="xfA"></param>
        /// <param name="radiusA"></param>
        /// <param name="xfB"></param>
        /// <param name="radiusB"></param>
        public WorldManifold(ref Manifold manifold,
            ref Transform xfA, float radiusA,
            ref Transform xfB, float radiusB)
        {
            _points = new FixedArray2<Vector2>();

            if (manifold._pointCount == 0)
            {
                _normal = Vector2.UnitY;
                return;
            }

            switch (manifold._type)
            {
                case ManifoldType.Circles:
                    {
                        Vector2 pointA = MathUtils.Multiply(ref xfA, manifold._localPoint);
                        Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
                        _normal = new Vector2(1.0f, 0.0f);
                        if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_epsilon * Settings.b2_epsilon)
                        {
                            _normal = pointB - pointA;
                            _normal.Normalize();
                        }

                        Vector2 cA = pointA + radiusA * _normal;
                        Vector2 cB = pointB - radiusB * _normal;
                        _points[0] = 0.5f * (cA + cB);
                    }
                    break;

                case ManifoldType.FaceA:
                    {
                        _normal = MathUtils.Multiply(ref xfA.R, manifold._localNormal);
                        Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);

                        for (int i = 0; i < manifold._pointCount; ++i)
                        {
                            Vector2 clipPoint = MathUtils.Multiply(ref xfB, manifold._points[i].LocalPoint);
                            Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
                            Vector2 cB = clipPoint - radiusB * _normal;
                            _points[i] = 0.5f * (cA + cB);
                        }
                    }
                    break;

                case ManifoldType.FaceB:
                    {
                        _normal = MathUtils.Multiply(ref xfB.R, manifold._localNormal);
                        Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);

                        for (int i = 0; i < manifold._pointCount; ++i)
                        {
                            Vector2 clipPoint = MathUtils.Multiply(ref xfA, manifold._points[i].LocalPoint);
                            Vector2 cA = clipPoint - radiusA * _normal;
                            Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
                            _points[i] = 0.5f * (cA + cB);
                        }
                        // Ensure normal points from A to B.
                        _normal *= -1;
                    }
                    break;
                default:
                    _normal = Vector2.UnitY;
                    break;
            }
        }
示例#5
0
        // Compute contact points for edge versus circle.
        // This accounts for edge connectivity.
        public static void CollideEdgeAndCircle(ref Manifold manifold,
            EdgeShape edgeA, ref Transform xfA,
            CircleShape circleB, ref Transform xfB)
        {
            manifold._pointCount = 0;

            // Compute circle in frame of edge
            Vector2 Q = MathUtils.MultiplyT(ref xfA, MathUtils.Multiply(ref xfB, circleB._p));

            Vector2 A = edgeA._vertex1, B = edgeA._vertex2;
            Vector2 e = B - A;

            // Barycentric coordinates
            float u = Vector2.Dot(e, B - Q);
            float v = Vector2.Dot(e, Q - A);

            float radius = edgeA._radius + circleB._radius;

            ContactFeature cf;
            cf.indexB = 0;
            cf.typeB = (byte)ContactFeatureType.Vertex;

            Vector2 P, d;

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

                // Is there an edge connected to A?
                if (edgeA._hasVertex0)
                {
                    Vector2 A1 = edgeA._vertex0;
                    Vector2 B1 = A;
                    Vector2 e1 = B1 - A1;
                    float u1 = Vector2.Dot(e1, B1 - Q);

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

                cf.indexA = 0;
                cf.typeA = (byte)ContactFeatureType.Vertex;
                manifold._pointCount = 1;
                manifold._type = ManifoldType.Circles;
                manifold._localNormal = Vector2.Zero;
                manifold._localPoint = P;
                var mp = new ManifoldPoint();
                mp.Id.Key = 0;
                mp.Id.Features = cf;
                mp.LocalPoint = circleB._p;
                manifold._points[0] = mp;
                return;
            }

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

                // Is there an edge connected to B?
                if (edgeA._hasVertex3)
                {
                    Vector2 B2 = edgeA._vertex3;
                    Vector2 A2 = B;
                    Vector2 e2 = B2 - A2;
                    float v2 = Vector2.Dot(e2, Q - A2);

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

                cf.indexA = 1;
                cf.typeA = (byte)ContactFeatureType.Vertex;
                manifold._pointCount = 1;
                manifold._type = ManifoldType.Circles;
                manifold._localNormal = Vector2.Zero;
                manifold._localPoint = P;
                var mp = new ManifoldPoint();
                mp.Id.Key = 0;
                mp.Id.Features = cf;
                mp.LocalPoint = circleB._p;
                manifold._points[0] = mp;
                return;
            }

            // Region AB
            float den = Vector2.Dot(e, e);
            Debug.Assert(den > 0.0f);
            P = (1.0f / den) * (u * A + v * B);
            d = Q - P;
            float dd2 = Vector2.Dot(d, d);
            if (dd2 > radius * radius)
            {
                return;
            }

            Vector2 n = new Vector2(-e.Y, e.X);
            if (Vector2.Dot(n, Q - A) < 0.0f)
            {
                n = new Vector2(-n.X, -n.Y);
            }
            n.Normalize();

            cf.indexA = 0;
            cf.typeA = (byte)ContactFeatureType.Face;
            manifold._pointCount = 1;
            manifold._type = ManifoldType.FaceA;
            manifold._localNormal = n;
            manifold._localPoint = A;
            var mp2 = new ManifoldPoint();
            mp2.Id.Key = 0;
            mp2.Id.Features = cf;
            mp2.LocalPoint = circleB._p;
            manifold._points[0] = mp2;
        }
示例#6
0
        public static void CollideEdgeAndPolygon(ref Manifold manifold,
            EdgeShape edgeA, ref Transform xfA,
            PolygonShape polygonB_in, ref Transform xfB)
        {
            manifold._pointCount = 0;

            Transform xf;
            MathUtils.MultiplyT(ref xfA, ref xfB, out xf);

            // Create a polygon for edge shape A
            s_polygonA.SetAsEdge(edgeA._vertex1, edgeA._vertex2);

            // Build polygonB in frame A
            s_polygonB._radius = polygonB_in._radius;
            s_polygonB._vertexCount = polygonB_in._vertexCount;
            s_polygonB._centroid = MathUtils.Multiply(ref xf, polygonB_in._centroid);
            for (int i = 0; i < s_polygonB._vertexCount; ++i)
            {
                s_polygonB._vertices[i] = MathUtils.Multiply(ref xf, polygonB_in._vertices[i]);
                s_polygonB._normals[i] = MathUtils.Multiply(ref xf.R, polygonB_in._normals[i]);
            }

            float totalRadius = s_polygonA._radius + s_polygonB._radius;

            // Edge geometry
            Vector2 v1 = edgeA._vertex1;
            Vector2 v2 = edgeA._vertex2;
            Vector2 e = v2 - v1;
            Vector2 edgeNormal = new Vector2(e.Y, -e.X);
            edgeNormal.Normalize();

            // Determine side
            bool isFrontSide = Vector2.Dot(edgeNormal, s_polygonB._centroid - v1) >= 0.0f;
            if (isFrontSide == false)
            {
                edgeNormal = -edgeNormal;
            }

            // Compute primary separating axis
            EPAxis edgeAxis = ComputeEdgeSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius);
            if (edgeAxis.separation > totalRadius)
            {
                // Shapes are separated
                return;
            }

            // Classify adjacent edges
            FixedArray2<EdgeType> types = new FixedArray2<EdgeType>();
            //types[0] = EdgeType.Isolated;
            //types[1] = EdgeType.Isolated;
            if (edgeA._hasVertex0)
            {
                Vector2 v0 = edgeA._vertex0;
                float s = Vector2.Dot(edgeNormal, v0 - v1);

                if (s > 0.1f * Settings.b2_linearSlop)
                {
                    types[0] = EdgeType.Concave;
                }
                else if (s >= -0.1f * Settings.b2_linearSlop)
                {
                    types[0] = EdgeType.Flat;
                }
                else
                {
                    types[0] = EdgeType.Convex;
                }
            }

            if (edgeA._hasVertex3)
            {
                Vector2 v3 = edgeA._vertex3;
                float s = Vector2.Dot(edgeNormal, v3 - v2);
                if (s > 0.1f * Settings.b2_linearSlop)
                {
                    types[1] = EdgeType.Concave;
                }
                else if (s >= -0.1f * Settings.b2_linearSlop)
                {
                    types[1] = EdgeType.Flat;
                }
                else
                {
                    types[1] = EdgeType.Convex;
                }
            }

            if (types[0] == EdgeType.Convex)
            {
                // Check separation on previous edge.
                Vector2 v0 = edgeA._vertex0;
                Vector2 e0 = v1 - v0;

                Vector2 n0 = new Vector2(e0.Y, -e0.X);
                n0.Normalize();
                if (isFrontSide == false)
                {
                    n0 = -n0;
                }

                EPAxis axis1 = ComputeEdgeSeperation(v0, v1, n0, s_polygonB, totalRadius);
                if (axis1.separation > edgeAxis.separation)
                {
                    // The polygon should collide with previous edge
                    return;
                }
            }

            if (types[1] == EdgeType.Convex)
            {
                // Check separation on next edge.
                Vector2 v3 = edgeA._vertex3;
                Vector2 e2 = v3 - v2;

                Vector2 n2 = new Vector2(e2.Y, -e2.X);
                n2.Normalize();
                if (isFrontSide == false)
                {
                    n2 = -n2;
                }

                EPAxis axis2 = ComputeEdgeSeperation(v2, v3, n2, s_polygonB, totalRadius);
                if (axis2.separation > edgeAxis.separation)
                {
                    // The polygon should collide with the next edge
                    return;
                }
            }

            EPAxis polygonAxis = ComputePolygonSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius);
            if (polygonAxis.separation > totalRadius)
            {
                return;
            }

            // Use hysteresis for jitter reduction.
            float k_relativeTol = 0.98f;
            float k_absoluteTol = 0.001f;

            EPAxis primaryAxis;
            if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
            {
                primaryAxis = polygonAxis;
            }
            else
            {
                primaryAxis = edgeAxis;
            }

            PolygonShape poly1;
            PolygonShape poly2;
            if (primaryAxis.type == EPAxisType.EdgeA)
            {
                poly1 = s_polygonA;
                poly2 = s_polygonB;
                if (isFrontSide == false)
                {
                    primaryAxis.index = 1;
                }
                manifold._type = ManifoldType.FaceA;
            }
            else
            {
                poly1 = s_polygonB;
                poly2 = s_polygonA;
                manifold._type = ManifoldType.FaceB;
            }

            int edge1 = primaryAxis.index;

            FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
            FindIncidentEdge(ref incidentEdge, poly1, primaryAxis.index, poly2);
            int count1 = poly1._vertexCount;
            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            Vector2 v11 = poly1._vertices[iv1];
            Vector2 v12 = poly1._vertices[iv2];

            Vector2 tangent = v12 - v11;
            tangent.Normalize();

            Vector2 normal = MathUtils.Cross(tangent, 1.0f);
            Vector2 planePoint = 0.5f * (v11 + v12);

            // Face offset.
            float frontOffset = Vector2.Dot(normal, v11);

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

            // 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, -tangent, sideOffset1, iv1);

            if (np < Settings.b2_maxManifoldPoints)
            {
                return;
            }

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

            if (np < Settings.b2_maxManifoldPoints)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.type == EPAxisType.EdgeA)
            {
                manifold._localNormal = normal;
                manifold._localPoint = planePoint;
            }
            else
            {
                manifold._localNormal = MathUtils.MultiplyT(ref xf.R, normal);
                manifold._localPoint = MathUtils.MultiplyT(ref xf, planePoint);
            }

            int pointCount = 0;
            for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
            {
                float separation;

                separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold._points[pointCount];

                    if (primaryAxis.type == EPAxisType.EdgeA)
                    {
                        cp.LocalPoint = MathUtils.MultiplyT(ref xf, clipPoints2[i].v);
                        cp.Id = clipPoints2[i].id;
                    }
                    else
                    {
                        cp.LocalPoint = clipPoints2[i].v;
                        cp.Id.Features.typeA = clipPoints2[i].id.Features.typeB;
                        cp.Id.Features.typeB = clipPoints2[i].id.Features.typeA;
                        cp.Id.Features.indexA = clipPoints2[i].id.Features.indexB;
                        cp.Id.Features.indexB = clipPoints2[i].id.Features.indexA;
                    }

                    manifold._points[pointCount] = cp;
                    if (cp.Id.Features.typeA == (byte)ContactFeatureType.Vertex && types[cp.Id.Features.indexA] == EdgeType.Flat)
                    {
                        continue;
                    }

                    ++pointCount;
                }
            }

            manifold._pointCount = pointCount;
        }
示例#7
0
 /// Evaluate this contact with your own manifold and transforms.   
 internal void Evaluate(ref Manifold manifold, ref Transform xfA, ref Transform xfB)
 {
     switch (_type)
     {
         case ContactType.Polygon:
             Collision.CollidePolygons(ref manifold,
                     (PolygonShape)_fixtureA.GetShape(), ref xfA,
                     (PolygonShape)_fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.PolygonAndCircle:
             Collision.CollidePolygonAndCircle(ref manifold,
                     (PolygonShape)_fixtureA.GetShape(), ref xfA,
                     (CircleShape) _fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.EdgeAndCircle:
             Collision.CollideEdgeAndCircle(ref manifold,
                     (EdgeShape)  _fixtureA.GetShape(), ref xfA,
                     (CircleShape)_fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.EdgeAndPolygon:
             Collision.CollideEdgeAndPolygon(ref manifold,
                     (EdgeShape)   _fixtureA.GetShape(), ref xfA,
                     (PolygonShape)_fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.LoopAndCircle:
             var loop = (LoopShape)_fixtureA.GetShape();
             loop.GetChildEdge(ref s_edge, _indexA);
             Collision.CollideEdgeAndCircle(ref manifold, s_edge, ref xfA,
                     (CircleShape)_fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.LoopAndPolygon:
             var loop2 = (LoopShape)_fixtureA.GetShape();
             loop2.GetChildEdge(ref s_edge, _indexA);
             Collision.CollideEdgeAndPolygon(ref manifold, s_edge, ref xfA,
                     (PolygonShape)_fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.Circle:
             Collision.CollideCircles(ref manifold,
                     (CircleShape)_fixtureA.GetShape(), ref xfA,
                     (CircleShape)_fixtureB.GetShape(), ref xfB);
             break;
     }
 }
示例#8
0
文件: Collision.cs 项目: Bitsits/web
        /// <summary>
        /// Compute the collision manifold between a polygon and a circle.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="polygonA"></param>
        /// <param name="xfA"></param>
        /// <param name="circleB"></param>
        /// <param name="xfB"></param>
        public static void CollidePolygonAndCircle(ref Manifold manifold,
                                                   PolygonShape polygonA, ref Transform xfA,
                                                   CircleShape circleB, ref Transform xfB)
        {
            manifold._pointCount = 0;

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

            // Find the min separating edge.
            int   normalIndex = 0;
            float separation  = -Settings.b2_maxFloat;
            float radius      = polygonA._radius + circleB._radius;
            int   vertexCount = polygonA._vertexCount;

            for (int i = 0; i < vertexCount; ++i)
            {
                float s = Vector2.Dot(polygonA._normals[i], cLocal - polygonA._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         = polygonA._vertices[vertIndex1];
            Vector2 v2         = polygonA._vertices[vertIndex2];

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

                var p0 = manifold._points[0];

                p0.LocalPoint = circleB._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._localNormal = cLocal - v1;
                manifold._localNormal.Normalize();
                manifold._localPoint = v1;

                var p0b = manifold._points[0];

                p0b.LocalPoint = circleB._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._localNormal = cLocal - v2;
                manifold._localNormal.Normalize();
                manifold._localPoint = v2;

                var p0c = manifold._points[0];

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

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

                manifold._pointCount  = 1;
                manifold._type        = ManifoldType.FaceA;
                manifold._localNormal = polygonA._normals[vertIndex1];
                manifold._localPoint  = faceCenter;

                var p0d = manifold._points[0];

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

                manifold._points[0] = p0d;
            }
        }
示例#9
0
 public virtual void PreSolve(Contact contact, ref Manifold oldManifold)
 {
     // call this in the base class if points are needed
     // PreSolveCalcPoints(contact, ref oldManifold);
 }
示例#10
0
        protected void PreSolveCalcPoints(Contact contact, ref Manifold oldManifold)
        {
            Manifold manifold;
            contact.GetManifold(out manifold);

            if (manifold._pointCount == 0)
            {
                return;
            }

            Fixture fixtureA = contact.GetFixtureA();
            Fixture fixtureB = contact.GetFixtureB();

            FixedArray2<PointState> state1, state2;
            Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold);

            WorldManifold worldManifold;
            contact.GetWorldManifold(out worldManifold);

            for (int i = 0; i < manifold._pointCount && _pointCount < k_maxContactPoints; ++i)
            {
                if (fixtureA == null)
                {
                    _points[i] = new ContactPoint();
                }
                ContactPoint cp = _points[_pointCount];
                cp.fixtureA = fixtureA;
                cp.fixtureB = fixtureB;
                cp.position = worldManifold._points[i];
                cp.normal = worldManifold._normal;
                cp.state = state2[i];
                _points[_pointCount] = cp;
                ++_pointCount;
            }
        }
 /// <summary>
 /// Evaluate this contact with your own manifold and transforms.
 /// </summary>
 /// <param name="manifold"></param>
 /// <param name="xfA"></param>
 /// <param name="xfB"></param>
 internal void Evaluate(ref Manifold manifold, ref Transform xfA, ref Transform xfB)
 {
     switch (_type)
     {
         case ContactType.Polygon:
             Collision.CollidePolygons(ref manifold,
                 (PolygonShape)_fixtureA.GetShape(), ref xfA,
                 (PolygonShape)_fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.PolygonAndCircle:
             Collision.CollidePolygonAndCircle(ref manifold,
                     (PolygonShape)_fixtureA.GetShape(), ref xfA,
                     (CircleShape)_fixtureB.GetShape(), ref xfB);
             break;
         case ContactType.Circle:
             Collision.CollideCircles(ref manifold,
                     (CircleShape)_fixtureA.GetShape(), ref xfA,
                     (CircleShape)_fixtureB.GetShape(), ref xfB);
             break;
     }
 }
 public void PreSolve(Contact contact, ref Manifold oldManifold)
 {
     var unitA = (Unit) contact.GetFixtureA().GetBody().GetUserData();
     var unitB = (Unit) contact.GetFixtureB().GetBody().GetUserData();
     if (!(unitA.EnableCollision && unitB.EnableCollision
           && GroupManager.Instance.ShouldContact(unitA.Group, unitB.Group)))
     {
         contact.SetEnabled(false);
     }
 }
示例#13
0
文件: Collision.cs 项目: Bitsits/web
        /// <summary>
        /// Compute the collision manifold between two polygons.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="polyA"></param>
        /// <param name="xfA"></param>
        /// <param name="polyB"></param>
        /// <param name="xfB"></param>
        public static void CollidePolygons(ref Manifold manifold,
                                           PolygonShape polyA, ref Transform xfA,
                                           PolygonShape polyB, ref Transform 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
            Transform    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 = 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 localTangent = v12 - v11;

            localTangent.Normalize();

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

            Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent);
            Vector2 normal  = MathUtils.Cross(tangent, 1.0f);

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

            // Face offset.
            float frontOffset = Vector2.Dot(normal, v11);

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

            // 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, -tangent, sideOffset1);

            if (np < 2)
            {
                return;
            }

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

            if (np < 2)
            {
                return;
            }

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

            int pointCount = 0;

            for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
            {
                float separation = Vector2.Dot(normal, 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;
        }
        /// <summary>
        /// Compute the collision manifold between two polygons.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="polyA"></param>
        /// <param name="xfA"></param>
        /// <param name="polyB"></param>
        /// <param name="xfB"></param>
        public static void CollidePolygons(ref Manifold manifold,
            PolygonShape polyA, ref Transform xfA,
            PolygonShape polyB, ref Transform 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
            Transform 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 = 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 localTangent = v12 - v11;
            localTangent.Normalize();

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

            Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent);
            Vector2 normal = MathUtils.Cross(tangent, 1.0f);

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

            // Face offset.
            float frontOffset = Vector2.Dot(normal, v11);

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

            // 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, -tangent, sideOffset1);

            if (np < 2)
                return;

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

            if (np < 2)
            {
                return;
            }

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

            int pointCount = 0;
            for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
            {
                float separation = Vector2.Dot(normal, 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;
        }
示例#15
0
        /// Get the contact manifold. Do not modify the manifold unless you understand the
        /// internals of Box2D.
	    public void GetManifold(out Manifold manifold)
        {
            manifold = _manifold;
        }
        public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2,
            ref Manifold manifold1, ref Manifold manifold2)
        {
            state1 = new FixedArray2<PointState>();
            state2 = new FixedArray2<PointState>();

            // Detect persists and removes.
            for (int i = 0; i < manifold1._pointCount; ++i)
            {
                ContactID id = manifold1._points[i].Id;

                state1[i] = PointState.Remove;

                for (int j = 0; j < manifold2._pointCount; ++j)
                {
                    if (manifold2._points[j].Id.Key == id.Key)
                    {
                        state1[i] = PointState.Persist;
                        break;
                    }
                }
            }

            // Detect persists and adds.
            for (int i = 0; i < manifold2._pointCount; ++i)
            {
                ContactID id = manifold2._points[i].Id;

                state2[i] = PointState.Add;

                for (int j = 0; j < manifold1._pointCount; ++j)
                {
                    if (manifold1._points[j].Id.Key == id.Key)
                    {
                        state2[i] = PointState.Persist;
                        break;
                    }
                }
            }
        }
示例#17
0
文件: Collision.cs 项目: Bitsits/web
        /// <summary>
        /// Evaluate the manifold with supplied transforms. This assumes
        /// modest motion from the original state. This does not change the
        /// point count, impulses, etc. The radii must come from the shapes
        /// that generated the manifold.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="xfA"></param>
        /// <param name="radiusA"></param>
        /// <param name="xfB"></param>
        /// <param name="radiusB"></param>
        public WorldManifold(ref Manifold manifold,
                             ref Transform xfA, float radiusA,
                             ref Transform xfB, float radiusB)
        {
            _points = new FixedArray2 <Vector2>();

            if (manifold._pointCount == 0)
            {
                _normal = Vector2.UnitY;
                return;
            }

            switch (manifold._type)
            {
            case ManifoldType.Circles:
            {
                Vector2 pointA = MathUtils.Multiply(ref xfA, manifold._localPoint);
                Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
                _normal = new Vector2(1.0f, 0.0f);
                if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_epsilon * Settings.b2_epsilon)
                {
                    _normal = pointB - pointA;
                    _normal.Normalize();
                }

                Vector2 cA = pointA + radiusA * _normal;
                Vector2 cB = pointB - radiusB * _normal;
                _points[0] = 0.5f * (cA + cB);
            }
            break;

            case ManifoldType.FaceA:
            {
                _normal = MathUtils.Multiply(ref xfA.R, manifold._localNormal);
                Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);

                for (int i = 0; i < manifold._pointCount; ++i)
                {
                    Vector2 clipPoint = MathUtils.Multiply(ref xfB, manifold._points[i].LocalPoint);
                    Vector2 cA        = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
                    Vector2 cB        = clipPoint - radiusB * _normal;
                    _points[i] = 0.5f * (cA + cB);
                }
            }
            break;

            case ManifoldType.FaceB:
            {
                _normal = MathUtils.Multiply(ref xfB.R, manifold._localNormal);
                Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);

                for (int i = 0; i < manifold._pointCount; ++i)
                {
                    Vector2 clipPoint = MathUtils.Multiply(ref xfA, manifold._points[i].LocalPoint);
                    Vector2 cA        = clipPoint - radiusA * _normal;
                    Vector2 cB        = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
                    _points[i] = 0.5f * (cA + cB);
                }
                // Ensure normal points from A to B.
                _normal *= -1;
            }
            break;

            default:
                _normal = Vector2.UnitY;
                break;
            }
        }