public static void CollideCircles(out Manifold manifold,
            CircleShape circle1, Transform xf1,
            CircleShape circle2, Transform xf2)
        {
            manifold = new Manifold();
            manifold.PointCount = 0;

            Vec2 p1 = Math.Mul(xf1, circle1._p);
            Vec2 p2 = Math.Mul(xf2, circle2._p);

            Vec2 d = p2 - p1;
            float distSqr = Vec2.Dot(d, d);
            float radius = circle1._radius + circle2._radius;
            if (distSqr > radius * radius)
            {
                return;
            }

            manifold.Type = Manifold.ManifoldType.Circles;
            manifold.LocalPoint = circle1._p;
            manifold.LocalPlaneNormal.SetZero();
            manifold.PointCount = 1;

            manifold.Points[0].LocalPoint = circle2._p;
            manifold.Points[0].ID.Key = 0;
        }
Ejemplo n.º 2
0
 public Manifold Clone()
 {
     Manifold manifold = new Manifold();
     manifold.Normal = this.Normal;
     manifold.PointCount = this.PointCount;
     int num = this.Points.Length;
     ManifoldPoint[] array = new ManifoldPoint[num];
     for (int i = 0; i < num; i++)
     {
         array[i] = this.Points[i].Clone();
     }
     manifold.Points = array;
     return manifold;
 }
Ejemplo n.º 3
0
		public static void CollideCircles(ref Manifold manifold,
			CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
		{
			manifold.PointCount = 0;

			Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition());
			Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition());

			Vec2 d = p2 - p1;
			float distSqr = Vec2.Dot(d, d);
			float r1 = circle1.GetRadius();
			float r2 = circle2.GetRadius();
			float radiusSum = r1 + r2;
			if (distSqr > radiusSum * radiusSum)
			{
				return;
			}

			float separation;
			if (distSqr < Common.Settings.FLT_EPSILON)
			{
				separation = -radiusSum;
				manifold.Normal.Set(0.0f, 1.0f);
			}
			else
			{
				float dist = Common.Math.Sqrt(distSqr);
				separation = dist - radiusSum;
				float a = 1.0f / dist;
				manifold.Normal.X = a * d.X;
				manifold.Normal.Y = a * d.Y;
			}

			manifold.PointCount = 1;
			manifold.Points[0].ID.Key = 0;
			manifold.Points[0].Separation = separation;

			p1 += r1 * manifold.Normal;
			p2 -= r2 * manifold.Normal;

			Vec2 p = 0.5f * (p1 + p2);

			manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p);
			manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p);
		}
