Example #1
0
		private void GetEpaVertexFromMinkowsky(
			SupportTriangle triangle,
			SimulationObject shape1,
			SimulationObject shape2,
			ref EngineCollisionPoint epaCollisionPoint)
		{
			Vector3 a1 = Helper.GetVertexPosition(shape1, triangle.a.a);
			Vector3 ba1 = Helper.GetVertexPosition(shape1, triangle.b.a) - a1;
			Vector3 ca1 = Helper.GetVertexPosition(shape1, triangle.c.a) - a1;

			Vector3 a2 = Helper.GetVertexPosition(shape2, triangle.a.b);
			Vector3 ba2 = Helper.GetVertexPosition(shape2, triangle.b.b) - a2;
			Vector3 ca2 = Helper.GetVertexPosition(shape2, triangle.c.b) - a2;

			epaCollisionPoint.SetA (a1 + (ba1 * triangle.s) + (ca1 * triangle.t));
			epaCollisionPoint.SetB (a2 + (ba2 * triangle.s) + (ca2 * triangle.t));
		}
Example #2
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;
		}
Example #3
0
		/// <summary>
		/// Executes the EPA engine.
		/// </summary>
		/// <param name="shape1">Shape1.</param>
		/// <param name="shape2">Shape2.</param>
		/// <param name="startPoint">Start point.</param>
		private EngineCollisionPoint ExecuteEngine(
			SimulationObject shape1,
			SimulationObject shape2,
            int geometryIndexA,
            int geometryIndexB,
            List<SupportTriangle> triangles,
			Vector3 centroid)
		{
			var epaCollisionPoint = new EngineCollisionPoint();

			double s = 0.0;
			double t = 0.0;

			var direction = new Vector3 ();
			var oldDirection = new Vector3 ();
			var vDistance = new Vector3 ();

			SupportTriangle epaBuffer;

			if (triangles.Count > 0) 
			{
				for (int k = 0; k < MaxIterations; k++) 
				{
					double minDistance = double.MaxValue;

					for (int i = 0; i < triangles.Count; i++) 
					{
						epaBuffer = triangles [i];

						if (!GeometryUtilities.TestCollinearity (
							    epaBuffer.a.s,
							    epaBuffer.b.s,
							    epaBuffer.c.s)) 
						{
							vDistance = GeometryUtilities.GetPointTriangleIntersection (
								epaBuffer.a.s,
								epaBuffer.b.s,
								epaBuffer.c.s,
								origin,
								ref s,
								ref t).Value;
							
							epaBuffer.SetValueS (s);
							epaBuffer.SetValueT (t);
						} 
						else 
						{
							continue;
						}
							
						triangles [i] = epaBuffer;

						double distance = Vector3.Length (vDistance);

						if (distance < minDistance) 
						{
							minDistance = distance;

							direction = vDistance;
							epaCollisionPoint.SetDist (vDistance);
							epaCollisionPoint.SetNormal (Vector3.Normalize (vDistance));

							GetEpaVertexFromMinkowsky(
								triangles[i],
								shape1,
								shape2,
								ref epaCollisionPoint);
						}
					}

					//L'origine risiede su uno dei bordi del triangolo
					if (Vector3.Length(direction) < constTolerance)
					{
						direction = origin - centroid;
					}

					if (direction == oldDirection)
						break;

                    Support vt = Helper.GetMinkowskiFarthestPoint(
                             shape1,
                             shape2,
                             geometryIndexA,
                             geometryIndexB,
                             direction.Normalize());

                    triangles = Helper.AddPointToConvexPolygon (
						triangles,
                        vt,
						centroid);

					oldDirection = direction;
				}
			}
			triangles.Clear ();

			return epaCollisionPoint;
		}