private Simplex GJK(Prism prismA, Prism prismB, Vector3 dir) { Simplex s = new Simplex(); //First point on the edge of the minkowski difference. 1-Simplex.\ s.Add(GetSupport(prismA, prismB, dir)); //Compute negative direction of d directionVector = -dir; while (true) { //Add the point to the simplex. 2-Simplex. s.Add(GetSupport(prismA, prismB, directionVector)); if (Vector3.Dot(s.GetLast(), directionVector) <= 0) { //point does not pass origin so do not add it. return(null); } else { //CheckOrigin automatically updates the directionVector on every call. If it doesnt that means we found the origin. if (CheckOrigin(s, directionVector)) { return(s); } } } }
//public void SetDirectionX(string s) { // float x; // float.TryParse(s, out x); // direction = new Vector3(x, direction.y, direction.z); //} //public void SetDirectionY(string s) { // float y; // float.TryParse(s, out y); // direction = new Vector3(direction.x, y, direction.z); //} //public void SetDirectionZ(string s) { // float z; // float.TryParse(s, out z); // direction = new Vector3(direction.x, direction.y, z); //} public static bool Intersects(IConvexRegion regionOne, Transform oneTrans, IConvexRegion regionTwo, Transform twoTrans, GJKState state) { // Get an initial point on the Minkowski difference. Vector3 s = Support(regionOne, regionTwo, twoTrans.position - oneTrans.position, state, out sa); // Create our initial simplex. Simplex simplex = new Simplex(); simplex.Add(s); // TODO: Choose an initial direction. direction = -s; state.simplices.Add(simplex.Clone()); // Choose a maximim number of iterations to avoid an // infinite loop during a non-convergent search. int maxIterations = 32; for (int i = 0; i < maxIterations; i++) { // Get our next simplex point toward the origin. Vector3 a = Support(regionOne, regionTwo, direction, state, out sa, out sb); // If we move toward the origin and didn't pass it // then we never will and there's no intersection. if (a.IsInOppositeDirection(direction)) { return false; } // otherwise we add the new point to the simplex and process it. simplex.Add(a); state.simplices.Add(simplex.Clone()); // Here we either find a collision or we find the closest feature of // the simplex to the origin, make that the new simplex and update the direction // to move toward the origin from that feature. if (ProcessSimplex(simplex, ref direction)) { float tolerance = 0.000001f; // Or another such small number while (true) { Edge e = simplex.FindClosestEdge(); Vector3 p = Support(regionOne, regionTwo, e.normal, state, out sa, out sb); float proj = Vector3.Dot(p, e.normal); if (proj - e.distance < tolerance) { } } else { } } return true; }
private Vector3 EPA(Simplex s, Prism A, Prism B) { while (true) { var e = s.ClosestEdge(); var point = GetSupport(A, B, e.direction); var d = Vector3.Dot(point, e.direction); if (d - e.distance < Mathf.Pow(10, 6)) { Debug.Log(s.numPoints); var normal = e.direction; var depth = d; print("in epa"); return(normal * depth); } else { // Insert point between points in closestEdge. Debug.Log("in epa adding point" + point + " " + e.index); s.Add(point, e.index); } } }
private Simplex GJK(Prism prismA, Prism prismB, Vector3 dir) { Simplex s = new Simplex { direction = dir }; //First point on the edge of the minkowski difference. 1-Simplex. s.Add(GetSupport(prismA, prismB, dir)); //s.points.ForEach(x => print(x)); //Point in opposite direction s.direction = -s.direction; while (true) { //Add the point to the simplex. 2-Simplex. s.Add(GetSupport(prismA, prismB, s.direction)); if (Vector3.Dot(s.GetLast(), s.direction) <= 0) { //point does not pass origin so do not add it. return(null); } else { //CheckOrigin automatically updates the direction parameter on every call. If it doesnt that means we found the origin. if (CheckOrigin(s)) { print("found origin"); return(s); } Debug.Log(prismA.name + " " + prismB.name); } } }
public static bool GJK(MGFObject a, MGFObject b) { Simplex s = new Simplex(); Fix64Vector2 dir = a.GetPos() - b.GetPos(); s.Add(Support(a, b, dir)); dir = -dir; while (true) { s.Add(Support(a, b, dir)); if (Fix64Vector2.Dot(s.GetLast(), dir) <= Fix64.Zero) { return(false); } else { if (ContainsOrigin(s, ref dir)) { return(true); } } } }
private Vector3 EPA(Simplex s, Prism A, Prism B) { while (true) { Edge e = s.ClosestEdge(); var point = GetSupport(A, B, e.normal); float d = Vector3.Dot(point, e.normal); if (d - e.distance < Mathf.Pow(10, 5)) { var normal = e.normal; var depth = d; return(normal * depth); } else { // we haven't reached the edge of the Minkowski Difference // so continue expanding by adding the new point to the simplex // in between the points that made the closest edge s.Add(point, e.index); } } }