Ejemplo n.º 4
0
		/// <summary>
		/// 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.
		/// </summary>
		public static void GetPointStates(PointState[/*b2_maxManifoldPoints*/] state1, PointState[/*b2_maxManifoldPoints*/] state2,
					  Manifold manifold1, Manifold manifold2)
		{
			for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i)
			{
				state1[i] = PointState.NullState;
				state2[i] = PointState.NullState;
			}

			// Detect persists and removes.
			for (int i = 0; i < manifold1.PointCount; ++i)
			{
				ContactID id = manifold1.Points[i].ID;

				state1[i] = PointState.RemoveState;

				for (int j = 0; j < manifold2.PointCount; ++j)
				{
					if (manifold2.Points[j].ID.Key == id.Key)
					{
						state1[i] = PointState.PersistState;
						break;
					}
				}
			}

			// Detect persists and adds.
			for (int i = 0; i < manifold2.PointCount; ++i)
			{
				ContactID id = manifold2.Points[i].ID;

				state2[i] = PointState.AddState;

				for (int j = 0; j < manifold1.PointCount; ++j)
				{
					if (manifold1.Points[j].ID.Key == id.Key)
					{
						state2[i] = PointState.PersistState;
						break;
					}
				}
			}
		}
        public static void CollideCircles(ref Manifold manifold, CircleShape circle1, Transform xf1, CircleShape circle2, Transform xf2)
        {
            manifold.PointCount = 0;

            Vector2 p1 = xf1.TransformPoint(circle1._position);
            Vector2 p2 = xf2.TransformPoint(circle2._position);

            Vector2 d = p2 - p1;
            float distSqr = Vector2.Dot(d, d);
            float radius = circle1._radius + circle2._radius;
            if (distSqr > radius * radius)
            {
                return;
            }

            manifold.Type = ManifoldType.Circles;
            manifold.LocalPoint = circle1._position;
            manifold.LocalPlaneNormal = Vector2.zero;
            manifold.PointCount = 1;

            manifold.Points[0].LocalPoint = circle2._position;
            manifold.Points[0].ID.Key = 0;
        }
        // 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 CollidePolygons(out Manifold manifold,
            PolygonShape polyA, Transform xfA,
            PolygonShape polyB, Transform xfB)
        {
            manifold = new Manifold();
            manifold.PointCount = 0;
            float totalRadius = polyA._radius + polyB._radius;

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

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

            PolygonShape poly1;	// reference poly
            PolygonShape poly2;	// incident poly
            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 = Manifold.ManifoldType.FaceB;
                flip = 1;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1 = xfA;
                xf2 = xfB;
                edge1 = edgeA;
                manifold.Type = Manifold.ManifoldType.FaceA;
                flip = 0;
            }

            ClipVertex[] incidentEdge;
            FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);

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

            Vec2 v11 = vertices1[edge1];
            Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];

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

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

            Vec2 tangent = Math.Mul(xf1.R, localTangent);
            Vec2 normal = Vec2.Cross(tangent, 1.0f);

            v11 = Math.Mul(xf1, v11);
            v12 = Math.Mul(xf1, v12);

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

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

            // Clip incident edge against extruded edge1 side edges.
            ClipVertex[] clipPoints1;
            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.LocalPlaneNormal = localNormal;
            manifold.LocalPoint = planePoint;

            int pointCount = 0;
            for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
            {
                float separation = Vec2.Dot(normal, clipPoints2[i].V) - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    cp.LocalPoint = Math.MulT(xf2, clipPoints2[i].V);
                    cp.ID = clipPoints2[i].ID;
                    cp.ID.Features.Flip = flip;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
Ejemplo n.º 7
0
        /// 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.
        public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
        {
            if (manifold.PointCount == 0)
            {
                return;
            }

            switch (manifold.Type)
            {
                case ManifoldType.Circles:
                    {
                        Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint);
                        Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint);
                        Vector2 normal = new Vector2(1.0f, 0.0f);
                        if ((pointA - pointB).sqrMagnitude > (Mathf.Epsilon * Mathf.Epsilon))
                        {
                            normal = pointB - pointA;
                            normal.Normalize();
                        }

                        Normal = normal;

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

                case ManifoldType.FaceA:
                    {
                        Vector2 normal = xfA.TransformDirection(manifold.LocalPlaneNormal);
                        Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint);

                        // Ensure normal points from A to B.
                        Normal = normal;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vector2 clipPoint = xfB.TransformPoint(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:
                    {
                        Vector2 normal = xfB.TransformDirection(manifold.LocalPlaneNormal);
                        Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint);

                        // Ensure normal points from A to B.
                        Normal = -normal;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vector2 clipPoint = xfA.TransformPoint(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);
                        }
                    }
                    break;
            }
        }
Ejemplo n.º 8
0
        /// <summary>
        /// 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.
        /// </summary>
        public static void GetPointStates(PointState[] /*b2_maxManifoldPoints*/ state1, PointState[] /*b2_maxManifoldPoints*/ state2, Manifold manifold1, Manifold manifold2)
        {
            for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i)
            {
                state1[i] = PointState.NullState;
                state2[i] = PointState.NullState;
            }

            // Detect persists and removes.
            for (int i = 0; i < manifold1.PointCount; ++i)
            {
                ContactID id = manifold1.Points[i].ID;

                state1[i] = PointState.RemoveState;

                for (int j = 0; j < manifold2.PointCount; ++j)
                {
                    if (manifold2.Points[j].ID.Key == id.Key)
                    {
                        state1[i] = PointState.PersistState;
                        break;
                    }
                }
            }

            // Detect persists and adds.
            for (int i = 0; i < manifold2.PointCount; ++i)
            {
                ContactID id = manifold2.Points[i].ID;

                state2[i] = PointState.AddState;

                for (int j = 0; j < manifold1.PointCount; ++j)
                {
                    if (manifold1.Points[j].ID.Key == id.Key)
                    {
                        state2[i] = PointState.PersistState;
                        break;
                    }
                }
            }
        }
Ejemplo n.º 9
0
        /// 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.
        public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
        {
            if (manifold.PointCount == 0)
            {
                return;
            }

            switch (manifold.Type)
            {
            case ManifoldType.Circles:
            {
                Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint);
                Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint);
                Vector2 normal = new Vector2(1.0f, 0.0f);
                if ((pointA - pointB).sqrMagnitude > (Mathf.Epsilon * Mathf.Epsilon))
                {
                    normal = pointB - pointA;
                    normal.Normalize();
                }

                Normal = normal;

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

            case ManifoldType.FaceA:
            {
                Vector2 normal     = xfA.TransformDirection(manifold.LocalPlaneNormal);
                Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint);

                // Ensure normal points from A to B.
                Normal = normal;

                for (int i = 0; i < manifold.PointCount; ++i)
                {
                    Vector2 clipPoint = xfB.TransformPoint(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:
            {
                Vector2 normal     = xfB.TransformDirection(manifold.LocalPlaneNormal);
                Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint);

                // Ensure normal points from A to B.
                Normal = -normal;

                for (int i = 0; i < manifold.PointCount; ++i)
                {
                    Vector2 clipPoint = xfA.TransformPoint(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);
                }
            }
            break;
            }
        }
