Beispiel #1
0
        public static bool DoOverlapTest(CollisionFunctor cf, SpherePart a, PolyhedronPart b, Vector3 offset)
        {
            Vector3 pa, pb, normal, v, center;
            float   depth, r2 = a.World.Radius * a.World.Radius;
            bool    foundAny = false;

            for (int i = 0; i < b.FaceCount; i++)
            {
                b.World(b.Face(i)[0], out v);
                b.FaceNormal(i, out normal);
                var plane = new Plane(v, normal);

                Vector3.Add(ref a.World.Center, ref offset, out center);
                Vector3.Subtract(ref center, ref v, out v);
                Vector3.Dot(ref normal, ref v, out depth);

                if (depth < 0f || depth - a.World.Radius >= Constants.Epsilon)
                {
                    continue;
                }

                plane.ClosestPointTo(ref center, out pb);
                if (b.IsPointOnFace(i, ref pb, true))
                {
                    Vector3.Subtract(ref pb, ref center, out v);
                    if (v.LengthSquared() - r2 < Constants.Epsilon)
                    {
                        Vector3.Multiply(ref normal, -a.World.Radius, out pa);
                        Vector3.Add(ref a.World.Center, ref pa, out pa);
                        cf.WritePoint(ref pa, ref pb, ref normal);
                        return(true);
                    }
                }
                else
                {
                    int[] face = b.Face(i);
                    for (int j = 0; j < face.Length; j++)
                    {
                        float   s;
                        Segment edge;
                        b.World(face[j == 0 ? face.Length - 1 : j - 1], out edge.P1);
                        b.World(face[j], out edge.P2);
                        edge.ClosestPointTo(ref center, out s, out pb);
                        Vector3.Subtract(ref center, ref pb, out normal);
                        if (normal.LengthSquared() - r2 < Constants.Epsilon)
                        {
                            normal.Normalize();
                            Vector3.Multiply(ref normal, -a.World.Radius, out pa);
                            Vector3.Add(ref a.World.Center, ref pa, out pa);
                            cf.WritePoint(ref pa, ref pb, ref normal);
                            foundAny = true;
                        }
                    }
                }
            }
            return(foundAny);
        }
		public static bool DoOverlapTest(CollisionFunctor cf, SpherePart a, PolyhedronPart b, Vector3 offset)
		{
			Vector3 pa, pb, normal, v, center;
			float depth, r2 = a.World.Radius * a.World.Radius;
			bool foundAny = false;

			for (int i = 0; i < b.FaceCount; i++)
			{
				b.World(b.Face(i)[0], out v);
				b.FaceNormal(i, out normal);
				var plane = new Plane(v, normal);

				Vector3.Add(ref a.World.Center, ref offset, out center);
				Vector3.Subtract(ref center, ref v, out v);
				Vector3.Dot(ref normal, ref v, out depth);

				if (depth < 0f || depth - a.World.Radius >= Constants.Epsilon)
					continue;

				plane.ClosestPointTo(ref center, out pb);
				if (b.IsPointOnFace(i, ref pb, true))
				{
					Vector3.Subtract(ref pb, ref center, out v);
					if (v.LengthSquared() - r2 < Constants.Epsilon)
					{
						Vector3.Multiply(ref normal, -a.World.Radius, out pa);
						Vector3.Add(ref a.World.Center, ref pa, out pa);
						cf.WritePoint(ref pa, ref pb, ref normal);
						return true;
					}
				}
				else
				{
					int[] face = b.Face(i);
					for (int j = 0; j < face.Length; j++)
					{
						float s;
						Segment edge;
						b.World(face[j == 0 ? face.Length - 1 : j - 1], out edge.P1);
						b.World(face[j], out edge.P2);
						edge.ClosestPointTo(ref center, out s, out pb);
						Vector3.Subtract(ref center, ref pb, out normal);
						if (normal.LengthSquared() - r2 < Constants.Epsilon)
						{
							normal.Normalize();
							Vector3.Multiply(ref normal, -a.World.Radius, out pa);
							Vector3.Add(ref a.World.Center, ref pa, out pa);
							cf.WritePoint(ref pa, ref pb, ref normal);
							foundAny = true;
						}
					}
				}
			}
			return foundAny;
		}
        private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
        {
            int[] faceA = a.Face(ci.FeatureA.Index);
            int[] faceB = b.Face(ci.FeatureB.Index);

            Vector3 pa, pb, n;

            a.World(faceA[0], out pa);
            a.FaceNormal(ci.FeatureA.Index, out n);
            Plane planeA = new Plane(pa, n);

            b.World(faceB[0], out pb);
            Plane planeB = new Plane(pb, ci.Normal);

            // vertices of A contained in face of B
            for (int i = 0; i < faceA.Length; i++)
            {
                a.World(faceA[i], out pa);
                planeB.ClosestPointTo(ref pa, out pb);
                if (b.IsPointOnFace(ci.FeatureB.Index, ref pb, true))
                {
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }

            // vertices of B contained in face of A
            for (int i = 0; i < faceB.Length; i++)
            {
                b.World(faceB[i], out pb);
                planeA.ClosestPointTo(ref pb, out pa);
                if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
                {
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }

            // intersections of edges from both faces
            Segment ea, eb;

            for (int i = 0; i < faceA.Length; i++)
            {
                a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
                a.World(faceA[i], out ea.P2);

                for (int j = 0; j < faceB.Length; j++)
                {
                    b.World(faceB[j == 0 ? faceB.Length - 1 : j - 1], out eb.P1);
                    b.World(faceB[j], out eb.P2);

                    float sa, sb;
                    Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
                    if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
                    {
                        cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                    }
                }
            }
        }
        private static void CalculateVertexFacePoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
        {
            Vector3 pa, pb;

            a.World(ci.FeatureA.Index, out pa);
            b.World(b.Face(ci.FeatureB.Index)[0], out pb);
            var plane = new Plane(pb, ci.Normal);

            plane.ClosestPointTo(ref pa, out pb);
            cf.WritePoint(ref pa, ref pb, ref ci.Normal);
        }
Beispiel #5
0
        private static void CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
        {
            Vector3 pa, pb;

            tri.Vertex(ci.FeatureB.Index, out pb);
            a.World(a.Face(ci.FeatureA.Index)[0], out pa);
            var plane = new Plane(pa, ci.Normal);

            plane.ClosestPointTo(ref pb, out pa);
            cf.WritePoint(ref pa, ref pb, ref ci.Normal);
        }
Beispiel #6
0
        private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
        {
            int[]   faceA = a.Face(ci.FeatureA.Index);
            Vector3 pa, pb, n;

            a.World(faceA[0], out pa);
            a.FaceNormal(ci.FeatureA.Index, out n);
            Plane planeA = new Plane(pa, n);
            Plane planeB = new Plane(tri.V1, tri.Normal);

            // vertices of A contained in face of B
            for (int i = 0; i < faceA.Length; i++)
            {
                a.World(faceA[i], out pa);
                planeB.ClosestPointTo(ref pa, out pb);
                if (tri.Contains(ref pb))
                {
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }

            for (int i = 1; i <= 3; i++)
            {
                tri.Vertex(i, out pb);
                planeA.ClosestPointTo(ref pb, out pa);
                if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
                {
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }

            // intersection of edges from both faces
            Segment ea, eb;

            for (int i = 0; i < faceA.Length; i++)
            {
                a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
                a.World(faceA[i], out ea.P2);

                for (int j = 1; j <= 3; j++)
                {
                    tri.Edge(j, out eb);

                    float sa, sb;
                    Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
                    if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
                    {
                        cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                    }
                }
            }
        }
Beispiel #7
0
        private static void DoOverlapTest(CollisionFunctor cf, PolyhedronPart a, PlanePart b, float offset)
        {
            Vector3 normalNeg;

            Vector3.Negate(ref b.Plane.Normal, out normalNeg);
            int vIndex = a.ExtremeVertex(ref normalNeg).Index;

            float   ax, bx;
            Vector3 p = b.Plane.P;

            if (offset > 0f)
            {
                Vector3.Multiply(ref b.Plane.Normal, offset, out p);
                Vector3.Add(ref b.Plane.P, ref p, out p);
            }

            Vector3.Dot(ref b.Plane.Normal, ref p, out bx);

            for (int i = 0; i < a.FaceCount; i++)
            {
                var  face = a.Face(i);
                bool skip = true;
                for (int j = 0; j < face.Length; j++)
                {
                    if (face[j] == vIndex)
                    {
                        skip = false;
                        break;
                    }
                }
                if (skip)
                {
                    continue;
                }

                for (int j = 0; j < face.Length; j++)
                {
                    Vector3 pa, pb;
                    a.World(face[j], out pa);
                    Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
                    if (ax - bx < Constants.Epsilon)
                    {
                        b.Plane.ClosestPointTo(ref pa, out pb);
                        cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
                    }
                }
            }
        }
		private static void DoOverlapTest(CollisionFunctor cf, PolyhedronPart a, PlanePart b, float offset)
		{
			Vector3 normalNeg;
			Vector3.Negate(ref b.Plane.Normal, out normalNeg);
			int vIndex = a.ExtremeVertex(ref normalNeg).Index;

			float ax, bx;
			Vector3 p = b.Plane.P;
			if(offset > 0f)
			{
				Vector3.Multiply(ref b.Plane.Normal, offset, out p);
				Vector3.Add(ref b.Plane.P, ref p, out p);
			}

			Vector3.Dot(ref b.Plane.Normal, ref p, out bx);

			for (int i = 0; i < a.FaceCount; i++)
			{
				var face = a.Face(i);
				bool skip = true;
				for (int j = 0; j < face.Length; j++)
				{
					if (face[j] == vIndex)
					{
						skip = false;
						break;
					}
				}
				if (skip)
					continue;

				for (int j = 0; j < face.Length; j++)
				{
					Vector3 pa, pb;
					a.World(face[j], out pa);
					Vector3.Dot(ref b.Plane.Normal, ref pa, out ax);
					if (ax - bx < Constants.Epsilon)
					{
						b.Plane.ClosestPointTo(ref pa, out pb);
						cf.WritePoint(ref pa, ref pb, ref b.Plane.Normal);
					}
				}
			}
		}
        private static void CalculateEdgeFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
        {
            Vector3 pa, pb;
            Segment ea, eb, eab;

            int[] edgeA = a.Edge(ci.FeatureA.Index);
            a.World(edgeA[0], out ea.P1);
            a.World(edgeA[1], out ea.P2);
            int[] faceB = b.Face(ci.FeatureB.Index);
            b.World(faceB[0], out pb);
            var planeB = new Plane(pb, ci.Normal);

            planeB.ClosestPointTo(ref ea.P1, out eab.P1);
            planeB.ClosestPointTo(ref ea.P2, out eab.P2);

            int count = 0;

            if (b.IsPointOnFace(ci.FeatureB.Index, ref eab.P1, true))
            {
                count++;
                cf.WritePoint(ref ea.P1, ref eab.P1, ref ci.Normal);
            }
            if (b.IsPointOnFace(ci.FeatureB.Index, ref eab.P2, true))
            {
                count++;
                cf.WritePoint(ref ea.P2, ref eab.P2, ref ci.Normal);
            }

            for (int i = 0; i < faceB.Length && count < 2; i++)
            {
                b.World(faceB[i == 0 ? faceB.Length - 1 : i - 1], out eb.P1);
                b.World(faceB[i], out eb.P2);

                float sa, sb;
                Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
                if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
                {
                    count++;
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }
        }
Beispiel #10
0
        private static void CalculateFaceEdgePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
        {
            Vector3 pa, pb;
            Segment ea, eba, eb;

            int[] faceA = a.Face(ci.FeatureA.Index);
            a.World(faceA[0], out pa);
            Plane planeA = new Plane(pa, ci.Normal);

            tri.Edge(ci.FeatureB.Index, out eb);
            planeA.ClosestPointTo(ref eb.P1, out eba.P1);
            planeA.ClosestPointTo(ref eb.P2, out eba.P2);

            int count = 0;

            if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P1, true))
            {
                count++;
                cf.WritePoint(ref eba.P1, ref eb.P1, ref ci.Normal);
            }
            if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P2, true))
            {
                count++;
                cf.WritePoint(ref eba.P2, ref eb.P2, ref ci.Normal);
            }

            for (int i = 0; i < faceA.Length && count < 2; i++)
            {
                a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
                a.World(faceA[i], out ea.P2);

                float sa, sb;
                Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
                if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
                {
                    count++;
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }
        }
        private static void CalculateVertexFacePoint(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
        {
            Vector3 pa, pb;

            a.World(ci.FeatureA.Index, out pa);
            b.World(b.Face(ci.FeatureB.Index)[0], out pb);
            var plane = new Plane(pb, ci.Normal);
            plane.ClosestPointTo(ref pa, out pb);
            cf.WritePoint(ref pa, ref pb, ref ci.Normal);
        }
        private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
        {
            int[] faceA = a.Face(ci.FeatureA.Index);
            int[] faceB = b.Face(ci.FeatureB.Index);

            Vector3 pa, pb, n;

            a.World(faceA[0], out pa);
            a.FaceNormal(ci.FeatureA.Index, out n);
            Plane planeA = new Plane(pa, n);
            b.World(faceB[0], out pb);
            Plane planeB = new Plane(pb, ci.Normal);

            // vertices of A contained in face of B
            for (int i = 0; i < faceA.Length; i++)
            {
                a.World(faceA[i], out pa);
                planeB.ClosestPointTo(ref pa, out pb);
                if(b.IsPointOnFace(ci.FeatureB.Index, ref pb, true))
                {
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }

            // vertices of B contained in face of A
            for (int i = 0; i < faceB.Length; i++)
            {
                b.World(faceB[i], out pb);
                planeA.ClosestPointTo(ref pb, out pa);
                if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
                {
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }

            // intersections of edges from both faces
            Segment ea, eb;
            for (int i = 0; i < faceA.Length; i++)
            {
                a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
                a.World(faceA[i], out ea.P2);

                for (int j = 0; j < faceB.Length; j++)
                {
                    b.World(faceB[j == 0 ? faceB.Length - 1 : j - 1], out eb.P1);
                    b.World(faceB[j], out eb.P2);

                    float sa, sb;
                    Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
                    if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
                    {
                        cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                    }
                }
            }
        }
        private static void CalculateFaceEdgePoints(CollisionFunctor cf, PolyhedronPart a, PolyhedronPart b, ref CollisionInfo ci)
        {
            Vector3 pa, pb;
            Segment ea, eb, eba;
            int[] edgeB = b.Edge(ci.FeatureB.Index);
            b.World(edgeB[0], out eb.P1);
            b.World(edgeB[1], out eb.P2);
            int[] faceA = a.Face(ci.FeatureA.Index);
            a.World(faceA[0], out pa);
            var planeA = new Plane(pa, ci.Normal);
            planeA.ClosestPointTo(ref eb.P1, out eba.P1);
            planeA.ClosestPointTo(ref eb.P2, out eba.P2);

            int count = 0;
            if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P1, true))
            {
                count++;
                cf.WritePoint(ref eba.P1, ref eb.P1, ref ci.Normal);
            }
            if (a.IsPointOnFace(ci.FeatureA.Index, ref eba.P2, true))
            {
                count++;
                cf.WritePoint(ref eba.P2, ref eb.P2, ref ci.Normal);
            }

            for(int i = 0; i < faceA.Length && count < 2; i++)
            {
                a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
                a.World(faceA[i], out ea.P2);

                float sa, sb;
                Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
                if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
                {
                    count++;
                    cf.WritePoint(ref pa, ref pb, ref ci.Normal);
                }
            }
        }
Beispiel #14
0
        public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset)
        {
            Vector3 v, normal;
            float   ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius;

            Segment capa;

            Vector3.Add(ref a.World.P1, ref offset, out capa.P1);
            Vector3.Add(ref a.World.P2, ref offset, out capa.P2);

            float curDepth, finalDepth = float.MaxValue;
            int   faceIdx = -1;

            // find the face with the least penetration depth along its normal
            for (int i = 0; i < b.FaceCount; i++)
            {
                b.World(b.Face(i)[0], out v);
                b.FaceNormal(i, out normal);
                Vector3.Dot(ref normal, ref v, out bx);
                Vector3.Dot(ref normal, ref capa.P1, out ax1);
                Vector3.Dot(ref normal, ref capa.P2, out ax2);
                bx      += a.World.Radius;
                curDepth = Math.Max(bx - ax1, bx - ax2);
                if (curDepth < 0f)
                {
                    return(false);
                }
                if (curDepth < finalDepth)
                {
                    faceIdx    = i;
                    finalDepth = curDepth;
                }
            }

            bool    got1 = false, got2 = false;
            Vector3 pa, pb, pa1, pa2, pb1, pb2;

            pa1 = pa2 = pb1 = pb2 = Vector3.Zero;
            var face = b.Face(faceIdx);

            b.World(face[0], out v);
            b.FaceNormal(faceIdx, out normal);

            var plane = new Plane(v, normal);

            Vector3.Dot(ref normal, ref v, out bx);
            bx += a.World.Radius;

            // determine if either capsule point is inside the face
            pa1 = capa.P1;
            plane.ClosestPointTo(ref pa1, out pb1);
            Vector3.Dot(ref normal, ref pa1, out ax1);
            got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true);

            pa2 = capa.P2;
            plane.ClosestPointTo(ref pa2, out pb2);
            Vector3.Dot(ref normal, ref pa2, out ax1);
            if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true))
            {
                if (got1)
                {
                    got2 = true;
                }
                else
                {
                    got1 = true;
                    pa1  = pa2;
                    pb1  = pb2;
                }
            }

            // if one capsule point is inside the face but one is not, try to generate a second point on an edge
            if (got1 ^ got2)
            {
                float sbPrev   = float.NaN;
                int   edgePrev = -1;
                for (int i = 0; i < face.Length; i++)
                {
                    float   dist, sa, sb;
                    Segment edge;
                    b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
                    b.World(face[i], out edge.P2);
                    Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
                    Vector3.DistanceSquared(ref pa, ref pb, out dist);
                    if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon)
                    {
                        if (i == face.Length - 1 && edgePrev == 0 && sb == 1f)
                        {
                            continue;
                        }
                        if (!got2 || (edgePrev == i - 1 && sbPrev == 1f))
                        {
                            pa2 = pa; pb2 = pb; got2 = true;
                        }
                        sbPrev   = sb;
                        edgePrev = i;
                    }
                }
            }
            else if (!got1 && !got2)
            {
                // neither point is inside the face, so try all edges
                float sbPrev = float.NaN, edgePrev = float.NaN;
                for (int i = 0; i < face.Length; i++)
                {
                    float   dist, sa, sb;
                    Segment edge;
                    b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
                    b.World(face[i], out edge.P2);
                    Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
                    Vector3.DistanceSquared(ref pa, ref pb, out dist);
                    if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon)
                    {
                        if (i == face.Length - 1 && edgePrev == 0 && sb == 1f)
                        {
                            continue;
                        }
                        if (!got1 || (edgePrev == i - 1 && sbPrev == 1f))
                        {
                            pa1 = pa; pb1 = pb; got1 = true;
                        }
                        else if (!got2)
                        {
                            pa2 = pa; pb2 = pb; got2 = true;
                        }
                        sbPrev   = sb;
                        edgePrev = i;
                    }
                }

                // if only one edge was crossed, create a new normal instead of using the face normal
                if (got1 ^ got2)
                {
                    Vector3.Subtract(ref pa1, ref pb1, out normal);
                    normal.Normalize();
                }
            }

            // write out points
            if (got1 || got2)
            {
                Vector3.Multiply(ref normal, -a.World.Radius, out v);

                if (got1)
                {
                    Vector3.Add(ref pa1, ref v, out pa1);
                    Vector3.Subtract(ref pa1, ref offset, out pa1);
                    cf.WritePoint(ref pa1, ref pb1, ref normal);
                }
                if (got2)
                {
                    Vector3.Add(ref pa2, ref v, out pa2);
                    Vector3.Subtract(ref pa2, ref offset, out pa2);
                    cf.WritePoint(ref pa2, ref pb2, ref normal);
                }
            }

            return(got1 || got2);
        }
		public static bool DoOverlapTest(CollisionFunctor cf, CapsulePart a, PolyhedronPart b, Vector3 offset)
		{
			Vector3 v, normal;
			float ax1, ax2, bx, r2 = a.World.Radius * a.World.Radius;

			Segment capa;
			Vector3.Add(ref a.World.P1, ref offset, out capa.P1);
			Vector3.Add(ref a.World.P2, ref offset, out capa.P2);

			float curDepth, finalDepth = float.MaxValue;
			int faceIdx = -1;

			// find the face with the least penetration depth along its normal
			for (int i = 0; i < b.FaceCount; i++)
			{
				b.World(b.Face(i)[0], out v);
				b.FaceNormal(i, out normal);
				Vector3.Dot(ref normal, ref v, out bx);
				Vector3.Dot(ref normal, ref capa.P1, out ax1);
				Vector3.Dot(ref normal, ref capa.P2, out ax2);
				bx += a.World.Radius;
				curDepth = Math.Max(bx - ax1, bx - ax2);
				if (curDepth < 0f)
					return false;
				if (curDepth < finalDepth)
				{
					faceIdx = i;
					finalDepth = curDepth;
				}
			}

			bool got1 = false, got2 = false;
			Vector3 pa, pb, pa1, pa2, pb1, pb2;
			pa1 = pa2 = pb1 = pb2 = Vector3.Zero;
			var face = b.Face(faceIdx);
			b.World(face[0], out v);
			b.FaceNormal(faceIdx, out normal);

			var plane = new Plane(v, normal);
			Vector3.Dot(ref normal, ref v, out bx);
			bx += a.World.Radius;

			// determine if either capsule point is inside the face
			pa1 = capa.P1;
			plane.ClosestPointTo(ref pa1, out pb1);
			Vector3.Dot(ref normal, ref pa1, out ax1);
			got1 = ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb1, true);

			pa2 = capa.P2;
			plane.ClosestPointTo(ref pa2, out pb2);
			Vector3.Dot(ref normal, ref pa2, out ax1);
			if (ax1 - bx < Constants.Epsilon && b.IsPointOnFace(faceIdx, ref pb2, true))
			{
				if (got1)
					got2 = true;
				else
				{
					got1 = true;
					pa1 = pa2;
					pb1 = pb2;
				}
			}

			// if one capsule point is inside the face but one is not, try to generate a second point on an edge
			if (got1 ^ got2)
			{
				float sbPrev = float.NaN;
				int edgePrev = -1;
				for (int i = 0; i < face.Length; i++)
				{
					float dist, sa, sb;
					Segment edge;
					b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
					b.World(face[i], out edge.P2);
					Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
					Vector3.DistanceSquared(ref pa, ref pb, out dist);
					if (dist - r2 < Constants.Epsilon && sa >= Constants.Epsilon && sa < 1 - Constants.Epsilon)
					{
						if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue;
						if (!got2 || (edgePrev == i - 1 && sbPrev == 1f)) { pa2 = pa; pb2 = pb; got2 = true; }
						sbPrev = sb;
						edgePrev = i;
					}
				}
			}
			else if (!got1 && !got2)
			{
				// neither point is inside the face, so try all edges
				float sbPrev = float.NaN, edgePrev = float.NaN;
				for (int i = 0; i < face.Length; i++)
				{
					float dist, sa, sb;
					Segment edge;
					b.World(face[i == 0 ? face.Length - 1 : i - 1], out edge.P1);
					b.World(face[i], out edge.P2);
					Segment.ClosestPoints(ref capa, ref edge, out sa, out pa, out sb, out pb);
					Vector3.DistanceSquared(ref pa, ref pb, out dist);
					if (dist - r2 < Constants.Epsilon && sb > Constants.Epsilon)
					{
						if (i == face.Length - 1 && edgePrev == 0 && sb == 1f) continue;
						if (!got1 || (edgePrev == i - 1 && sbPrev == 1f)) { pa1 = pa; pb1 = pb; got1 = true; }
						else if (!got2) { pa2 = pa; pb2 = pb; got2 = true; }
						sbPrev = sb;
						edgePrev = i;
					}
				}

				// if only one edge was crossed, create a new normal instead of using the face normal
				if (got1 ^ got2)
				{
					Vector3.Subtract(ref pa1, ref pb1, out normal);
					normal.Normalize();
				}
			}

			// write out points
			if (got1 || got2)
			{
				Vector3.Multiply(ref normal, -a.World.Radius, out v);

				if (got1)
				{
					Vector3.Add(ref pa1, ref v, out pa1);
					Vector3.Subtract(ref pa1, ref offset, out pa1);
					cf.WritePoint(ref pa1, ref pb1, ref normal);
				}
				if (got2)
				{
					Vector3.Add(ref pa2, ref v, out pa2);
					Vector3.Subtract(ref pa2, ref offset, out pa2);
					cf.WritePoint(ref pa2, ref pb2, ref normal);
				}
			}

			return got1 || got2;
		}
		private static void CalculateFaceVertexPoint(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
		{
			Vector3 pa, pb;

			tri.Vertex(ci.FeatureB.Index, out pb);
			a.World(a.Face(ci.FeatureA.Index)[0], out pa);
			var plane = new Plane(pa, ci.Normal);
			plane.ClosestPointTo(ref pb, out pa);
			cf.WritePoint(ref pa, ref pb, ref ci.Normal);
		}
Beispiel #17
0
        private static void OverlapTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri)
        {
            CollisionInfo cur = new CollisionInfo(), final;
            Vector3       v;
            float         d;

            // axis: face normal of B
            cur.Normal = tri.Normal;
            Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
            cur.FeatureA      = a.ExtremeVertex(ref cur.NormalNeg);
            cur.FeatureA.X    = -cur.FeatureA.X;
            cur.FeatureB.Type = TriangleFeatureType.Face;
            Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
            cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
            if (cur.Depth <= -Constants.Epsilon)
            {
                return;
            }
            final = cur;

            // axes: face normals of A
            for (int i = 0; i < a.FaceCount; i++)
            {
                a.FaceNormal(i, out cur.NormalNeg);
                Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
                a.World(a.Face(i)[0], out v);
                cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
                Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
                cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
                cur.Depth    = cur.FeatureB.X - cur.FeatureA.X;

                if (cur.Depth <= -Constants.Epsilon)
                {
                    return;
                }
                else if (cur.Depth < final.Depth)
                {
                    final = cur;
                }
            }

            // crossed edges from A and B
            Vector3 centerA, centerB;

            a.Center(out centerA);
            tri.Center(out centerB);
            for (int i = 0; i < a.EdgeVectorCount; i++)
            {
                for (int j = 1; j <= 3; j++)
                {
                    // calculate normal from the two edge vectors
                    Vector3 eva, evb;
                    a.EdgeVector(i, out eva);
                    tri.EdgeVector(j, out evb);
                    Vector3.Cross(ref eva, ref evb, out cur.Normal);
                    if (cur.Normal.LengthSquared() < Constants.Epsilon)
                    {
                        continue;
                    }
                    cur.Normal.Normalize();

                    float ca, cb;
                    Vector3.Dot(ref cur.Normal, ref centerA, out ca);
                    Vector3.Dot(ref cur.Normal, ref centerB, out cb);
                    if (ca < cb)
                    {
                        cur.NormalNeg = cur.Normal;
                        Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
                    }
                    else
                    {
                        Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
                    }

                    // skip this normal if it's close to one we already have
                    Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
                    if (Math.Abs(1f - d) < Constants.Epsilon)
                    {
                        continue;
                    }

                    cur.FeatureA   = a.ExtremeVertex(ref cur.NormalNeg);
                    cur.FeatureA.X = -cur.FeatureA.X;
                    cur.FeatureB   = tri.ExtremeVertex(ref cur.Normal);
                    cur.Depth      = cur.FeatureB.X - cur.FeatureA.X;

                    if (cur.Depth <= -Constants.Epsilon)
                    {
                        return;
                    }
                    else if (final.Depth - cur.Depth >= Constants.Epsilon)
                    {
                        final = cur;
                    }
                }
            }

            if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
            {
                final.FeatureA   = a.ExtremeFeature(ref final.NormalNeg, -final.FeatureB.X);
                final.FeatureA.X = -final.FeatureA.X;
            }
            if (final.FeatureB.Type == TriangleFeatureType.Vertex)
            {
                final.FeatureB = tri.ExtremeFeature(ref final.Normal, final.FeatureA.X);
            }

            // make sure the normal points outward
            Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
            if (d < 0f)
            {
                Vector3.Multiply(ref tri.Normal, -2f * d, out v);
                Vector3.Add(ref final.Normal, ref v, out final.Normal);
            }

            CalculateContactPoints(cf, a, b, ref tri, ref final);
        }
		private static bool SweptTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref Vector3 delta)
		{
			CollisionInfo cur = new CollisionInfo(), final = new CollisionInfo() { Depth = float.MaxValue };
			Vector3 v;
			float d, dx, curTime, finalTime = 0f, tlast = float.MaxValue;

			// axis: face normal of B
			cur.Normal = tri.Normal;
			Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
			cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
			cur.FeatureA.X = -cur.FeatureA.X;
			cur.FeatureB.Type = TriangleFeatureType.Face;
			Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
			cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
			Vector3.Dot(ref cur.Normal, ref delta, out dx);
			curTime = cur.Depth / dx;

			final = cur;
			if (cur.Depth >= 0f)
			{
				if (dx > 0f)
					tlast = curTime;
			}
			else
			{
				if (dx >= 0f || curTime > 1f)
					return false;
				finalTime = curTime;
			}

			// axes: face normals of A
			for (int i = 0; i < a.FaceCount; i++)
			{
				a.FaceNormal(i, out cur.NormalNeg);
				Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
				a.World(a.Face(i)[0], out v);
				cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
				Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
				cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
				cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
				Vector3.Dot(ref cur.Normal, ref delta, out dx);
				curTime = cur.Depth / dx;

				if (cur.Depth >= 0f)
				{
					if (finalTime <= 0f && cur.Depth < final.Depth)
					{
						final = cur;
						finalTime = 0f;
					}
					if (dx > 0f && curTime < tlast)
						tlast = curTime;
				}
				else
				{
					if (dx >= 0f || curTime > 1f)
						return false;
					if (curTime > finalTime)
					{
						final = cur;
						finalTime = curTime;
					}
				} 
			}

			// crossed edges from A and B
			Vector3 centerA, centerB;
			a.Center(out centerA);
			tri.Center(out centerB);
			for (int i = 0; i < a.EdgeVectorCount; i++)
			{
				for (int j = 1; j <= 3; j++)
				{
					// calculate normal from the two edge vectors
					Vector3 eva, evb;
					a.EdgeVector(i, out eva);
					tri.EdgeVector(j, out evb);
					Vector3.Cross(ref eva, ref evb, out cur.Normal);
					if (cur.Normal.LengthSquared() < Constants.Epsilon)
						continue;
					cur.Normal.Normalize();

					float ca, cb;
					Vector3.Dot(ref cur.Normal, ref centerA, out ca);
					Vector3.Dot(ref cur.Normal, ref centerB, out cb);
					if (ca < cb)
					{
						cur.NormalNeg = cur.Normal;
						Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
					}
					else
						Vector3.Negate(ref cur.Normal, out cur.NormalNeg);

					// skip this normal if it's close to one we already have
					Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
					if (Math.Abs(1f - d) < Constants.Epsilon)
						continue;

					cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
					cur.FeatureA.X = -cur.FeatureA.X;
					cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
					cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
					Vector3.Dot(ref cur.Normal, ref delta, out dx);
					curTime = cur.Depth / dx;

					if (cur.Depth >= 0f)
					{
						if (finalTime <= 0f && cur.Depth < final.Depth)
						{
							final = cur;
							finalTime = 0f;
						}
						if (dx > 0f && curTime < tlast)
							tlast = curTime;
					}
					else
					{
						if (dx >= 0f || curTime > 1f)
							return false;
						if (curTime > finalTime)
						{
							final = cur;
							finalTime = curTime;
						}
					}
				}
			}

			if (finalTime >= tlast)
				return false;

			Vector3.Dot(ref final.Normal, ref delta, out dx);
			if (finalTime <= 0f)
				dx = 0f;

			if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
			{
				final.FeatureA = a.ExtremeFeature(ref final.NormalNeg, dx - final.FeatureB.X);
				final.FeatureA.X = -final.FeatureA.X;
			}
			if (final.FeatureB.Type == TriangleFeatureType.Vertex)
			{
				final.FeatureB = tri.ExtremeFeature(ref final.Normal, dx + final.FeatureA.X);
			}

			// make sure the normal points outward
			Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
			if (d < 0f)
			{
				Vector3.Multiply(ref tri.Normal, -2f * d, out v);
				Vector3.Add(ref final.Normal, ref v, out final.Normal);
			}

			CalculateContactPoints(cf, a, b, ref tri, ref final);
			return true;
		}
