public List<CollisionPoint> GetManifoldPoints(
			SimulationObject objectA, 
			SimulationObject objectB,
			CollisionPoint collisionPoint)
		{
			List<Vector3> collisionA = getNearestPoint (
				objectA,
				collisionPoint.CollisionPointA,
				collisionPoint.CollisionNormal);

			List<Vector3> collisionB = getNearestPoint (
				objectB,
				collisionPoint.CollisionPointB,
				collisionPoint.CollisionNormal);

			List<CollisionPoint> collisionPointsList = findCollisionPoints (
				collisionA.ToArray (),
				collisionB.ToArray (),
				collisionPoint.CollisionNormal,
				collisionPoint);

			collisionA.Clear ();
			collisionB.Clear ();

			return collisionPointsList;
		}
示例#2
0
		public GJKOutput (
			double collisionDistance,
			CollisionPoint collisionPoint,
			Vector3 collisionNormal,
			Vector3 centroid,
			bool intersection,
			List<SupportTriangle> supportTriangles)
		{
			CollisionDistance = collisionDistance;
			CollisionPoint = collisionPoint;
			CollisionNormal = collisionNormal;
			Centroid = centroid;
			Intersection = intersection;
			SupportTriangles = supportTriangles;
		}
示例#3
0
		/// <summary>
		/// Detects the collision.
		/// </summary>
		/// <returns>The collision.</returns>
		/// <param name="objectA">Object a.</param>
		/// <param name="objectB">Object b.</param>
		public GJKOutput Execute(
			SimulationObject objectA, 
			SimulationObject objectB,
            int geometryIndexA,
            int geometryIndexB)
		{
			var collisionPoint = new CollisionPoint();
			var collisionNormal = new Vector3();
			var supportTriangles = new List<SupportTriangle>();
			var centroid = new Vector3();
			bool isIntersection = false;

			double collisionDistance = ExecuteGJKAlgorithm (
				                          objectA,
				                          objectB,
                                          geometryIndexA,
                                          geometryIndexB,
				                          ref collisionNormal,
				                          ref collisionPoint,
				                          ref supportTriangles,
										  ref centroid,
				                          ref isIntersection);

			return new GJKOutput (
				collisionDistance,
				collisionPoint,
				collisionNormal,
				centroid,
				isIntersection,
				supportTriangles);
		}
示例#4
0
		/// <summary>
		/// Ritorna la distanza se distanza 0.0f allora vi è intersezione o compenetrazione tra gli oggetti, 
		/// se distanza 0.0 allora i due oggetti non collidono
		/// </summary>
		/// <returns>The GJK algorithm.</returns>
		/// <param name="shape1">Shape1.</param>
		/// <param name="shape2">Shape2.</param>
		/// <param name="cp">Cp.</param>
		/// <param name="isIntersection">If set to <c>true</c> is itersection.</param>
		private double ExecuteGJKAlgorithm(
			SimulationObject shape1,
			SimulationObject shape2,
            int geometryIndexA,
            int geometryIndexB,
			ref Vector3 collisionNormal,
			ref CollisionPoint cp,
			ref List<SupportTriangle> triangles,
			ref Vector3 centroid,
			ref bool isIntersection)
		{
			double minDistance = double.MaxValue;
			int minTriangleIndex = -1;
			var result = new EngineCollisionPoint();
			var oldDirection = new Vector3();
			var simplex = new Simplex();

			//Primo punto del simplex
			simplex.Support.Add(GetFarthestPoint(shape1, shape2));

			//Secondo punto del simplex
			Vector3 direction = Vector3.Normalize(simplex.Support[0].s * -1.0);
            if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
                return -1.0;

			//Terzo punto del simplex
			direction = Vector3.Normalize(GetDirectionOnSimplex2(simplex));
			if(!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
                return -1.0;

            	//Quarto punto del simplex
			direction = Vector3.Normalize(GeometryUtilities.CalculateNormal(
				simplex.Support[0].s,
				simplex.Support[1].s,
				simplex.Support[2].s));

			if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
				simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, -1.0 * direction));

			//Costruisco il poliedro
			centroid = Helper.SetStartTriangle(
								   	ref triangles,
									simplex.Support.ToArray());

			//Verifico che l'origine sia contenuta nel poliedro
			if (Helper.IsInConvexPoly(origin, triangles))
			{
				isIntersection = true;
				return -1.0;
			}

			Vector3 triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex);

			result.SetDist(triangleDistance);
			result.SetNormal(Vector3.Normalize(triangleDistance));
			Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], shape1, shape2, ref result);

			minDistance = triangleDistance.Length();

			for (int i = 0; i < MaxIterations; i++) 
			{
				direction = -1.0 * triangleDistance.Normalize();

				if (Vector3.Length(direction) < constTolerance)
				{
					direction = origin - centroid;
				}

				if (direction == oldDirection)
					break;

				oldDirection = direction;

				if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
				{
					for (int j = 0; j < triangles.Count; j++)
					{
						direction = triangles[j].normal;
						if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction)))
						{
							if (simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, -1.0 * direction)))
							   break;
							
							continue;
						}
						break;
					}
				}

				triangles = Helper.AddPointToConvexPolygon(triangles, simplex.Support[simplex.Support.Count - 1], centroid);

				//Verifico che l'origine sia contenuta nel poliedro
				if (Helper.IsInConvexPoly(origin, triangles))
				{
					isIntersection = true;
					return -1.0;
				}

				triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex);

				double mod = triangleDistance.Length();

				if (mod < minDistance)
				{
					result.SetDist(triangleDistance);
					result.SetNormal(Vector3.Normalize(triangleDistance));
					Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], shape1, shape2, ref result);

					minDistance = mod;
				}
			}

			collisionNormal = -1.0 * result.normal;

			cp = new CollisionPoint(
				result.a,
				result.b,
				collisionNormal);
			
			return minDistance;
		}
