Exemple #1
0
        static void FindIncidentEdge(out FixedArray2 <ClipVertex> c,
                                     PolygonShape poly1, ref Transform xf1, int edge1,
                                     PolygonShape poly2, ref Transform 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_maxFloat;

            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;
        }
Exemple #2
0
        /// <summary>
        /// Clipping for contact manifolds.
        /// </summary>
        /// <param name="vOut"></param>
        /// <param name="vIn"></param>
        /// <param name="normal"></param>
        /// <param name="offset"></param>
        /// <returns></returns>
        public static int ClipSegmentToLine(out FixedArray2 <ClipVertex> vOut, ref FixedArray2 <ClipVertex> vIn,
                                            Vector2 normal, float offset)
        {
            vOut = new FixedArray2 <ClipVertex>();

            // Start with no output points
            int numOut = 0;

            // Calculate the distance of end points to the line
            float distance0 = Vector2.Dot(normal, vIn[0].v) - offset;
            float distance1 = Vector2.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
                float interp = distance0 / (distance0 - distance1);

                var cv = vOut[numOut];

                cv.v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
                if (distance0 > 0.0f)
                {
                    cv.id = vIn[0].id;
                }
                else
                {
                    cv.id = vIn[1].id;
                }

                vOut[numOut] = cv;

                ++numOut;
            }

            return(numOut);
        }
Exemple #3
0
        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;
                    }
                }
            }
        }
Exemple #4
0
        /// <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;
            }
        }