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