예제 #1
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);
        }
예제 #2
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);
		}
예제 #3
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);
        }
예제 #4
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;
		}