private int counter; // to know how many L shapes we've created thus far
// Start is called before the first frame update
void Start()
{
counter = 0;
numberOfShapes = Random.Range(3, 5); // to get either 3 or 4 L shapes
VisibilityGraphGenerator.setNumberOfLShapes(numberOfShapes);
SpawnLShapes();
}
// just to check if we can reach our goal from spawning point
// edge case for spawning point
// private bool canReachDestination(){
// float distance = Vector3.Distance(transform.position, destination.pos);
// Vector3 direction = destination.pos - transform.position;
// RaycastHit hitInfo;
// // currently can walk through air
// if (Physics.SphereCast(transform.position, 3.0f, destination.pos, out hitInfo, distance)){
// // then we can move directly, we assume we won't collide with other agents
// // if we can reach our goal
// Debug.Log("CanReachWillReturn " + hitInfo.transform.tag != "Wall" && hitInfo.transform.tag != "Agent" && hitInfo.transform.tag != "LShape");
// Debug.Log(hitInfo.transform.tag);
// return hitInfo.transform.tag != "Wall" && hitInfo.transform.tag != "Agent" && hitInfo.transform.tag != "LShape";
// }
// return false;
// }
// This is will look at nearby vertices that are accessible from the agent's current position
// and return the nearest one
private PathVertex getNearbyVertex()
{
List <PathVertex> allVertices = VisibilityGraphGenerator.getVertices();
List <PathVertex> potentialVertices = new List <PathVertex>();
RaycastHit hitInfo;
for (int i = 0; i < allVertices.Count; i++)
{
Vector3 direction = allVertices[i].pos - transform.position;
float distance = Vector3.Distance(transform.position, allVertices[i].pos);
if (Physics.SphereCast(transform.position, 3.0f, direction, out hitInfo, distance))
{
if (hitInfo.transform.tag != "Wall" && hitInfo.transform.tag != "LShape")
{
potentialVertices.Add(allVertices[i]);
}
}
}
float min = float.MaxValue;
int index = 0;
for (int i = 0; i < potentialVertices.Count; i++)
{
float dist = Vector3.Distance(transform.position, potentialVertices[i].pos);
if (dist < min)
{
min = dist;
index = i;
}
}
return(potentialVertices[index]);
}
private void reset()
{
VisibilityGraphGenerator.removePathVertex(destination);
destination = Terrain.getValidSquare();
goal.transform.position = destination.pos;
VisibilityGraphGenerator.addPathVertex(destination);
// canReachGoal = canReachDestination();
walkToNearbyNeighbor = false;
setPathPlan();
i = 0;
}
// This creates the VisibilityVertex objects along the segments.
internal VisibilityGraph Generate(IEnumerable <ScanSegment> hSegments, IEnumerable <ScanSegment> vSegments)
{
foreach (ScanSegment seg in vSegments)
{
eventList.Add(new SegEvent(SegEventType.VOpen, seg));
eventList.Add(new SegEvent(SegEventType.VClose, seg));
}
foreach (ScanSegment seg in hSegments)
{
eventList.Add(new SegEvent(SegEventType.HOpen, seg));
}
if (0 == eventList.Count)
{
return(null); // empty
}
eventList.Sort(this);
// Note: We don't need any sentinels in the scanline here, because the lowest VOpen
// events are loaded before the first HOpen is.
// Process all events.
visGraph = VisibilityGraphGenerator.NewVisibilityGraph();
foreach (SegEvent evt in eventList)
{
switch (evt.EventType)
{
case SegEventType.VOpen:
OnSegmentOpen(evt.Segment);
ScanInsert(evt.Segment);
break;
case SegEventType.VClose:
OnSegmentClose(evt.Segment);
ScanRemove(evt.Segment);
break;
case SegEventType.HOpen:
OnSegmentOpen(evt.Segment);
ScanIntersect(evt.Segment);
break;
default:
Debug.Assert(false, "Unknown SegEventType");
// ReSharper disable HeuristicUnreachableCode
break;
// ReSharper restore HeuristicUnreachableCode
}
} // endforeach
return(visGraph);
}
// spawns agent and their destination square and gives the destination square to the agent
void spawnAgents()
{
while (numberOfAgents-- > 0)
{
Vector3 spawnPoint = Terrain.getValidSquare().pos;
Agent tempAgent = Instantiate(agentPrefab, spawnPoint, Quaternion.identity);
PathVertex destinationPoint = Terrain.getValidSquare();
VisibilityGraphGenerator.addPathVertex(destinationPoint);
tempAgent.setDestination(destinationPoint);
GameObject goalSquare = Instantiate(goalPrefab, destinationPoint.pos, Quaternion.identity);
tempAgent.setGoalSquare(goalSquare);
// once our agent is spawned and has a destination, we make it walk and we forget about it
// the rest of the code logic is delegated to Agent.cs
}
}
/// <summary>
/// Constructor specifying graph and shape information.
/// </summary>
/// <param name="obstacles">The collection of shapes to route around. Contains all source and target shapes
/// as well as any intervening obstacles.</param>
/// <param name="padding">The minimum padding from an obstacle's curve to its enclosing polyline.</param>
/// <param name="cornerFitRadius">The radius of the arc inscribed into path corners</param>
/// <param name="useSparseVisibilityGraph">If true, use a sparse visibility graph, which saves memory for large graphs
/// but may select suboptimal paths</param>
/// <param name="useObstacleRectangles">Use obstacle bounding boxes in visibility graph</param>
public RectilinearEdgeRouter(IEnumerable <Shape> obstacles, double padding, double cornerFitRadius,
bool useSparseVisibilityGraph, bool useObstacleRectangles)
{
Padding = padding;
CornerFitRadius = cornerFitRadius;
BendPenaltyAsAPercentageOfDistance = SsstRectilinearPath.DefaultBendPenaltyAsAPercentageOfDistance;
if (useSparseVisibilityGraph)
{
this.GraphGenerator = new SparseVisibilityGraphGenerator();
}
else
{
this.GraphGenerator = new FullVisibilityGraphGenerator();
}
this.UseObstacleRectangles = useObstacleRectangles;
PortManager = new PortManager(GraphGenerator);
AddShapes(obstacles);
}
public int Compare(BasicObstacleSide first, BasicObstacleSide second)
{
ValidateArg.IsNotNull(first, "first");
ValidateArg.IsNotNull(second, "second");
// If these are two sides of the same obstacle then the ordering is obvious.
if (first.Obstacle == second.Obstacle)
{
if (first == second)
{
return(0);
}
return((first is LowObstacleSide) ? -1 : 1);
}
Debug_VerifySidesDoNotIntersect(first, second);
// Other than intersecting sides at vertices of the same obstacle, there should be no interior intersections...
Point firstIntersect = VisibilityGraphGenerator.ScanLineIntersectSide(this.linePositionAtLastInsertOrRemove, first, scanDirection);
Point secondIntersect = VisibilityGraphGenerator.ScanLineIntersectSide(this.linePositionAtLastInsertOrRemove, second, scanDirection);
var cmp = firstIntersect.CompareTo(secondIntersect);
// ... but we may still have rectangular sides that coincide, or angled sides that are close enough here but
// are not detected by the convex-hull overlap calculations. In those cases, we refine the comparison by side
// type, with High coming before Low, and then by obstacle ordinal if needed. Because there are no interior
// intersections, this ordering will remain valid as long as the side(s) are in the scanline.
if (0 == cmp)
{
bool firstIsLow = first is LowObstacleSide;
bool secondIsLow = second is LowObstacleSide;
cmp = firstIsLow.CompareTo(secondIsLow);
if (0 == cmp)
{
cmp = first.Obstacle.Ordinal.CompareTo(second.Obstacle.Ordinal);
}
}
DevTraceInfo(4, "Compare {0} @ {1:F5} {2:F5} and {3:F5} {4:F5}: {5} {6}",
cmp, firstIntersect.X, firstIntersect.Y, secondIntersect.X, secondIntersect.Y, first, second);
return(cmp);
}
void OnDrawGizmos()
{
if (m_mat == null)
{
m_mat = new Material(Shader.Find("Unlit/Color"));
}
m_mat.SetColor("_Color", Color.black);
List <Polygon> obstacles = new List <Polygon>()
{
new Polygon(new Vector2[]
{
new Vector2(100, 50),
new Vector2(100, 100),
new Vector2(180, 100),
new Vector2(180, 50),
}),
new Polygon(new Vector2[]
{
new Vector2(250, 120),
new Vector2(230, 140),
new Vector2(250, 160),
new Vector2(290, 150),
}),
new Polygon(new Vector2[]
{
new Vector2(90, 130),
new Vector2(70, 150),
new Vector2(110, 150),
}),
new Polygon(new Vector2[]
{
new Vector2(150, 200),
new Vector2(150, 250),
new Vector2(280, 250),
new Vector2(280, 200),
}),
};
for (int i = 0; i < obstacles.Count; i++)
{
GraphicsTool.DrawPolygon(obstacles[i].Points, m_mat, true, true);
}
m_mat.SetColor("_Color", Color.yellow);
List <GraphNode> nodes = VisibilityGraphGenerator.Generate(m_start, m_end, obstacles);
for (int a = 0; a < nodes.Count; a++)
{
var n = nodes[a];
GraphicsTool.DrawPoint(nodes[a].Center, 3, m_mat);
for (int i = 0; i < n.Neighbors.Count; i++)
{
GraphicsTool.DrawLine(n.Center, n.Neighbors[i].Center, m_mat);
}
}
m_mat.SetColor("_Color", Color.red);
GraphicsTool.DrawPoint(m_start, 4, m_mat);
GraphicsTool.DrawPoint(m_end, 4, m_mat);
GraphNode startNode = nodes[nodes.Count - 2];
GraphNode endNode = nodes[nodes.Count - 1];
GraphAStar astar = new GraphAStar(startNode, endNode);
astar.Process();
List <Vector2> path = new List <Vector2>();
while (endNode != null)
{
path.Add(endNode.Center);
endNode = endNode.Parent;
}
GraphicsTool.DrawPolygon(path, m_mat, false);
}
internal TransientGraphUtility(VisibilityGraphGenerator graphGen)
{
GraphGenerator = graphGen;
}
// Runs A* on our graph : f(n) = g(n) + h(n)
// g(n) : distance from the source
// h(n) : estimate of the actual cost to get to the destination, it will be calculated from Vector3.Distance()
// our destination node is added into our graph but it isnt displayed
// returns the order of nodes that we need to walk through to reach our destination
public List <PathVertex> pathFind()
{
System.DateTime before = System.DateTime.Now;
System.DateTime after;
PathVertex startVertex = new PathVertex(start);
startVertex.gCost = 0.0f; // by definition
startVertex.fCost = Vector3.Distance(startVertex.pos, destination);
startVertex.setFCost();
fringe.Add(startVertex);
List <PathVertex> pathToTake = new List <PathVertex>();
// here we set the g cost to ALL our nodes to be infinity and we calculate the fcost
List <PathVertex> allNodes = VisibilityGraphGenerator.getVertices();
foreach (PathVertex v in allNodes)
{
v.parentVertex = null;
v.gCost = float.MaxValue;
v.setFCost();
}
// PSEUDO CODE FROM CLASS (DEAR TA, YOU DON'T NEED TO READ THIS, IT'S JUST FOR LOGIC/DEBUGGING)
// we might need to see node more than once
// g(start) = 0
// f(start) = g(start) + h(start) = h(start)
// g(n =/= start) = infinity, thus f(n =/= start) = infinity also
// fringe = {start} we start from the frige
// we keep going, while (fringe is not empty set):
// c = find node in the fringe that has the smallest f function (initially its start)
// if c == destination => done
// else : look at all neighbours n of C:
// compute new distance = g(c) + cost(getting from c to n)
// if dist < g(n) :
// if n is not in the fringe and dist + h(n) < f (n) :
// add n to the fringe
// either way, we're gonna update g(n) = dist
// f(n) = g(n) + h(n)
// our heuristic will be the straight distance to our destination
// basically f(start) = g(start) + h(start) = h(start) since g(start) = 0
while (fringe.Count > 0)
{
PathVertex current = getVertexWithSmallestFCost(fringe); // returns the one with smallest f function
if (current.pos == destination)
{
// we're done
after = System.DateTime.Now;
pathPlaningTime = after.Subtract(before).Milliseconds;
return(getPath(current));
}
// if current != destination THEN :
fringe.Remove(current); // we remove bc we've already searched it -> prevent infinite loop
// exploredVertices.Add(current);
// now we look at all neighbours of C
foreach (PathVertex neighbor in VisibilityGraphGenerator.getNeighbors(current))
{
// we only want the neighbors that we haven't explored already
// if (exploredVertices.Contains(neighbor)){
// continue;
// }
float transitionDistance = Vector3.Distance(current.pos, neighbor.pos);
float alternativeBestDistance = current.gCost + transitionDistance; // g(neighbor)
// if we found new best distance for neighbor then we update our neighbor
if (alternativeBestDistance < neighbor.gCost)
{
neighbor.gCost = alternativeBestDistance;
neighbor.parentVertex = current;
neighbor.hCost = Vector3.Distance(current.pos, destination);
neighbor.setFCost();
// if neighbor isn't in fringe, this just returns false, we don't care
// our way of updating the neighbor whether it was in our fringe or not
fringe.Remove(neighbor);
fringe.Add(neighbor);
}
}
}
// outside of while -> empty fringe -> couldn't find path -> return null
// sometimes path is not found, why?
after = System.DateTime.Now;
pathPlaningTime = after.Subtract(before).Milliseconds;
return(null);
}