Ejemplo n.º 10
0
        /// 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.
        public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
        {
            if (manifold.PointCount == 0)
            {
                return;
            }

            switch (manifold.Type)
            {
                case Manifold.ManifoldType.Circles:
                    {
                        Vec2 pointA = Math.Mul(xfA, manifold.LocalPoint);
                        Vec2 pointB = Math.Mul(xfB, manifold.Points[0].LocalPoint);
                        Vec2 normal = new Vec2(1.0f, 0.0f);
                        if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON * Settings.FLT_EPSILON)
                        {
                            normal = pointB - pointA;
                            normal.Normalize();
                        }

                        Normal = normal;

                        Vec2 cA = pointA + radiusA * normal;
                        Vec2 cB = pointB - radiusB * normal;
                        Points[0] = 0.5f * (cA + cB);
                    }
                    break;

                case Manifold.ManifoldType.FaceA:
                    {
                        Vec2 normal = Math.Mul(xfA.R, manifold.LocalPlaneNormal);
                        Vec2 planePoint = Math.Mul(xfA, manifold.LocalPoint);

                        // Ensure normal points from A to B.
                        Normal = normal;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vec2 clipPoint = Math.Mul(xfB, manifold.Points[i].LocalPoint);
                            Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
                            Vec2 cB = clipPoint - radiusB * normal;
                            Points[i] = 0.5f * (cA + cB);
                        }
                    }
                    break;

                case Manifold.ManifoldType.FaceB:
                    {
                        Vec2 normal = Math.Mul(xfB.R, manifold.LocalPlaneNormal);
                        Vec2 planePoint = Math.Mul(xfB, manifold.LocalPoint);

                        // Ensure normal points from A to B.
                        Normal = -normal;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vec2 clipPoint = Math.Mul(xfA, manifold.Points[i].LocalPoint);
                            Vec2 cA = clipPoint - radiusA * normal;
                            Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
                            Points[i] = 0.5f * (cA + cB);
                        }
                    }
                    break;
            }
        }
