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);
* } */
}