public void AddCollisionPoint(CollisionPoint point) { var lst = CollisionPoints.ToList(); lst.Add(point); CollisionPoints = lst.ToArray(); }
/// <summary> /// Detects the collision. /// </summary> /// <returns>The collision.</returns> /// <param name="objectA">Object a.</param> /// <param name="objectB">Object b.</param> public GJKOutput Execute( VertexProperties[] vertexObjA, VertexProperties[] vertexObjB) { var collisionPoint = new CollisionPoint(); var collisionNormal = new Vector3d(); var supportTriangles = new List <SupportTriangle>(); var centroid = new Vector3d(); bool isIntersection = false; //TODO refactoring double collisionDistance = ExecuteGJKAlgorithm( vertexObjA, vertexObjB, ref collisionNormal, ref collisionPoint, ref supportTriangles, ref centroid, ref isIntersection); return(new GJKOutput( collisionDistance, collisionPoint, collisionNormal, centroid, isIntersection, supportTriangles)); }
public List <CollisionPoint> GetManifoldPoints( Vector3d[] vertexObjA, Vector3d[] vertexObjB, CollisionPoint collisionPoint) { if (ManifoldPointNumber > 1) { List <Vector3d> collisionA = GetNearestPoint( vertexObjA, collisionPoint.CollisionPointA.Vertex, collisionPoint.CollisionNormal); List <Vector3d> collisionB = GetNearestPoint( vertexObjB, collisionPoint.CollisionPointB.Vertex, collisionPoint.CollisionNormal); List <CollisionPoint> collisionPointsList = FindCollisionPoints( collisionA.ToArray(), collisionB.ToArray(), collisionPoint); collisionA.Clear(); collisionB.Clear(); SetCollisionNormal(collisionPointsList, collisionPoint.CollisionNormal); return(collisionPointsList); } return(null); }
public CollisionPointBaseStructure( CollisionPoint collisionPoint, CollisionPoint[] collisionPoints) { CollisionPoint = collisionPoint; CollisionPoints = collisionPoints; }
private CollisionPoint TestEdgesIntersection( Vector3d p1, Vector3d p2, Vector3d p3, Vector3d p4, CollisionPoint point) { var a = new Vector3d(); var b = new Vector3d(); double mua = 0.0; double mub = 0.0; if (GeometryUtils.TestEdgesIntersect( p1, p2, p3, p4, ref a, ref b, ref mua, ref mub)) { if (!(mua < 0.0 || mua > 1.0 || mub < 0.0 || mub > 1.0)) { return(new CollisionPoint( new VertexProperties(a), new VertexProperties(b), point.CollisionNormal, 0.0, false)); } } return(null); }
/// <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( VertexProperties[] vertexObjA, VertexProperties[] vertexObjB, List <SupportTriangle> startTriangles, Vector3d centroid) { EngineCollisionPoint epaCollisionPoint = ExecuteEngine( vertexObjA, vertexObjB, startTriangles, centroid); var distance = Vector3d.Length(epaCollisionPoint.Dist); var collisionPoint = new CollisionPoint( epaCollisionPoint.A, epaCollisionPoint.B, epaCollisionPoint.Normal, distance, true); return(new EPAOutput( distance, collisionPoint)); }
public GJKOutput( double collisionDistance, CollisionPoint collisionPoint, Vector3d collisionNormal, Vector3d centroid, bool intersection, List <SupportTriangle> supportTriangles) { CollisionDistance = collisionDistance; CollisionPoint = collisionPoint; CollisionNormal = collisionNormal; Centroid = centroid; Intersection = intersection; SupportTriangles = supportTriangles; }
/// <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( VertexProperties[] vertexShape1, VertexProperties[] vertexShape2, ref Vector3d collisionNormal, ref CollisionPoint cp, ref List <SupportTriangle> triangles, ref Vector3d centroid, ref bool isIntersection) { double minDistance = double.MaxValue; int minTriangleIndex = -1; var result = new EngineCollisionPoint(); var oldDirection = new Vector3d(); var simplex = new Simplex(); //Primo punto del simplex simplex.Support.Add(GetFarthestPoint(vertexShape1, vertexShape2, vertexShape2.Length / 2)); //Secondo punto del simplex Vector3d direction = Vector3d.Normalize(simplex.Support[0].s * -1.0); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { return(-1.0); } //Terzo punto del simplex direction = Vector3d.Normalize(GetDirectionOnSimplex2(simplex)); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { return(-1.0); } //Quarto punto del simplex direction = Vector3d.Normalize(GeometryUtils.CalculateTriangleNormal( simplex.Support[0].s, simplex.Support[1].s, simplex.Support[2].s)); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, -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); } Vector3d triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex); result.SetDist(triangleDistance); result.SetNormal(Vector3d.Normalize(triangleDistance)); Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], vertexShape1, vertexShape2, ref result); minDistance = triangleDistance.Length(); for (int i = 0; i < MaxIterations; i++) { direction = -1.0 * triangleDistance.Normalize(); if (Vector3d.Length(direction) < constTolerance) { direction = origin - centroid; } if (direction == oldDirection) { break; } oldDirection = direction; if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { for (int j = 0; j < triangles.Count; j++) { direction = triangles[j].Normal; if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { if (simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, -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(triangles[minTriangleIndex].Normal); Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], vertexShape1, vertexShape2, ref result); minDistance = mod; } } collisionNormal = -1.0 * result.Normal; cp = new CollisionPoint( result.A, result.B, collisionNormal, 0.0, false); return(minDistance); }
private List <CollisionPoint> ExtractFourCollisionPoints( List <CollisionPoint> cpList, Vector3d normal) { if (cpList.Count > 4) { var result = new List <CollisionPoint>(); //Point 1 Vector3d A = cpList[0].CollisionPointA.Vertex; result.Add(cpList[0]); cpList.RemoveAt(0); //Point 2 double maxDist = double.MinValue; CollisionPoint farthestPoint = null; int index = -1; for (int i = 0; i < cpList.Count; i++) { double dist = (cpList[i].CollisionPointA.Vertex - A).Length(); if (dist > maxDist) { maxDist = dist; farthestPoint = cpList[i]; index = i; } } Vector3d B = farthestPoint.CollisionPointA.Vertex; result.Add(farthestPoint); cpList.RemoveAt(index); //Point 3 double maxArea = double.MinValue; CollisionPoint third = null; index = -1; for (int i = 0; i < cpList.Count; i++) { double area = CalculateArea(A, B, cpList[i].CollisionPointA.Vertex, normal); if (area > maxArea) { maxArea = area; third = cpList[i]; index = i; } } Vector3d C = third.CollisionPointA.Vertex; result.Add(third); cpList.RemoveAt(index); //Point 4 CollisionPoint fourth = null; maxArea = double.MinValue; for (int i = 0; i < cpList.Count; i++) { // A-B-D double area = CalculateArea(A, B, cpList[i].CollisionPointA.Vertex, normal); if (area > maxArea) { maxArea = area; fourth = cpList[i]; } // A-C-D area = CalculateArea(A, C, cpList[i].CollisionPointA.Vertex, normal); if (area > maxArea) { maxArea = area; fourth = cpList[i]; } // B-C-D area = CalculateArea(B, C, cpList[i].CollisionPointA.Vertex, normal); if (area > maxArea) { maxArea = area; fourth = cpList[i]; } } result.Add(fourth); return(result); } return(cpList); }
/// <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( Vector3d[] ca, Vector3d[] cb, CollisionPoint initPoint, Vector3d na) { var result = new List <CollisionPoint>(); if (cb.Length > 2) { for (int i = 0; i < ca.Length; i++) { Vector3d project = ca[i] - (na * (Vector3d.Dot(na, ca[i]) + Vector3d.Dot(na * -1.0, initPoint.CollisionPointB.Vertex))); double angle = GeometryUtils.TestPointInsidePolygon( cb, project, na, initPoint.CollisionPointB.Vertex); //Inserito il minore per gestire problemi di approssimazione if (angle + ManifoldStabilizeValue >= ConstValues.PI2) { var cp = new CollisionPoint( new VertexProperties(ca[i]), new VertexProperties(project), na, 0.0, false); result.Add(cp); } } } if (ca.Length > 2) { for (int i = 0; i < cb.Length; i++) { Vector3d project = cb[i] - (na * (Vector3d.Dot(na, cb[i]) + Vector3d.Dot(na * -1.0, initPoint.CollisionPointA.Vertex))); double angle = GeometryUtils.TestPointInsidePolygon( ca, project, na, initPoint.CollisionPointA.Vertex); if (angle + ManifoldStabilizeValue >= ConstValues.PI2) { var cp = new CollisionPoint( new VertexProperties(project), new VertexProperties(cb[i]), na, 0.0, false); result.Add(cp); } } } return(result); }
private List <CollisionPoint> FindCollisionPoints( Vector3d[] ca, Vector3d[] cb, CollisionPoint cp) { var result = new List <CollisionPoint> (); if (ca.Length == 2 && cb.Length == 2) { CollisionPoint collisionP = TestEdgesIntersection( ca [0], ca [1], cb [0], cb [1], cp); if (collisionP != null) { result.Add(collisionP); } } else if (ca.Length > 2 && cb.Length == 2) { ca = GeometryUtils.TurnVectorClockWise( ca, cp.CollisionNormal); result.AddRange(TestPointIsOnPlane( ca, cb, cp, cp.CollisionNormal)); if (result.Count < ca.Length) { for (int i = 0; i < ca.Length; i++) { CollisionPoint collisionP = TestEdgesIntersection( ca [i], ca [(i + 1) % ca.Length], cb [0], cb [1], cp); if (collisionP != null) { result.Add(collisionP); } } } } else if (ca.Length == 2 && cb.Length > 2) { cb = GeometryUtils.TurnVectorClockWise( cb, cp.CollisionNormal); result.AddRange(TestPointIsOnPlane( ca, cb, cp, cp.CollisionNormal)); if (result.Count < cb.Length) { for (int i = 0; i < cb.Length; i++) { CollisionPoint collisionP = TestEdgesIntersection( ca [0], ca [1], cb [i], cb [(i + 1) % cb.Length], cp); if (collisionP != null) { result.Add(collisionP); } } } } else if (ca.Length > 2 && cb.Length > 2) { ca = GeometryUtils.TurnVectorClockWise( ca, cp.CollisionNormal); cb = GeometryUtils.TurnVectorClockWise( cb, cp.CollisionNormal); result.AddRange(TestPointIsOnPlane( ca, cb, cp, cp.CollisionNormal)); for (int i = 0; i < ca.Length; i++) { for (int j = 0; j < cb.Length; j++) { CollisionPoint collisionP = TestEdgesIntersection( ca [i], ca [(i + 1) % ca.Length], cb [j], cb [(j + 1) % cb.Length], cp); if (collisionP != null) { result.Add(collisionP); } } } } else { result.Add(cp); } if (ManifoldPointNumber == 4 && result.Count > 4) { result = ExtractFourCollisionPoints(result, cp.CollisionNormal); } else { result = ExtractCollisionPoints(result, cp.CollisionNormal); } return(result); }