Ejemplo n.º 11
0
		public Manifold Clone()
		{
			Manifold newManifold = new Manifold();
			newManifold.LocalPlaneNormal = this.LocalPlaneNormal;
			newManifold.LocalPoint = this.LocalPoint;
			newManifold.Type = this.Type;
			newManifold.PointCount = this.PointCount;
			int pointCount = this.Points.Length;
			ManifoldPoint[] tmp = new ManifoldPoint[pointCount];
			for (int i = 0; i < pointCount; i++)
			{
				tmp[i] = this.Points[i].Clone();
			}
			newManifold.Points = tmp;
			return newManifold;
		}
        // 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 CollidePolygons(ref Manifold manifold,
			PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB)
        {
            manifold.PointCount = 0;
            float totalRadius = polyA._radius + polyB._radius;

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

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

            PolygonShape poly1;	// reference poly
            PolygonShape poly2;	// incident poly
            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;
            }

            ClipVertex[] incidentEdge;
            Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);

            int count1 = poly1._vertexCount;
            Vector2[] vertices1 = poly1._vertices;

            Vector2 v11 = vertices1[edge1];
            Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];

            Vector2 dv = v12 - v11;

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

            Vector2 sideNormal = xf1.TransformDirection(v12 - v11);
            sideNormal.Normalize();
            Vector2 frontNormal = sideNormal.CrossScalarPostMultiply(1.0f);

            v11 = Common.Math.Mul(xf1, v11);
            v12 = Common.Math.Mul(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.
            ClipVertex[] clipPoints1;
            ClipVertex[] clipPoints2;
            int np;

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

            if (np < 2)
                return;

            // Clip to negative box side 1
            np = ClipSegmentToLine(out clipPoints2, 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.MaxManifoldPoints; ++i)
            {
                float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    cp.LocalPoint = xf2.InverseTransformPoint(clipPoints2[i].V);
                    cp.ID = clipPoints2[i].ID;
                    cp.ID.Features.Flip = flip;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
        // This implements 2-sided edge vs circle collision.
        public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
        {
            manifold.PointCount = 0;
            Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
            Vector2 normal = edge._normal;
            Vector2 v1 = edge._v1;
            Vector2 v2 = edge._v2;
            float radius = edge._radius + circle._radius;

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

            if (u1 <= 0.0f)
            {
                // Behind v1
                if ((cLocal- v1).sqrMagnitude > radius * radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalPlaneNormal = cLocal - v1;
                manifold.LocalPlaneNormal.Normalize();
                manifold.LocalPoint = v1;
                manifold.Points[0].LocalPoint = circle._position;
                manifold.Points[0].ID.Key = 0;
            }
            else if (u2 <= 0.0f)
            {
                // Ahead of v2
                if ((cLocal- v2).sqrMagnitude > radius * radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalPlaneNormal = cLocal - v2;
                manifold.LocalPlaneNormal.Normalize();
                manifold.LocalPoint = v2;
                manifold.Points[0].LocalPoint = circle._position;
                manifold.Points[0].ID.Key = 0;
            }
            else
            {
                float separation = Vector2.Dot(cLocal - v1, normal);
                if (separation < -radius || radius < separation)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
                manifold.LocalPoint = 0.5f * (v1 + v2);
                manifold.Points[0].LocalPoint = circle._position;
                manifold.Points[0].ID.Key = 0;
            }
        }
Ejemplo n.º 14
0
		public static void CollidePolygonAndCircle(ref Manifold manifold,
			PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
		{
			manifold.PointCount = 0;

			// Compute circle position in the frame of the polygon.
			Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition());
			Vec2 cLocal = Common.Math.MulT(xf1, c);

			// Find the min separating edge.
			int normalIndex = 0;
			float separation = -Settings.FLT_MAX;
			float radius = circle.GetRadius();
			int vertexCount = polygon.VertexCount;
			Vec2[] vertices = polygon.GetVertices();
			Vec2[] normals = polygon.Normals;

			for (int i = 0; i < vertexCount; ++i)
			{
				float s = Vec2.Dot(normals[i], cLocal - vertices[i]);
				if (s > radius)
				{
					// Early out.
					return;
				}

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

			// If the center is inside the polygon ...
			if (separation < Common.Settings.FLT_EPSILON)
			{
				manifold.PointCount = 1;
				manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]);
				manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
				manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
				manifold.Points[0].ID.Features.ReferenceEdge = 0;
				manifold.Points[0].ID.Features.Flip = 0;
				Vec2 position = c - radius * manifold.Normal;
				manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position);
				manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position);
				manifold.Points[0].Separation = separation - radius;
				return;
			}

			// Project the circle center onto the edge segment.
			int vertIndex1 = normalIndex;
			int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
			Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];

			float length = e.Normalize();
			Box2DXDebug.Assert(length > Settings.FLT_EPSILON);

			// Project the center onto the edge.
			float u = Vec2.Dot(cLocal - vertices[vertIndex1], e);
			Vec2 p;
			if (u <= 0.0f)
			{
				p = vertices[vertIndex1];
				manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
				manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
			}
			else if (u >= length)
			{
				p = vertices[vertIndex2];
				manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
				manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2;
			}
			else
			{
				p = vertices[vertIndex1] + u * e;
				manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
				manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
			}

			Vec2 d = cLocal - p;
			float dist = d.Normalize();
			if (dist > radius)
			{
				return;
			}

			manifold.PointCount = 1;
			manifold.Normal = Common.Math.Mul(xf1.R, d);
			Vec2 position_ = c - radius * manifold.Normal;
			manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_);
			manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_);
			manifold.Points[0].Separation = dist - radius;
			manifold.Points[0].ID.Features.ReferenceEdge = 0;
			manifold.Points[0].ID.Features.Flip = 0;
		}
 private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
 {
     Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2);
 }
