Ejemplo n.º 1
0
		// Algorithm:
		// 1. Classify v1 and v2
		// 2. Classify polygon centroid as front or back
		// 3. Flip normal if necessary
		// 4. Initialize normal range to [-pi, pi] about face normal
		// 5. Adjust normal range according to adjacent edges
		// 6. Visit each separating axes, only accept axes within the range
		// 7. Return if _any_ axis indicates separation
		// 8. Clip
		public void Collide(out Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB){
			manifold = new Manifold();
			m_xf = Utilities.MulT(xfA, xfB);
	
			m_centroidB = Utilities.Mul(m_xf, polygonB.m_centroid);
	
			m_v0 = edgeA.m_vertex0;
			m_v1 = edgeA.m_vertex1;
			m_v2 = edgeA.m_vertex2;
			m_v3 = edgeA.m_vertex3;
	
			bool hasVertex0 = edgeA.m_hasVertex0;
			bool hasVertex3 = edgeA.m_hasVertex3;
	
			Vec2 edge1 = m_v2 - m_v1;
			edge1.Normalize();
			m_normal1.Set(edge1.Y, -edge1.X);
			float offset1 = Utilities.Dot(m_normal1, m_centroidB - m_v1);
			float offset0 = 0.0f, offset2 = 0.0f;
			bool convex1 = false, convex2 = false;
	
			// Is there a preceding edge?
			if (hasVertex0)
			{
				Vec2 edge0 = m_v1 - m_v0;
				edge0.Normalize();
				m_normal0.Set(edge0.Y, -edge0.X);
				convex1 = Utilities.Cross(edge0, edge1) >= 0.0f;
				offset0 = Utilities.Dot(m_normal0, m_centroidB - m_v0);
			}
	
			// Is there a following edge?
			if (hasVertex3)
			{
				Vec2 edge2 = m_v3 - m_v2;
				edge2.Normalize();
				m_normal2.Set(edge2.Y, -edge2.X);
				convex2 = Utilities.Cross(edge1, edge2) > 0.0f;
				offset2 = Utilities.Dot(m_normal2, m_centroidB - m_v2);
			}
	
			// Determine front or back collision. Determine collision normal limits.
			if (hasVertex0 && hasVertex3)
			{
				if (convex1 && convex2)
				{
					m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal0;
						m_upperLimit = m_normal2;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = -m_normal1;
					}
				}
				else if (convex1)
				{
					m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal0;
						m_upperLimit = m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal2;
						m_upperLimit = -m_normal1;
					}
				}
				else if (convex2)
				{
					m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = m_normal2;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = -m_normal0;
					}
				}
				else
				{
					m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal2;
						m_upperLimit = -m_normal0;
					}
				}
			}
			else if (hasVertex0)
			{
				if (convex1)
				{
					m_front = offset0 >= 0.0f || offset1 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal0;
						m_upperLimit = -m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = -m_normal1;
					}
				}
				else
				{
					m_front = offset0 >= 0.0f && offset1 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = -m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = m_normal1;
						m_upperLimit = -m_normal0;
					}
				}
			}
			else if (hasVertex3)
			{
				if (convex2)
				{
					m_front = offset1 >= 0.0f || offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = m_normal2;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = m_normal1;
					}
				}
				else
				{
					m_front = offset1 >= 0.0f && offset2 >= 0.0f;
					if (m_front)
					{
						m_normal = m_normal1;
						m_lowerLimit = -m_normal1;
						m_upperLimit = m_normal1;
					}
					else
					{
						m_normal = -m_normal1;
						m_lowerLimit = -m_normal2;
						m_upperLimit = m_normal1;
					}
				}		
			}
			else
			{
				m_front = offset1 >= 0.0f;
				if (m_front)
				{
					m_normal = m_normal1;
					m_lowerLimit = -m_normal1;
					m_upperLimit = -m_normal1;
				}
				else
				{
					m_normal = -m_normal1;
					m_lowerLimit = m_normal1;
					m_upperLimit = m_normal1;
				}
			}
	
			// Get polygonB in frameA
			m_polygonB.count = polygonB.m_count;
			for (int i = 0; i < polygonB.m_count; ++i)
			{
				m_polygonB.vertices[i] = Utilities.Mul(m_xf, polygonB.m_vertices[i]);
				m_polygonB.normals[i] = Utilities.Mul(m_xf.q, polygonB.m_normals[i]);
			}
	
			m_radius = 2.0f * Settings._polygonRadius;

			manifold.points.Clear();
	
			EPAxis edgeAxis = ComputeEdgeSeparation();
	
			// If no valid normal can be found than this edge should not collide.
			if (edgeAxis.type == EPAxisType.e_unknown)
			{
				return;
			}
	
			if (edgeAxis.separation > m_radius)
			{
				return;
			}
	
			EPAxis polygonAxis = ComputePolygonSeparation();
			if (polygonAxis.type != EPAxisType.e_unknown && polygonAxis.separation > m_radius)
			{
				return;
			}
	
			// Use hysteresis for jitter reduction.
			const float k_relativeTol = 0.98f;
			const float k_absoluteTol = 0.001f;
	
			EPAxis primaryAxis;
			if (polygonAxis.type == EPAxisType.e_unknown)
			{
				primaryAxis = edgeAxis;
			}
			else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
			{
				primaryAxis = polygonAxis;
			}
			else
			{
				primaryAxis = edgeAxis;
			}
	
			ClipVertex[] ie = new ClipVertex[2];
			ReferenceFace rf = new ReferenceFace();
			if (primaryAxis.type == EPAxisType.e_edgeA)
			{
				manifold.type = Manifold.ManifoldType.e_faceA;
		
				// Search for the polygon normal that is most anti-parallel to the edge normal.
				int bestIndex = 0;
				float bestValue = Utilities.Dot(m_normal, m_polygonB.normals[0]);
				for (int i = 1; i < m_polygonB.count; ++i)
				{
					float value = Utilities.Dot(m_normal, m_polygonB.normals[i]);
					if (value < bestValue)
					{
						bestValue = value;
						bestIndex = i;
					}
				}
		
				int i1 = bestIndex;
				int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
		
				ie[0].v = m_polygonB.vertices[i1];
				ie[0].id.cf.indexA = 0;
				ie[0].id.cf.indexB = (byte)i1;
				ie[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
				ie[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
		
				ie[1].v = m_polygonB.vertices[i2];
				ie[1].id.cf.indexA = 0;
				ie[1].id.cf.indexB = (byte)i2;
				ie[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
				ie[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
		
				if (m_front)
				{
					rf.i1 = 0;
					rf.i2 = 1;
					rf.v1 = m_v1;
					rf.v2 = m_v2;
					rf.normal = m_normal1;
				}
				else
				{
					rf.i1 = 1;
					rf.i2 = 0;
					rf.v1 = m_v2;
					rf.v2 = m_v1;
					rf.normal = -m_normal1;
				}		
			}
			else
			{
				manifold.type = Manifold.ManifoldType.e_faceB;
		
				ie[0].v = m_v1;
				ie[0].id.cf.indexA = 0;
				ie[0].id.cf.indexB = (byte)primaryAxis.index;
				ie[0].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
				ie[0].id.cf.typeB = ContactFeature.FeatureType.e_face;
		
				ie[1].v = m_v2;
				ie[1].id.cf.indexA = 0;
				ie[1].id.cf.indexB = (byte)primaryAxis.index;		
				ie[1].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
				ie[1].id.cf.typeB = ContactFeature.FeatureType.e_face;
		
				rf.i1 = primaryAxis.index;
				rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
				rf.v1 = m_polygonB.vertices[rf.i1];
				rf.v2 = m_polygonB.vertices[rf.i2];
				rf.normal = m_polygonB.normals[rf.i1];
			}
	
			rf.sideNormal1.Set(rf.normal.Y, -rf.normal.X);
			rf.sideNormal2 = -rf.sideNormal1;
			rf.sideOffset1 = Utilities.Dot(rf.sideNormal1, rf.v1);
			rf.sideOffset2 = Utilities.Dot(rf.sideNormal2, rf.v2);
	
			// Clip incident edge against extruded edge1 side edges.
			ClipVertex[] clipPoints1 = new ClipVertex[2];
			ClipVertex[] clipPoints2 = new ClipVertex[2];
			int np;
	
			// Clip to box side 1
			np = Collision.ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
	
			if (np < Settings._maxManifoldPoints)
			{
				return;
			}
	
			// Clip to negative box side 1
			np = Collision.ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
	
			if (np < Settings._maxManifoldPoints)
			{
				return;
			}
	
			// Now clipPoints2 contains the clipped points.
			if (primaryAxis.type == EPAxisType.e_edgeA)
			{
				manifold.localNormal = rf.normal;
				manifold.localPoint = rf.v1;
			}
			else
			{
				manifold.localNormal = polygonB.m_normals[rf.i1];
				manifold.localPoint = polygonB.m_vertices[rf.i1];
			}

			manifold.points.Clear();
			for (int i = 0; i < Settings._maxManifoldPoints; ++i)
			{
				float separation;
		
				separation = Utilities.Dot(rf.normal, clipPoints2[i].v - rf.v1);
		
				if (separation <= m_radius)
				{
					ManifoldPoint cp = new ManifoldPoint();
			
					if (primaryAxis.type == EPAxisType.e_edgeA)
					{
						cp.localPoint = Utilities.MulT(m_xf, clipPoints2[i].v);
						cp.id = clipPoints2[i].id;
					}
					else
					{
						cp.localPoint = clipPoints2[i].v;
						cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
						cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
						cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
						cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
					}

					manifold.points.Add(cp);
				}
			}
		}
Ejemplo n.º 2
0
		/// Clipping for contact manifolds.
		// Sutherland-Hodgman clipping.
		public static int ClipSegmentToLine(ClipVertex[/*2*/] vOut, ClipVertex[/*2*/] vIn,
									Vec2 normal, float offset, int vertexIndexA){
			// Start with no output points
			int numOut = 0;

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

				// VertexA is hitting edgeB.
				vOut[numOut].id.cf.indexA = (byte)vertexIndexA;
				vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
				vOut[numOut].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
				vOut[numOut].id.cf.typeB = ContactFeature.FeatureType.e_face;
				++numOut;
			}

			return numOut;
		}
Ejemplo n.º 3
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
		/// Compute the collision manifold between two polygons.
		public static void CollidePolygons(out Manifold manifold,
							   PolygonShape polyA, Transform xfA,
							   PolygonShape polyB, Transform xfB) {
			manifold = new Manifold();
			float totalRadius = polyA.m_radius + polyB.m_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 polygon
			PolygonShape poly2;	// incident polygon
			Transform xf1, xf2;
			int edge1;		// reference edge
			bool 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.e_faceB;
			    flip = true;
			}
			else
			{
			    poly1 = polyA;
			    poly2 = polyB;
			    xf1 = xfA;
			    xf2 = xfB;
			    edge1 = edgeA;
			    manifold.type = Manifold.ManifoldType.e_faceA;
			    flip = false;
			}

			ClipVertex[] incidentEdge = new ClipVertex[2];
			FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);

			int count1 = poly1.m_count;
			Vec2[] vertices1 = poly1.m_vertices;

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

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

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

			Vec2 tangent = Utilities.Mul(xf1.q, localTangent);
			Vec2 normal = Utilities.Cross(tangent, 1.0f);
	
			v11 = Utilities.Mul(xf1, v11);
			v12 = Utilities.Mul(xf1, v12);

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

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

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

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

			if (np < 2)
			    return;

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

			if (np < 2)
			{
			    return;
			}

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

			manifold.points.Clear();
			for (int i = 0; i < Settings._maxManifoldPoints; ++i)
			{
			    float separation = Utilities.Dot(normal, clipPoints2[i].v) - frontOffset;

			    if (separation <= totalRadius)
			    {
					ManifoldPoint cp = new ManifoldPoint();
			        cp.localPoint = Utilities.MulT(xf2, clipPoints2[i].v);
			        cp.id = clipPoints2[i].id;
			        if (flip)
			        {
			            // Swap features
			            ContactFeature cf = cp.id.cf;
			            cp.id.cf.indexA = cf.indexB;
			            cp.id.cf.indexB = cf.indexA;
			            cp.id.cf.typeA = cf.typeB;
			            cp.id.cf.typeB = cf.typeA;
			        }
					manifold.points.Add(cp);
			    }
			}
		}
