void MoveToPathPoint(float deltaTime)
{
Vector2i pathPoint = m_pathList[0];
Common.Vector2 distance = new Common.Vector2(pathPoint.m_x, pathPoint.m_y) - m_curPos;
if (distance.sqrMagnitude < 0.1f)
{
m_pathList.RemoveAt(0);
if (m_pathList.Count == 0)
{
Common.Vector2 curPos = GetCurPos();
Vector2i srcPos = new Vector2i((int)curPos.x, (int)curPos.y);
Vector2i desPos = new Vector2i((int)m_desPos.x, (int)m_desPos.y);
GridPathResult result = m_mgr.GetPathFinder().GridAstarFind(this, srcPos, desPos);
//if(result != null)
if (result.m_result == FindPathResult.OnTheWay)
{
m_pathList = result.m_pathList;
//Debug.Log("MoveToPathPoint " + pathPoint);
//MoveToPathPoint();
UnRegisterPos();
m_gridPos = m_pathList[0];
RegisterPos();
return;
}
else if (result.m_result == FindPathResult.Arrived)
{
StopAgent(m_arriveCallback);
return;
}
else if (result.m_result == FindPathResult.Blocked)
{
StopAgent(m_blockCallback);
return;
}
}
else
{
Vector2i pathPoint2 = m_pathList[0];
Common.Vector2 distance2 = new Common.Vector2(pathPoint2.m_x, pathPoint2.m_y) - m_curPos;
distance2.Normalize();
m_curPos += (deltaTime * m_speed) * distance2;
}
}
else
{
distance.Normalize();
m_curPos += (deltaTime * m_speed) * distance;
}
}
public void MoveTo(Common.Vector2 pos, System.Action arriveCallback = null, System.Action blockCallback = null)
{
m_state = State.Moving;
m_arriveCallback = arriveCallback;
m_blockCallback = blockCallback;
m_desPos = pos;
Common.Vector2 curPos = GetCurPos();
Vector2i srcPos = new Vector2i((int)curPos.x, (int)curPos.y);
Vector2i desPos = new Vector2i((int)m_desPos.x, (int)m_desPos.y);
m_pathList.Clear();
GridPathResult result = m_mgr.GetPathFinder().GridAstarFind(this, srcPos, desPos);
//if(result != null)
{
if (result.m_result == FindPathResult.OnTheWay)
{
m_pathList = result.m_pathList;
//CheckPathPoint();
UnRegisterPos();
m_gridPos = m_pathList[0];
RegisterPos();
return;
}
else if (result.m_result == FindPathResult.Arrived)
{
StopAgent(m_arriveCallback);
return;
}
else if (result.m_result == FindPathResult.Blocked)
{
StopAgent(m_blockCallback);
return;
}
}
}
public GridPathResult GridAstarFind(PathFindAgent agent, Vector2i srcPos, Vector2i desPos)
{
GridPathResult ret = new GridPathResult();
ClearNode();
Vector2i nearest_final_pos = new Vector2i();
if (!computeNearestFreePos(srcPos, desPos, agent, out nearest_final_pos))
{
ret.m_result = FindPathResult.Blocked;
return(ret);
}
if (srcPos == nearest_final_pos)
{
ret.m_result = FindPathResult.Arrived;
return(ret);
}
//a) push starting pos into openNodes
Node first_node = m_nodePool.getNewNode();
first_node.m_next = null;
first_node.m_prev = null;
first_node.m_pos = srcPos;
first_node.m_heuristic = (nearest_final_pos - first_node.m_pos).magnitude();
AddOpenNode(first_node);
//b) loop
bool path_found = true;
bool note_limited_reached = false;
Node node = null;
while (!note_limited_reached)
{
//b1) is open nodes is empty => failed to find the path
if (m_open_list.Count == 0)
{
path_found = false;
break;
}
//b2) get the minimum heuristic node
node = minHeuristic();
//b3) if minHeuristic is the finalNode => path was found
if (node.m_pos == nearest_final_pos)
{
path_found = true;
break;
}
//b4) move this node from closedNodes to openNodes
// add all succesors that are not in closedNodes or openNodes to openNodes
AddCloseNode(node);
RemoveOpenNode(node);
for (int i = -1; i <= 1 && !note_limited_reached; ++i)
{
for (int j = -1; j <= 1 && !note_limited_reached; ++j)
{
Vector2i suc_pos = node.m_pos + new Vector2i(i, j);
if (!isOpenOrClosePos(suc_pos))
{
if (m_mapMgr.TestAgentCells(suc_pos, agent))
{
Node suc_node = m_nodePool.getNewNode();
if (suc_node != null)
{
suc_node.m_pos = suc_pos;
suc_node.m_heuristic = heuristic(suc_node.m_pos, nearest_final_pos);
suc_node.m_prev = node;
suc_node.m_next = null;
AddOpenNode(suc_node);
}
else
{
note_limited_reached = true;
}
}
}
}
}
}
//if consumed all nodes find best node (to avoid strage behaviour)
Node last_node = node;
if (note_limited_reached)
{
for (int i = 0; i < m_close_list.Count; ++i)
{
if (m_close_list[i].m_heuristic < last_node.m_heuristic)
{
last_node = m_close_list[i];
}
}
}
if (path_found == false || last_node == first_node)
{
ret.m_result = FindPathResult.Blocked;
return(ret);
}
else
{
Node cur_node = last_node;
while (cur_node.m_prev != null)
{
cur_node.m_prev.m_next = cur_node;
cur_node = cur_node.m_prev;
}
cur_node = first_node;
for (int i = 0; cur_node.m_next != null && i < PATH_MAX_NODE; ++i)
{
ret.m_pathList.Add(cur_node.m_next.m_pos);
cur_node = cur_node.m_next;
}
ret.m_result = FindPathResult.OnTheWay;
if (ret.m_result == FindPathResult.OnTheWay && ret.m_pathList.Count == 0)
{
UnityEngine.Debug.LogWarning("没算出路径点");
}
return(ret);
}
}