Ejemplo n.º 16
0
        // Callbacks for derived classes.
        public override void PreSolve(Contact contact, Manifold oldManifold)
        {
            Manifold manifold = contact.GetManifold();

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

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

            PointState[] state1 = new PointState[Box2DX.Common.Settings.MaxManifoldPoints];
            PointState[] state2 = new PointState[Box2DX.Common.Settings.MaxManifoldPoints];

            Collision.GetPointStates(state1, state2, oldManifold, manifold);

            WorldManifold worldManifold;
            contact.GetWorldManifold(out worldManifold);

            for (int i = 0; i < manifold.PointCount && _pointCount < k_maxContactPoints; ++i)
            {
                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;
            }
        }
        // This implements 2-sided edge vs circle collision.
        public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
        {
            manifold.PointCount = 0;
            Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
            Vector2 normal = edge._normal;
            Vector2 v1     = edge._v1;
            Vector2 v2     = edge._v2;
            float   radius = edge._radius + circle._radius;

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

            if (u1 <= 0.0f)
            {
                // Behind v1
                if ((cLocal - v1).LengthSquared > radius * radius)
                {
                    return;
                }

                manifold.PointCount       = 1;
                manifold.Type             = ManifoldType.FaceA;
                manifold.LocalPlaneNormal = cLocal - v1;
                manifold.LocalPlaneNormal.Normalize();
                manifold.LocalPoint           = v1;
                manifold.Points[0].LocalPoint = circle._position;
                manifold.Points[0].ID.Key     = 0;
            }
            else if (u2 <= 0.0f)
            {
                // Ahead of v2
                if ((cLocal - v2).LengthSquared > radius * radius)
                {
                    return;
                }

                manifold.PointCount       = 1;
                manifold.Type             = ManifoldType.FaceA;
                manifold.LocalPlaneNormal = cLocal - v2;
                manifold.LocalPlaneNormal.Normalize();
                manifold.LocalPoint           = v2;
                manifold.Points[0].LocalPoint = circle._position;
                manifold.Points[0].ID.Key     = 0;
            }
            else
            {
                float separation = Vector2.Dot(cLocal - v1, normal);
                if (separation < -radius || radius < separation)
                {
                    return;
                }

                manifold.PointCount           = 1;
                manifold.Type                 = ManifoldType.FaceA;
                manifold.LocalPlaneNormal     = separation < 0.0f ? -normal : normal;
                manifold.LocalPoint           = 0.5f * (v1 + v2);
                manifold.Points[0].LocalPoint = circle._position;
                manifold.Points[0].ID.Key     = 0;
            }
        }
 private static void CollideEdgeAndCircle(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
 {
     Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)shape1, xf1, (CircleShape)shape2, xf2);
 }
		internal void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc) 
		{
			// this is kind of yucky but we do know these were setup before entry to this method
			var bodyA = cc.BodyA;
			var bodyB = cc.BodyB;
				
			Vector2 vA = bodyA._linearVelocity;
			Vector2 vB = bodyB._linearVelocity;
			float wA = bodyA._angularVelocity;
			float wB = bodyB._angularVelocity;
			
			ContactConstraintPoint[] ccPointsPtr = cc.Points;
			for (int j = 0; j < cc.PointCount; ++j)
			{
				ManifoldPoint cp = manifold.Points[j];
				ContactConstraintPoint ccp = ccPointsPtr[j];

				ccp.NormalImpulse = cp.NormalImpulse;
				ccp.TangentImpulse = cp.TangentImpulse;

				ccp.LocalPoint = cp.LocalPoint;
							
				ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
				ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;

				float rnA = ccp.RA.Cross(cc.Normal);
				float rnB = ccp.RB.Cross(cc.Normal);
				rnA *= rnA;
				rnB *= rnB;

				float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;

				Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
				ccp.NormalMass = 1.0f / kNormal;

				float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
				kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;

				Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
				ccp.EqualizedMass = 1.0f / kEqualized;

				Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f);

				float rtA = ccp.RA.Cross(tangent);
				float rtB = ccp.RB.Cross(tangent);
				rtA *= rtA;
				rtB *= rtB;

				float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

				Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
				ccp.TangentMass = 1.0f / kTangent;

				// Setup a velocity bias for restitution.
				ccp.VelocityBias = 0.0f;
				float vRel = Vector2.Dot(cc.Normal, vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA));
				if (vRel < -Common.Settings.VelocityThreshold)
				{
					ccp.VelocityBias = -cc.Restitution * vRel;
				}
			}

			// If we have two points, then prepare the block solver.
			if (cc.PointCount == 2)
			{
				ContactConstraintPoint ccp1 = ccPointsPtr[0];
				ContactConstraintPoint ccp2 = ccPointsPtr[1];

				float invMassA = bodyA._invMass;
				float invIA = bodyA._invI;
				float invMassB = bodyB._invMass;
				float invIB = bodyB._invI;

				float rn1A = ccp1.RA.Cross(cc.Normal);
				float rn1B = ccp1.RB.Cross(cc.Normal);
				float rn2A = ccp2.RA.Cross(cc.Normal);
				float rn2B = ccp2.RB.Cross(cc.Normal);

				float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
				float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
				float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

				// Ensure a reasonable condition number.
				const float k_maxConditionNumber = 100.0f;
				if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
				{
					// K is safe to invert.
					cc.K.Col1 = new Vector2(k11, k12);
					cc.K.Col2 = new Vector2(k12, k22);
					cc.NormalMass = cc.K.GetInverse();
				}
				else
				{
					// The constraints are redundant, just use one.
					// TODO_ERIN use deepest?
					cc.PointCount = 1;
				}
			}
		}
