C# Plane.ClosestPointTo方法代码示例

		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 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);
                }
            }
        }

		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;
//.........这里部分代码省略.........

		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 CalculateEdgeFacePoints(CollisionFunctor cf, PolyhedronPart a, MeshPart b, ref Triangle tri, ref CollisionInfo ci)
		{
			Vector3 pa, pb;
			Segment ea, eb, eab;

			int[] edge = a.Edge(ci.FeatureA.Index);
			a.World(edge[0], out ea.P1);
			a.World(edge[1], out ea.P2);
			Plane planeB = new Plane(tri.V1, tri.Normal);
			planeB.ClosestPointTo(ref ea.P1, out eab.P1);
			planeB.ClosestPointTo(ref ea.P2, out eab.P2);

			int count = 0;
			if (tri.Contains(ref eab.P1))
			{
				count++;
				cf.WritePoint(ref ea.P1, ref eab.P1, ref ci.Normal);
			}
			if (tri.Contains(ref eab.P2))
			{
				count++;
				cf.WritePoint(ref ea.P2, ref eab.P2, ref ci.Normal);
			}

			for (int i = 1; i <= 3 && count < 2; i++)
			{
				tri.Edge(i, 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)
				{
					count++;
					cf.WritePoint(ref pa, ref pb, ref ci.Normal);
				}
			}
		}

		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);
		}