public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
var startCluster = this.ClusterGraph.Quantize(node);
var goalCluster = this.ClusterGraph.Quantize(goalNode);
float h;
float shortestDistance = 0f;
//for now just returns the euclidean distance
if (object.ReferenceEquals(startCluster, null) || object.ReferenceEquals(goalCluster, null) || startCluster == goalCluster)
{
return(EuclideanDistance(node.LocalPosition, goalNode.LocalPosition));
}
//TODO implement this properly
else
{
var gatewayDistanceTable = this.ClusterGraph.gatewayDistanceTable;
for (int k = 0; k < startCluster.gateways.Count; k++)
{
var startGateway = startCluster.gateways[k];
for (int l = 0; l < goalCluster.gateways.Count; l++)
{
var goalGateway = goalCluster.gateways[l];
h = EuclideanDistance(node.LocalPosition, startGateway.center) + EuclideanDistance(goalNode.LocalPosition, goalGateway.center) + gatewayDistanceTable[startGateway.id].entries[goalGateway.id].shortestDistance;
shortestDistance = Mathf.Min(shortestDistance, -h);
}
}
return(-shortestDistance);
}
}
public NodeRecord SearchInOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
//here I cannot use the == comparer because the nodeRecord will likely be a different computational object
//and therefore pointer comparison will not work, we need to use Equals
//LINQ with a lambda expression
return this.NodeRecords.FirstOrDefault(n => n.Equals(nodeRecord));
}
public NodeRecord SearchInClosed(NavigationGraphNode key, NodeRecord nodeRecord)
{
if (this.NodeRecords.Keys.Contains(key))
return (NodeRecord)this.NodeRecords[key];
else
return null;
}
protected void ProcessChildNode(NodeRecord parent, NavigationGraphEdge connectionEdge, int connectionIndex)
{
NavigationGraphNode childNode = connectionEdge.ToNode;
var childNodeRecord = this.NodeRecordArray.GetNodeRecord(childNode);
var open = Open.SearchInOpen(childNodeRecord);
var close = Closed.SearchInClosed(childNodeRecord);
if (open == null && close == null)
{
float g = parent.gValue + (childNodeRecord.node.LocalPosition - parent.node.LocalPosition).magnitude;
UpdateNode(parent, childNodeRecord, g, 0, g, connectionIndex);
Open.AddToOpen(childNodeRecord);
}
else if (open != null)
{
var g = parent.gValue + (childNodeRecord.node.LocalPosition - parent.node.LocalPosition).magnitude;
if (g < childNodeRecord.gValue)
{
UpdateNode(parent, childNodeRecord, g, 0, g, connectionIndex);
Open.Replace(childNodeRecord, childNodeRecord);
}
}
}
private const float SquareRoot2 = 1.4142135623730950488016887242097f; // sqrt(2)
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
var zDistance = goalNode.Position.z - node.Position.z;
var xDistance = goalNode.Position.x - node.Position.x;
return(Math.Max(zDistance, xDistance) + (SquareRoot2 - 1) * Math.Min(zDistance, xDistance));
}
protected void ProcessChildNode(NodeRecord BestNode, NavigationGraphEdge connectionEdge, int connectionIndex)
{
NavigationGraphNode childNode = connectionEdge.ToNode;
NodeRecord childRecord = this.NodeRecordArray.GetNodeRecord(connectionEdge.ToNode);
if (childRecord == null)
{
childRecord = new NodeRecord
{
node = childNode,
parent = BestNode,
status = NodeStatus.Unvisited
};
this.NodeRecordArray.AddSpecialCaseNode(childRecord);
}
if (childRecord.status == NodeStatus.Closed)
{
return;
}
float g = BestNode.gValue + (childRecord.node.LocalPosition - BestNode.node.LocalPosition).magnitude;
if (childRecord.status == NodeStatus.Unvisited)
{
UpdateNodeRecord(childRecord, BestNode, g, connectionIndex);
this.Open.AddToOpen(childRecord);
}
else if (childRecord.gValue > g)
{
UpdateNodeRecord(childRecord, BestNode, g, connectionIndex);
this.Open.Replace(childRecord, childRecord);
}
}
private void Initialize(NavigationGraphNode startNode)
{
var startNodeRecord = new NodeRecord
{
fValue = 0,
gValue = 0,
hValue = 0,
StartNodeOutConnectionIndex = -1,
node = startNode,
status = NodeStatus.Closed
};
this.Open.Initialize();
this.Closed.Initialize();
this.Closed.AddToClosed(startNodeRecord);
for (var i = 0; i < startNode.OutEdgeCount; i++)
{
var outEdge = startNode.EdgeOut(i);
var childNodeRecord = this.NodeRecordArray.GetNodeRecord(outEdge.ToNode);
var gValue = startNodeRecord.gValue + (childNodeRecord.node.LocalPosition - startNodeRecord.node.LocalPosition).magnitude;
childNodeRecord.hValue = 0;
childNodeRecord.gValue = gValue;
childNodeRecord.fValue = gValue;
childNodeRecord.StartNodeOutConnectionIndex = i;
childNodeRecord.parent = startNodeRecord;
this.UpdateBoundingBoxes(childNodeRecord, i);
this.Open.AddToOpen(childNodeRecord);
}
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
NodeRecord startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeRecord.StartNodeOutConnectionIndex = -1;
startNodeRecord.fValue = 0;
startNodeRecord.parent = null;
for (int i = 0; i < startNode.OutEdgeCount; i++)
{
NodeRecord nodeChildRecord = this.NodeRecordArray.GetNodeRecord(startNode.EdgeOut(i).ToNode);
nodeChildRecord.parent = startNodeRecord;
nodeChildRecord.StartNodeOutConnectionIndex = i;
nodeChildRecord.fValue = (startNodeRecord.node.Position - nodeChildRecord.node.Position).magnitude;
this.Open.AddToOpen(nodeChildRecord);
}
this.Closed.AddToClosed(startNodeRecord);
while (this.Open.CountOpen() > 0)
{
var bestRecord = this.Open.GetBestAndRemove();
for (int i = 0; i < bestRecord.node.OutEdgeCount; i++)
{
this.ProcessChildNode(bestRecord, bestRecord.node.EdgeOut(i), i);
}
nodeGoalBounds.connectionBounds [bestRecord.StartNodeOutConnectionIndex].UpdateBounds(bestRecord.node.Position);
this.Closed.AddToClosed(bestRecord);
}
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
var distance = (goalNode.Position - node.Position).sqrMagnitude;
// 1 + 1/distance should solve the ties problem
return((1 + 1 / distance) * distance);
}
public NodeRecord GetNodeRecord(NavigationGraphNode node)
{
//do not change this method
//here we have the "special case" node handling
#pragma warning disable 0618 //NodeIndex is deprecated
if (node.NodeIndex == -1)
#pragma warning restore 0618
{
for (int i = 0; i < this.SpecialCaseNodes.Count; i++)
{
if (node == this.SpecialCaseNodes[i].node)
{
return this.SpecialCaseNodes[i];
}
}
return null;
}
else
{
#pragma warning disable 0618 //NodeIndex is deprecated
return this.NodeRecords[node.NodeIndex];
#pragma warning restore 0618
}
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
//for now just returns the euclidean distance
Cluster StartCluster = ClusterGraph.Quantize(node);
Cluster GoalCluster = ClusterGraph.Quantize(goalNode);
if (object.ReferenceEquals(null, StartCluster) || object.ReferenceEquals(null, GoalCluster) || StartCluster.Equals(GoalCluster))
{
return(EuclideanDistance(node.LocalPosition, goalNode.LocalPosition));
}
else
{
var startPos = node.LocalPosition;
var goalPos = goalNode.LocalPosition;
float min = float.MaxValue;
foreach (Gateway g1 in StartCluster.gateways)
{
foreach (Gateway g2 in GoalCluster.gateways)
{
float h =
EuclideanDistance(startPos, g1.center) +
ClusterGraph.gatewayDistanceTable[g1.id].entries[g2.id].shortestDistance +
EuclideanDistance(g2.center, goalPos);
if (h < min)
{
min = h;
}
}
}
return(min);
}
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
this.StartNode = startNode;
this.NodeGoalBounds = nodeGoalBounds;
// we fill the initial childNodes with start edge indexes,
// and add them to the open list. the start node is added to closed list
this.Initialize(startNode);
var closed = this.Closed.All();
var openCount = this.Open.CountOpen();
while (openCount > 0)
{
var currentNode = this.Open.GetBestAndRemove();
this.Closed.AddToClosed(currentNode);
for (var i = 0; i < currentNode.node.OutEdgeCount; i++)
{
ProcessChildNode(currentNode, currentNode.node.EdgeOut(i), currentNode.StartNodeOutConnectionIndex);
}
openCount = this.Open.CountOpen();
}
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
NodeRecord startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeRecord.node = startNode;
startNodeRecord.gValue = 0f;
this.Open.AddToOpen(startNodeRecord);
while (this.Open.CountOpen() > 0)
{
NodeRecord Node = this.Open.GetBestAndRemove();
this.Closed.AddToClosed(Node);
if (Node.id != -1 && nodeGoalBounds.connectionBounds.Length > Node.id)
{
nodeGoalBounds.connectionBounds[Node.id].UpdateBounds(Node.node.LocalPosition);
}
for (int i = 0; i < Node.node.OutEdgeCount; i++)
{
ProcessChildNode(Node, Node.node.EdgeOut(i), i);
}
}
this.Open.Initialize();
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
this.Open.Initialize();
this.Closed.Initialize();
NodeRecord startNodeRecord = this.NodeRecordArray.GetNodeRecord(startNode);
startNodeRecord.gValue = 0;
startNodeRecord.fValue = 0;
startNodeRecord.parent = null;
startNodeRecord.StartNodeOutConnectionIndex = 0;
Open.AddToOpen(startNodeRecord);
while (true)
{
if (this.Open.CountOpen() == 0)
{
return;
}
NodeRecord currNode = this.Open.GetBestAndRemove();
this.Closed.AddToClosed(currNode);
nodeGoalBounds.connectionBounds[currNode.StartNodeOutConnectionIndex].UpdateBounds(currNode.node.Position);
int nOutConnections = currNode.node.OutEdgeCount;
for (int i = 0; i < nOutConnections; i++)
{
this.ProcessChildNode(currNode, currNode.node.EdgeOut(i), i);
}
}
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
this.Open.Initialize();
this.Closed.Initialize();
NodeRecord StartNode = NodeRecordArray.GetNodeRecord(startNode);
StartNode.StartNodeOutConnectionIndex = -1;
Open.AddToOpen(StartNode);
int OpenSize = Open.All().Count;
while (OpenSize > 0)
{
NodeRecord currentNode = Open.GetBestAndRemove();
Open.RemoveFromOpen(currentNode);
Closed.AddToClosed(currentNode);
if (currentNode.StartNodeOutConnectionIndex != -1)
{
nodeGoalBounds.connectionBounds[currentNode.StartNodeOutConnectionIndex].UpdateBounds(currentNode.node.Position);
}
//Initialize start node edge colors:
var outConnections = currentNode.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
this.ProcessChildNode(currentNode, currentNode.node.EdgeOut(i), i);
}
OpenSize = Open.All().Count;
}
}
public DijkstraSearchThread(NavigationGraphNode startNode, NodeGoalBounds bounds, GoalBoundsDijkstraMapFlooding dijkstra, GoalBoundingTable goalBoundingTable, int index)
{
this.startNode = startNode;
this.bounds = bounds;
this.dijkstra = dijkstra;
this.goalBoundingTable = goalBoundingTable;
this.index = index;
}
public void RemoveFromOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
int index = this.Open.BinarySearch(nodeRecord);
if (index >= 0)
{
this.Open.RemoveAt(index);
}
}
public NodeRecord SearchInOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
int index = this.Open.BinarySearch(nodeRecord);
if (index >= 0)
{
return this.Open[index];
}
return Open.FirstOrDefault(n => n.Equals(nodeRecord));
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
int i = 0;
int j = 0;
float finalDistance = float.MaxValue;
float tempDistance = 0;
Cluster clusterStart;
Cluster clusterEnd;
clusterStart = this.ClusterGraph.Quantize(node);
clusterEnd = this.ClusterGraph.Quantize(goalNode);
if ((object.ReferenceEquals(null, clusterStart)) || (object.ReferenceEquals(null, clusterEnd)))
{
return(EuclideanDistance(node.LocalPosition, goalNode.LocalPosition));
}
if (clusterStart.center == clusterEnd.center)
{
return(EuclideanDistance(node.LocalPosition, goalNode.LocalPosition));
}
else
{
foreach (Gateway startGateway in clusterStart.gateways)
{
float startDistance = EuclideanDistance(node.LocalPosition, startGateway.center);
foreach (Gateway endGateway in clusterEnd.gateways)
{
float endDistance = EuclideanDistance(goalNode.LocalPosition, endGateway.center);
while (true)
{
if (ClusterGraph.gateways[i].center == startGateway.center)
{
break;
}
i++;
}
while (true)
{
if (ClusterGraph.gateways[j].center == endGateway.center)
{
break;
}
j++;
}
float middleDistance = ClusterGraph.gatewayDistanceTable[i].entries[j].shortestDistance;
i = 0;
j = 0;
tempDistance = startDistance + middleDistance + endDistance;
if (tempDistance < finalDistance)
{
finalDistance = tempDistance;
}
}
}
return(finalDistance);
}
}
public static Vector3 GetShortestDistancePositionInEdge(NavigationGraphNode node, LocalPath L)
{
NavMeshEdge navEdge = node as NavMeshEdge;
if (navEdge != null)
{
return MathHelper.ClosestPointInLineSegment2ToLineSegment1(
L.StartPosition, L.EndPosition,
navEdge.PointOne, navEdge.PointTwo,
node.Position);
}
return Vector3.zero;
}
public Cluster Quantize(NavigationGraphNode node)
{
foreach (Cluster c in this.clusters)
{
if (node.Position.x + 5 >= c.min.x && node.Position.z + 5 >= c.min.z && node.Position.x - 5 <= c.max.x && node.Position.z - 5 <= c.max.z)
{
return(c);
}
}
Debug.Log(" NUUUULLLLLLLLLLLLL " + node.Position);
return(null);
}
public void ToggleVisibility(NavigationGraphNode node)
{
node = TransformNode(node);
if (node == selectedNode)
{
Draw = !Draw;
}
else
{
selectedNode = node;
Draw = true;
}
}
protected float CalculateInfluenceCost(NavigationGraphNode node, NavigationGraphNode child)
{
float nodeRedInfluence = this.RedMap.GetInfluence(this.Quantize(node.Position));
float nodeGreenInfluence = this.GreenMap.GetInfluence(this.Quantize(node.Position));
float childRedInfluence = this.RedMap.GetInfluence(this.Quantize(child.Position));
float childGreenInfluence = this.GreenMap.GetInfluence(this.Quantize(child.Position));
float securityNodetoChild = nodeRedInfluence - nodeGreenInfluence;
float securityChildtoNode = childRedInfluence - childGreenInfluence;
return((securityChildtoNode + securityNodetoChild) / 2);
}
public void AddToOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
//a little help here
//is very nice that the List<T> already implements a binary search method
int index = this.Open.BinarySearch(nodeRecord);
if (index < 0)
{
this.Open.Insert(~index, nodeRecord);
}
else
this.Replace(this.Open[index], nodeRecord);
}
public NodeRecord GetNodeRecord(NavigationGraphNode node)
{
//do not change this method
//here we have the "special case" node handling
if (node.NodeIndex == -1)
{
return(this.SpecialCaseNodes.FirstOrDefault(x => x.node == node));
}
else
{
return(this.NodeRecords[node.NodeIndex]);
}
}
protected float CalculateInfluenceCost(NavigationGraphNode node, NavigationGraphNode child)
{
float nodeRedInfluence = this.RedMap.GetInfluence(this.Quantize(node.Position));
float nodeGreenInfluence = this.GreenMap.GetInfluence(this.Quantize(node.Position));
float childRedInfluence = this.RedMap.GetInfluence(this.Quantize(child.Position));
float childGreenInfluence = this.GreenMap.GetInfluence(this.Quantize(child.Position));
float securityNodetoChild = nodeRedInfluence - nodeGreenInfluence;
float securityChildtoNode = childRedInfluence - childGreenInfluence;
return (securityChildtoNode + securityNodetoChild) / 2;
}
public override ActionResult Execute(AI ai)
{
if (!MoveTargetVariable.IsValid || !MoveTargetVariable.IsVariable)
{
return(ActionResult.FAILURE);
}
WaypointSet waypointSet = GetWaypointSetFromExpression(ai);
if (waypointSet == null)
{
return(ActionResult.FAILURE);
}
if (waypointSet != lastWaypointSet)
{
lastWaypoint = -1;
lastWaypointSet = waypointSet;
}
if (lastWaypoint == -1)
{
lastWaypoint = waypointSet.GetClosestWaypointIndex(ai.Kinematic.Position);
if (lastWaypoint < 0)
{
return(ActionResult.FAILURE);
}
moveTarget.VectorTarget = waypointSet.Waypoints[lastWaypoint].position;
moveTarget.CloseEnoughDistance = Mathf.Max(waypointSet.Waypoints[lastWaypoint].range, ai.Motor.CloseEnoughDistance);
if (!ai.Motor.IsAt(moveTarget))
{
ai.WorkingMemory.SetItem <MoveLookTarget>(MoveTargetVariable.VariableName, moveTarget);
return(ActionResult.SUCCESS);
}
}
//**REMOVED EXTRA LINE HERE
NavigationGraphNode tNode = waypointSet.Graph.GetNode(lastWaypoint);
if (tNode.OutEdgeCount > 0)
{
int tRandomEdge = UnityEngine.Random.Range(0, tNode.OutEdgeCount); //**FIXED THIS LINE
lastWaypoint = ((VectorPathNode)tNode.EdgeOut(tRandomEdge).ToNode).NodeIndex;
}
moveTarget.VectorTarget = waypointSet.Waypoints[lastWaypoint].position;
moveTarget.CloseEnoughDistance = Mathf.Max(waypointSet.Waypoints[lastWaypoint].range, ai.Motor.CloseEnoughDistance);
ai.WorkingMemory.SetItem <MoveLookTarget>(MoveTargetVariable.VariableName, moveTarget);
return(ActionResult.SUCCESS);
}
// Uses only clusters
public Cluster Quantize(NavigationGraphNode node)
{
Vector3 position = node.LocalPosition;
//TODO: MAYBE IMPROVE BY DIVIDING CLUSTERS INTO ZONES
foreach (var cluster in this.clusters)
{
if (MathHelper.PointInsideBoundingBox(position, cluster.min, cluster.max))
{
return(cluster);
}
}
return(null);
}
public Cluster Quantize(NavigationGraphNode node)
{
foreach (Cluster c in clusters)
{
if (node.Position.x <= c.max.x && node.Position.x >= c.min.x)
{
if (node.Position.z <= c.max.z && node.Position.z >= c.min.z)
{
return(c);
}
}
}
return(null);
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
Vector3 dir = goalNode.Position - node.Position;
float distance = dir.magnitude;
if (!Physics.Raycast(node.Position, dir, dir.magnitude))
{
return(distance);
}
else
{
return(distance * 1.2f);
}
}
public float GetInfluence(NavigationGraphNode node)
{
LocationRecord locationRecord = new LocationRecord();
locationRecord.Location = node;
LocationRecord mapRecord = this.Closed.SearchInClosed(locationRecord);
if (mapRecord == null)
{
return(0.0f);
}
else
{
return(mapRecord.Influence);
}
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
float distanceValue = (goalNode.LocalPosition - node.LocalPosition).magnitude;
if (character.SecurityInfluence.Count > 0)
{
float securityValue = 0;
character.SecurityInfluence.TryGetValue(goalNode.Position, out securityValue);
//influencia pode ser muito pequena face à distancia.
return 0.3f * distanceValue + 0.7f * securityValue;
}
else
{
return distanceValue;
}
}
protected virtual void ProcessChildNode(NodeRecord bestNode, NavigationGraphEdge connectionEdge)
{
//this is where you process a child node
var childNode = GenerateChildNodeRecord(bestNode, connectionEdge);
//Get the cost estimate for the end node
NavigationGraphNode endNode = connectionEdge.ToNode;
float endNodeCost = bestNode.gValue + connectionEdge.Cost;
//If the node is closed we may have to skip, or remove it from the close list.
if (Closed.contains(endNode))
{
NodeRecord endNodeRecord = Closed.find(endNode); //should find this in the closed list
if (endNodeRecord.gValue <= endNodeCost)
{
continue;
}
Closed.Remove(endNodeRecord);
float endNodeHeuristic = endNodeRecord.fValue - endNodeRecord.gValue;
}
else if (Open.contains(endNode))
{
NodeRecord endNodeRecord = Open.find(endNode);
if (endNodeRecord.gValue <= endNodeCost)
{
continue;
}
float endNodeHeuristic = endNodeRecord.fValue - endNodeRecord.gValue;
}
else
{
NodeRecord endNodeRecord = new NodeRecord();
endNodeRecord.node = endNode;
var endNodeHeuristic = this.Heuristic.H(endNode, GoalNode);
endNodeRecord.gValue = endNodeCost;
endNodeRecord.parent = bestNode;
endNodeRecord.fValue = endNodeCost + endNodeHeuristic;
if (!Open.contains(endNode))
{
Open.Add(endNodeRecord);
}
}
}
public NodeRecord GetNodeRecord(NavigationGraphNode node)
{
if (node.NodeIndex == -1)
{
for (int i = 0; i < this.SpecialCaseNodes.Count; i++)
{
if (node == this.SpecialCaseNodes[i].node)
{
return(this.SpecialCaseNodes[i]);
}
}
return(null);
}
else
{
return(this.NodeRecords[node.NodeIndex]);
}
}
public NodeRecord GetNodeRecord(NavigationGraphNode node)
{
if (node.NodeIndex == -1)
{
for (int i = 0; i < this.SpecialCaseNodes.Count; i++)
{
if (node == this.SpecialCaseNodes[i].node)
{
return this.SpecialCaseNodes[i];
}
}
return null;
}
else
{
return this.NodeRecords[node.NodeIndex];
}
}
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
this.Open.Initialize();
this.Closed.Initialize();
//Initialize starting node for Dijkstra
NodeRecord StartNode = NodeRecordArray.GetNodeRecord(startNode);
StartNode.gValue = 0;
StartNode.StartNodeOutConnectionIndex = -1; // -1 corresponds to not having a "color". Valid indices will start at 0
Open.AddToOpen(StartNode);
//Dijkstra
while (Open.CountOpen() > 0)
{
NodeRecord currentNode = Open.GetBestAndRemove();
Open.RemoveFromOpen(currentNode);
Closed.AddToClosed(currentNode);
//We don't fill out the starting position as it is colorless
if (currentNode.StartNodeOutConnectionIndex != -1)
{
var outFillConnections = currentNode.node.OutEdgeCount;
Vector3 edgePosition;
//Update the bounding box with all positions of EdgeOuts of the current Node
for (int i = 0; i < outFillConnections; i++)
{
edgePosition = currentNode.node.EdgeOut(i).ToNode.Position;
nodeGoalBounds.connectionBounds[currentNode.StartNodeOutConnectionIndex].UpdateBounds(edgePosition); //update only the bound corresponding to the ConnectionIndex
}
}
//Process Child Nodes
var outConnections = currentNode.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
this.ProcessChildNode(currentNode, currentNode.node.EdgeOut(i), i);
}
}
}
//cria rectangulos
public void Search(NavigationGraphNode startNode, NodeGoalBounds nodeGoalBounds)
{
//TODO: Implement the algorithm that calculates the goal bounds using a dijkstra
//Given that the nodes in the graph correspond to the edges of a polygon, we won't be able to use the vertices of the polygon to update the bounding boxes
// mete os vizinhos do no inicial. Inicializacao da lista
var outConnections = startNode.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
NavigationGraphEdge edge = startNode.EdgeOut(i);
var childNode = edge.ToNode;
var childNodeRecord = this.NodeRecordArray.GetNodeRecord(childNode);
//adicionar ao open
NodeRecordArray.AddToOpen(childNodeRecord);
//transformar em vector3 para inicializar cada rectangulo
childNodeRecord.StartNodeOutConnectionIndex = i;
}
//giro: var startTime = Time.realtimeSinceStartup;
//enquanto houver nos no conj open
while (this.Open.CountOpen() > 0)
{
NodeRecord bestNode = this.Open.GetBestAndRemove();
//aumentar o rectangulo
nodeGoalBounds.connectionBounds[bestNode.StartNodeOutConnectionIndex].UpdateBounds(bestNode.node.LocalPosition); //isto e a cor do rectangulo. falta updateBounds
this.Closed.AddToClosed(bestNode);
//para ver as ligacoes do no que acabamos de ver
var outConnections2 = bestNode.node.OutEdgeCount;
for (int j = 0; j < outConnections2; j++)
{
this.ProcessChildNode(bestNode, bestNode.node.EdgeOut(j), bestNode.StartNodeOutConnectionIndex);
}
// giro: this.MaxOpenNodes = Mathf.Max(this.Open.CountOpen(), this.MaxOpenNodes);
}
}
public Cluster Quantize(NavigationGraphNode node)
{
//TODO implement this
var nodeX = node.LocalPosition.x;
var nodeZ = node.LocalPosition.z;
for (int i = 0; i < this.clusters.Count; i++)
{
//if the node position is within the cluster min and max then the node belongs to the cluster
if (this.clusters[i].min.x < nodeX && nodeX < this.clusters[i].max.x)
{
if (this.clusters[i].min.z < nodeZ && nodeZ < this.clusters[i].max.z)
{
return(this.clusters[i]);
}
}
}
return(null);
}
public NodeRecord GetNodeRecord(NavigationGraphNode node)
{
//do not change this method
//here we have the "special case" node handling
if (node.NodeIndex == -1)
{
for (int i = 0; i < this.SpecialCaseNodes.Count; i++)
{
if (node == this.SpecialCaseNodes [i].node)
{
return this.SpecialCaseNodes [i];
}
}
return null;
} else
{
return this.NodeRecords [node.NodeIndex];
}
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
Cluster startCluster = ClusterGraph.Quantize2(node);
Cluster goalCluster = ClusterGraph.Quantize2(goalNode);
Vector3 startPosition = node.LocalPosition;
Vector3 goalPosition = goalNode.LocalPosition;
if (startCluster == goalCluster)
{
return((goalPosition - startPosition).magnitude);
}
Vector3 startGatewayPosition;
Vector3 goalGatewayPosition;
List <Gateway> startGateways = startCluster.gateways;
List <Gateway> goalGateways = goalCluster.gateways;
float h = int.MaxValue;
float h1 = 0; float h2 = 0;
float startGatewaysCount = startGateways.Count;
float goalGatewaysCount = goalGateways.Count;
int i, j;
for (i = 0; i < startGatewaysCount; i++)
{
startGatewayPosition = startGateways[i].center;
h1 = (startGatewayPosition - startPosition).magnitude;
for (j = 0; j < goalGatewaysCount; j++)
{
goalGatewayPosition = goalGateways[j].center;
h2 = h1 + ClusterGraph.DistanceBetweenGateways(startGateways[i].id, goalGateways[j].id)
+ (goalPosition - goalGatewayPosition).magnitude;
if (h2 < h)
{
h = h2;
}
}
}
return(h);
}
public NodeRecord GetNodeRecord(NavigationGraphNode node)
{
//do not change this method
//here we have the "special case" node handling
if (node.NodeIndex == -1)
{
for (int i = 0; i < this.SpecialCaseNodes.Count; i++)
{
if (node == this.SpecialCaseNodes[i].node)
{
return(this.SpecialCaseNodes[i]);
}
}
return(null);
}
else
{
return(this.NodeRecords[node.NodeIndex]);
}
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
Cluster nodeCluster = graph.Quantize(node);
Cluster nodeGoalCluster = graph.Quantize(goalNode);
//Debug.Log("DISTS====" + nodeCluster.center + nodeGoalCluster.center);
bool check = nodeCluster.center.Equals(nodeGoalCluster.center);
if (check)
{
return((node.Position - goalNode.Position).magnitude);
}
else
{
List <Gateway> nodeClusterGateways = nodeCluster.gateways;
List <Gateway> nodeGoalClusterGateways = nodeGoalCluster.gateways;
float min = 10000f;
Gateway gateMin = ScriptableObject.CreateInstance <Gateway>();
Gateway gate2Min = ScriptableObject.CreateInstance <Gateway>();
foreach (Gateway gate in nodeClusterGateways)
{
foreach (Gateway gate2 in nodeGoalClusterGateways)
{
float distance = graph.gatewayDistanceTable[gate.id].entries[gate2.id].shortestDistance;
//Debug.Log("DISTANCEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE = " + distance);
//Debug.Log("NODENODENODENODEONDEO: " + 5 * (Math.Abs((nodeCluster.center - nodeGoalCluster.center).x) + Math.Abs((nodeCluster.center - nodeGoalCluster.center).z)));
if (distance < min)
{
min = distance;
gateMin = gate;
gate2Min = gate2;
}
}
}
float hNode = (node.Position - gateMin.center).magnitude;
float hGoal = (goalNode.Position - gate2Min.center).magnitude;
//Debug.Log("MINNNN: " + min);
return(hNode + min + hGoal);
}
}
public void AddToOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
this.NodeRecords.Add(nodeRecord);
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
return 0;
}
public NodeRecord GetNodeRecord(NavigationGraphNode n)
{
throw new NotImplementedException();
}
public float GetInfluence(NavigationGraphNode node)
{
LocationRecord locationRecord = new LocationRecord();
locationRecord.Location = node;
LocationRecord mapRecord = this.Closed.SearchInClosed(locationRecord);
if (mapRecord == null) return 0.0f;
else return mapRecord.Influence;
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
return (goalNode.LocalPosition - node.LocalPosition).magnitude;
}
public void AddToOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
nodeRecord.status = NodeStatus.Open;
Open.AddToOpen(key, nodeRecord);
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
return Vector3.Distance(node.Position, goalNode.Position);
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
return (float)Math.Sqrt(Math.Pow(goalNode.LocalPosition.x - node.LocalPosition.x, 2.0) +
Math.Pow(goalNode.LocalPosition.y - node.LocalPosition.y, 2.0));
}
public void AddToOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
this.OpenHeap.Enqueue(nodeRecord);
}
private static NavigationGraphNode TransformNode(NavigationGraphNode node)
{
NavMeshPoly poly = node as NavMeshPoly;
if (poly == null)
{
return node;
}
if(poly.EdgeCount > 0)
{
return poly.GetEdgeNode(0);
}
return node;
}
private void ApplyDijkstraNodeArray(NavigationGraphNode node)
{
Open.Initialize();
Closed.Initialize();
var currentNode = node;
GoalBoundingRecord currentNodeRecord = this.recordArray.GetNodeRecord(currentNode.NodeIndex);
Closed.AddToClosed(currentNodeRecord);
int outConnections = currentNode.OutEdgeCount;
if (outConnections <= 0)
{
return;
}
int numOfBB = outConnections > maxNumberOfBoundingBoxes ? maxNumberOfBoundingBoxes : outConnections;
boundingBoxes[currentNode.NodeIndex] = new List<BoundingBox>(new BoundingBox[numOfBB]);
for (int j = 0; j < numOfBB; j++)
{
boundingBoxes[currentNode.NodeIndex][j] = new BoundingBox();
NavigationGraphNode neighborNode = currentNode.EdgeOut(j).ToNode;
GoalBoundingRecord neighborNodeRecord = recordArray.GetNodeRecord(neighborNode.NodeIndex);
neighborNodeRecord.Previous = currentNodeRecord;
neighborNodeRecord.OriginBoundingBox = boundingBoxes[currentNode.NodeIndex][j];
Open.AddToOpen(neighborNodeRecord);
}
while (Open.CountOpen() > 0)
{
processedNodes++;
var currentRecord = Open.GetBestAndRemove();
Closed.AddToClosed(currentRecord);
//needs a function to get adjacent nodes
// NavMeshNode
outConnections = currentRecord.Location.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
var childNode = currentRecord.Location.EdgeOut(i).ToNode;
var childNodeRecord = recordArray.GetNodeRecord(childNode.NodeIndex);
float distance = GoalBoundingRecord.CalculateDistance(currentRecord, childNodeRecord);
// if child state is not in open or in closed then insert in open
if (childNodeRecord.Status == NodeStatus.Unvisited)
{
childNodeRecord.UpdatePrevious(currentRecord);
this.recordArray.AddToOpen(childNodeRecord);
}
else
{
// if child state is in open with higher F-value replace old one
if (childNodeRecord.Status == NodeStatus.Open && childNodeRecord.Distance > distance)
{
childNodeRecord.UpdatePrevious(currentRecord);
this.recordArray.Replace(childNodeRecord, childNodeRecord);
}
//else
//{
// // if child state is in close with higher F-value then delete old and add new to open
// if (childNodeRecord.Status == NodeStatus.Closed && childNodeRecord.fValue > f)
// {
// UpdateNodeRecord(childNodeRecord, bestNode, g, h);
// NodeRecordArray.RemoveFromClosed(childNodeRecord);
// NodeRecordArray.AddToOpen(childNodeRecord);
// }
//}
}
}
}
}
public NodeRecord SearchInOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
return Open.SearchInOpen(key, nodeRecord);
}
public void AddToClosed(NavigationGraphNode key, NodeRecord nodeRecord)
{
nodeRecord.status = NodeStatus.Closed;
Open.RemoveFromOpen(key, nodeRecord);
}
public float H(NavigationGraphNode node, NavigationGraphNode goalNode)
{
return Mathf.Sqrt(Mathf.Pow(node.Position.x - goalNode.Position.x, 2) + Mathf.Pow(node.Position.y - goalNode.Position.y, 2) + Mathf.Pow(node.Position.z - goalNode.Position.z, 2));
}
public NodeRecord SearchInClosed(NavigationGraphNode key, NodeRecord nodeRecord)
{
//errado
return Open.SearchInOpen(key, nodeRecord);
}
public NodeRecord SearchInOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
return this.OpenHeap.SearchForEqual(nodeRecord);
}
public void RemoveFromClosed(NavigationGraphNode key, NodeRecord nodeRecord)
{
nodeRecord.status = NodeStatus.Unvisited;
this.Open.RemoveFromOpen(key, nodeRecord);
}
public void RemoveFromOpen(NavigationGraphNode key, NodeRecord nodeRecord)
{
this.NodeRecords.Remove(nodeRecord);
}