Ejemplo n.º 20
0
        private static bool IsBlocked(ref BoundingBox2D bbox)
        {
            const int maxShapeCount = 1024;
            AABB physBounds;
            physBounds.LowerBound = new Vector2( bbox.Min.X, bbox.Min.Y );
            physBounds.UpperBound = new Vector2( bbox.Max.X, bbox.Max.Y );

            Shape[] tempShapes = new Shape[maxShapeCount];
            int numBroadphase = Angel.World.Instance.PhysicsWorld.Query( physBounds, tempShapes, maxShapeCount );

            //No bodies here
            if( numBroadphase == 0 )
                return false;

            PolygonDef shapeBoundsDef = new PolygonDef();
            shapeBoundsDef.VertexCount = 4;
            shapeBoundsDef.Vertices[0] = new Vector2( physBounds.LowerBound.X, physBounds.LowerBound.Y );
            shapeBoundsDef.Vertices[1] = new Vector2( physBounds.UpperBound.X, physBounds.LowerBound.Y );
            shapeBoundsDef.Vertices[2] = new Vector2( physBounds.UpperBound.X, physBounds.UpperBound.Y );
            shapeBoundsDef.Vertices[3] = new Vector2( physBounds.LowerBound.X, physBounds.UpperBound.Y );

            BodyDef fakeBodyDef = new BodyDef();
            //b2Vec2 center = physBounds.lowerBound + (0.5f * shapeBoundsDef.extents);
            fakeBodyDef.Position = Vector2.Zero;
            Body fakeBody = new Body( fakeBodyDef, Angel.World.Instance.PhysicsWorld );
            PolygonShape shapeBounds = new PolygonShape( shapeBoundsDef );

            for( int i = 0; i < numBroadphase; i++ )
            {
                Shape Sh = tempShapes[i];
                if( Sh.Type == ShapeType.PolygonShape  )
                {
                    PolygonShape PolyShape = (PolygonShape)Sh;

                    Manifold m0 = new Manifold();
                    XForm xf1 = fakeBody.XForm;
                    XForm xf2 = PolyShape.Body.XForm;
                    Collision.CollidePolygons(ref m0, shapeBounds, ref xf1, PolyShape, ref xf2);

                    if( m0.PointCount > 0 )
                        return true;

                }
                else if( Sh.Type == ShapeType.CircleShape )
                {
                    CircleShape CircleShape = (CircleShape)Sh;
                    Manifold m0 = new Manifold();
                    Collision.CollidePolygonAndCircle( ref m0, shapeBounds, fakeBody.XForm, CircleShape, CircleShape.Body.XForm );
                    if( m0.PointCount > 0 )
                        return true;
                }
            }

            return false;
        }
Ejemplo n.º 21
0
		private static void Collide(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) { }
        // Polygon versus 2-sided edge.
        public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, Transform TransformA, EdgeShape edge, Transform TransformB)
        {
            PolygonShape polygonB = new PolygonShape();
            polygonB.SetAsEdge(edge._v1, edge._v2);

            CollidePolygons(ref manifold, polygon, TransformA, polygonB, TransformB);
        }
Ejemplo n.º 23
0
		private static void CollidePolygons(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
		{
			Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2);
		}
        // 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 CollidePolygons(ref Manifold manifold,
			PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
        {
            manifold.PointCount = 0;

            int edgeA = 0;
            float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
            if (separationA > 0.0f)
                return;

            int edgeB = 0;
            float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
            if (separationB > 0.0f)
                return;

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

            // TODO_ERIN use "radius" of poly for absolute tolerance.
            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1 = polyB;
                poly2 = polyA;
                xf1 = xfB;
                xf2 = xfA;
                edge1 = edgeB;
                flip = 1;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1 = xfA;
                xf2 = xfB;
                edge1 = edgeA;
                flip = 0;
            }

            ClipVertex[] incidentEdge;
            Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);

            int count1 = poly1.VertexCount;
            Vec2[] vertices1 = poly1.GetVertices();

            Vec2 v11 = vertices1[edge1];
            Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];

            Vec2 dv = v12 - v11;
            Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11);
            sideNormal.Normalize();
            Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f);

            v11 = Common.Math.Mul(xf1, v11);
            v12 = Common.Math.Mul(xf1, v12);

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

            // Clip incident edge against extruded edge1 side edges.
            ClipVertex[] clipPoints1;
            ClipVertex[] clipPoints2;
            int np;

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

            if (np < 2)
                return;

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

            if (np < 2)
                return;

            // Now clipPoints2 contains the clipped points.
            manifold.Normal = flip!=0 ? -frontNormal : frontNormal;

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

                if (separation <= 0.0f)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    cp.Separation = separation;
                    cp.LocalPoint1 = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V);
                    cp.LocalPoint2 = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V);
                    cp.ID = clipPoints2[i].ID;
                    cp.ID.Features.Flip = flip;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
