public void Reset()
{
this.opened = false;
this.closed = false;
this.gScore = float.MaxValue;
this.fScore = float.MaxValue;
this.parent = null;
}
public void Modify(TNavNode node)
{
int index = this.nodes.IndexOf(node);
if (index != -1)
{
this.bubbleUp(index);
}
}
public bool FindPortal(TNavNode node, ref int left, ref int right)
{
for (int i = 0; i < this.neighbors.Count; i++)
{
if (this.neighbors[i] == node)
{
int edgeIndex = neighborEdges[i];
left = this.vertexIndex[edgeIndex];
right = this.vertexIndex[(edgeIndex + 1) % 3];
return(true);
}
}
return(false);
}
private void trickleDown(int index)
{
int childLeft = index * 2 + 1;
while (childLeft < this.nodes.Count)
{
TNavNode node = this.nodes[index];
int minNodeIndex = index;
TNavNode minNode = node;
TNavNode childLeftNode = this.nodes[childLeft];
if (childLeftNode.fScore < minNode.fScore)
{
minNode = childLeftNode;
minNodeIndex = childLeft;
}
int childRight = childLeft + 1;
if (childRight < this.nodes.Count)
{
TNavNode childRightNode = this.nodes[childRight];
if (childRightNode.fScore < minNode.fScore)
{
minNode = childRightNode;
minNodeIndex = childRight;
}
}
if (minNodeIndex != index)
{
this.nodes[minNodeIndex] = node;
this.nodes[index] = minNode;
index = minNodeIndex;
childLeft = index * 2 + 1;
}
else
{
break;
}
}
}
private static void BuildGraph()
{
for (int i = 0; i < triangleIndices.Count; i += 3)
{
TNavNode node = new TNavNode(triangleIndices[i], triangleIndices[i + 1], triangleIndices[i + 2]);
navNodes.Add(node);
//自己顺时针对邻接三角形来讲是逆时针........
//索引都是其实都是uint16
//navNodeDict.Add((node.index1 << 16) | node.index0, node);
//navNodeDict.Add((node.index2 << 16) | node.index1, node);
//navNodeDict.Add((node.index0 << 16) | node.index2, node);
navNodeDict.Add(node.index1 * 65536 + node.index0, node);
navNodeDict.Add(node.index2 * 65536 + node.index1, node);
navNodeDict.Add(node.index0 * 65536 + node.index2, node);
}
foreach (TNavNode node in navNodes)
{
TNavNode neighbor = null;
//index = (node.index0 << 16) | node.index1;
if (navNodeDict.TryGetValue(node.index0 * 65536 + node.index1, out neighbor))
{
node.AddNeighbor(neighbor, 0);
}
//index = (node.index1 << 16) | node.index2;
if (navNodeDict.TryGetValue(node.index1 * 65536 + node.index2, out neighbor))
{
node.AddNeighbor(neighbor, 1);
}
//index = (node.index2 << 16) | node.index0;
if (navNodeDict.TryGetValue(node.index2 * 65536 + node.index0, out neighbor))
{
node.AddNeighbor(neighbor, 2);
}
}
navNodeDict.Clear();
}
private static List <Vector3> CalculatePortals(Vector3 target)
{
navPortals.Clear();
for (int i = 0; i < pathNodes.Count - 1; i++)
{
TNavNode node0 = pathNodes[i];
TNavNode node1 = pathNodes[i + 1];
int left = -1;
int right = -1;
node0.FindPortal(node1, ref left, ref right);
navPortals.Add(TNavMesh.vertices[left]);
navPortals.Add(TNavMesh.vertices[right]);
}
navPortals.Add(target);
navPortals.Add(target);
return(navPortals);
}
private void bubbleUp(int index)
{
while (index > 0)
{
TNavNode node = this.nodes[index];
int parent = (index - 1) / 2;
TNavNode parentNode = this.nodes[parent];
if (node.fScore < parentNode.fScore)
{
this.nodes[index] = parentNode;
this.nodes[parent] = node;
index = parent;
}
else
{
break;
}
}
}
public TNavNode Pop()
{
TNavNode node = this.nodes[0];
int lastIndex = this.nodes.Count - 1;
this.nodes[0] = this.nodes[lastIndex];
this.nodes.RemoveAt(lastIndex);
if (this.nodes.Count > 1)
{
this.trickleDown(0);
}
//foreach(var n in this.nodes)
//{
// if(n.fScore < node.fScore)
// {
// int i = 0;
// }
//}
return(node);
}
public static List <Vector3> GetDebugNeighbor(Vector3 pos)
{
List <Vector3> lines = new List <Vector3>();
TNavNode node = TNavMesh.GetNode(pos);
if (node != null)
{
for (int i = 0; i < node.GetNeighborCount(); i++)
{
TNavNode neightbor = node.GetNeighbor(i);
lines.Add(vertices[neightbor.index0]);
lines.Add(vertices[neightbor.index1]);
lines.Add(vertices[neightbor.index1]);
lines.Add(vertices[neightbor.index2]);
lines.Add(vertices[neightbor.index2]);
lines.Add(vertices[neightbor.index0]);
}
}
return(lines);
}
public bool FindPath(TNavNode sourceNode, TNavNode targetNode, List <TNavNode> path)
{
openSet.Clear();
sourceNode.gScore = 0;
sourceNode.fScore = Vector3.Distance(sourceNode.position, targetNode.position);
openSet.Add(sourceNode);
sourceNode.opened = true;
while (openSet.Count != 0)
{
TNavNode current = this.openSet.Pop();
if (current == targetNode)
{
path.Add(current);
while (current.parent != null)
{
current = current.parent;
path.Add(current);
}
path.Reverse();
return(true);
}
current.opened = false;
current.closed = true;
for (int i = 0; i < current.GetNeighborCount(); i++)
{
TNavNode neighbor = current.GetNeighbor(i);
if (neighbor.closed == true)
{
continue;
}
Vector3 portalLeft, portalRight;
current.GetProtal(i, out portalLeft, out portalRight);
//http://digestingduck.blogspot.fi/2010/05/towards-better-navmesh-path-planning.html
//http://digestingduck.blogspot.fi/2010/08/visibility-optimized-graph-experiment.html
//if (TNavMeshUtility.isectSegSeg(current.position, targetNode.position, portalLeft, portalRight, ref neighbor.position) == false)
//{
// Vector3 newLeft = portalLeft + (portalRight - portalLeft) * 0.1f;
// Vector3 newRight = portalRight + (portalLeft - portalRight) * 0.1f;
// float leftDistance = (targetNode.position - newLeft).sqrMagnitude;
// float rightDistance = (targetNode.position - newRight).sqrMagnitude;
// if (leftDistance < rightDistance)
// neighbor.position = newLeft;
// else
// neighbor.position = newRight;
//}
//neighbor.position = current.position在potal的投影
//路径看起来和上面的方法好像差不多......
Vector3 edge = portalRight - portalLeft;
float edgeLengthSquare = edge.sqrMagnitude;
if (edgeLengthSquare > 0.0001f)
{
Vector3 v = current.position - portalLeft;
float pdot = Vector3.Dot(v, edge) / edgeLengthSquare;
pdot = Mathf.Clamp(pdot, 0.05f, 0.95f);
neighbor.position = portalLeft + pdot * edge;
}
else
{
neighbor.position = portalLeft;
}
float gScore = current.gScore + this.HeuristicCostEstimate(current.position, neighbor.position);
if (gScore >= neighbor.gScore)//neighbor.opened == true的话,neighbor.gCost=MaxValue，gCost一定小于neighbor.gCost
{
continue;
}
float fScore = gScore + this.HeuristicCostEstimate(neighbor.position, targetNode.position);
//need????
if (fScore >= neighbor.fScore)
{
continue;
}
neighbor.parent = current;
neighbor.gScore = gScore;
neighbor.fScore = fScore;
if (neighbor.opened == false)
{
this.openSet.Add(neighbor);
neighbor.opened = true;
}
else
{
this.openSet.Modify(neighbor);
}
}
}
return(false);
}
public void AddNeighbor(TNavNode neighbor, int edgeIndex)
{
this.neighbors.Add(neighbor);
this.neighborEdges.Add(edgeIndex);
}
public void Add(TNavNode node)
{
this.nodes.Add(node);
this.bubbleUp(this.nodes.Count - 1);
}