void OnDrawGizmos() { if (Inc) { DebugI++; Inc = false; } Tri = -1; if (!gen()) { ; // return; } Gizmos.color = Color.white; foreach (Node n in Nodes) { drawCross(n.Centre); for (int i = 3; i-- > 0;) { var o = n.Nbrs[i]; if (o != null) { Gizmos.DrawLine(n.Centre, o.Centre); } } } if (Nodes.Count == 0 || !Obj1 || !Obj2) { return; } Gizmos.color = Color.red; Node a = findNode(Obj1.position), b = findNode(Obj2.position); if (a == null || b == null) { return; } SortedList <float, SearchNode> search = new SortedList <float, SearchNode>(new DuplicateKeyComparer <float>()); Vector2 desP = Obj2.position, strtP = Obj1.position; var cur = new SearchNode(); cur.D = 0; cur.N = a; cur.Prev = null; cur.P = Obj1.position; cur.N.SN = cur; float cD = float.MaxValue; SearchNode cP = null; for (; ;) { var n = cur.N; for (int i = 3; i-- > 0;) { var o = n.Nbrs[i]; if (o == null) { continue; } if (o.SN != null) { // var d = cur.D + (cur.P - sn.P).magnitude; continue; } // Gizmos.DrawLine(cur.P, n.Edge[i] ); var sn = new SearchNode(); sn.P = n.Edge[i]; sn.D = cur.D + (cur.P - sn.P).magnitude; sn.N = o; sn.Prev = cur; sn.N.SN = sn; sn.NbrI = i; var d = sn.D + (desP - sn.P).magnitude; if (d > cD) { continue; } if (o == b) { cD = d; cP = sn; // Gizmos.DrawLine(sn.P, desP); continue; } search.Add(d, sn); } if (search.Count == 0) { break; } cur = search.Values[search.Count - 1]; if (search.Keys[search.Count - 1] > cD) { break; } search.RemoveAt(search.Count - 1); } if (cP == null) { return; } List <SmoothNode> smooth = new List <SmoothNode>(); //Gizmos.DrawLine( desP, cP.P ); SmoothNode smt = new SmoothNode(); smt.P = desP; smooth.Add(smt); for (; ;) { var n = cP.Prev; if (n != null) { //Gizmos.DrawLine( n.P, cP.P ); // Gizmos.DrawLine(Verts[ n.N.Vi[ cP.NbrI ] ],Verts[ n.N.Vi[ (cP.NbrI+2)%3 ] ] ); smt = new SmoothNode(); smt.P = cP.P; smt.E1 = Verts[n.N.Vi[cP.NbrI]]; smt.E2 = Verts[n.N.Vi[(cP.NbrI + 2) % 3]];// -smt.E1; smooth.Add(smt); cP = n; } else { //Gizmos.DrawLine( strtP, cP.P ); smt = new SmoothNode(); smt.P = strtP; smooth.Add(smt); break; } } Gizmos.color = Color.red; /* * for(int iter =10; iter-->0;) { * // break; * for(int i = smooth.Count-1;--i > 0;) { * smt = smooth[i]; * * * smt.P = (smt.P*0 + smooth[i+1].P+ smooth[i-1].P )*0.5f; * * //var op = smt.P; * * // Util.lineLineIntersection( smooth[i+1].P, smooth[i-1].P, smt.E1, smt.E1 + smt.E2, ref smt.P ); * * var vec = smt.P - smt.E1; * var dt = Vector2.Dot( vec, smt.E2 ); dt /= smt.E2.sqrMagnitude; * * * * * dt = Mathf.Clamp01( dt ); * // Gizmos.DrawLine( smt.E1, smt.E1 + smt.E2 ); * * // Gizmos.DrawLine(smooth[i + 1].P, smooth[i - 1].P); * smt.P =smt.E1 + smt.E2*dt; * * // Gizmos.DrawLine(smt.P, op ); * break; * } * break; * } * * Gizmos.color = Color.green; * for(int i = smooth.Count-1;i-- > 0;) { * Gizmos.DrawLine( smooth[i+1].P, smooth[i].P ); * } */ Vector2 vec, fnlA, fnlB; Vector2 tPos = smooth[0].P, cPos = smooth[smooth.Count - 1].P; int CurNode = smooth.Count - 2; // tPos = smooth[CurNode].P; fnlA = smooth[CurNode].E1; fnlB = smooth[CurNode].E2; float sgn = -1; for (int ci = CurNode - 1; ; ci--) { if (ci <= 0) { break; } //Vector2 tPos = smooth[CurNode].P; Vector2 fnlA2 = smooth[ci].E1; Vector2 fnlB2 = smooth[ci].E2; if ((fnlA2 - fnlA).sqrMagnitude > (fnlB2 - fnlB).sqrMagnitude) { Debug.DrawLine(cPos, fnlA2, Color.black); sgn = Util.sign(fnlA2, cPos, fnlA); if (Util.sign(fnlA2, cPos, fnlA) > 0) { if (Util.sign(fnlB, cPos, fnlA) < 0) { tPos = fnlB; break; } fnlA = fnlA2; } else { // tPos = fnlA; // break; } } else { Debug.DrawLine(cPos, fnlB2, Color.black); sgn = Util.sign(fnlB, cPos, fnlB2); if (Util.sign(fnlB, cPos, fnlB2) > 0) { if (Util.sign(fnlB, cPos, fnlA) < 0) { // tPos = fnlA; // break; } fnlB = fnlB2; } else { // tPos = fnlB; // break; } } } if (Util.sign(fnlB, cPos, tPos) < 0) { tPos = fnlB; } if (Util.sign(tPos, cPos, fnlA) < 0) { tPos = fnlA; } // Debug.Log("sgn " + sgn); Debug.DrawLine(cPos, fnlA, Color.green); Debug.DrawLine(cPos, fnlB, Color.red); Debug.DrawLine(cPos, tPos, Color.white); }
public Path getPath(Vector2 from, Vector2 to, Node toNode) { if (Nodes.Count == 0) { return(null); } Node a = findNode(from), b = toNode; if (a == null || b == null || a == b) { return(null); } SortedList <float, SearchNode> search = new SortedList <float, SearchNode>(new DuplicateKeyComparer <float>()); var cur = new SearchNode(); cur.D = 0; cur.N = a; cur.Prev = null; cur.P = from; cur.N.SN = cur; float cD = float.MaxValue; SearchNode cP = null; for (; ;) { var n = cur.N; for (int i = 3; i-- > 0;) { var o = n.Nbrs[i]; if (o == null) { continue; } if (o.SN != null) { // var d = cur.D + (cur.P - sn.P).magnitude; continue; } // Gizmos.DrawLine(cur.P, n.Edge[i]); var sn = new SearchNode(); sn.P = n.Edge[i]; sn.D = cur.D + (cur.P - sn.P).magnitude; sn.N = o; sn.Prev = cur; sn.N.SN = sn; sn.NbrI = i; var d = sn.D + (to - sn.P).magnitude; if (d > cD) { continue; } if (o == b) { cD = d; cP = sn; // Gizmos.DrawLine(sn.P, to); continue; } search.Add(d, sn); } if (search.Count == 0) { break; } cur = search.Values[search.Count - 1]; if (search.Keys[search.Count - 1] > cD) { break; } search.RemoveAt(search.Count - 1); } foreach (Node n in Nodes) { n.SN = null; } if (cP == null) { return(null); } Path ret = new Path(); List <SmoothNode> smooth = ret.Smooth = new List <SmoothNode>(); // Gizmos.color = Color.green; //Gizmos.DrawLine( desP, cP.P ); SmoothNode smt = new SmoothNode(); smt.P2 = smt.P = to; smooth.Add(smt); for (; ;) { var n = cP.Prev; if (n != null) { //Gizmos.DrawLine( n.P, cP.P ); // Gizmos.DrawLine(Verts[ n.N.Vi[ cP.NbrI ] ],Verts[ n.N.Vi[ (cP.NbrI+2)%3 ] ] ); smt = new SmoothNode(); smt.P2 = smt.P = cP.P; smt.E1 = Verts[n.N.Vi[cP.NbrI]]; smt.E2 = Verts[n.N.Vi[(cP.NbrI + 2) % 3]];// -smt.E1; smooth.Add(smt); cP = n; } else { //Gizmos.DrawLine( strtP, cP.P ); smt = new SmoothNode(); smt.P2 = smt.P = from; smooth.Add(smt); break; } } return(ret); for (int iter = 60; iter-- > 0;) { /* for(int i = smooth.Count - 1; --i > 0; ) { * smt = smooth[i]; * smt.P2 = (smooth[i + 1].P + smooth[i - 1].P) * 0.5f; * * var vec = smt.P2 - smt.E1; * var dt = Vector2.Dot(vec, smt.E2); dt /= smt.E2.sqrMagnitude; * dt = Mathf.Clamp01(dt); * // Gizmos.DrawLine( smt.E1, smt.E1 + smt.E2 ); * smt.P2 = smt.E1 + smt.E2 * dt; * } * for(int i = smooth.Count - 1; --i > 0; ) { * smt = smooth[i]; * smt.P = (smooth[i + 1].P2 + smooth[i - 1].P2) * 0.5f; * * var vec = smt.P - smt.E1; * var dt = Vector2.Dot(vec, smt.E2); dt /= smt.E2.sqrMagnitude; * dt = Mathf.Clamp01(dt); * // Gizmos.DrawLine( smt.E1, smt.E1 + smt.E2 ); * smt.P = smt.E1 + smt.E2 * dt; * }*/ for (int i = smooth.Count - 1; --i > 0;) { smt = smooth[i]; smt.P = (smt.P * 0.5f + smooth[i + 1].P + smooth[i - 1].P) * 0.4f; var vec = smt.P - smt.E1; var dt = Vector2.Dot(vec, smt.E2); dt /= smt.E2.sqrMagnitude; float edge = 0.25f / smt.E2.magnitude; dt = Mathf.Clamp(dt, edge, 1.0f - edge); // Gizmos.DrawLine( smt.E1, smt.E1 + smt.E2 ); smt.P = smt.E1 + smt.E2 * dt; break; } break; } return(ret); /* * for(int i = smooth.Count - 1; i-- > 0; ) { * Gizmos.DrawLine(smooth[i + 1].P, smooth[i].P); * } */ }