Ejemplo n.º 25
0
        public Contact(Fixture fixtureA, Fixture fixtureB)
        {
            Flags = 0;

            if (fixtureA.IsSensor || fixtureB.IsSensor)
            {
                Flags |= ContactFlag.SensorFlag;
            }

            Body bodyA = fixtureA.GetBody();
            Body bodyB = fixtureB.GetBody();

            if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
            {
                Flags |= ContactFlag.ContinuousFlag;
            }
            else
            {
                Flags &= ~ContactFlag.ContinuousFlag;
            }

            _fixtureA = fixtureA;
            _fixtureB = fixtureB;

            Manifold = new Manifold();
            Manifold.PointCount = 0;

            Prev = null;
            Next = null;

            NodeA = new ContactEdge();
            NodeA.Contact = null;
            NodeA.Prev = null;
            NodeA.Next = null;
            NodeA.Other = null;

            NodeB = new ContactEdge();
            NodeB.Contact = null;
            NodeB.Prev = null;
            NodeB.Next = null;
            NodeB.Other = null;
        }
Ejemplo n.º 26
0
 public void PreSolve(Contact contact, Manifold oldManifold)
 {
     //eh
 }
Ejemplo n.º 27
0
        public void Update(ContactListener listener)
        {
            //Note: Manifold is a class, not a struct. It will reference the old manifest, not copy it - DONE
            Manifold oldManifold = new Manifold();
            oldManifold.LocalPlaneNormal = Manifold.LocalPlaneNormal;
            oldManifold.LocalPoint = Manifold.LocalPoint;
            oldManifold.PointCount = Manifold.PointCount;
            oldManifold.Points = Manifold.Points;
            oldManifold.Type = Manifold.Type;

            // Re-enable this contact.
            Flags &= ~ContactFlag.DisabledFlag;

            if (Collision.Collision.TestOverlap(_fixtureA.Aabb, _fixtureB.Aabb))
            {
                Evaluate();
            }
            else
            {
                Manifold.PointCount = 0;
            }

            Body bodyA = _fixtureA.GetBody();
            Body bodyB = _fixtureB.GetBody();

            int oldCount = oldManifold.PointCount;
            int newCount = Manifold.PointCount;

            if (newCount == 0 && oldCount > 0)
            {
                bodyA.WakeUp();
                bodyB.WakeUp();
            }

            // Slow contacts don't generate TOI events.
            if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
            {
                Flags |= ContactFlag.ContinuousFlag;
            }
            else
            {
                Flags &= ~ContactFlag.ContinuousFlag;
            }

            // Match old contact ids to new contact ids and copy the
            // stored impulses to warm start the solver.
            for (int i = 0; i < Manifold.PointCount; ++i)
            {
                ManifoldPoint mp2 = Manifold.Points[i];
                mp2.NormalImpulse = 0.0f;
                mp2.TangentImpulse = 0.0f;
                ContactID id2 = mp2.ID;

                for (int j = 0; j < oldManifold.PointCount; ++j)
                {
                    ManifoldPoint mp1 = oldManifold.Points[j];

                    if (mp1.ID.Key == id2.Key)
                    {
                        mp2.NormalImpulse = mp1.NormalImpulse;
                        mp2.TangentImpulse = mp1.TangentImpulse;
                        break;
                    }
                }
            }

            if (newCount > 0)
            {
                Flags |= ContactFlag.TouchingFlag;
            }
            else
            {
                Flags &= ~ContactFlag.TouchingFlag;
            }

            if (oldCount == 0 && newCount > 0)
            {
                listener.BeginContact(this);
            }

            if (oldCount > 0 && newCount == 0)
            {
                listener.EndContact(this);
            }

            if ((Flags & ContactFlag.SensorFlag) == 0)
            {
                listener.PreSolve(this, oldManifold);
            }
        }
Ejemplo n.º 28
0
 /// This is called after a contact is updated. This allows you to inspect a
 /// contact before it goes to the solver. If you are careful, you can modify the
 /// contact manifold (e.g. disable contact).
 /// A copy of the old manifold is provided so that you can detect changes.
 /// Note: this is called only for awake bodies.
 /// Note: this is called even when the number of contact points is zero.
 /// Note: this is not called for sensors.
 /// Note: if you set the number of contact points to zero, you will not
 /// get an EndContact callback. However, you may get a BeginContact callback
 /// the next step.
 public virtual void PreSolve(Contact contact, Manifold oldManifold)
 {
     //B2_NOT_USED(contact);
     //B2_NOT_USED(oldManifold);
 }
