void ClearGoalNode()
{
goalNode = null;
path = null;
navMesh = null;
manager[moveBehavior].SetEnabled(false);
}
private static bool MakePointInTriangle(ref Int3 result, NavMeshNode node, int minX, int maxX, int minZ, int maxZ, Int3 offset)
{
Int3 num;
Int3 num2;
Int3 num3;
node.GetPoints(out num, out num2, out num3);
long num4 = num2.x - num.x;
long num5 = num3.x - num2.x;
long num6 = num.x - num3.x;
long num7 = num2.z - num.z;
long num8 = num3.z - num2.z;
long num9 = num.z - num3.z;
for (int i = minX; i <= maxX; i++)
{
for (int j = minZ; j <= maxZ; j++)
{
int num12 = i + offset.x;
int num13 = j + offset.z;
if (((((num4 * (num13 - num.z)) - ((num12 - num.x) * num7)) <= 0L) && (((num5 * (num13 - num2.z)) - ((num12 - num2.x) * num8)) <= 0L)) && (((num6 * (num13 - num3.z)) - ((num12 - num3.x) * num9)) <= 0L))
{
result.x = num12;
result.z = num13;
return(true);
}
}
}
return(false);
}
void OnGUI()
{
if (Event.current.type != EventType.MouseDown)
{
return;
}
if (Event.current.button != (int)moveButton)
{
return;
}
Vector3 pos = Event.current.mousePosition;
pos.y = Screen.height - pos.y;
Ray ray = Camera.main.ScreenPointToRay(pos);
RaycastHit hit;
if (Physics.Raycast(ray, out hit, Mathf.Infinity, clickLayerMask.value))
{
NavMeshNode node = grid.Quantize(hit.point);
if (node != null)
{
SendMessage("SetNavMesh", grid);
SendMessage("SetGoalNode", node);
}
}
Event.current.Use();
}
// --------------------------------------------------------------------------------
public void LoadGraph(out NavMeshGraph graph)
{
graph = new NavMeshGraph();
// map graph nodes to an id
var mappedNodes = new List <NodeToUID>();
for (int i = 0; i < m_nodeData.Count; ++i)
{
mappedNodes.Add(new NodeToUID()
{
m_node = new NavMeshNode(m_nodeData[i].m_position),
m_uid = m_nodeData[i].m_uid,
});
}
// generate graph
for (int i = 0; i < mappedNodes.Count; ++i)
{
NavMeshNode node = mappedNodes[i].m_node;
NavMeshNodeData nodeData = m_nodeData.Find(nd => nd.m_uid == mappedNodes[i].m_uid);
for (int j = 0; j < nodeData.m_connections.Count; ++j)
{
var connectedNode = mappedNodes.Find(mn => mn.m_uid == nodeData.m_connections[j]).m_node;
node.AddConnection(new NavMeshEdge(node, connectedNode));
}
graph.Add(node);
}
}
void Update()
{
if (Input.GetMouseButtonDown(1))
{
RaycastHit hit;
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out hit, 100f))
{
if (hit.collider.tag == "Navigate")
{
CurrentNode = NavMesh.Instance.FindBestNode(transform.position);
TargetNode = NavMesh.Instance.FindBestNode(hit.point);
UpdatePath();
ReachDestination();
ReachDestination();
}
}
}
if (!moving)
{
return;
}
if (Vector3.Distance(transform.position, CurrentNode.transform.position) > 0.1f)
{
MoveToNode();
}
else
{
ReachDestination();
}
}
public float Heuristic(Node n1, Node n2)
{
NavMeshNode node1 = (NavMeshNode)n1;
NavMeshNode node2 = (NavMeshNode)n2;
return(DistanceMetrics.Euclidean(node1.Position, node2.Position));
}
/// <summary>
/// Sets the path.
/// </summary>
/// <param name="newPath">The new path.</param>
public void SetPath(Path path)
{
segments.Clear();
totalLength = 0.0f;
if (path == null)
{
return;
}
for (int i = 0; i < path.StepCount; i++)
{
Edge edge = path.GetStep(i);
Segment s = new Segment();
NavMeshNode src = (NavMeshNode)edge.Source;
NavMeshNode tgt = (NavMeshNode)edge.Target;
Vector3 pos1 = src.Position;
Vector3 pos2 = tgt.Position;
s.origin = pos1;
s.dir = Vector3.Normalize(pos2 - pos1);
s.start = totalLength;
totalLength += (pos2 - pos1).magnitude;
s.end = totalLength;
segments.Add(s);
}
}
void ClearGoalNode()
{
goalNode = null;
path = null;
navMesh = null;
manager[moveBehavior].SetEnabled(false);
}
//Connects the nodes
private void CreateNavMeshEdges()
{
for (int i = 0; i < (navMeshNodes.Count - 1); i++)
{
NavMeshNode tmpNode = navMeshNodes[i];
for (int j = i + 1; j < navMeshNodes.Count; j++)
{
int verticeMatchCount = 0;
foreach (Vector3 v1 in tmpNode.Vertices)
{
foreach (Vector3 v2 in navMeshNodes[j].Vertices)
{
if (v1 == v2)
{
verticeMatchCount++;
}
}
}
//create edge if 2 vertices is shared
if (verticeMatchCount == 2)
{
//edge is created in both directions, to enable one way routes
navMeshEdges.Add(new NavMeshEdge(tmpNode, navMeshNodes[j]));
navMeshEdges.Add(new NavMeshEdge(navMeshNodes[j], tmpNode));
}
}
}
}
void DoTriangleTest()
{
var graph = machine.navgationGraph as NavMeshGraph;
var info = graph.GetNearest(to, null);
var node = info.node;
hit = Polygon.ClosestPointOnTriangle(node.v0, node.v1, node.v2, to);
startNode = node;
}
public NNInfo(NavMeshNode node)
{
this.node = node;
constrainedNode = null;
clampedPosition = Int3.zero;
constClampedPosition = Int3.zero;
UpdateInfo();
}
public static void UpdateMesh()
{
NavMesh mesh = Selection.activeTransform.GetComponent <NavMesh> ();
if (mesh != null)
{
GameObject[] nodes = GameObject.FindGameObjectsWithTag("Node");
mesh.nodes.Clear();
foreach (GameObject obj in nodes)
{
NavMeshNode node = obj.GetComponent <NavMeshNode> ();
mesh.nodes.Add(node);
node.neighbors.Clear();
foreach (GameObject other in nodes)
{
if (other != obj && Vector3.Distance(node.transform.position, other.transform.position) < 6.0f)
{
bool obstructed = false;
RaycastHit hit;
Ray newRay = new Ray(obj.transform.position, (other.transform.position - obj.transform.position).normalized);
Physics.Raycast(newRay, out hit, Vector3.Distance(obj.transform.position, other.transform.position) + 0.1f);
if (hit.collider != null)
{
Debug.LogWarning(obj.name + " hit a wall on its way to " + other.name);
obstructed = true;
}
Vector3 cross1 = Vector3.Cross((other.transform.position - obj.transform.position).normalized, Vector3.up);
Vector3 cross2 = Vector3.Cross((obj.transform.position - other.transform.position).normalized, Vector3.up);
Vector3[] crosses = new Vector3[] { cross1, cross2 };
foreach (Vector3 cross in crosses)
{
Vector3 pos1 = obj.transform.position + cross * 0.6f;
Vector3 pos2 = other.transform.position + cross * 0.6f;
RaycastHit hit2;
Ray newRay2 = new Ray(pos1, (pos2 - pos1).normalized);
Physics.Raycast(newRay2, out hit2, Vector3.Distance(pos1, pos2) + 0.1f);
if (hit2.collider != null)
{
Debug.LogWarning(obj.name + " hit a wall on its way to " + other.name);
obstructed = true;
}
}
if (!obstructed)
{
node.neighbors.Add(other.GetComponent <NavMeshNode> ());
}
}
}
}
}
}
/** Returns whether or not the graph conforms to this NNConstraint's rules.
* Note that only the first 31 graphs are considered using this function.
* If the graphMask has bit 31 set (i.e the last graph possible to fit in the mask), all graphs
* above index 31 will also be considered suitable.
*/
/*public virtual bool SuitableGraph(int graphIndex, NavGraph graph)
* {
* return ((graphMask >> graphIndex) & 1) != 0;
* }*/
/** Returns whether or not the node conforms to this NNConstraint's rules */
public virtual bool Suitable(NavMeshNode node)
{
/*if (constrainWalkability && node.Walkable != walkable) return false;
*
* if (constrainArea && area >= 0 && node.Area != area) return false;
*
* if (constrainTags && ((tags >> (int)node.Tag) & 0x1) == 0) return false;*/
return(true);
}
void ReachDestination()
{
if (path.Count > 0)
{
CurrentNode = path.Pop();
}
else
{
moving = false;
}
}
/// <summary>
/// 所有共用vertex的三角形
/// </summary>
private static void GetAllNodesByVert(ref List <CheckNodeInfo> nodeInfos, NavMeshNode startNode, Int3 vertex)
{
if (nodeInfos == null)
{
nodeInfos = new List <CheckNodeInfo>();
}
for (int i = 0; i < nodeInfos.Count; i++)
{
CheckNodeInfo info2 = nodeInfos[i];
if (info2.node == startNode)
{
return;
}
}
int num2 = -1;
if (startNode.v0 == vertex)
{
num2 = 0;
}
else if (startNode.v1 == vertex)
{
num2 = 1;
}
else if (startNode.v2 == vertex)
{
num2 = 2;
}
else
{
return;
}
CheckNodeInfo item = new CheckNodeInfo();
item.vi = num2;
item.node = startNode;
item.v0 = startNode.GetVertex(num2 % 3);
item.v1 = startNode.GetVertex((num2 + 1) % 3);
item.v2 = startNode.GetVertex((num2 + 2) % 3);
nodeInfos.Add(item);
if (startNode.connections != null)
{
for (int j = 0; j < startNode.connections.Length; j++)
{
NavMeshNode node = startNode.graph.GetNode(startNode.connections[j]) as NavMeshNode;
if (node != null)
{
GetAllNodesByVert(ref nodeInfos, node, vertex);
}
}
}
}
void SetGoalNode(NavMeshNode goal)
{
if (goal == null)
{
ClearGoalNode();
return;
}
manager[moveBehavior].SetEnabled(false);
this.goalNode = goal;
FindNewPath();
manager[moveBehavior].SetEnabled(true);
}
public NavMeshNode FindBestNode(Vector3 point)
{
float bestDistance = 1000f;
NavMeshNode best = nodes[0];
foreach (NavMeshNode node in nodes)
{
float distance = Vector3.Distance(node.transform.position, point);
if (distance < bestDistance)
{
best = node;
bestDistance = distance;
}
}
return(best);
}
private static float CalculateY(Vector3 pf, NavMeshNode node)
{
Vector3 vector;
Vector3 vector2;
Vector3 vector3;
node.GetPoints(out vector, out vector2, out vector3);
float num = ((vector2.z - vector3.z) * (vector.x - vector3.x)) + ((vector3.x - vector2.x) * (vector.z - vector3.z));
float num2 = 1f / num;
float num3 = ((vector2.z - vector3.z) * (pf.x - vector3.x)) + ((vector3.x - vector2.x) * (pf.z - vector3.z));
num3 *= num2;
float num4 = ((vector3.z - vector.z) * (pf.x - vector3.x)) + ((vector.x - vector3.x) * (pf.z - vector3.z));
num4 *= num2;
float num5 = (1f - num3) - num4;
return(((num3 * vector.y) + (num4 * vector2.y)) + (num5 * vector3.y));
}
void OnDrawGizmosSelected()
{
if (path == null)
{
return;
}
Gizmos.color = Color.yellow;
Gizmos.DrawSphere(target.Point, 0.2f);
Gizmos.color = Color.white;
for (int i = 0; i < path.StepCount; i++)
{
Edge edge = path.GetStep(i);
NavMeshNode src = (NavMeshNode)edge.Source;
NavMeshNode tgt = (NavMeshNode)edge.Target;
Gizmos.DrawSphere(tgt.Position, 0.1f);
Gizmos.DrawLine(src.Position, tgt.Position);
}
}
private static Int3 InternalMove(Int3 srcLoc, Int3 delta, ref Int groundY)
{
Int3 num4;
if ((delta.x == 0) && (delta.z == 0))
{
return(delta);
}
Int3 end = srcLoc + delta;
int edge = -1;
NavMeshGraph graph = PathFindingMachine.Instance.navgationGraph as NavMeshGraph;
NavMeshNode node = graph.bbTree.QueryInside(srcLoc, null);
if (node == null)
{
NavMeshNode node2 = graph.bbTree.QueryInside(end, null);
if (node2 == null)
{
return(Int3.zero);
}
else
{
edge = node2.EdgeIntersect(srcLoc, end);
node = node2;
}
}
MoveFromNode(node, edge, srcLoc, end, out num4);
checkedNodes.Clear();
groundY = num4.y;
if (!MoveAxisY)
{
num4.y = srcLoc.y;
}
return(num4 - srcLoc);
}
void DoIt()
{
startNode = null;
endNode = null;
#if UNITY_EDITOR
line.ClearLines();
#endif
Stopwatch watch = new Stopwatch();
watch.Start();
fixResult.Clear();
if (!machine.FindPath(from, to, ref fixResult, TwGame.Team.Neutral))
{
UnityEngine.Debug.Log("ms " + watch.ElapsedMilliseconds);
to = PathFindingMachine.Instance.GetNearestPosition(to);
bool ret = machine.FindPath(from, to, ref fixResult, TwGame.Team.Neutral);
if (!ret)
{
UnityEngine.Debug.Log("cannot find a path to " + to.ToString());
}
}
if (fixResult.Count > 0)
{
#if UNITY_EDITOR
line.ClearLines();
line.AddLine(fixResult.ToArray(), Color.red);
#endif
}
watch.Stop();
if (fixResult.Count > 0)
{
UnityEngine.Debug.Log("ms " + watch.ElapsedMilliseconds);
}
}
public PriorityNode(NavMeshNode node, float priority)
{
this.node = node;
this.priority = priority;
}
void UpdatePath()
{
if (TargetNode == null)
{
return;
}
List <PriorityNode> frontier = new List <PriorityNode> ();
frontier.Add(new PriorityNode(CurrentNode, 0f));
Dictionary <NavMeshNode, NavMeshNode> visitedDict = new Dictionary <NavMeshNode, NavMeshNode> ();
visitedDict.Add(CurrentNode, null);
Dictionary <NavMeshNode, float> costDict = new Dictionary <NavMeshNode, float> ();
costDict.Add(CurrentNode, 0);
bool found = false;
while (frontier.Count > 0 && !found)
{
NavMeshNode current = frontier [0].node;
frontier.RemoveAt(0);
foreach (NavMeshNode neighbor in current.neighbors)
{
float newCost = costDict [current] + CalculateCost(current, neighbor);
if (!costDict.ContainsKey(neighbor) || newCost < costDict[neighbor])
{
costDict [neighbor] = newCost;
frontier.Add(new PriorityNode(neighbor, newCost + DistToGoal(neighbor)));
frontier.Sort(delegate(PriorityNode x, PriorityNode y) {
return(x.priority.CompareTo(y.priority));
});
if (visitedDict.ContainsKey(neighbor))
{
visitedDict [neighbor] = current;
}
else
{
visitedDict.Add(neighbor, current);
}
}
if (neighbor == TargetNode)
{
found = true;
}
}
}
if (found)
{
path.Clear();
NavMeshNode next = TargetNode;
while (next != null)
{
path.Push(next);
next = visitedDict [next];
}
moving = true;
}
}
float DistToGoal(NavMeshNode node)
{
return(Vector3.Distance(node.transform.position, TargetNode.transform.position));
}
float CalculateCost(NavMeshNode current, NavMeshNode node)
{
return(Vector3.Distance(current.transform.position, node.transform.position));
}
/** Sets the constrained node */
public void SetConstrained(NavMeshNode constrainedNode, Int3 clampedPosition)
{
this.constrainedNode = constrainedNode;
constClampedPosition = clampedPosition;
}
public NavMeshEdge(NavMeshNode startNode, NavMeshNode endNode)
{
StartNode = startNode;
EndNode = endNode;
Distance = Vector3.Distance(startNode.Centroid, endNode.Centroid);
}
protected virtual void OnMoved(Vector3 diff)
{
if (Simulation != null && Simulation.NavMesh != null && (NavMeshNode == null || !NavMeshNode.IsInside(Position)))
{
NavMeshNode = Simulation.NavMesh.BoundingRegion.GetNodeAt(Position);
}
}
/// <summary>
///
/// </summary>
/// <param name="node"></param>
/// <param name="startEdge"></param>
/// <param name="srcLoc"></param>
/// <param name="destLoc"></param>
/// <param name="state"></param>
/// <param name="result">实际可移动到的点</param>
private static void MoveFromNode(NavMeshNode node, int startEdge, Int3 srcLoc, Int3 destLoc, out Int3 result)
{
result = srcLoc;
while (node != null)
{
int num;
int num10;
int count = 2;
// 当前在顶点上
if (node.IsVertex(srcLoc, out num))
{
// 找到所有包含srcLoc这顶点的三角形
Int3 vertex = node.GetVertex(num);
List <CheckNodeInfo> nodeInfos = null;
GetAllNodesByVert(ref nodeInfos, node, vertex);
// 找到其中将要穿过的三角形
NavMeshNode node2 = null;
int vi = -1;
for (int i = 0; i < nodeInfos.Count; i++)
{
CheckNodeInfo info = nodeInfos[i];
if ((!checkedNodes.Contains(info.node) && !VectorMath.RightXZ(info.v0, info.v2, destLoc)) && VectorMath.RightOrColinearXZ(info.v0, info.v1, destLoc))
{
node2 = info.node;
vi = info.vi;
break;
}
}
if (node2 != null)
{
node = node2;
// 将要穿过的边
startEdge = (vi + 1) % 3;
count = 1;
}
else
{
int num6 = -1;
VFactor factor3 = new VFactor();
factor3.nom = -2L;
factor3.den = 1L;
VFactor factor = factor3;
for (int j = 0; j < nodeInfos.Count; j++)
{
CheckNodeInfo info2 = nodeInfos[j];
if (!checkedNodes.Contains(info2.node))
{
int num8;
VFactor factor2 = info2.GetCosineAngle(destLoc, out num8);
if (factor2 > factor)
{
factor = factor2;
num6 = num8;
node2 = info2.node;
}
}
}
if (node2 != null)
{
MoveAlongEdge(node2, num6, srcLoc, destLoc, out result, true);
break;
}
}
}
// 将要穿过的边
int edge = -1;
if (startEdge == -1)
{
edge = node.EdgeIntersect(srcLoc, destLoc);
}
else
{
edge = node.EdgeIntersect(srcLoc, destLoc, startEdge, count);
}
// 不会穿越边
if (edge == -1)
{
if (node.ContainsPoint(destLoc))
{
// 三角形内移动
result = destLoc;
if (MoveAxisY)
{
CalculateY(ref result, node);
}
}
else
{
// 沿边所在直线移动
edge = node.GetColinearEdge(srcLoc, destLoc);
if (edge != -1)
{
MoveAlongEdge(node, edge, srcLoc, destLoc, out result, true);
}
}
break;
}
// 会穿过边，则看相邻三角形
NavMeshNode neighborByEdge = node.GetNeighborByEdge(edge, out num10);
if (neighborByEdge != null)
{
node = neighborByEdge;
startEdge = num10 + 1;
count = 2;
}
else
{
MoveAlongEdge(node, edge, srcLoc, destLoc, out result, true);
break;
}
}
}
/// <summary>
///
/// </summary>
/// <param name="node">所在node</param>
/// <param name="edge">所沿着的边</param>
/// <param name="srcLoc">from</param>
/// <param name="destLoc">to</param>
/// <param name="result">实际可到达的点</param>
private static void MoveAlongEdge(NavMeshNode node, int edge, Int3 srcLoc, Int3 destLoc, out Int3 result, bool checkAnotherEdge = true)
{
bool flag;
//DebugHelper.Assert((edge >= 0) && (edge <= 2));
Int3 vertex = node.GetVertex(edge);
Int3 num2 = node.GetVertex((edge + 1) % 3);
Int3 a = destLoc - srcLoc;
a.y = 0;
Int3 lhs = num2 - vertex;
lhs.y = 0;
lhs.NormalizeTo(1000);
int num5 = 0;
// 点乘移动向量和边向量
num5 = (lhs.x * a.x) + (lhs.z * a.z);
// 移动向量和边向量的交点
Int3 rhs = Polygon.IntersectionPoint(ref vertex, ref num2, ref srcLoc, ref destLoc, out flag);
// 不相交？
if (!flag)
{
// 边和from to存在不共线，则不能移动
if (!VectorMath.IsColinearXZ(vertex, num2, srcLoc) || !VectorMath.IsColinearXZ(vertex, num2, destLoc))
{
result = srcLoc;
return;
}
// 计算本三角形内可移动到的点 rhs
// 移动向量和边向量 同向
if (num5 >= 0)
{
int num8 = (lhs.x * (num2.x - vertex.x)) + (lhs.z * (num2.z - vertex.z));
int num9 = (lhs.x * (destLoc.x - vertex.x)) + (lhs.z * (destLoc.z - vertex.z));
rhs = (num8 <= num9) ? num2 : destLoc;
//DebugHelper.Assert((num8 >= 0) && (num9 >= 0));
}
else
{
// 移动向量和边向量 异向
int num10 = (-lhs.x * (vertex.x - num2.x)) - (lhs.z * (vertex.z - num2.z));
int num11 = (-lhs.x * (destLoc.x - num2.x)) - (lhs.z * (destLoc.z - num2.z));
rhs = (Mathf.Abs(num10) <= Mathf.Abs(num11)) ? vertex : destLoc;
//DebugHelper.Assert((num10 >= 0) && (num11 >= 0));
}
}
// 计算交点到边的顶点的距离1000倍
int num12 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 0xf4240L); // 一百万
int num13 = IntMath.Sqrt(num2.XZSqrMagnitude(rhs) * 0xf4240L);
// num5是在边上的投影
if ((num5 >= num12) && (num5 <= num13))
{
result = IntMath.Divide(lhs, (long)num5, 0xf4240L) + rhs;
if (!node.ContainsPoint(result))
{
int num16;
int num17;
int num18;
int num19;
Vector3 vector = (Vector3)(num2 - vertex);
vector.y = 0f;
vector.Normalize();
Int3 num14 = num2 - vertex;
num14.y = 0;
num14 *= 10000;
long magnitude = num14.magnitude;
VFactor factor = new VFactor();
factor.nom = num5;
factor.den = magnitude * 1000L;
getMinMax(out num16, out num18, (long)num14.x, ref factor);
getMinMax(out num17, out num19, (long)num14.z, ref factor);
if (!MakePointInTriangle(ref result, node, num16, num18, num17, num19, srcLoc) && !MakePointInTriangle(ref result, node, num16 - 4, num18 + 4, num17 - 4, num19 + 4, srcLoc))
{
result = srcLoc;
}
}
if (MoveAxisY)
{
CalculateY(ref result, node);
}
}
else
{
int num20;
int num21;
Int3 num22;
int num24;
if (num5 < num12)
{
num20 = num5 - num12;
num21 = (edge + 2) % 3;
num22 = vertex;
}
else
{
num20 = num5 - num13;
num21 = (edge + 1) % 3;
num22 = num2;
}
Int3 num23 = (Int3)((lhs * num20) / 1000000f);
NavMeshNode neighborByEdge = node.GetNeighborByEdge(num21, out num24);
if (neighborByEdge != null)
{
checkedNodes.Add(node);
MoveFromNode(neighborByEdge, num24, num22, num23 + num22, out result);
}
else
{
if (checkAnotherEdge)
{
Int3 num27 = node.GetVertex((edge + 2) % 3) - num22;
if (Int3.Dot(num27.NormalizeTo(1000), num23) > 0)
{
checkedNodes.Add(node);
MoveAlongEdge(node, num21, num22, num23 + num22, out result, false);
return;
}
}
result = num22;
}
}
}
private static void CalculateY(ref Int3 point, NavMeshNode node)
{
float num = CalculateY((Vector3)point, node);
point.y = Mathf.RoundToInt(num * 1000f);
}
void SetGoalNode(NavMeshNode goal)
{
if(goal == null) {
ClearGoalNode();
return;
}
manager[moveBehavior].SetEnabled(false);
this.goalNode = goal;
FindNewPath();
manager[moveBehavior].SetEnabled(true);
}
