void RemoveEdge(WaypointEdge e) { e.Disconnect(); m_waypointEdges.Remove(e); CleanNode(e.m_a); CleanNode(e.m_b); }
public void AddEdge(Waypoint a, Waypoint b) { if (a != b && !a.IsConnectedTo(b)) { WaypointEdge e = new WaypointEdge(a, b); a.AddEdge(e); b.AddEdge(e); m_waypointEdges.Add(e); } }
private WaypointEdge GetWaypointEdgeInRegion (Guid regionID, Guid sourceWaypoindID, Guid sinkWaypointID, ConnectionType[] avoidConnectionTypes) { WaypointEdge waypointEdge = new WaypointEdge(); Tuple <Guid, Guid> edgeKeyFromNode1 = new Tuple <Guid, Guid>(sourceWaypoindID, sinkWaypointID); Tuple <Guid, Guid> edgeKeyFromNode2 = new Tuple <Guid, Guid>(sinkWaypointID, sourceWaypoindID); if (_navigraphs[regionID]._edges.ContainsKey(edgeKeyFromNode1)) { // XML file contains (W1, W2) and the query input is (W1, W2) // as well. waypointEdge = _navigraphs[regionID]._edges[edgeKeyFromNode1]; } else if (_navigraphs[regionID]._edges.ContainsKey(edgeKeyFromNode2)) { // XML file contains (W1, W2) but the query string is (W2, W1). waypointEdge = _navigraphs[regionID]._edges[edgeKeyFromNode2]; if (System.Convert.ToInt32(waypointEdge._direction) + 4 < 8) { waypointEdge._direction = (CardinalDirection) (4 + waypointEdge._direction); } else { waypointEdge._direction = (CardinalDirection) (4 + waypointEdge._direction - 8); } } return(waypointEdge); }
public void Start() { if (!doMagic) { return; } SphereCollider col = GetComponent <SphereCollider> (); WaypointGraph graph = GameObject.FindGameObjectWithTag("Planet").GetComponent <WaypointGraph>(); Ray ray; RaycastHit hit; WaypointNode other; WaypointEdge edge; string[] masks = { "Waypoint" }; LayerMask mask = LayerMask.GetMask(masks); if (right == null) { if (Physics.Raycast(transform.position, Vector3.right, out hit, 50.0f, mask.value)) { other = hit.collider.gameObject.GetComponent <WaypointNode> (); if (other != null) { right = other; other.left = this; edge = new WaypointEdge(); edge.one = this; edge.two = other; graph.edges.Add(edge); } } } if (left == null) { ray = new Ray(transform.position, Vector3.left); if (Physics.Raycast(transform.position, Vector3.left, out hit, 50.0f, mask.value)) { other = hit.collider.gameObject.GetComponent <WaypointNode> (); if (other != null) { left = other; other.right = this; edge = new WaypointEdge(); edge.one = this; edge.two = other; graph.edges.Add(edge); } } } if (front == null) { ray = new Ray(transform.position, Vector3.forward); if (Physics.Raycast(transform.position, Vector3.forward, out hit, 50.0f, mask.value)) { other = hit.collider.gameObject.GetComponent <WaypointNode> (); if (other != null) { front = other; other.back = this; edge = new WaypointEdge(); edge.one = this; edge.two = other; graph.edges.Add(edge); } } } if (back == null) { ray = new Ray(transform.position, Vector3.back); if (Physics.Raycast(transform.position, Vector3.back, out hit, 50.0f, mask.value)) { other = hit.collider.gameObject.GetComponent <WaypointNode> (); if (other != null) { back = other; other.front = this; edge = new WaypointEdge(); edge.one = this; edge.two = other; graph.edges.Add(edge); } } } }
public void RemoveEdge(WaypointEdge e) { m_edges.Remove(e); }
public void AddEdge(WaypointEdge e) { m_edges.Add(e); }
// Dijkstra's algorithm for shortest path public WaypointNode[] ShortestPath(Vector3 source, Vector3 target) { WaypointNode[] nodes = graph.Nodes; // Find the closest waypoints int sourceIndex = 0, targetIndex = 0; float sourceDist = float.MaxValue, targetDist = float.MaxValue; for (int i = 0; i < nodes.Length; i++) { float distance = Vector3.Distance(source, nodes[i].position); if (distance < sourceDist) { sourceDist = distance; sourceIndex = i; } distance = Vector3.Distance(target, nodes[i].position); if (distance < targetDist) { targetDist = distance; targetIndex = i; } } // Initializations float[] dist = new float[nodes.Length]; int[] previous = new int[nodes.Length]; for (int v = 0; v < nodes.Length; v++) { dist[v] = float.MaxValue; previous[v] = -1; } dist[sourceIndex] = 0; var Q = new Collection <int>(); for (int i = 0; i < dist.Length; i++) { Q.Add(i); } // Main loop while (Q.Count > 0) { int u = Q[0]; for (int i = 0; i < Q.Count; i++) { if (dist[Q[i]] < dist[u]) { u = Q[i]; } } Q.Remove(u); // We found our target if (u == targetIndex) { Stack <WaypointNode> sp = new Stack <WaypointNode>(); while (previous[u] >= 0) { sp.Push(nodes[u]); u = previous[u]; } WaypointNode[] result = new WaypointNode[sp.Count]; sp.CopyTo(result, 0); return(result); } if (dist[u] > float.MaxValue - 1) { break; } int[] theRest = new int[Q.Count]; Q.CopyTo(theRest, 0); List <WaypointEdge> neighbours = GetConnectedEdges(u, theRest); for (int j = 0; j < neighbours.Count; j++) { WaypointEdge e = neighbours[j]; int neighbour = (e.FirstIndex == u ? e.SecondIndex : e.FirstIndex); float alt = dist[u] + e.Cost; if (alt < dist[neighbour]) { dist[neighbour] = alt; previous[neighbour] = u; } } } return(nodes); }
public Graph <Guid, string> GenerateNavigraph (Guid regionID, ConnectionType[] avoidConnectionTypes) { Graph <Guid, string> graph = new Graph <Guid, string>(); foreach (KeyValuePair <Guid, Waypoint> waypointItem in _navigraphs[regionID]._waypoints) { graph.AddNode(waypointItem.Key); } foreach (KeyValuePair <Tuple <Guid, Guid>, WaypointEdge> waypointEdgeItem in _navigraphs[regionID]._edges) { Guid node1 = waypointEdgeItem.Key.Item1; Guid node2 = waypointEdgeItem.Key.Item2; uint node1Key = graph.Where(node => node.Item.Equals(node1)) .Select(node => node.Key).First(); uint node2Key = graph.Where(node => node.Item.Equals(node2)) .Select(node => node.Key).First(); // should refine distance, bi-direction, direction, connection // type later int distance = Int32.MaxValue; Tuple <Guid, Guid> edgeKey = new Tuple <Guid, Guid>(node1, node2); WaypointEdge edgeItem = _navigraphs[regionID]._edges[edgeKey]; if (!avoidConnectionTypes.Contains(edgeItem._connectionType)) { distance = System.Convert.ToInt32(edgeItem._distance); if (DirectionalConnection.BiDirection == edgeItem._biDirection) { // Graph.Connect is on-way, not bi-drectional graph.Connect(node1Key, node2Key, distance, String.Empty); graph.Connect(node2Key, node1Key, distance, String.Empty); } else if (DirectionalConnection.OneWay == edgeItem._biDirection) { if (1 == edgeItem._source) { graph.Connect(node1Key, node2Key, distance, String.Empty); } else if (2 == edgeItem._source) { graph.Connect(node2Key, node1Key, distance, String.Empty); } } } } return(graph); }
public InstructionInformation GetInstructionInformation( int currentNavigationStep, Guid currentRegionID, Guid currentWaypointID, Guid previousRegionID, Guid previousWaypointID, Guid nextRegionID, Guid nextWaypointID, ConnectionType[] avoidConnectionTypes) { InstructionInformation information = new InstructionInformation(); information._floor = _regions[nextRegionID]._floor; information._regionName = _regions[nextRegionID]._name; if (!currentRegionID.Equals(nextRegionID)) { // currentWaypoint and nextWaypoint are in different regions if (!_regions[currentRegionID]. _floor.Equals(_regions[nextRegionID]._floor)) { // currentWaypoint and nextWaypoint are in different regions // with different floors if (_regions[nextRegionID]._floor > _regions[currentRegionID]._floor) { information._turnDirection = TurnDirection.Up; } else { information._turnDirection = TurnDirection.Down; } RegionEdge currentEdge = GetRegionEdgeMostNearSourceWaypoint(currentRegionID, currentWaypointID, nextRegionID, avoidConnectionTypes); information._connectionType = currentEdge._connectionType; information._distance = System.Convert .ToInt32(currentEdge._distance); } else { // currentWaypoint and nextWaypoint are across regions // but on the same floor // When step==0, it means that the turn direction is first // direction. if (0 == currentNavigationStep) { // currentWaypoint is the first waypoing from the // beginning need to refine the turndirection in this // case information._turnDirection = TurnDirection.FirstDirection; RegionEdge currentEdge = GetRegionEdgeMostNearSourceWaypoint (currentRegionID, currentWaypointID, nextRegionID, avoidConnectionTypes); information._relatedDirectionOfFirstDirection = currentEdge._direction; information._connectionType = currentEdge._connectionType; information._distance = System.Convert .ToInt32(currentEdge._distance); } else { if (!previousRegionID.Equals(currentRegionID)) { // previouWaypoint and currentWaypoint are acrss // regions if (!_regions[previousRegionID]._floor.Equals( _regions[currentRegionID]._floor)) { if (!currentRegionID.Equals(nextRegionID)) { information._turnDirection = TurnDirection.FirstDirection; RegionEdge regionEdge = GetRegionEdgeMostNearSourceWaypoint (currentRegionID, currentWaypointID, nextRegionID, avoidConnectionTypes); information._connectionType = regionEdge._connectionType; information._distance = System.Convert.ToInt32 (regionEdge._distance); information. _relatedDirectionOfFirstDirection = regionEdge._direction; } else { information._turnDirection = TurnDirection.FirstDirection; WaypointEdge currentEdge = GetWaypointEdgeInRegion (currentRegionID, currentWaypointID, nextWaypointID, avoidConnectionTypes); information._connectionType = currentEdge._connectionType; information. _relatedDirectionOfFirstDirection = currentEdge._direction; information._distance = System.Convert .ToInt32(currentEdge._distance); } // previousWaypoint and currentWaypoint are on // different floor need to refine the // turndirection in this case } else { // previousWaypoint and currentWaypoint are on the same floor RegionEdge prevEdge = GetRegionEdgeMostNearSourceWaypoint (previousRegionID, previousWaypointID, currentRegionID, avoidConnectionTypes); CardinalDirection prevEdgeDirection = prevEdge._direction; RegionEdge currentEdge = GetRegionEdgeMostNearSourceWaypoint (currentRegionID, currentWaypointID, nextRegionID, avoidConnectionTypes); CardinalDirection currentEdgeDirection = currentEdge._direction; int prevDirection = System.Convert.ToInt32(prevEdgeDirection); int currentDirection = System.Convert.ToInt32(currentEdgeDirection); if (currentDirection - prevDirection >= 0) { information._turnDirection = (TurnDirection) (currentDirection - prevDirection); } else { information._turnDirection = (TurnDirection) (currentDirection - prevDirection + 8); } information._connectionType = currentEdge._connectionType; information._distance = System.Convert .ToInt32(currentEdge._distance); } } else { // previousWaypoint and currentWaypoint are in the // same region WaypointEdge prevEdge = GetWaypointEdgeInRegion(previousRegionID, previousWaypointID, currentWaypointID, avoidConnectionTypes); CardinalDirection prevEdgeDirection = prevEdge._direction; RegionEdge currentEdge = GetRegionEdgeMostNearSourceWaypoint (currentRegionID, currentWaypointID, nextRegionID, avoidConnectionTypes); CardinalDirection currentEdgeDirection = currentEdge._direction; int prevDirection = System.Convert.ToInt32(prevEdgeDirection); int currentDirection = System.Convert.ToInt32(currentEdgeDirection); if (currentDirection - prevDirection >= 0) { information._turnDirection = (TurnDirection) (currentDirection - prevDirection); } else { information._turnDirection = (TurnDirection) (currentDirection - prevDirection + 8); } information._connectionType = currentEdge._connectionType; information._distance = System.Convert.ToInt32(currentEdge._distance); } } } } else { // currentWaypoint and nextWaypoint are in the same region if (0 == currentNavigationStep) { // first waypoint from the beginning // need to refine the turndirection in this case information._turnDirection = TurnDirection.FirstDirection; WaypointEdge currentEdge = GetWaypointEdgeInRegion(currentRegionID, currentWaypointID, nextWaypointID, avoidConnectionTypes); information._connectionType = currentEdge._connectionType; information._relatedDirectionOfFirstDirection = currentEdge._direction; information._distance = System.Convert .ToInt32(currentEdge._distance); } else { Console.WriteLine("current = next case"); if (!previousRegionID.Equals(currentRegionID)) { Console.WriteLine("previous != current case"); // currentWaypoint and nextWaypoint are in the same // region. // previouWaypoint and currentWaypoint are acrss regions if (!_regions[previousRegionID]._floor.Equals( _regions[currentRegionID]._floor)) { // previousWaypoint and currentWaypoint are on // different floor need to refine the turndirection // in this case information._turnDirection = TurnDirection.FirstDirection; WaypointEdge currentEdge = GetWaypointEdgeInRegion(currentRegionID, currentWaypointID, nextWaypointID, avoidConnectionTypes); information._connectionType = currentEdge._connectionType; information._relatedDirectionOfFirstDirection = currentEdge._direction; information._distance = System.Convert .ToInt32(currentEdge._distance); } else { // previousWaypoint and currentWaypoint are on the // same floor RegionEdge prevEdge = GetRegionEdgeMostNearSourceWaypoint (previousRegionID, previousWaypointID, currentRegionID, avoidConnectionTypes); CardinalDirection prevEdgeDirection = prevEdge._direction; WaypointEdge currentEdge = GetWaypointEdgeInRegion(currentRegionID, currentWaypointID, nextWaypointID, avoidConnectionTypes); CardinalDirection currentEdgeDirection = currentEdge._direction; int prevDirection = System.Convert.ToInt32(prevEdgeDirection); int currentDirection = System.Convert.ToInt32(currentEdgeDirection); if (currentDirection - prevDirection >= 0) { information._turnDirection = (TurnDirection) (currentDirection - prevDirection); } else { information._turnDirection = (TurnDirection) (currentDirection - prevDirection + 8); } information._connectionType = currentEdge._connectionType; information._distance = System.Convert.ToInt32(currentEdge._distance); } } else { Console.WriteLine("previous = current case"); // currentWaypoint and nextWaypoint are in the same // region // previousWaypoint and currentWaypoint are in the same // region WaypointEdge prevEdge = GetWaypointEdgeInRegion(previousRegionID, previousWaypointID, currentWaypointID, avoidConnectionTypes); CardinalDirection prevEdgeDirection = prevEdge._direction; WaypointEdge currentEdge = GetWaypointEdgeInRegion(currentRegionID, currentWaypointID, nextWaypointID, avoidConnectionTypes); CardinalDirection currentEdgeDirection = currentEdge._direction; int prevDirection = System.Convert.ToInt32(prevEdgeDirection); int currentDirection = System.Convert.ToInt32(currentEdgeDirection); if (currentDirection - prevDirection >= 0) { information._turnDirection = (TurnDirection) (currentDirection - prevDirection); } else { information._turnDirection = (TurnDirection) (currentDirection - prevDirection + 8); } information._connectionType = currentEdge._connectionType; information._distance = System.Convert.ToInt32(currentEdge._distance); } } } return(information); }
public int GetDistanceOfLongHallway(RegionWaypointPoint currentGuid, int nextStep, List <RegionWaypointPoint> allRoute, ConnectionType[] avoidConnectionType) { int distance = 0; if (nextStep <= 0) { nextStep = 1; } for (int i = nextStep - 1; i < allRoute.Count(); i++) { if (allRoute[i]._regionID != allRoute[i + 1]._regionID) { if (_regions[allRoute[i]._regionID]._floor == _regions[allRoute[i + 1]._regionID]._floor) { RegionEdge regionEdge = GetRegionEdgeMostNearSourceWaypoint (allRoute[i]. _regionID, allRoute[i]. _waypointID, allRoute[i + 1]. _regionID, avoidConnectionType); distance = System.Convert.ToInt32(regionEdge._distance); } else { break; } } else { WaypointEdge waypointEdge = GetWaypointEdgeInRegion(allRoute[i]._regionID, allRoute[i]._waypointID, allRoute[i + 1]._waypointID, avoidConnectionType); distance = distance + System.Convert.ToInt32(waypointEdge._distance); if (i + 2 >= allRoute.Count()) { break; } else { WaypointEdge currentWaypointEdge = GetWaypointEdgeInRegion(allRoute[i]._regionID, allRoute[i]._waypointID, allRoute[i + 1]._waypointID, avoidConnectionType); WaypointEdge nextWaypointEdge = GetWaypointEdgeInRegion(allRoute[i + 1]._regionID, allRoute[i + 1]._waypointID, allRoute[i + 2]._waypointID, avoidConnectionType); if (currentWaypointEdge._direction != nextWaypointEdge._direction) { break; } } } } return(distance); }