Ejemplo n.º 29
0
		public static void CollidePolygonAndCircle(ref Manifold manifold,
			PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
		{
			manifold.PointCount = 0;

			// Compute circle position in the frame of the polygon.
			Vec2 c = Common.Math.Mul(xf2, circle._position);
			Vec2 cLocal = Common.Math.MulT(xf1, c);

			// Find the min separating edge.
			int normalIndex = 0;
			float separation = -Settings.FLT_MAX;
			float radius = polygon._radius + circle._radius;
			int vertexCount = polygon._vertexCount;
			Vec2[] vertices = polygon._vertices;
			Vec2[] normals = polygon._normals;

			for (int i = 0; i < vertexCount; ++i)
			{
				float s = Vec2.Dot(normals[i], cLocal - 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;
			Vec2 v1 = vertices[vertIndex1];
			Vec2 v2 = vertices[vertIndex2];

			// If the center is inside the polygon ...
			if (separation < Common.Settings.FLT_EPSILON)
			{
				manifold.PointCount = 1;
				manifold.Type = ManifoldType.FaceA;
				manifold.LocalPlaneNormal = normals[normalIndex];
				manifold.LocalPoint = 0.5f * (v1 + v2);
				manifold.Points[0].LocalPoint = circle._position;
				manifold.Points[0].ID.Key = 0;
				return;
			}

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

				manifold.PointCount = 1;
				manifold.Type = ManifoldType.FaceA;
				manifold.LocalPlaneNormal = cLocal - v1;
				manifold.LocalPlaneNormal.Normalize();
				manifold.LocalPoint = v1;
				manifold.Points[0].LocalPoint = circle._position;
				manifold.Points[0].ID.Key = 0;
			}
			else if (u2 <= 0.0f)
			{
				if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
				{
					return;
				}

				manifold.PointCount = 1;
				manifold.Type = ManifoldType.FaceA;
				manifold.LocalPlaneNormal = cLocal - v2;
				manifold.LocalPlaneNormal.Normalize();
				manifold.LocalPoint = v2;
				manifold.Points[0].LocalPoint = circle._position;
				manifold.Points[0].ID.Key = 0;
			}
			else
			{
				Vec2 faceCenter = 0.5f * (v1 + v2);
				float separation_ = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]);
				if (separation_ > radius)
				{
					return;
				}

				manifold.PointCount = 1;
				manifold.Type = ManifoldType.FaceA;
				manifold.LocalPlaneNormal = normals[vertIndex1];
				manifold.LocalPoint = faceCenter;
				manifold.Points[0].LocalPoint = circle._position;
				manifold.Points[0].ID.Key = 0;
			}
		}
Ejemplo n.º 30
0
		private static void CollideCircles(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
		{
			Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2);
		}
Ejemplo n.º 31
0
        /// 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.
        public void Initialize(Manifold manifold, XForm xfA, float radiusA, XForm xfB, float radiusB)
        {
            if (manifold.PointCount == 0)
            {
                return;
            }

            switch (manifold.Type)
            {
            case ManifoldType.Circles:
            {
                Vec2 pointA = Common.Math.Mul(xfA, manifold.LocalPoint);
                Vec2 pointB = Common.Math.Mul(xfB, manifold.Points[0].LocalPoint);
                Vec2 normal = new Vec2(1.0f, 0.0f);
                if (Vec2.DistanceSquared(pointA, pointB) > Common.Settings.FLT_EPSILON_SQUARED)
                {
                    normal = pointB - pointA;
                    normal.Normalize();
                }

                Normal = normal;

                Vec2 cA = pointA + radiusA * normal;
                Vec2 cB = pointB - radiusB * normal;
                Points[0] = 0.5f * (cA + cB);
            }
            break;

            case ManifoldType.FaceA:
            {
                Vec2 normal     = Common.Math.Mul(xfA.R, manifold.LocalPlaneNormal);
                Vec2 planePoint = Common.Math.Mul(xfA, manifold.LocalPoint);

                // Ensure normal points from A to B.
                Normal = normal;

                for (int i = 0; i < manifold.PointCount; ++i)
                {
                    Vec2 clipPoint = Common.Math.Mul(xfB, manifold.Points[i].LocalPoint);
                    Vec2 cA        = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
                    Vec2 cB        = clipPoint - radiusB * normal;
                    Points[i] = 0.5f * (cA + cB);
                }
            }
            break;

            case ManifoldType.FaceB:
            {
                Vec2 normal     = Common.Math.Mul(xfB.R, manifold.LocalPlaneNormal);
                Vec2 planePoint = Common.Math.Mul(xfB, manifold.LocalPoint);

                // Ensure normal points from A to B.
                Normal = -normal;

                for (int i = 0; i < manifold.PointCount; ++i)
                {
                    Vec2 clipPoint = Common.Math.Mul(xfA, manifold.Points[i].LocalPoint);
                    Vec2 cA        = clipPoint - radiusA * normal;
                    Vec2 cB        = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
                    Points[i] = 0.5f * (cA + cB);
                }
            }
            break;
            }
        }