532 lines
16 KiB
C++
532 lines
16 KiB
C++
#include "precompile.h"
|
|
|
|
#include <math.h>
|
|
#include <memory.h>
|
|
|
|
#include "DetourNavMeshBuilder.h"
|
|
#include "DetourNavMeshQuery.h"
|
|
#include "DetourCommon.h"
|
|
#include "DetourNavMesh.h"
|
|
|
|
#include "mapservice.h"
|
|
#include "collider.h"
|
|
#include "entity.h"
|
|
#include "human.h"
|
|
#include "room.h"
|
|
#include "roomobstacle.h"
|
|
|
|
const int MAX_POLYS = 256;
|
|
static const int INVALID_NAVMESH_POLYREF = 0;
|
|
|
|
MapService::MapService()
|
|
{
|
|
INIT_LIST_HEAD(&findpath_list_);
|
|
}
|
|
|
|
MapService::~MapService()
|
|
{
|
|
for (auto pair : node_hash_) {
|
|
CellNode* node = pair.second;
|
|
while (node) {
|
|
CellNode* pdelnode = node;
|
|
node = node->next;
|
|
|
|
list_del_init(&pdelnode->entry);
|
|
delete pdelnode;
|
|
}
|
|
}
|
|
node_hash_.clear();
|
|
|
|
if (map_cells_) {
|
|
free(map_cells_);
|
|
map_cells_ = nullptr;
|
|
}
|
|
if (navmesh_) {
|
|
dtFreeNavMesh(navmesh_);
|
|
navmesh_ = nullptr;
|
|
}
|
|
}
|
|
|
|
void MapService::Init(int width, int height, int cell_width)
|
|
{
|
|
if (width < 1 || height < 1 || cell_width < 1) {
|
|
abort();
|
|
}
|
|
map_width_ = width;
|
|
map_height_ = height;
|
|
cell_width_ = cell_width;
|
|
max_grid_id_ = map_width_ * map_height_;
|
|
|
|
grid_offset_arr_[0] = - (map_width_ + 1);
|
|
grid_offset_arr_[1] = - (map_width_ + 0);
|
|
grid_offset_arr_[2] = - (map_width_ + -1);
|
|
|
|
grid_offset_arr_[3] = - 1;
|
|
grid_offset_arr_[4] = 0;
|
|
grid_offset_arr_[5] = 1;
|
|
|
|
grid_offset_arr_[6] = map_width_ - 1;
|
|
grid_offset_arr_[7] = map_width_;
|
|
grid_offset_arr_[8] = map_width_ + 1;
|
|
|
|
map_cells_ = (list_head*)malloc(sizeof(list_head) * width * height);
|
|
memset(map_cells_, 0, sizeof(list_head) * width * height);
|
|
for (int i = 0; i < max_grid_id_; ++i) {
|
|
INIT_LIST_HEAD(&map_cells_[i]);
|
|
}
|
|
}
|
|
|
|
void MapService::UnInit()
|
|
{
|
|
|
|
}
|
|
|
|
void MapService::AddCollider(ColliderComponent* collider)
|
|
{
|
|
if (!(collider->owner->IsEntityType(ET_Obstacle) ||
|
|
collider->owner->IsEntityType(ET_Building))) {
|
|
abort();
|
|
}
|
|
CellNode* top_node = nullptr;
|
|
CellNode* bot_node = nullptr;
|
|
switch (collider->type) {
|
|
case CT_Aabb:
|
|
case CT_Circle:
|
|
{
|
|
AabbCollider aabb;
|
|
if (collider->type == CT_Aabb) {
|
|
aabb = *((AabbCollider*)collider);
|
|
} else if (collider->type == CT_Circle) {
|
|
CircleCollider* circle_box = (CircleCollider*)collider;
|
|
aabb.owner = circle_box->owner;
|
|
aabb._min.x = 0 - circle_box->rad;
|
|
aabb._min.y = 0 - circle_box->rad;
|
|
aabb._max.x = 0 + circle_box->rad;
|
|
aabb._max.y = 0 + circle_box->rad;
|
|
} else {
|
|
return;
|
|
}
|
|
|
|
float min_x = (collider->owner->GetPos() + aabb._min).x - 2;
|
|
float min_y = (collider->owner->GetPos() + aabb._min).y - 2;
|
|
float max_x = (collider->owner->GetPos() + aabb._max).x + 2;
|
|
float max_y = (collider->owner->GetPos() + aabb._max).y + 2;
|
|
#if 1
|
|
int min_grid_x = floor(min_x / cell_width_);
|
|
int min_grid_y = floor(min_y / cell_width_);
|
|
int max_grid_x = ceil(max_x / cell_width_);
|
|
int max_grid_y = ceil(max_y / cell_width_);
|
|
#else
|
|
int min_grid_x = ceil(min_x / cell_width_) - 1;
|
|
int min_grid_y = ceil(min_y / cell_width_) - 1;
|
|
int max_grid_x = ceil(max_x / cell_width_) - 1;
|
|
int max_grid_y = ceil(max_y / cell_width_) - 1;
|
|
#endif
|
|
if (min_grid_x < 0) {
|
|
abort();
|
|
}
|
|
if (max_grid_x >= map_width_) {
|
|
max_grid_x = map_width_ - 1;
|
|
}
|
|
if (min_grid_y < 0) {
|
|
abort();
|
|
}
|
|
if (max_grid_y >= map_height_) {
|
|
max_grid_y = map_height_ - 1;
|
|
}
|
|
for (int x = min_grid_x; x <= max_grid_x; ++x) {
|
|
for (int y = min_grid_y; y <= max_grid_y; ++y) {
|
|
int grid_id = x + y * map_width_;
|
|
list_head* head = &map_cells_[grid_id];
|
|
CellNode* node = new CellNode();
|
|
node->collider = collider;
|
|
list_add_tail(&node->entry, head);
|
|
node->next = top_node;
|
|
#ifdef DEBUG
|
|
node->x = x;
|
|
node->y = y;
|
|
#endif
|
|
top_node = node;
|
|
if (!bot_node) {
|
|
bot_node = node;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
if (top_node) {
|
|
auto pair = node_hash_.find(collider);
|
|
if (pair != node_hash_.end()) {
|
|
bot_node->next = pair->second;
|
|
pair->second = top_node;
|
|
} else {
|
|
node_hash_[collider] = top_node;
|
|
}
|
|
}
|
|
}
|
|
|
|
void MapService::RemoveCollider(ColliderComponent* collider)
|
|
{
|
|
auto pair = node_hash_.find(collider);
|
|
if (pair != node_hash_.end()) {
|
|
CellNode* node = pair->second;
|
|
while (node) {
|
|
CellNode* pdelnode = node;
|
|
node = node->next;
|
|
|
|
list_del_init(&pdelnode->entry);
|
|
delete pdelnode;
|
|
}
|
|
node_hash_.erase(pair);
|
|
}
|
|
}
|
|
|
|
void MapService::GetColliders(Room* room,
|
|
float world_x,
|
|
float world_y,
|
|
std::set<ColliderComponent*>& colliders)
|
|
{
|
|
int center_grid_id = GetGridId(world_x, world_y);
|
|
if (center_grid_id < 0 || center_grid_id >= max_grid_id_) {
|
|
return;
|
|
}
|
|
for (int i = 0; i < a8::ArraySize(grid_offset_arr_); ++i) {
|
|
int grid_id = center_grid_id + grid_offset_arr_[i];
|
|
if (grid_id >= 0 && grid_id < max_grid_id_) {
|
|
list_head* head = &map_cells_[grid_id];
|
|
if (list_empty(head)) {
|
|
continue;
|
|
}
|
|
struct CellNode *node, *tmp;
|
|
list_for_each_entry_safe(node, tmp, head, entry) {
|
|
switch (node->collider->owner->GetEntityType()) {
|
|
case ET_Obstacle:
|
|
{
|
|
switch (node->collider->owner->GetEntitySubType()) {
|
|
case EST_PermanentObstacle:
|
|
{
|
|
colliders.insert(node->collider);
|
|
}
|
|
break;
|
|
case EST_RoomObstacle:
|
|
{
|
|
RoomObstacle* obstacle = (RoomObstacle*)node->collider->owner;
|
|
if (obstacle->room == room) {
|
|
colliders.insert(node->collider);
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
abort();
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
case ET_Building:
|
|
{
|
|
colliders.insert(node->collider);
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
abort();
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int MapService::GetMap(int x, int y)
|
|
{
|
|
if (!IsValidPos(x, y)) {
|
|
#ifdef DEBUG1
|
|
abort();
|
|
#endif
|
|
return TILE_STATE_CLOSED;
|
|
}
|
|
list_head* head = &map_cells_[x + map_width_ * y];
|
|
if (list_empty(head)) {
|
|
return TILE_STATE_OPENED_COST1;
|
|
} else {
|
|
CellNode* node = list_first_entry(head, CellNode, entry);
|
|
switch (node->collider->owner->GetEntityType()) {
|
|
case ET_Obstacle:
|
|
{
|
|
Obstacle* obstacle = (Obstacle*)node->collider->owner;
|
|
if (!obstacle->IsPermanent()) {
|
|
return TILE_STATE_OPENED_COST1;
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
}
|
|
break;
|
|
}
|
|
return TILE_STATE_CLOSED;
|
|
}
|
|
}
|
|
|
|
int MapService::GetGridId(float world_x, float world_y)
|
|
{
|
|
int grid_id = (int)(world_x/cell_width_) + (int)(world_y/cell_width_) * map_width_;
|
|
return grid_id;
|
|
}
|
|
|
|
bool MapService::IsValidPos(int x, int y)
|
|
{
|
|
if (x < 0 ||
|
|
y < 0 ||
|
|
x >= map_width_ ||
|
|
y >= map_height_) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
FindPathStatus* MapService::AddFindPathRequest(Human* hum,
|
|
const a8::Vec2& start_pos,
|
|
const a8::Vec2& end_pos,
|
|
int max_step_num)
|
|
{
|
|
FindPathStatus* status = new FindPathStatus();
|
|
status->frameno = hum->room->GetFrameNo();
|
|
status->hum = hum;
|
|
status->start_pos = start_pos;
|
|
status->end_pos = end_pos;
|
|
status->search_step = 0;
|
|
status->max_step_num = max_step_num;
|
|
if (use_navmesh_) {
|
|
status->navmesh_query = new dtNavMeshQuery();
|
|
status->navmesh_query->init(navmesh_, 1024);
|
|
} else {
|
|
MapSearchNode node_start;
|
|
MapSearchNode node_goal;
|
|
node_start.hum = hum;
|
|
node_goal.hum = hum;
|
|
|
|
node_start.x = int(start_pos.x / cell_width_);
|
|
node_start.y = int(start_pos.y / cell_width_);
|
|
|
|
node_goal.x = int(end_pos.x / cell_width_);
|
|
node_goal.y = int(end_pos.y / cell_width_);
|
|
|
|
status->astar_search.SetStartAndGoalStates(node_start, node_goal);
|
|
}
|
|
list_add_tail(&status->entry, &findpath_list_);
|
|
findpath_requests_hash_[hum] = status;
|
|
return status;
|
|
}
|
|
|
|
void MapService::DelFindPathRequest(Human* hum, FindPathStatus* status)
|
|
{
|
|
auto itr = findpath_requests_hash_.find(hum);
|
|
if (itr == findpath_requests_hash_.end()) {
|
|
abort();
|
|
}
|
|
if (itr->second != status) {
|
|
abort();
|
|
}
|
|
if (use_navmesh_) {
|
|
if (status->navmesh_query) {
|
|
dtFreeNavMeshQuery(status->navmesh_query);
|
|
status->navmesh_query = nullptr;
|
|
}
|
|
}
|
|
list_del_init(&status->entry);
|
|
A8_SAFE_DELETE(status);
|
|
findpath_requests_hash_.erase(itr);
|
|
}
|
|
|
|
void MapService::FindPathUpdate(FindPathStatus* find_status)
|
|
{
|
|
if (use_navmesh_) {
|
|
UpdateNavmesh(find_status);
|
|
} else {
|
|
UpdateAStar(find_status);
|
|
}
|
|
}
|
|
|
|
bool MapService::FindOk(FindPathStatus* find_status)
|
|
{
|
|
if (use_navmesh_) {
|
|
return find_status->navmesh_search_state == kNavMesh_SearchOk;
|
|
} else {
|
|
return find_status->search_state == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED;
|
|
}
|
|
}
|
|
|
|
bool MapService::FindFailed(FindPathStatus* find_status)
|
|
{
|
|
if (use_navmesh_) {
|
|
return find_status->navmesh_search_state == kNavMesh_SearchFailed;
|
|
} else {
|
|
return find_status->search_state == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED;
|
|
}
|
|
}
|
|
|
|
bool MapService::MoveDone(FindPathStatus* find_status)
|
|
{
|
|
if (find_status->path_index >= (int)find_status->out_points.size()) {
|
|
return true;
|
|
}
|
|
if (find_status->path_index >= (int)find_status->out_points.size() ||
|
|
find_status->out_points.empty()) {
|
|
abort();
|
|
}
|
|
if (find_status->path_index + 1 == (int)find_status->out_points.size()) {
|
|
MovePathPoint* point = &find_status->out_points[find_status->path_index];
|
|
if (point->pos.ManhattanDistance(find_status->hum->GetPos()) > 2.0f) {
|
|
return false;
|
|
}
|
|
float distance = point->pos.Distance(find_status->hum->GetPos());
|
|
if (distance < 0.000001f) {
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void MapService::DoMove(FindPathStatus* find_status)
|
|
{
|
|
if (find_status->path_index >= (int)find_status->out_points.size() ||
|
|
find_status->out_points.empty()) {
|
|
abort();
|
|
}
|
|
Human* myself = find_status->hum;
|
|
MovePathPoint* point = &find_status->out_points[find_status->path_index];
|
|
myself->move_dir = point->pos - myself->GetPos();
|
|
myself->move_dir.Normalize();
|
|
float target_distance = point->pos.Distance(myself->GetPos());
|
|
float speed = std::min(myself->GetSpeed(), target_distance);
|
|
a8::Vec2 new_pos = myself->GetPos() + myself->move_dir * speed;
|
|
myself->SetPos(new_pos);
|
|
target_distance = point->pos.Distance(myself->GetPos());
|
|
if (target_distance < 1.0001f) {
|
|
myself->SetPos(point->pos);
|
|
++find_status->path_index;
|
|
#ifdef DEBUG
|
|
if (myself->IsCollisionInMapService()) {
|
|
myself->IsCollisionInMapService();
|
|
abort();
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void MapService::UpdateAStar(FindPathStatus* find_status)
|
|
{
|
|
do {
|
|
find_status->search_state = find_status->astar_search.SearchStep();
|
|
++find_status->search_step;
|
|
} while (find_status->search_state == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING);
|
|
|
|
if (FindOk(find_status)) {
|
|
MapSearchNode *node = find_status->astar_search.GetSolutionStart();
|
|
int steps = 0;
|
|
for( ;; ) {
|
|
node = find_status->astar_search.GetSolutionNext();
|
|
if (!node) {
|
|
break;
|
|
}
|
|
|
|
steps++;
|
|
MovePathPoint point;
|
|
point.pos = a8::Vec2(
|
|
node->x * cell_width_ + cell_width_ / 2,
|
|
node->y * cell_width_ + cell_width_ / 2
|
|
);
|
|
find_status->out_points.push_back(point);
|
|
};
|
|
}
|
|
}
|
|
|
|
void MapService::UpdateNavmesh(FindPathStatus* find_status)
|
|
{
|
|
dtNavMeshQuery* navmesh_query = find_status->navmesh_query;
|
|
|
|
float spos[3];
|
|
spos[0] = find_status->start_pos.x;
|
|
spos[1] = find_status->start_pos.y;
|
|
spos[2] = 0;
|
|
|
|
float epos[3];
|
|
epos[0] = find_status->end_pos.x;
|
|
epos[1] = find_status->end_pos.x;
|
|
epos[2] = 0;
|
|
|
|
dtQueryFilter filter;
|
|
filter.setIncludeFlags(0xffff);
|
|
filter.setExcludeFlags(0);
|
|
|
|
const float extents[3] = {2.f, 4.f, 2.f};
|
|
|
|
dtPolyRef start_ref = INVALID_NAVMESH_POLYREF;
|
|
dtPolyRef end_ref = INVALID_NAVMESH_POLYREF;
|
|
|
|
float start_nearest_pt[3];
|
|
float end_nearest_pt[3];
|
|
navmesh_query->findNearestPoly(spos, extents, &filter, &start_ref, start_nearest_pt);
|
|
navmesh_query->findNearestPoly(epos, extents, &filter, &end_ref, end_nearest_pt);
|
|
if (!start_ref || !end_ref) {
|
|
find_status->navmesh_search_state = kNavMesh_SearchFailed;
|
|
return;
|
|
}
|
|
dtPolyRef polys[MAX_POLYS];
|
|
int npolys;
|
|
float straight_path[MAX_POLYS * 3];
|
|
unsigned char straight_path_flags[MAX_POLYS];
|
|
dtPolyRef straight_path_polys[MAX_POLYS];
|
|
int nstraight_path;
|
|
|
|
navmesh_query->findPath(start_ref,
|
|
end_ref,
|
|
start_nearest_pt,
|
|
end_nearest_pt,
|
|
&filter,
|
|
polys,
|
|
&npolys,
|
|
MAX_POLYS);
|
|
nstraight_path = 0;
|
|
|
|
if (npolys) {
|
|
float epos1[3];
|
|
dtVcopy(epos1, end_nearest_pt);
|
|
|
|
if (polys[npolys-1] != end_ref) {
|
|
navmesh_query->closestPointOnPoly(polys[npolys-1], end_nearest_pt, epos1, 0);
|
|
}
|
|
|
|
navmesh_query->findStraightPath(start_nearest_pt,
|
|
end_nearest_pt,
|
|
polys,
|
|
npolys,
|
|
straight_path,
|
|
straight_path_flags,
|
|
straight_path_polys,
|
|
&nstraight_path,
|
|
MAX_POLYS);
|
|
|
|
for (int i = 0; i < nstraight_path * 3; ) {
|
|
MovePathPoint point;
|
|
point.pos.x = straight_path[i++];
|
|
point.pos.y = straight_path[i++];
|
|
straight_path[i++];
|
|
find_status->out_points.push_back(point);
|
|
}
|
|
find_status->navmesh_search_state = kNavMesh_SearchOk;
|
|
} else {
|
|
find_status->navmesh_search_state = kNavMesh_SearchFailed;
|
|
}
|
|
}
|