Beispiel #19
0
        private static bool SweptTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref Vector3 delta)
        {
            CollisionInfo cur = new CollisionInfo(), final = new CollisionInfo()
            {
                Depth = float.MaxValue
            };
            Vector3 v;
            float   d, dx, curTime, finalTime = 0f, tlast = float.MaxValue;

            // axis: face normal of B
            cur.Normal = tri.Normal;
            Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
            cur.FeatureA      = a.ExtremeVertex(ref cur.NormalNeg);
            cur.FeatureA.X    = -cur.FeatureA.X;
            cur.FeatureB.Type = TriangleFeatureType.Face;
            Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
            cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
            Vector3.Dot(ref cur.Normal, ref delta, out dx);
            curTime = cur.Depth / dx;

            final = cur;
            if (cur.Depth >= 0f)
            {
                if (dx > 0f)
                {
                    tlast = curTime;
                }
            }
            else
            {
                if (dx >= 0f || curTime > 1f)
                {
                    return(false);
                }
                finalTime = curTime;
            }

            // axes: face normals of A
            for (int i = 0; i < a.FaceCount; i++)
            {
                a.FaceNormal(i, out cur.NormalNeg);
                Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
                a.World(a.Face(i)[0], out v);
                cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
                Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
                cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
                cur.Depth    = cur.FeatureB.X - cur.FeatureA.X;
                Vector3.Dot(ref cur.Normal, ref delta, out dx);
                curTime = cur.Depth / dx;

                if (cur.Depth >= 0f)
                {
                    if (finalTime <= 0f && cur.Depth < final.Depth)
                    {
                        final     = cur;
                        finalTime = 0f;
                    }
                    if (dx > 0f && curTime < tlast)
                    {
                        tlast = curTime;
                    }
                }
                else
                {
                    if (dx >= 0f || curTime > 1f)
                    {
                        return(false);
                    }
                    if (curTime > finalTime)
                    {
                        final     = cur;
                        finalTime = curTime;
                    }
                }
            }

            // crossed edges from A and B
            Vector3 centerA, centerB;

            a.Center(out centerA);
            tri.Center(out centerB);
            for (int i = 0; i < a.EdgeVectorCount; i++)
            {
                for (int j = 1; j <= 3; j++)
                {
                    // calculate normal from the two edge vectors
                    Vector3 eva, evb;
                    a.EdgeVector(i, out eva);
                    tri.EdgeVector(j, out evb);
                    Vector3.Cross(ref eva, ref evb, out cur.Normal);
                    if (cur.Normal.LengthSquared() < Constants.Epsilon)
                    {
                        continue;
                    }
                    cur.Normal.Normalize();

                    float ca, cb;
                    Vector3.Dot(ref cur.Normal, ref centerA, out ca);
                    Vector3.Dot(ref cur.Normal, ref centerB, out cb);
                    if (ca < cb)
                    {
                        cur.NormalNeg = cur.Normal;
                        Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
                    }
                    else
                    {
                        Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
                    }

                    // skip this normal if it's close to one we already have
                    Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
                    if (Math.Abs(1f - d) < Constants.Epsilon)
                    {
                        continue;
                    }

                    cur.FeatureA   = a.ExtremeVertex(ref cur.NormalNeg);
                    cur.FeatureA.X = -cur.FeatureA.X;
                    cur.FeatureB   = tri.ExtremeVertex(ref cur.Normal);
                    cur.Depth      = cur.FeatureB.X - cur.FeatureA.X;
                    Vector3.Dot(ref cur.Normal, ref delta, out dx);
                    curTime = cur.Depth / dx;

                    if (cur.Depth >= 0f)
                    {
                        if (finalTime <= 0f && cur.Depth < final.Depth)
                        {
                            final     = cur;
                            finalTime = 0f;
                        }
                        if (dx > 0f && curTime < tlast)
                        {
                            tlast = curTime;
                        }
                    }
                    else
                    {
                        if (dx >= 0f || curTime > 1f)
                        {
                            return(false);
                        }
                        if (curTime > finalTime)
                        {
                            final     = cur;
                            finalTime = curTime;
                        }
                    }
                }
            }

            if (finalTime >= tlast)
            {
                return(false);
            }

            Vector3.Dot(ref final.Normal, ref delta, out dx);
            if (finalTime <= 0f)
            {
                dx = 0f;
            }

            if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
            {
                final.FeatureA   = a.ExtremeFeature(ref final.NormalNeg, dx - final.FeatureB.X);
                final.FeatureA.X = -final.FeatureA.X;
            }
            if (final.FeatureB.Type == TriangleFeatureType.Vertex)
            {
                final.FeatureB = tri.ExtremeFeature(ref final.Normal, dx + final.FeatureA.X);
            }

            // make sure the normal points outward
            Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
            if (d < 0f)
            {
                Vector3.Multiply(ref tri.Normal, -2f * d, out v);
                Vector3.Add(ref final.Normal, ref v, out final.Normal);
            }

            CalculateContactPoints(cf, a, b, ref tri, ref final);
            return(true);
        }
		private static void CalculateFaceFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
		{
			int[] faceA = a.Face(ci.FeatureA.Index);
			Vector3 pa, pb, n;

			a.World(faceA[0], out pa);
			a.FaceNormal(ci.FeatureA.Index, out n);
			Plane planeA = new Plane(pa, n);
			Plane planeB = new Plane(tri.V1, tri.Normal);

			// vertices of A contained in face of B
			for (int i = 0; i < faceA.Length; i++)
			{
				a.World(faceA[i], out pa);
				planeB.ClosestPointTo(ref pa, out pb);
				if (tri.Contains(ref pb))
				{
					cf.WritePoint(ref pa, ref pb, ref ci.Normal);
				}
			}

			for (int i = 1; i <= 3; i++)
			{
				tri.Vertex(i, out pb);
				planeA.ClosestPointTo(ref pb, out pa);
				if (a.IsPointOnFace(ci.FeatureA.Index, ref pa, true))
				{
					cf.WritePoint(ref pa, ref pb, ref ci.Normal);
				}
			}

			// intersection of edges from both faces
			Segment ea, eb;
			for (int i = 0; i < faceA.Length; i++)
			{
				a.World(faceA[i == 0 ? faceA.Length - 1 : i - 1], out ea.P1);
				a.World(faceA[i], out ea.P2);

				for (int j = 1; j <= 3; j++)
				{
					tri.Edge(j, out eb);

					float sa, sb;
					Segment.ClosestPoints(ref ea, ref eb, out sa, out pa, out sb, out pb);
					if (sa > 0f && sa < 1f && sb > 0f && sb < 1f)
					{
						cf.WritePoint(ref pa, ref pb, ref ci.Normal);
					}
				}
			}
		}
		private static void OverlapTest(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri)
		{
			CollisionInfo cur = new CollisionInfo(), final;
			Vector3 v;
			float d;

			// axis: face normal of B
			cur.Normal = tri.Normal;
			Vector3.Negate(ref cur.Normal, out cur.NormalNeg);
			cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
			cur.FeatureA.X = -cur.FeatureA.X;
			cur.FeatureB.Type = TriangleFeatureType.Face;
			Vector3.Dot(ref cur.Normal, ref tri.V1, out cur.FeatureB.X);
			cur.Depth = cur.FeatureB.X - cur.FeatureA.X;
			if (cur.Depth <= -Constants.Epsilon)
				return;
			final = cur;

			// axes: face normals of A
			for (int i = 0; i < a.FaceCount; i++)
			{
				a.FaceNormal(i, out cur.NormalNeg);
				Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
				a.World(a.Face(i)[0], out v);
				cur.FeatureA = new PolyhedronFeature(PolyhedronFeatureType.Face, i, 0f);
				Vector3.Dot(ref cur.Normal, ref v, out cur.FeatureA.X);
				cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
				cur.Depth = cur.FeatureB.X - cur.FeatureA.X;

				if (cur.Depth <= -Constants.Epsilon)
					return;
				else if (cur.Depth < final.Depth)
					final = cur;
			}

			// crossed edges from A and B
			Vector3 centerA, centerB;
			a.Center(out centerA);
			tri.Center(out centerB);
			for (int i = 0; i < a.EdgeVectorCount; i++)
			{
				for (int j = 1; j <= 3; j++)
				{
					// calculate normal from the two edge vectors
					Vector3 eva, evb;
					a.EdgeVector(i, out eva);
					tri.EdgeVector(j, out evb);
					Vector3.Cross(ref eva, ref evb, out cur.Normal);
					if (cur.Normal.LengthSquared() < Constants.Epsilon)
						continue;
					cur.Normal.Normalize();

					float ca, cb;
					Vector3.Dot(ref cur.Normal, ref centerA, out ca);
					Vector3.Dot(ref cur.Normal, ref centerB, out cb);
					if (ca < cb)
					{
						cur.NormalNeg = cur.Normal;
						Vector3.Negate(ref cur.NormalNeg, out cur.Normal);
					}
					else
						Vector3.Negate(ref cur.Normal, out cur.NormalNeg);

					// skip this normal if it's close to one we already have
					Vector3.Dot(ref cur.Normal, ref final.Normal, out d);
					if (Math.Abs(1f - d) < Constants.Epsilon)
						continue;

					cur.FeatureA = a.ExtremeVertex(ref cur.NormalNeg);
					cur.FeatureA.X = -cur.FeatureA.X;
					cur.FeatureB = tri.ExtremeVertex(ref cur.Normal);
					cur.Depth = cur.FeatureB.X - cur.FeatureA.X;

					if (cur.Depth <= -Constants.Epsilon)
						return;
					else if (final.Depth - cur.Depth >= Constants.Epsilon)
						final = cur;
				}
			}

			if (final.FeatureA.Type == PolyhedronFeatureType.Vertex)
			{
				final.FeatureA = a.ExtremeFeature(ref final.NormalNeg, -final.FeatureB.X);
				final.FeatureA.X = -final.FeatureA.X;
			}
			if (final.FeatureB.Type == TriangleFeatureType.Vertex)
			{
				final.FeatureB = tri.ExtremeFeature(ref final.Normal, final.FeatureA.X);
			}

			// make sure the normal points outward
			Vector3.Dot(ref tri.Normal, ref final.Normal, out d);
			if (d < 0f)
			{
				Vector3.Multiply(ref tri.Normal, -2f * d, out v);
				Vector3.Add(ref final.Normal, ref v, out final.Normal);
			}

			CalculateContactPoints(cf, a, b, ref tri, ref final);
		}