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