Mosaic Game 中的寻路系统
mosaic_game的寻路方案完全基于RecastNavigation, 不过为了适应大世界的精度需求使用了double来替换原来的float，修改后的仓库见https://github.com/huangfeidian/recastnavigation。基于此项目的RecastDemo导出的NavMesh文件需要放到工程目录的/data/map/navmesh文件夹下，而对应的Obj文件则需要放到/data/map/wavefront文件夹下，指定编号的场景对应的地形文件和寻路文件映射则需要在/data/xlsx/场景表.xlsx中进行配置。每个Space都有一个对应的space_navi_component, 当一个Space被创建之后，space_navi_component的activate函数则会向map_server请求创建一个对应的群体寻路管理器：
void space_navi_component::activate()
{
json cur_msg, cur_param;
cur_msg["cmd"] = "request_create_navi_crowd";
cur_param["anchor"] = *m_owner->get_call_proxy();
cur_param["resource_path"] = m_navi_map;
cur_param["max_agent_num"] = m_navi_agent_num;
cur_param["max_agent_radius"] = m_max_agent_radius;
cur_param["extend"] = m_half_extend;
cur_msg["param"] = std::move(cur_param);
m_owner->call_map_server(cur_msg);
}
这里传递的参数都在space_navi_component::init的时候从对应的配置项中读取：
bool space_navi_component::init(const json& data)
{
auto cur_space_sysd = m_owner->space_sysd();
if(!cur_space_sysd.expect_value("navi_map", m_navi_map))
{
m_owner->logger()->error("cant find navi_map for space {} with value {}", m_owner->space_no(), cur_space_sysd.get_cell("navi_map").dump());
return false;
}
if(!cur_space_sysd.expect_value("navi_agent_num", m_navi_agent_num))
{
m_owner->logger()->error("cant find navi_agent_num for space {}", m_owner->space_no());
return false;
}
if(!cur_space_sysd.expect_value("navi_half_extend", m_half_extend))
{
m_owner->logger()->error("cant find navi_half_extend for space {}", m_owner->space_no());
return false;
}
if(!cur_space_sysd.expect_value("navi_agent_max_radius", m_max_agent_radius))
{
m_owner->logger()->error("cant find navi_agent_max_radius for space {}", m_owner->space_no());
return false;
}
m_handler_to_entities = std::vector<actor_entity*>(m_navi_agent_num+1, nullptr);
return true;
}
当map_server接收到创建新场景的群体寻路管理器时，并不会每个场景都尝试加载对应的NavMesh文件到内存中，而是会先判断对应的NavMesh是否已经被加载了，如果加载了则复用这份const dtNavMesh*来创建群体寻路管理器，如果没有加载则再尝试加载，通过寻路资源复用来减少内存占用。
resource_path = m_map_config.navi_resource_folder +"/"+ resource_path;
if(!std::filesystem::exists(resource_path))
{
m_logger->error("on_request_create_navi_crowd cant find map {} ", resource_path);
return;
}
auto loaded_nav_iter = m_loaded_navi_maps.find(resource_path);
if(loaded_nav_iter == m_loaded_navi_maps.end()) // 如果没有加载
{
auto loading_nav_iter = m_pending_load_navmesh.find(resource_path);
if(loading_nav_iter == m_pending_load_navmesh.end())// 如果不在加载过程中
{
// 执行真正的加载
add_task_to_async_loop([this, resource_path]()
{
begin_load_navi_map(resource_path);
});
// 同时记录同时有多少个请求在等待这个寻路资源
m_pending_load_navmesh[resource_path].push_back(msg);
}
else
{
// 添加当前消息到等待队列
loading_nav_iter->second.push_back(msg);
}
}
else
{
if(!loaded_nav_iter->second)
{
m_logger->error("on_request_create_navi_crowd invalid map {}", resource_path);
return;
}
// 给每个群体寻路管理器分配一个唯一id
auto cur_crowd_index = ++m_navi_crowd_counter;
auto cur_nav_crowd = std::make_shared<navi_crowd>(this, cur_crowd_index, loaded_nav_iter->second, max_agent_num, max_agent_radius, space_anchor);
m_navi_crowds[cur_crowd_index] = cur_nav_crowd;
utility::rpc_msg cur_msg;
cur_msg.cmd = "reply_create_navi_map";
cur_msg.set_args(cur_crowd_index); // 将这个id返回给space 后续的通信需要带上这个crowd index
call_entity(cur_nav_crowd->m_space_anchor, cur_msg);
finish_update_navi_crowd(cur_nav_crowd->m_crowd_index); // 将当前的navi_crowd放到等待更新的队列中
return;
}
然后map_server的每帧更新中，都会处理固定数量的navi_crowd的更新：
void map_server::on_new_frame()
{
json_server::on_new_frame();
update_navi_crowds();
}
void map_server::update_navi_crowds()
{
std::uint32_t i = 10;
auto cur_ts = utility::timer_manager::now_ts();
while(i > 1 && !m_navi_crowd_update_queue.empty())
{
i--;
if(m_navi_crowd_update_queue.front().second > cur_ts)
{
// 当前task还没到执行时间 后续的task也不需要继续处理了
return;
}
auto temp_navi_crowd_index = m_navi_crowd_update_queue.front().first;
m_navi_crowd_update_queue.pop();
auto temp_iter = m_navi_crowds.find(temp_navi_crowd_index);
if(temp_iter == m_navi_crowds.end())
{
continue;;
}
auto temp_crowd = temp_iter->second;
add_task_to_async_loop([this, temp_crowd]()
{
temp_crowd->update();
add_task_to_main_loop([this, temp_crowd]()
{
finish_update_navi_crowd(temp_crowd->m_crowd_index);
});
}, temp_crowd->m_crowd_index);
}
}
上面的add_task_to_async_loop会将一个任务扔到异步线程池中的一个线程中执行，同时为了保证同一个navi_crowd的所有操作都是在同一个线程中执行的，这里需要传递m_crowd_index来指定线程：
void basic_stub::add_task_to_async_loop(std::function<void()>&& cur_task, std::uint64_t channel_index)
{
if(m_async_task_channels.empty())
{
m_logger->error("add_task_to_async_loop fail task channel is empty");
return;
}
if(m_async_task_channels.size() == 1)
{
channel_index = 0;
}
else
{
if(channel_index == 0)
{
channel_index = gen_unique_uint64() % m_async_task_channels.size();
}
else
{
channel_index = channel_index % m_async_threads.size();
}
}
m_async_task_channels[channel_index].push_msg(std::forward<std::function<void()>>(cur_task));
}
如果外部传递的索引是0，则代表这个任务不需要与其他任务串行化，内部则随机指定一个线程即可。
当单个的navi_crowd执行完成之后，需要将这个navi_crowd重新挂载到待更新的队列中，由于这个队列只在主线程维护，因此使用了add_task_to_main_loop来操作这个线程安全队列:
void basic_stub::add_task_to_main_loop(std::function<void()>&& cur_task)
{
m_main_loop_tasks.push_msg(std::forward<std::function<void()>>(cur_task));
}
在主线程的main_loop中会主动调用poll_mainloop_tasks去获取m_main_loop_tasks里的任务去执行：
std::size_t basic_stub::poll_mainloop_tasks()
{
const static std::uint32_t batch_task_num =10;
std::array<std::function<void()>, batch_task_num> temp_tasks;
std::uint64_t pop_get_num = 0;
std::uint64_t total_pop_num = 0;
while((pop_get_num = m_main_loop_tasks.pop_bulk_msg(temp_tasks.data(), batch_task_num)))
{
total_pop_num += pop_get_num;
for(std::uint32_t i = 0; i< pop_get_num; i++)
{
temp_tasks[i]();
}
}
return total_pop_num;
}
在space_navi_component发起创建群体寻路管理器的请求到接收到map_server的回应期间，所有actor_entity寻路相关的请求都会被缓存起来，当收到map_server的创建群体寻路管理器回答之后，再将这些请求发送到map_server上:
void space_navi_component::reply_create_navi_map(const utility::rpc_msg& msg, const std::uint64_t navi_map_handler)
{
m_navi_map_handler = navi_map_handler;
m_owner->logger()->info("reply_create_navi_map {} with {} buffered_reqs", m_navi_map_handler, m_buffered_reqs.size());
for(auto & one_req: m_buffered_reqs)
{
one_req["param"]["navi_crowd"] = m_navi_map_handler;
m_owner->call_map_server(one_req);
}
m_buffered_reqs.clear();
}
当一个actor_entity发起一个具体的寻路请求时，会调用到space_navi_component上的中转接口:
bool actor_navi_component::navi_to_pos(const std::array<double, 3>& dest, const double radius)
{
// 这里省略一些代码
cur_space_navi_comp->navi_to_pos(m_owner, cur_navi_prop->request_version(), dest, radius);
}
void space_navi_component::navi_to_pos(actor_entity* cur_entity, std::uint32_t req_version, const std::array<double, 3>& dest, const double radius)
{
utility::navi_request cur_nav_req;
cur_nav_req.cmd = utility::navi_cmd::move_to_pos;
cur_nav_req.req_version = req_version;
cur_nav_req.pos = dest;
cur_nav_req.dest_radius = radius;
cur_nav_req.from_handler = cur_entity->navi_handler();
send_navi_req(cur_entity, cur_nav_req);
}
这个中转接口负责打包出来一个navi_request,调用send_navi_req发送到map_server的对应navi_crowd去处理，为了正确的路由到对应的navi_crowd，请求中需要携带创建群体寻路管理器响应中初始化的navi_map_handler：
void space_navi_component::send_navi_req(actor_entity* cur_entity, const utility::navi_request& cur_req)
{
cur_entity->dispatcher().dispatch(cur_req.cmd, cur_req);
json cur_msg, cur_param;
cur_msg["cmd"] = "navi_request";
cur_param["navi_req"] = serialize::encode(cur_req);
cur_param["navi_crowd"] = m_navi_map_handler; // navi_crowd的唯一索引
if(m_owner)
{
if(m_owner->is_cell_space())
{
cur_param["anchor"] = *cur_entity->get_call_proxy();
}
else
{
cur_param["anchor"] = "";
}
}
else
{
cur_param["anchor"] = "";
}
cur_msg["param"] = std::move(cur_param);
if(m_navi_map_handler == 0)
{
m_owner->logger()->warn("send_navi_req {} while map handler invalid ", cur_req.encode().dump());
m_buffered_reqs.push_back(cur_msg);
return;
}
m_owner->call_map_server(cur_msg);
}
当map_server接收到具体的寻路请求之后，解析出其中传递的navi_crowd_index，然后创建一个异步任务将这个请求转发到对应的navi_crowd所在的线程:
void map_server::on_navi_request(const json& msg)
{
std::uint64_t temp_navi_crowd_index;
json navi_req_json;
utility::navi_request navi_req;
std::string callback_anchor;
try
{
msg.at("navi_crowd").get_to(temp_navi_crowd_index);
msg.at("navi_req").get_to(navi_req_json);
msg.at("anchor").get_to(callback_anchor);
}
catch(const std::exception& e)
{
m_logger->error("on_navi_request fail to parse msg {} with error {}", msg.dump(), e.what());
return;
}
if(!serialize::decode(navi_req_json, navi_req))
{
m_logger->error("on_navi_request fail to parse msg {} to navi_req", navi_req_json.dump());
return;
}
auto temp_iter = m_navi_crowds.find(temp_navi_crowd_index);
if(temp_iter == m_navi_crowds.end())
{
return;
}
auto temp_crowd = temp_iter->second;
add_task_to_async_loop([temp_crowd, navi_req, callback_anchor]()
{
temp_crowd->on_navi_request(callback_anchor, navi_req);
}, temp_crowd->m_crowd_index);
}
上面构造lambda的时候可以尝试使用右值引用来初始化被捕获的变量，以避免各种不必要的内存分配，不过这里偷懒就没有去做优化。
当navi_crowd处理传递过来的navi_request时，根据navi_request内部的cmd进行指令路由：
void navi_crowd::on_navi_request(const std::string& agent_anchor, const utility::navi_request& req_data)
{
switch(req_data.cmd)
{
case utility::navi_cmd::add:
{
return on_navi_add_req(agent_anchor, req_data);
}
case utility::navi_cmd::remove:
{
return on_navi_remove_req(agent_anchor, req_data);
}
// 此处省略其他cmd
}
}
当一个请求被对应的函数处理之后，会构造一个navi_reply对象发送到请求方:
//navi_crowd::on_navi_add_req
utility::navi_reply rep_data;
rep_data.cmd = req_data.cmd;
rep_data.req_version = req_data.req_version;
rep_data.entity_index = req_data.entity_index;
rep_data.from_handler = cur_agent_idx + 1;
if(agent_anchor.empty())
{
m_agent_anchors[rep_data.from_handler] = m_space_anchor;
}
else
{
m_agent_anchors[rep_data.from_handler] = std::make_shared<std::string>(agent_anchor);
}
m_agent_cmd_versions[rep_data.from_handler] = req_data.req_version;
send_reply(m_agent_anchors[rep_data.from_handler], rep_data);
上面的from_handler记录了本次创建的dtCrowdAgent的AgentIndex + 1，而entity_index则对应了对应entity的线上唯一索引，send_reply函数负责将请求传递到主线程去执行真正的消息发送:
void navi_crowd::send_reply(std::shared_ptr<std::string> anchor, const utility::navi_reply& rep_data)
{
if(!anchor)
{
anchor = m_space_anchor;
}
utility::rpc_msg rep_msg;
rep_msg.cmd = "navi_reply";
rep_msg.args.push_back(rep_data.encode());
m_server->add_task_to_main_loop([=, rep_msg = std::move(rep_msg)]()
{
m_server->call_entity(anchor, rep_msg);
});
}
当space_navi_component收到对应的navi_reply之后，会根据对应的from_handler找到发起请求的actor_entity来执行应答通知:
void space_navi_component::navi_reply(const utility::rpc_msg& msg, const json& rep_msg)
{
utility::navi_reply cur_reply;
if(!serialize::decode(rep_msg, cur_reply))
{
return;
}
if(cur_reply.cmd == utility::navi_cmd::add)
{
auto cur_entity = m_owner->get_entity(utility::entity_desc::gen_local_id(cur_reply.entity_index));
if(!cur_entity)
{
utility::navi_request cur_nav_req;
cur_nav_req.cmd = utility::navi_cmd::remove;
cur_nav_req.req_version = 0;
cur_nav_req.from_handler = cur_reply.from_handler;
send_navi_req(nullptr, cur_nav_req);
return;
}
m_handler_to_entities[cur_reply.from_handler] = cur_entity;
}
if(cur_reply.cmd == utility::navi_cmd::remove)
{
return;
}
m_handler_to_entities[cur_reply.from_handler]->on_navi_reply(cur_reply);
}
至此一个完成的寻路请求处理流程结束，下面是对应的调用时序图:
map_server中创建的navi_crowd其实并不负责真正的群体寻路管理器的更新，只是作为一个异步操作群体寻路管理器的封装而存在的，将请求解析好之后进一步转发到真正的DetourCrowd中:
void navi_crowd::on_navi_to_pos_req(const std::string& agent_anchor, const utility::navi_request& req_data)
{
if(req_data.from_handler == 0 || !m_agent_anchors[req_data.from_handler])
{
return;
}
m_agent_cmd_versions[req_data.from_handler] = req_data.req_version;
m_detour_crowd->requestMoveTarget(req_data.from_handler - 1, 0, req_data.pos.data(), req_data.dest_radius);
utility::navi_reply rep_data;
rep_data.cmd = req_data.cmd;
rep_data.req_version = req_data.req_version;
rep_data.from_handler = req_data.from_handler;
send_reply(m_agent_anchors[rep_data.from_handler], rep_data);
}
navi_crowd主要的逻辑除了这些寻路请求接口的转发之外，还负责在detourCrwod执行更新之后将所有的dtAgent的新位置收集起来通知对应的actor_entity，同时将寻路任务的完成状况通知到对应的actor_entity:
void navi_crowd::update()
{
auto cur_ms = utility::timer_manager::now_ts();
float dt = (cur_ms - m_last_update_ms) / 1000.0f;
m_last_update_ms = cur_ms;
m_detour_crowd->update(dt, nullptr); // 驱动detourcrowd的更新
auto active_num = m_detour_crowd->getActiveAgents(m_active_agent_vec.data(), m_active_agent_vec.size());
system::navigation::ExtendDetourCrowdAgent* idx_0_agent = m_detour_crowd->getEditableAgent(0);
m_temp_batch_replys.clear();
for (int i = 0; i < active_num; i++) // 将所有的正在移动的agent的最新位置收集起来
{
auto cur_agent = m_active_agent_vec[i];
if(cur_agent->targetState != spiritsaway::system::navigation::ExtendDetourMoveRequestState::TARGET_VALID)
{
continue;
}
utility::navi_reply one_ack;
one_ack.from_handler = std::distance(idx_0_agent, cur_agent) + 1;
one_ack.req_version = m_agent_cmd_versions[one_ack.from_handler];
one_ack.cmd = utility::navi_cmd::notify_sync_pos;
std::copy(cur_agent->npos, cur_agent->npos + 3, one_ack.pos.data());
std::copy(cur_agent->vel, cur_agent->vel + 3, one_ack.speed.data());
if(m_agent_anchors[one_ack.from_handler] == m_space_anchor)
{
// 对应的是非分布式场景 直接加入到这个数组中
m_temp_batch_replys.push_back(one_ack);
}
else
{
// 否则每个agent的位置单独走rpc推送更新
send_reply(m_agent_anchors[one_ack.from_handler], one_ack);
}
}
if(!m_temp_batch_replys.empty())
{
// 这里走批量更新 避免构造多个数据包 减少流量
send_replys(m_temp_batch_replys);
}
m_temp_batch_replys.clear();
// 收集所有已经达到寻路目标点的agent 通知其寻路任务结束
auto finish_num = m_detour_crowd->fetchAndClearArrived(m_finished_agent_vec.data(), m_finished_agent_vec.size());
for (int i = 0; i < finish_num; i++)
{
utility::navi_reply one_ack;
one_ack.from_handler = m_finished_agent_vec[i] + 1;
one_ack.req_version = m_agent_cmd_versions[one_ack.from_handler];
one_ack.cmd = utility::navi_cmd::notify_move_finish;
if(m_agent_anchors[one_ack.from_handler] == m_space_anchor)
{
m_temp_batch_replys.push_back(one_ack);
}
else
{
send_reply(m_agent_anchors[one_ack.from_handler], one_ack);
}
}
if(!m_temp_batch_replys.empty())
{
send_replys(m_temp_batch_replys);
}
}
由于detour_crowd的更新在map_server上，而actor_entity的寻路请求在space_server上，两者只能通过网络异步通信，所以有可能actor_entity取消了寻路并强制修改本地位置之后，之前的寻路请求导致的位置更新被推送下来，导致位置被覆写，出现拉扯的情况。为了避免这种情况的发生，我们在actor_entity上增加了寻路请求版本号req_version，每次发起寻路时这个版本号都会自增，初始时为0。
// bool actor_navi_component::navi_to_pos(const std::array<double, 3>& dest, const double radius)
cur_navi_prop_proxy->request_version().set(cur_navi_prop->request_version() + 1);
cur_navi_prop_proxy->state().set(std::uint8_t(utility::navi_cmd::move_to_pos));
cur_navi_prop_proxy->dest_pos().set(dest);
cur_navi_prop_proxy->dest_radius().set(radius);
cur_space_navi_comp->navi_to_pos(m_owner, cur_navi_prop->request_version(), dest, radius);
当actor_entity收到navi_reply的时候，需要检查一下版本号是否匹配：
void actor_navi_component::on_navi_reply(const utility::navi_reply& cur_navi_reply)
{
auto cur_navi_prop_proxy = m_owner->navi_prop_proxy();
if(cur_navi_reply.req_version != cur_navi_prop_proxy->request_version().get())
{
return;
}
// 省略其他代码
}
在actor_navi_component上提供了如下的几个寻路接口:
bool navi_to_pos(const std::array<double, 3>& dest, const double radius);
bool navi_to_entity(actor_entity* dest_entity, const double radius);
bool navi_follow_entity(actor_entity* dest_entity, const double radius);
void navi_cancel();
navi_to_pos负责开启一个走向固定点的寻路任务，对应的就是detourCrowd中的requestMoveTarget接口。而navi_to_entity和navi_follow_entity分别对应了移动到某个Entity周围和开启对某个Entity的跟随移动，这两个移动任务在原始的DetourCrowd中是没有的，属于mosaic_game中对DetourCrowd的扩展。我将这个扩展版本的DetourCrowd命名为ExtendDetourCrowd,对应的文件在工程目录的/common/extend_detour中。代码的大部分都来自于DetourCrowd，为了处理上述的趋近寻路和跟随寻路增加了下面的接口：
/// Submits a new move request for the specified agent.
/// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()]
/// @param[in] ref The position's polygon reference.
/// @param[in] pos The position within the polygon. [(x, y, z)]
/// @param[in] target_radius distance to pos less than raget_radius considered complete
/// @param[in] follow continue to follow when in target_radius
/// @return True if the request was successfully submitted.
bool requestChaseTarget(const int idx, const int target_idx, const dtReal_t target_radius = 0, const bool follow = false);
idx代表发起寻路的agent索引，target_idx代表目标agent索引，follow代表是否开启持续性的跟随。跟requestMoveTarget接口一样，这个接口只负责初始化相关的dtAgent成员变量：
ag->target_agent = target_idx; // 所追踪的目标agent的索引
m_agents[target_idx].follow_by_agent_count++; // 目标agent上记录当前被多少agent追踪
ag->continue_follow = follow; // 是否开启持续跟随
updateAgentFollowTarget(ag); // 根据目标agent来设置当前agent的终点poly和position信息
驱动跟随时位移其实就是定期的以趋近目标所在的poly和position来更新当前agent的目标位置并触发路径重新规划，这样就可以复用原始DetourCrowd的所有基于点目标进行移动的代码：
// 这个函数负责驱动趋近移动时corridor的更新
void ExtendDetourCrowd::checkFollowSteer(ExtendDetourCrowdAgent** agents, const int nagents)
{
for (int i = 0; i < nagents; ++i)
{
ExtendDetourCrowdAgent* ag = agents[i];
if (ag->targetState != ExtendDetourMoveRequestState::TARGET_VALID && ag->targetState != ExtendDetourMoveRequestState::TARGET_VELOCITY)
{
continue;
}
if (ag->state != ExtendDetourCrowdAgentState::STATE_WALKING)
{
continue;
}
if (ag->move_state != ExtendDetourCrowdAgentMoveState::STATE_MOVING)
{
continue;
}
if (ag->target_agent == -1)
{
continue;
}
auto target_ag = getAgent(ag->target_agent);
if (!target_ag || !target_ag->active)
{
ag->move_state = ExtendDetourCrowdAgentMoveState::STATE_FINISHED;
dtVset(ag->dvel, 0, 0, 0);
ag->targetState = ExtendDetourMoveRequestState::TARGET_FAILED;
continue;
}
// 两倍半径和作为阈值
auto dist_threhold = 2*(ag->params.radius + ag->target_radius + target_ag->params.radius);
auto cur_dis = dtVdist(ag->npos, target_ag->npos);
dtReal_t offset[3];
dtVsub(offset, target_ag->npos, ag->npos);
dtVnormalize(offset);
dtVscale(offset, offset, ag->params.maxSpeed);
if (ag->targetState == ExtendDetourMoveRequestState::TARGET_VALID)
{
if (cur_dis < dist_threhold)// 如果两个agent的距离小于上面计算的阈值 则开启存粹的基于速度的移动
{
// 根据两者的位置差异来确定移动速度方向
requestMoveVelocity(getAgentIndex(ag), offset);
continue;
}
}
else // 在基于速度的移动状态
{
if (cur_dis > 2 * dist_threhold) // 如果大于了两倍阈值 则切换到目标点移动状态
{
updateAgentFollowTarget(ag);
ag->targetState = ExtendDetourMoveRequestState::TARGET_VALID;
}
else
{
// 继续使用基于速度的插值位置更新
dtVcopy(ag->targetPos, offset);
continue;
}
}
auto now_to_cor_dis = dtVdist(ag->npos, ag->corridor.getTarget());
auto cor_to_target_dis = dtVdist(ag->corridor.getTarget(), target_ag->npos);
// 如果当前agent的corridor末尾点距离目标agent的距离大于指定阈值 且当前corridor的长度小于这个距离的10倍
// 则更新当前agent的corridor末尾为target_ag->pos
// 这样避免离很远时经常更新corridor
if (cor_to_target_dis < dist_threhold && now_to_cor_dis < 10 * cor_to_target_dis)
{
updateAgentFollowTarget(ag);
ag->corridor.moveTargetPosition(target_ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);
continue;
}
// 如果ag->npos ag->corridor target_ag->pos 三个点组成了一个倒角
// 则认为当前路径离最短路径差异较大 需要重新规划
if (cor_to_target_dis + now_to_cor_dis > 1.5 * cur_dis)
{
updateAgentFollowTarget(ag);
requestMoveTargetReplan(getAgentIndex(ag), ag->targetRef, ag->targetPos);
continue;
}
}
}
如果开启了持续跟随，当当前agent与目标agent之间的距离小于跟随范围时，当前agent会暂停移动，临时性的不再参与寻路系统的更新。为了将这种临时暂停与没有寻路任务区分开来，我们增加了一个枚举来代表当前agent的移动状态：
enum class ExtendDetourCrowdAgentMoveState
{
STATE_IDLE, // 无移动任务
STATE_MOVING, // 正在移动
STATE_WAITING, // 正在休眠
STATE_FINISHED, // 运动已经结束
};
struct ExtendDetourCrowdAgent
{
// 增加了一个move_state字段代表移动状态
ExtendDetourCrowdAgentMoveState move_state = ExtendDetourCrowdAgentMoveState::STATE_IDLE;
};
当跟随移动时两者距离小于指定阈值时进入STATE_WAITING状态，直到两者之间的距离差距拉大到了ag->params.radius + ag->target_radius + target_ag->params.radius时重新进入到STATE_MOVING。而STATE_FINISHED则代表当前寻路任务完成，这个状态由checkArrived函数进行设置，该函数在ExtendDetourCrowd::update的末尾进行调用：
void ExtendDetourCrowd::checkArrived(ExtendDetourCrowdAgent** agents, const int nagents)
{
for (int i = 0; i < nagents; ++i)
{
ExtendDetourCrowdAgent* ag = agents[i];
if (ag->state != ExtendDetourCrowdAgentState::STATE_WALKING)
{
continue;
}
if (ag->target_agent < 0) // 基于目标点的寻路
{
if (ag->targetState != ExtendDetourMoveRequestState::TARGET_VALID)
{
continue;
}
auto cur_dis = dtVdist(ag->npos, ag->targetPos);
bool close_tag = false; //是否已经到了阈值半径内
if (ag->target_radius != 0)
{
close_tag = cur_dis <= ag->target_radius;
}
else
{
close_tag = cur_dis <= ag->params.radius * 0.5;
}
if (close_tag)
{// 到了阈值半径内标记为寻路结束
ag->move_state = ExtendDetourCrowdAgentMoveState::STATE_FINISHED;
dtVset(ag->vel, 0, 0, 0);
}
}
else // 在执行趋近寻路
{
if (ag->targetState != ExtendDetourMoveRequestState::TARGET_VALID && ag->targetState != ExtendDetourMoveRequestState::TARGET_VELOCITY)
{
continue;
}
auto target_ag = getAgent(ag->target_agent);
if (!target_ag) // 趋近目标不存在时 认为寻路完成
{
ag->move_state = ExtendDetourCrowdAgentMoveState::STATE_FINISHED;
dtVset(ag->vel, 0, 0, 0);
continue;
}
if (ag->move_state == ExtendDetourCrowdAgentMoveState::STATE_WAITING) // 跟随等待状态 不处理
{
continue;
}
auto cur_dis = dtVdist(ag->npos, target_ag->npos);
bool close_tag = false;
if (ag->target_radius != 0)
{
close_tag = cur_dis <= ag->target_radius;
}
else
{
close_tag = cur_dis <= (ag->params.radius + target_ag->params.radius) * 0.5;
}
if (close_tag)//是否已经到了阈值半径内
{
if (ag->continue_follow) // 如果开启了持续跟随 则进入暂停等待状态
{
ag->move_state = ExtendDetourCrowdAgentMoveState::STATE_WAITING;
dtVset(ag->vel, 0, 0, 0);
}
else // 代表趋近寻路完成
{
ag->move_state = ExtendDetourCrowdAgentMoveState::STATE_FINISHED;
dtVset(ag->vel, 0, 0, 0);
}
}
}
}
}
外部的navi_crowd在触发了ExtendDetourCrowd的update之后，会调用ExtendDetourCrowd::fetchAndClearArrived来收集所有寻路完成的agent，然后向相关的actor_entity进行通知任务完成:
int ExtendDetourCrowd::fetchAndClearArrived(int* result, const int max_result)
{
ExtendDetourCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
int count = 0;
for (int i = 0; i < nagents; i++)
{
if (agents[i]->move_state == ExtendDetourCrowdAgentMoveState::STATE_MOVING)
{
if (agents[i]->targetState == ExtendDetourMoveRequestState::TARGET_FAILED)
{
agents[i]->move_state = ExtendDetourCrowdAgentMoveState::STATE_FINISHED;
}
}
if (agents[i]->move_state == ExtendDetourCrowdAgentMoveState::STATE_FINISHED)
{
auto cur_agent_idx = getAgentIndex(agents[i]);
result[count] = cur_agent_idx;
count++;
resetMoveTarget(cur_agent_idx);
if (count >= max_result)
{
break;
}
}
}
return count;
}