Ejemplo n.º 4
0
		static void FindIncidentEdge(ClipVertex[/*2*/] c,
							 PolygonShape poly1, Transform xf1, int edge1,
							 PolygonShape poly2, Transform xf2)
		{
			Vec2[] normals1 = poly1.m_normals;

			int count2 = poly2.m_count;
			Vec2[] vertices2 = poly2.m_vertices;
			Vec2[] normals2 = poly2.m_normals;

			Utilities.Assert(0 <= edge1 && edge1 < poly1.m_count);

			// Get the normal of the reference edge in poly2's frame.
			Vec2 normal1 = Utilities.MulT(xf2.q, Utilities.Mul(xf1.q, normals1[edge1]));

			// Find the incident edge on poly2.
			int index = 0;
			float minDot = Single.MaxValue;
			for (int i = 0; i < count2; ++i)
			{
				float dot = Utilities.Dot(normal1, normals2[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;

			c[0].v = Utilities.Mul(xf2, vertices2[i1]);
			c[0].id.cf.indexA = (byte)edge1;
			c[0].id.cf.indexB = (byte)i1;
			c[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
			c[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;

			c[1].v = Utilities.Mul(xf2, vertices2[i2]);
			c[1].id.cf.indexA = (byte)edge1;
			c[1].id.cf.indexB = (byte)i2;
			c[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
			c[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
		}
Ejemplo n.º 5
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
        /// Compute the collision manifold between two polygons.
        public static void CollidePolygons(out Manifold manifold,
                                           PolygonShape polyA, Transform xfA,
                                           PolygonShape polyB, Transform xfB)
        {
            manifold = new Manifold();
            float totalRadius = polyA.m_radius + polyB.m_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 polygon
            PolygonShape poly2;                 // incident polygon
            Transform    xf1, xf2;
            int          edge1;                 // reference edge
            bool         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.e_faceB;
                flip          = true;
            }
            else
            {
                poly1         = polyA;
                poly2         = polyB;
                xf1           = xfA;
                xf2           = xfB;
                edge1         = edgeA;
                manifold.type = Manifold.ManifoldType.e_faceA;
                flip          = false;
            }

            ClipVertex[] incidentEdge = new ClipVertex[2];
            FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);

            int count1 = poly1.m_count;

            Vec2[] vertices1 = poly1.m_vertices;

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

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

            Vec2 localTangent = v12 - v11;

            localTangent.Normalize();

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

            Vec2 tangent = Utilities.Mul(xf1.q, localTangent);
            Vec2 normal  = Utilities.Cross(tangent, 1.0f);

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

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

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

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

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

            if (np < 2)
            {
                return;
            }

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

            if (np < 2)
            {
                return;
            }

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

            manifold.points.Clear();
            for (int i = 0; i < Settings._maxManifoldPoints; ++i)
            {
                float separation = Utilities.Dot(normal, clipPoints2[i].v) - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = new ManifoldPoint();
                    cp.localPoint = Utilities.MulT(xf2, clipPoints2[i].v);
                    cp.id         = clipPoints2[i].id;
                    if (flip)
                    {
                        // Swap features
                        ContactFeature cf = cp.id.cf;
                        cp.id.cf.indexA = cf.indexB;
                        cp.id.cf.indexB = cf.indexA;
                        cp.id.cf.typeA  = cf.typeB;
                        cp.id.cf.typeB  = cf.typeA;
                    }
                    manifold.points.Add(cp);
                }
            }
        }
Ejemplo n.º 6
0
        // Algorithm:
        // 1. Classify v1 and v2
        // 2. Classify polygon centroid as front or back
        // 3. Flip normal if necessary
        // 4. Initialize normal range to [-pi, pi] about face normal
        // 5. Adjust normal range according to adjacent edges
        // 6. Visit each separating axes, only accept axes within the range
        // 7. Return if _any_ axis indicates separation
        // 8. Clip
        public void Collide(out Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
        {
            manifold = new Manifold();
            m_xf     = Utilities.MulT(xfA, xfB);

            m_centroidB = Utilities.Mul(m_xf, polygonB.m_centroid);

            m_v0 = edgeA.m_vertex0;
            m_v1 = edgeA.m_vertex1;
            m_v2 = edgeA.m_vertex2;
            m_v3 = edgeA.m_vertex3;

            bool hasVertex0 = edgeA.m_hasVertex0;
            bool hasVertex3 = edgeA.m_hasVertex3;

            Vec2 edge1 = m_v2 - m_v1;

            edge1.Normalize();
            m_normal1.Set(edge1.Y, -edge1.X);
            float offset1 = Utilities.Dot(m_normal1, m_centroidB - m_v1);
            float offset0 = 0.0f, offset2 = 0.0f;
            bool  convex1 = false, convex2 = false;

            // Is there a preceding edge?
            if (hasVertex0)
            {
                Vec2 edge0 = m_v1 - m_v0;
                edge0.Normalize();
                m_normal0.Set(edge0.Y, -edge0.X);
                convex1 = Utilities.Cross(edge0, edge1) >= 0.0f;
                offset0 = Utilities.Dot(m_normal0, m_centroidB - m_v0);
            }

            // Is there a following edge?
            if (hasVertex3)
            {
                Vec2 edge2 = m_v3 - m_v2;
                edge2.Normalize();
                m_normal2.Set(edge2.Y, -edge2.X);
                convex2 = Utilities.Cross(edge1, edge2) > 0.0f;
                offset2 = Utilities.Dot(m_normal2, m_centroidB - m_v2);
            }

            // Determine front or back collision. Determine collision normal limits.
            if (hasVertex0 && hasVertex3)
            {
                if (convex1 && convex2)
                {
                    m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = m_normal0;
                        m_upperLimit = m_normal2;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = -m_normal1;
                        m_upperLimit = -m_normal1;
                    }
                }
                else if (convex1)
                {
                    m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = m_normal0;
                        m_upperLimit = m_normal1;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = -m_normal2;
                        m_upperLimit = -m_normal1;
                    }
                }
                else if (convex2)
                {
                    m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = m_normal1;
                        m_upperLimit = m_normal2;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = -m_normal1;
                        m_upperLimit = -m_normal0;
                    }
                }
                else
                {
                    m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = m_normal1;
                        m_upperLimit = m_normal1;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = -m_normal2;
                        m_upperLimit = -m_normal0;
                    }
                }
            }
            else if (hasVertex0)
            {
                if (convex1)
                {
                    m_front = offset0 >= 0.0f || offset1 >= 0.0f;
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = m_normal0;
                        m_upperLimit = -m_normal1;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = m_normal1;
                        m_upperLimit = -m_normal1;
                    }
                }
                else
                {
                    m_front = offset0 >= 0.0f && offset1 >= 0.0f;
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = m_normal1;
                        m_upperLimit = -m_normal1;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = m_normal1;
                        m_upperLimit = -m_normal0;
                    }
                }
            }
            else if (hasVertex3)
            {
                if (convex2)
                {
                    m_front = offset1 >= 0.0f || offset2 >= 0.0f;
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = -m_normal1;
                        m_upperLimit = m_normal2;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = -m_normal1;
                        m_upperLimit = m_normal1;
                    }
                }
                else
                {
                    m_front = offset1 >= 0.0f && offset2 >= 0.0f;
                    if (m_front)
                    {
                        m_normal     = m_normal1;
                        m_lowerLimit = -m_normal1;
                        m_upperLimit = m_normal1;
                    }
                    else
                    {
                        m_normal     = -m_normal1;
                        m_lowerLimit = -m_normal2;
                        m_upperLimit = m_normal1;
                    }
                }
            }
            else
            {
                m_front = offset1 >= 0.0f;
                if (m_front)
                {
                    m_normal     = m_normal1;
                    m_lowerLimit = -m_normal1;
                    m_upperLimit = -m_normal1;
                }
                else
                {
                    m_normal     = -m_normal1;
                    m_lowerLimit = m_normal1;
                    m_upperLimit = m_normal1;
                }
            }

            // Get polygonB in frameA
            m_polygonB.count = polygonB.m_count;
            for (int i = 0; i < polygonB.m_count; ++i)
            {
                m_polygonB.vertices[i] = Utilities.Mul(m_xf, polygonB.m_vertices[i]);
                m_polygonB.normals[i]  = Utilities.Mul(m_xf.q, polygonB.m_normals[i]);
            }

            m_radius = 2.0f * Settings._polygonRadius;

            manifold.points.Clear();

            EPAxis edgeAxis = ComputeEdgeSeparation();

            // If no valid normal can be found than this edge should not collide.
            if (edgeAxis.type == EPAxisType.e_unknown)
            {
                return;
            }

            if (edgeAxis.separation > m_radius)
            {
                return;
            }

            EPAxis polygonAxis = ComputePolygonSeparation();

            if (polygonAxis.type != EPAxisType.e_unknown && polygonAxis.separation > m_radius)
            {
                return;
            }

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

            EPAxis primaryAxis;

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

            ClipVertex[]  ie = new ClipVertex[2];
            ReferenceFace rf = new ReferenceFace();

            if (primaryAxis.type == EPAxisType.e_edgeA)
            {
                manifold.type = Manifold.ManifoldType.e_faceA;

                // Search for the polygon normal that is most anti-parallel to the edge normal.
                int   bestIndex = 0;
                float bestValue = Utilities.Dot(m_normal, m_polygonB.normals[0]);
                for (int i = 1; i < m_polygonB.count; ++i)
                {
                    float value = Utilities.Dot(m_normal, m_polygonB.normals[i]);
                    if (value < bestValue)
                    {
                        bestValue = value;
                        bestIndex = i;
                    }
                }

                int i1 = bestIndex;
                int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;

                ie[0].v            = m_polygonB.vertices[i1];
                ie[0].id.cf.indexA = 0;
                ie[0].id.cf.indexB = (byte)i1;
                ie[0].id.cf.typeA  = ContactFeature.FeatureType.e_face;
                ie[0].id.cf.typeB  = ContactFeature.FeatureType.e_vertex;

                ie[1].v            = m_polygonB.vertices[i2];
                ie[1].id.cf.indexA = 0;
                ie[1].id.cf.indexB = (byte)i2;
                ie[1].id.cf.typeA  = ContactFeature.FeatureType.e_face;
                ie[1].id.cf.typeB  = ContactFeature.FeatureType.e_vertex;

                if (m_front)
                {
                    rf.i1     = 0;
                    rf.i2     = 1;
                    rf.v1     = m_v1;
                    rf.v2     = m_v2;
                    rf.normal = m_normal1;
                }
                else
                {
                    rf.i1     = 1;
                    rf.i2     = 0;
                    rf.v1     = m_v2;
                    rf.v2     = m_v1;
                    rf.normal = -m_normal1;
                }
            }
            else
            {
                manifold.type = Manifold.ManifoldType.e_faceB;

                ie[0].v            = m_v1;
                ie[0].id.cf.indexA = 0;
                ie[0].id.cf.indexB = (byte)primaryAxis.index;
                ie[0].id.cf.typeA  = ContactFeature.FeatureType.e_vertex;
                ie[0].id.cf.typeB  = ContactFeature.FeatureType.e_face;

                ie[1].v            = m_v2;
                ie[1].id.cf.indexA = 0;
                ie[1].id.cf.indexB = (byte)primaryAxis.index;
                ie[1].id.cf.typeA  = ContactFeature.FeatureType.e_vertex;
                ie[1].id.cf.typeB  = ContactFeature.FeatureType.e_face;

                rf.i1     = primaryAxis.index;
                rf.i2     = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
                rf.v1     = m_polygonB.vertices[rf.i1];
                rf.v2     = m_polygonB.vertices[rf.i2];
                rf.normal = m_polygonB.normals[rf.i1];
            }

            rf.sideNormal1.Set(rf.normal.Y, -rf.normal.X);
            rf.sideNormal2 = -rf.sideNormal1;
            rf.sideOffset1 = Utilities.Dot(rf.sideNormal1, rf.v1);
            rf.sideOffset2 = Utilities.Dot(rf.sideNormal2, rf.v2);

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

            // Clip to box side 1
            np = Collision.ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);

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

            // Clip to negative box side 1
            np = Collision.ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);

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

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.type == EPAxisType.e_edgeA)
            {
                manifold.localNormal = rf.normal;
                manifold.localPoint  = rf.v1;
            }
            else
            {
                manifold.localNormal = polygonB.m_normals[rf.i1];
                manifold.localPoint  = polygonB.m_vertices[rf.i1];
            }

            manifold.points.Clear();
            for (int i = 0; i < Settings._maxManifoldPoints; ++i)
            {
                float separation;

                separation = Utilities.Dot(rf.normal, clipPoints2[i].v - rf.v1);

                if (separation <= m_radius)
                {
                    ManifoldPoint cp = new ManifoldPoint();

                    if (primaryAxis.type == EPAxisType.e_edgeA)
                    {
                        cp.localPoint = Utilities.MulT(m_xf, clipPoints2[i].v);
                        cp.id         = clipPoints2[i].id;
                    }
                    else
                    {
                        cp.localPoint   = clipPoints2[i].v;
                        cp.id.cf.typeA  = clipPoints2[i].id.cf.typeB;
                        cp.id.cf.typeB  = clipPoints2[i].id.cf.typeA;
                        cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
                        cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
                    }

                    manifold.points.Add(cp);
                }
            }
        }