void AddEdge(Point a, Point b)
{
Debug.Assert(PortLocations.Contains(a));
/*********************
* A complication arises when we have overlaps. Loose obstacles become large enough to contain several
* ports. We need to avoid a situation when a port has degree more than one.
* To avoid this situation we redirect to b every edge incoming into a.
* Notice that we create a new graph for each AddDiriction call, so all this edges point roughly to the
* direction of the sweep and the above procedure just alignes the edges better.
* In the resulting graph, which contains the sum of the graphs passed to AddDirection, of course
* a port can have an incoming and outcoming edge at the same time
*******************/
VisibilityEdge ab = visibilityGraph.AddEdge(a, b);
VisibilityVertex av = ab.Source;
Debug.Assert(av.Point == a && ab.TargetPoint == b);
//all edges adjacent to a which are different from ab
VisibilityEdge[] edgesToFix =
av.InEdges.Where(e => e != ab).Concat(av.OutEdges.Where(e => e != ab)).ToArray();
foreach (VisibilityEdge edge in edgesToFix)
{
Point c = (edge.Target == av ? edge.Source : edge.Target).Point;
VisibilityGraph.RemoveEdge(edge);
visibilityGraph.AddEdge(c, b);
}
}
internal VisibilityEdge AddVisGraphEdge(Point ep0, Point ep1)
{
var v0 = GetOrFindVisibilityVertex(ep0) ?? AddNewVertex(ep0);
var v1 = GetOrFindVisibilityVertex(ep1) ?? AddNewVertex(ep1);
return(VisibilityGraph.AddEdge(v0, v1));
}
public void AddVisEdge(VisibilityVertex v0, VisibilityVertex v1)
{
if (v0 != v1)
{
VisibilityGraph.AddEdge(v0, v1);
}
}
void AddIntersectionOfBothDirectionSweepsToTheResult(VisibilityGraph vg0, VisibilityGraph vg1)
{
foreach (var edge in vg0.Edges)
{
if (vg1.FindEdge(edge.SourcePoint, edge.TargetPoint) != null)
{
_visibilityGraph.AddEdge(edge.SourcePoint, edge.TargetPoint);
}
}
}
public void AddVisEdge(VisibilityVertex v0, VisibilityVertex v1)
{
if (v0 != v1)
{
VisibilityGraph.AddEdge(v0, v1);
}
else
{
Debug.WriteLine("avoiding creating a small visibility edge");
}
}
internal void AddVisGraphEdgesFromNodeCenterToNodeBorder(LgNodeInfo nodeInfo)
{
var vc = VisGraph.AddVertex(nodeInfo.Center);
vc.IsTerminal = true; // we don't need to register this node in the tree
foreach (var pt in nodeInfo.BoundaryOnLayer.PolylinePoints)
{
var vv = GetOrFindVisibilityVertex(pt.Point);
var edge = VisibilityGraph.AddEdge(vc, vv);
edge.IsPassable = () => EdgeIsPassable(edge);
}
}
private void RestoreRemovedEdges()
{
foreach (var edge in this.edgesToRestore)
{
// We should only put TransientVisibilityEdges in this list, and should never encounter
// a non-transient edge in the graph after we've replaced it with a transient one, so
// the edge should not be in the graph until we re-insert it.
Debug.Assert(!(edge is TollFreeVisibilityEdge), "Unexpected Transient edge");
VisibilityGraph.AddEdge(edge);
this.DevTrace_VerifyEdge(edge);
}
this.edgesToRestore.Clear();
}
private VisibilityEdge AddVisibilityEdge(VisibilityVertex source, VisibilityVertex target)
{
Debug.Assert(source.Point != target.Point, "Self-edges are not allowed");
Debug.Assert(PointComparer.IsPureLower(source.Point, target.Point), "Impure or reversed direction encountered");
// Make sure we aren't adding two edges in the same direction to the same vertex.
Debug.Assert(null == StaticGraphUtility.FindAdjacentVertex(source, StaticGraphUtility.EdgeDirection(source, target))
, "Duplicate outEdge from Source vertex");
Debug.Assert(null == StaticGraphUtility.FindAdjacentVertex(target, StaticGraphUtility.EdgeDirection(target, source))
, "Duplicate inEdge to Target vertex");
var edge = new VisibilityEdge(source, target, this.Weight);
VisibilityGraph.AddEdge(edge);
return(edge);
}
public void DrawFromObstacles(
Vector2 source, Vector2 target, Color color, VisibilityGraph graph
)
{
Vector3[] sourceAndTarget = { source, target };
foreach (Vector3 pt in sourceAndTarget)
{
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);
// Between obstacle point and source/target
Vector3[] vertices = DrawVisibilityEdge(
pt, obstaclePtWorld,
Color.red, false, true, false, true
);
if (vertices != null)
{
graph.AddEdge(vertices[0], vertices[1]);
}
// Between obstacle adjcent reflex points
Vector3 obstaclePtWorldPrev
= obstacle.transform.TransformPoint(obstaclePtPrev);
Debug.DrawRay(
obstaclePtWorldPrev,
obstaclePtWorld - obstaclePtWorldPrev,
Color.red, 1000
);
obstaclePtPrev = obstaclePt;
}
obstaclePtIdx++;
}
}
}
}
// ReSharper restore InconsistentNaming
private VisibilityEdge CreateEdge(VisibilityVertex first, VisibilityVertex second, double weight)
{
// All edges in the graph are ascending.
VisibilityVertex source = first;
VisibilityVertex target = second;
if (!PointComparer.IsPureLower(source.Point, target.Point))
{
source = second;
target = first;
}
var edge = new TollFreeVisibilityEdge(source, target, weight);
VisibilityGraph.AddEdge(edge);
this.AddedEdges.Add(edge);
return(edge);
}
internal void ModifySkeletonWithNewBoundaryOnLayer(LgNodeInfo nodeInfo)
{
Dictionary <VisibilityEdge, VisibilityVertex> edgeSnapMap = GetEdgeSnapAtVertexMap(nodeInfo);
foreach (var t in edgeSnapMap)
{
VisibilityEdge edge = t.Key;
VisibilityGraph.RemoveEdge(edge);
var midleV = edgeSnapMap[edge];
if (nodeInfo.Center != edge.SourcePoint)
{
VisibilityGraph.AddEdge(edge.Source, midleV);
}
else
{
VisibilityGraph.AddEdge(midleV, edge.Target);
}
}
}
public void DrawFromBoundaries(
Vector2 source, Vector2 target, Color color, VisibilityGraph graph
)
{
Vector3[] sourceAndTarget = { source, target };
foreach (Vector3 boundaryReflexPt in boundaryReflexPts)
{
foreach (Vector3 pt in sourceAndTarget)
{
Vector3[] vertices = DrawVisibilityEdge(
boundaryReflexPt, pt, color, false, true, false, true
);
if (vertices != null)
{
graph.AddEdge(vertices[0], vertices[1]);
}
}
}
}
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();
}
public Vector3[] DrawVisibilityEdge(
Vector3 from, Vector3 to,
Color color,
bool bitangence = false, bool considerBoundaries = false,
bool addEdge = true, bool hideLine = false
)
{
/* FORWARDS RAYCAST */
bool hitCollider = false;
RaycastHit2D hit = Physics2D.Raycast(
from,
to - from,
considerBoundaries
? Vector3.Distance(to, from) + BoundaryTolerance : Mathf.Infinity,
1, -0.5f, 0.5f
);
if (hit.collider != null)
{
hitCollider = true;
}
if (
!considerBoundaries &&
hit.collider.gameObject.CompareTag("Boundaries")
)
{
hitCollider = false;
}
/* NOT AS GOOD CODE
* RaycastHit2D[] hits = Physics2D.RaycastAll(
* from,
* to - from,
* considerBoundaries
* ? Vector3.Distance(to, from) + BoundaryTolerance : Mathf.Infinity
* );
* foreach (RaycastHit2D hit in hits) {
* if (!hit.collider.gameObject.CompareTag("Agent")) {
* if (hit.collider != null) {
* hitCollider = true;
* }
* if (
* !considerBoundaries
* && hit.collider.gameObject.CompareTag("Boundaries")
* ) {
* hitCollider = false;
* }
*
* break;
* }
* }
*/
/* BACKWARDS RAYCAST */
bool backwardsHitCollider = false;
RaycastHit2D backwardsHit;
if (bitangence)
{
backwardsHit = Physics2D.Raycast(
from,
-(to - from),
considerBoundaries ? BoundaryTolerance : Mathf.Infinity,
1, -0.5f, 0.5f
);
if (backwardsHit.collider != null)
{
backwardsHitCollider = true;
}
if (
!considerBoundaries &&
backwardsHit.collider.gameObject.CompareTag("Boundaries")
)
{
backwardsHitCollider = false;
}
/* NOT AS GOOD CODE
* RaycastHit2D[] backwardsHits = Physics2D.RaycastAll(
* from,
* -(to - from),
* considerBoundaries ? BoundaryTolerance : Mathf.Infinity
* );
*
* foreach (RaycastHit2D backwardsHit in backwardsHits) {
* if (!backwardsHit.collider.gameObject.CompareTag("Agent")) {
* if (backwardsHit.collider != null) {
* backwardsHitCollider = true;
* }
*
* if (
* !considerBoundaries
* && backwardsHit.collider.gameObject
* .CompareTag("Boundaries")
* ) {
* backwardsHitCollider = false;
* }
*
* break;
* }
* }
*/
}
/* FINAL EVALUATION */
if (!hitCollider && !backwardsHitCollider)
{
if (!hideLine)
{
Debug.DrawRay(
from,
to - from,
color, 1000
);
}
if (addEdge)
{
Graph.AddEdge(from, to);
}
return(new Vector3[2] {
from, to
});
// Debug.Log(obstaclePtIdx + ", " + hit.collider);
}
return(null);
}