public void MoveTo(int agentId, Common.Vector2 pos, System.Action arriveCallback = null, System.Action blockCallback = null)
{
PathFindAgent agent = null;
if (m_agentDic.TryGetValue(agentId, out agent))
{
agent.MoveTo(pos, arriveCallback, blockCallback);
}
}
public int AddAgent(Common.Vector2 pos, float radius, float moveSpeed)
{
m_agendIdGenerator += 1;
PathFindAgent agent = new PathFindAgent();
agent.Init(m_agendIdGenerator, this, pos, radius, moveSpeed);
m_agentDic.Add(m_agendIdGenerator, agent);
return(m_agendIdGenerator);
}
public Common.Vector2 GetCurPos(int agentId)
{
PathFindAgent agent = null;
if (m_agentDic.TryGetValue(agentId, out agent))
{
return(agent.GetCurPos());
}
else
{
return(new Common.Vector2(0, 0));
}
}
public bool TestAgentCells(Vector2i posi, PathFindAgent agent)
{
if (posi.m_x >= MAP_LENGTH_X ||
posi.m_y >= MAP_WIDTH_Z)
{
return(false);
}
List <Vector2i> outLineList = agent.m_outlineList;
for (int i = 0; i < outLineList.Count; ++i)
{
outLineList[i] = posi + agent.m_outlineOffsetList[i];
}
bool canUse = true;
for (int i = 0; i < outLineList.Count; ++i)
{
int index = GetIndex(outLineList[i]);
if (index >= 0 && index < m_mapCellList.Count)
{
GridMapCell cellInfo = m_mapCellList[index];
if (cellInfo.m_whoseArea.Count == 0)
{
//return true;
}
else if (cellInfo.m_whoseArea.Count == 1 &&
cellInfo.m_whoseArea[0] == agent.m_agentId)
{
//return true;
}
else
{
//return false;
canUse = false;
}
}
else
{
//return false;
canUse = false;
}
}
return(canUse);
}
bool computeNearestFreePos(Vector2i scrPos, Vector2i final_pos, PathFindAgent agent, out Vector2i nearest_free_pos)
{
nearest_free_pos = new Vector2i(0, 0);
bool has_found = false;
float nearest_distance = 1000000;
for (int i = -NEAREST_POS_SEARCH_RADIUS; i <= NEAREST_POS_SEARCH_RADIUS; ++i)
{
for (int j = -NEAREST_POS_SEARCH_RADIUS; j <= NEAREST_POS_SEARCH_RADIUS; ++j)
{
Vector2i new_final_pos = final_pos + new Vector2i(i, j);
if (m_mapMgr.TestAgentCells(new_final_pos, agent))
{
float distance_sqr = (final_pos - new_final_pos).sqrMagnitude();
if (distance_sqr < nearest_distance)
{
has_found = true;
nearest_free_pos = new_final_pos;
nearest_distance = distance_sqr;
}
else if (distance_sqr == nearest_distance)
{
if ((new_final_pos - scrPos).sqrMagnitude() < (nearest_free_pos - scrPos).sqrMagnitude())
{
nearest_free_pos = new_final_pos;
}
}
}
}
}
if (!has_found)
{
nearest_free_pos = m_mapMgr.GetNearestCanMovePosFromDes(scrPos, final_pos, agent);
has_found = true;
}
return(has_found);
}
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);
}
}
public Vector2i GetNearestCanMovePosFromDes(Vector2i srcPosi, Vector2i desPosi, PathFindAgent agent)
{
//srcPos.y = 0;
//desPos.y = 0;
Common.Vector3 srcPos = new Common.Vector3(srcPosi.m_x, 0, srcPosi.m_y);
Common.Vector3 desPos = new Common.Vector3(desPosi.m_x, 0, desPosi.m_y);
Common.Vector3 retPos = desPos;
Common.Vector3 dirOrigin = srcPos - desPos;
if (dirOrigin.sqrMagnitude < 0.1 * 0.1)
{
return(new Vector2i((int)retPos.x, (int)retPos.z));
}
dirOrigin.Normalize();
Common.Vector3 dir = srcPos - retPos;
//dir.Normalize();
if (!TestAgentCells(new Vector2i((int)retPos.x, (int)retPos.z), agent))
{
while (Common.Vector3.Dot(dirOrigin, dir) > 0)
{
if (TestAgentCells(new Vector2i((int)retPos.x, (int)retPos.z), agent))
{
break;
}
else
{
retPos = retPos + dirOrigin;
dir = srcPos - retPos;
//dir.Normalize();
}
}
}
Vector2i retPosi = new Vector2i((int)retPos.x, (int)retPos.z);
return(retPosi);
}