示例#5
0
		/// <summary>
		/// Execute Expanding Polytope Algorithm (EPA).
		/// </summary>
		/// <param name="objectA">Object a.</param>
		/// <param name="objectB">Object b.</param>
		/// <param name="startTriangles">Start triangles.</param>
		public EPAOutput Execute(
			SimulationObject objectA,
			SimulationObject objectB,
            int geometryIndexA,
            int geometryIndexB,
            List<SupportTriangle> startTriangles,
			Vector3 centroid)
		{
			EngineCollisionPoint epaCollisionPoint = ExecuteEngine (
				                                      objectA,
				                                      objectB,
                                                      geometryIndexA,
                                                      geometryIndexB,
				                                      startTriangles,
													  centroid);

			var collisionPoint = new CollisionPoint (
				epaCollisionPoint.a,
				epaCollisionPoint.b,
				epaCollisionPoint.normal);

			return new EPAOutput (
				Vector3.Length (epaCollisionPoint.dist),
				collisionPoint);
		}
		private List<CollisionPoint> findCollisionPoints(
			Vector3[] ca,
			Vector3[] cb,
			Vector3 normal,
			CollisionPoint cp)
		{
			var result = new List<CollisionPoint> ();

			if (ca.Length == 2 && cb.Length == 2) 
			{
				CollisionPoint? collisionP = TestLineIntersection (
					ca [0],
					ca [1],
					cb [0],
					cb [1],
					cp.CollisionNormal);

				if (collisionP != null)
					result.Add (collisionP.Value);

			} 
			else if (ca.Length > 2 && cb.Length == 2) 
			{
				ca = GeometryUtilities.TurnVectorClockWise (
					ca,
					normal);

				result.AddRange(TestPointIsOnPlane (
					ca,
					cb,
					cp,
					normal));

				if (result.Count < ca.Length) {
					for (int i = 0; i < ca.Length; i++) {

						CollisionPoint? collisionP = TestLineIntersection (
							ca [i],
							ca [(i + 1) % ca.Length],
							cb [0],
							cb [1],
							cp.CollisionNormal);

						if (collisionP != null)
							result.Add (collisionP.Value);
					}
				}
			} 
			else if (ca.Length == 2 && cb.Length > 2) 
			{
				cb = GeometryUtilities.TurnVectorClockWise (
					cb,
					normal);

				result.AddRange(TestPointIsOnPlane (
					ca,
					cb,
					cp,
					normal));

				if (result.Count < cb.Length) {
					for (int i = 0; i < cb.Length; i++) {

						CollisionPoint? collisionP = TestLineIntersection (
							ca [0],
							ca [1],
							cb [i],
							cb [(i + 1) % cb.Length],
							cp.CollisionNormal);

						if (collisionP != null)
							result.Add (collisionP.Value);
					}
				}
			} 
			else if (ca.Length > 2 && cb.Length > 2) 
			{
				ca = GeometryUtilities.TurnVectorClockWise (
					ca,
					normal);

				cb = GeometryUtilities.TurnVectorClockWise (
					cb,
					normal);

				result.AddRange(TestPointIsOnPlane (
					ca,
					cb,
					cp,
					normal));

				for (int i = 0; i < ca.Length; i++) {
					for (int j = 0; j < cb.Length; j++) {

						CollisionPoint? collisionP = TestLineIntersection (
							ca [i],
							ca [(i + 1) % ca.Length],
							cb [j],
							cb [(j + 1) % cb.Length],
							cp.CollisionNormal);
						
						if (collisionP != null)
							result.Add (collisionP.Value);
					}
				}
			}

			result = PruneCollisionPoints (result);

			if (result.Count < 1)
				result.Add(cp);

			return result;
		}
		/// <summary>
		/// Tests the point is on plane.
		/// </summary>
		/// <returns>The point is on plane.</returns>
		/// <param name="ca">Ca.</param>
		/// <param name="cb">Cb.</param>
		/// <param name="initPoint">Init point.</param>
		/// <param name="na">Na.</param>
		private List<CollisionPoint> TestPointIsOnPlane(
			Vector3[] ca,
			Vector3[] cb,
			CollisionPoint initPoint,
			Vector3 na)
		{
			var result = new List<CollisionPoint>();

			if (cb.Length > 2)
			{
				for (int i = 0; i < ca.Length; i++)
				{
					Vector3 project = ca[i] -
						(na * (Vector3.Dot(na, ca[i]) +
							Vector3.Dot(na * -1.0, initPoint.CollisionPointB)));

					double angle = GeometryUtilities.TestPointInsidePolygon(
						cb,
						project,
						na,
						initPoint.CollisionPointB);

					//Inserito il minore per gestire problemi di approssimazione
					if (angle + ManifoldStabilizeValue >= 2.0 * Math.PI)
					{
						var cp = new CollisionPoint(
							ca[i],
							project,
							na);
						result.Add(cp);
					}
				}
			}

			if (ca.Length > 2)
			{
				for (int i = 0; i < cb.Length; i++)
				{
					Vector3 project = cb[i] -
						(na * (Vector3.Dot(na, cb[i]) +
							Vector3.Dot(na * -1.0, initPoint.CollisionPointA)));

					double angle = GeometryUtilities.TestPointInsidePolygon(
						ca,
						project,
						na,
						initPoint.CollisionPointA);

					if (angle + ManifoldStabilizeValue >= 2.0 * Math.PI)
					{
						var cp = new CollisionPoint(
							project,
							cb[i],
							na);
						result.Add(cp);
					}
				}
			}

			return result;
		}