void MakeSureThatNodeBoundariesAreInVisGraph(IEnumerable <LgNodeInfo> nodeInfos)
{
foreach (var nodeInfo in nodeInfos)
{
foreach (var polypoint in nodeInfo.BoundaryOnLayer.PolylinePoints)
{
_visGraph.AddVertex(polypoint.Point);
}
}
}
internal VisibilityVertex GetOrFindVisibilityVertex(Point p)
{
var v = _visGraph.FindVertex(p);
if (v != null)
{
return(v);
}
v = _visGraph.AddVertex(p);
RegisterInTree(v);
return(v);
}
private static VisibilityVertex GetCrossingInteriorVertex(VisibilityGraph vg, VisibilityVertex crossingVertex,
GroupBoundaryCrossing crossing)
{
Point interiorPoint = crossing.GetInteriorVertexPoint(crossingVertex.Point);
return(vg.FindVertex(interiorPoint) ?? vg.AddVertex(interiorPoint));
}
private void LoadEndOverlapVertexIfNeeded(VisibilityGraph vg)
{
// See comments in LoadStartOverlapVertexIfNeeded.
if (NeedEndOverlapVertex)
{
VisibilityVertex vertex = vg.FindVertex(End);
AppendVisibilityVertex(vg, vertex ?? vg.AddVertex(End));
}
}
private void LoadStartOverlapVertexIfNeeded(VisibilityGraph vg)
{
// For adjacent segments with different IsOverlapped, we need a vertex that
// joins the two so a path may be run. This is paired with the other segment's
// LoadEndOverlapVertexIfNeeded.
if (NeedStartOverlapVertex)
{
VisibilityVertex vertex = vg.FindVertex(Start);
AppendVisibilityVertex(vg, vertex ?? vg.AddVertex(Start));
}
}
private bool AppendGroupCrossingsThroughPoint(VisibilityGraph vg, Point lastPoint)
{
if (null == GroupBoundaryPointAndCrossingsList)
{
return(false);
}
bool found = false;
while (GroupBoundaryPointAndCrossingsList.CurrentIsBeforeOrAt(lastPoint))
{
// We will only create crossing Edges that the segment actually crosses, not those it ends before crossing.
// For those terminal crossings, the adjacent segment creates the interior vertex and crossing edge.
PointAndCrossings pac = GroupBoundaryPointAndCrossingsList.Pop();
GroupBoundaryCrossing[] lowDirCrossings = null;
GroupBoundaryCrossing[] highDirCrossings = null;
if (PointComparer.Compare(pac.Location, Start) > 0)
{
lowDirCrossings = PointAndCrossingsList.ToCrossingArray(pac.Crossings,
ScanDirection.OppositeDirection);
}
if (PointComparer.Compare(pac.Location, End) < 0)
{
highDirCrossings = PointAndCrossingsList.ToCrossingArray(pac.Crossings, ScanDirection.Direction);
}
found = true;
VisibilityVertex crossingVertex = vg.FindVertex(pac.Location) ?? vg.AddVertex(pac.Location);
if ((null != lowDirCrossings) || (null != highDirCrossings))
{
AddLowCrossings(vg, crossingVertex, lowDirCrossings);
AddHighCrossings(vg, crossingVertex, highDirCrossings);
}
else
{
// This is at this.Start with only lower-direction toward group interior(s), or at this.End with only
// higher-direction toward group interior(s). Therefore an adjacent ScanSegment will create the crossing
// edge, so create the crossing vertex here and we'll link to it.
if (null == LowestVisibilityVertex)
{
SetInitialVisibilityVertex(crossingVertex);
}
else
{
Debug.Assert(PointComparer.Equal(End, crossingVertex.Point), "Expected this.End crossingVertex");
AppendHighestVisibilityVertex(crossingVertex);
}
}
}
return(found);
}
internal void CreateSparseVerticesAndEdges(VisibilityGraph vg)
{
if (this.sparsePerpendicularCoords == null)
{
return;
}
AppendGroupCrossingsThroughPoint(vg, Start);
foreach (var perpCoord in this.sparsePerpendicularCoords.OrderBy(d => d))
{
var vertexLocation = this.CreatePointFromPerpCoord(perpCoord);
Debug.Assert(this.ContainsPoint(vertexLocation), "vertexLocation is not on Segment");
this.AppendVisibilityVertex(vg, vg.FindVertex(vertexLocation) ?? vg.AddVertex(vertexLocation));
}
AppendGroupCrossingsThroughPoint(vg, End);
GroupBoundaryPointAndCrossingsList = null;
this.sparsePerpendicularCoords.Clear();
this.sparsePerpendicularCoords = null;
}
private void Initialize()
{
obstacles = new List <LineRenderer>();
obstacles.Add(Obstacle1);
obstacles.Add(Obstacle2);
obstacles.Add(Obstacle3);
if (Random.value > 0.5f) // so we have 3-4 obstacles
{
obstacles.Add(Obstacle4);
}
else
{
Destroy(Obstacle4.gameObject);
}
obstaclePts = new Vector3[6];
obstacles[0].GetPositions(obstaclePts); // obstacles 1-4 have the same local points though and this returns local points
Color colorAAA = new Color(0.66f, 0.66f, 0.66f);
int[] boundaryReflexPtsIdx =
{
0, 3, 5, 8, 9, 12, 13, 16, 18,
21, 22, 25, 27, 30, 31, 34, 35, 38, 41
}; // boundary linerenderer vertices
int iters = boundaryReflexPtsIdx.Length;
boundaryReflexPts = new Vector3[iters];
int count = 0;
foreach (int idx in boundaryReflexPtsIdx)
{
boundaryReflexPts[count] = Boundaries.GetPosition(idx);
count++;
}
/* ADD VERTICES TO VISIBILITY GRAPH */
Graph = new VisibilityGraph();
foreach (LineRenderer obstacle in obstacles)
{
foreach (Vector3 obstaclePt in obstaclePts)
{
Graph.AddVertex(
obstacle.transform.TransformPoint(obstaclePt)
);
}
}
foreach (Vector3 pt in boundaryReflexPts)
{
Graph.AddVertex(pt);
}
var vertices = new List <Vector2>(Graph.Vertices);
// Debug.Log(VisibilityGraph.Vertices.Count);
// VisibilityGraph.PrintVertices();
/* DRAWING LINE SEGMENTS */
// Between obstacle adjcent reflex points
foreach (LineRenderer obstacle in obstacles)
{
int obstaclePtIdx = 0;
Vector3 obstaclePtPrev = obstaclePts[5];
foreach (Vector3 obstaclePt in obstaclePts)
{
if (obstaclePtIdx != 4) // 4th point is not reflex
{
Vector3 obstaclePtWorld
= obstacle.transform.TransformPoint(obstaclePt);
Vector3 obstaclePtWorldPrev
= obstacle.transform.TransformPoint(obstaclePtPrev);
Debug.DrawRay(
obstaclePtWorldPrev,
obstaclePtWorld - obstaclePtWorldPrev,
Color.red, 1000
);
Graph.AddEdge(obstaclePtWorldPrev, obstaclePtWorld);
obstaclePtPrev = obstaclePt;
}
obstaclePtIdx++;
}
}
// Between obstacles (bitangent)
for (int i = 0; i < obstacles.Count - 1; i++)
{
for (int j = i + 1; j < obstacles.Count; j++)
{
for (int k = 0; k < 6; k++)
{
for (int l = 0; l < 6; l++)
{
Vector3 obstaclePtWorld
= obstacles[i].transform.TransformPoint(
obstaclePts[k]
);
Vector3 obstaclePtWorldToCompare
= obstacles[j].transform.TransformPoint(
obstaclePts[l]
);
DrawVisibilityEdge(
obstaclePtWorld, obstaclePtWorldToCompare,
Color.yellow, true, true
);
}
}
}
}
// Boundaries to Boundaries
for (int i = 0; i < iters; i++)
{
for (int j = i + 1; j < iters; j++)
{
DrawVisibilityEdge(
boundaryReflexPts[i],
boundaryReflexPts[j],
colorAAA,
true,
true
);
}
}
DrawVisibilityEdge( // close the loop
boundaryReflexPts[iters - 1],
boundaryReflexPts[0],
colorAAA,
true,
true
);
// Boundaries to obstacles
for (int i = 0; i < iters; i++)
{
foreach (LineRenderer obstacle in obstacles)
{
int obstaclePtIdx = 0;
foreach (Vector3 obstaclePt in obstaclePts)
{
if (obstaclePtIdx != 4) // 4th point is not reflex
{
Vector3 obstaclePtWorld
= obstacle.transform.TransformPoint(obstaclePt);
DrawVisibilityEdge(
boundaryReflexPts[i],
obstaclePtWorld,
colorAAA,
true,
true
);
}
obstaclePtIdx++;
}
}
}
// VisibilityGraph.PrintEdges();
}