00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00014
00015 #include "aiPathFinder.h"
00016
00017 PathFinder::PathFinder(NavMesh nav_mesh) {
00018 _grid = nav_mesh;
00019 }
00020
00021 PathFinder::~PathFinder() {
00022 }
00023
00029
00031
00032 void PathFinder::find_path(Node *src_node, Node *dest_node) {
00033 _src_node = src_node;
00034 _dest_node = dest_node;
00035
00036
00037
00038 Node *_dummy_node = new Node(-1, -1, LVecBase3f(0.0, 0.0, 0.0), 0, 0, 0);
00039 _dummy_node->_status = _dummy_node->open;
00040 _dummy_node->_score = -1;
00041 _open_list.push_back(_dummy_node);
00042
00043
00044 add_to_olist(_src_node);
00045
00046
00047 generate_path();
00048 }
00049
00055
00057
00058 void PathFinder::generate_path() {
00059
00060
00061 while(_open_list.size() > 1) {
00062
00063
00064
00065 Node* nxt_node = _open_list[1];
00066
00067 if(nxt_node->_grid_x == _dest_node->_grid_x &&
00068 nxt_node->_grid_y == _dest_node->_grid_y) {
00069
00070 remove_from_olist();
00071
00072
00073 add_to_clist(nxt_node);
00074
00075
00076 return;
00077 }
00078 else {
00079 identify_neighbors(nxt_node);
00080
00081
00082 add_to_clist(nxt_node);
00083 }
00084 }
00085 cout<<"DESTINATION NOT REACHABLE MATE!"<<endl;
00086 _closed_list.clear();
00087 }
00088
00094
00096
00097 void PathFinder::identify_neighbors(Node *parent_node) {
00098
00099
00100 remove_from_olist();
00101 for(int i = 0; i < 8; ++i) {
00102 if(parent_node->_neighbours[i] != NULL) {
00103 if(parent_node->_neighbours[i]->_status == parent_node->_neighbours[i]->neutral
00104 && parent_node->_neighbours[i]->_type == true) {
00105
00106 parent_node->_neighbours[i]->_prv_node = parent_node;
00107
00108 calc_node_score(parent_node->_neighbours[i]);
00109
00110 add_to_olist(parent_node->_neighbours[i]);
00111 }
00112 }
00113 }
00114 }
00115
00121
00123
00124 void PathFinder::calc_node_score(Node *nd) {
00125 nd->_cost = calc_cost_frm_src(nd);
00126 nd->_heuristic = calc_heuristic(nd);
00127 nd->_score = nd->_cost + nd->_heuristic;
00128 }
00129
00137
00139
00140 int PathFinder::calc_cost_frm_src(Node *nd) {
00141 int cost = 0;
00142 Node *start_node = nd;
00143 while(start_node->_prv_node != _src_node) {
00144 if(is_diagonal_node(start_node)) {
00145 cost += 14;
00146 }
00147 else {
00148 cost += 10;
00149 }
00150 start_node = start_node->_prv_node;
00151 }
00152
00153 if(is_diagonal_node(start_node)) {
00154 cost += 14;
00155 }
00156 else {
00157 cost += 10;
00158 }
00159 return cost;
00160 }
00161
00168
00170
00171 int PathFinder::calc_heuristic(Node *nd) {
00172 int row_diff = abs(_dest_node->_grid_x - nd->_grid_x);
00173 int col_diff = abs(_dest_node->_grid_y - nd->_grid_y);
00174
00175 int heuristic = 10 * (row_diff + col_diff);
00176 return heuristic;
00177 }
00178
00183
00185
00186 bool PathFinder::is_diagonal_node(Node *nd) {
00187
00188 float row_diff = nd->_grid_x - nd->_prv_node->_grid_x;
00189 float col_diff = nd->_grid_y - nd->_prv_node->_grid_y;
00190
00191
00192 if(row_diff == 0 || col_diff == 0) {
00193 return false;
00194 }
00195 else {
00196 return true;
00197 }
00198 }
00199
00205
00207
00208
00209 void PathFinder::add_to_olist(Node *nd) {
00210
00211 Node *child_node, *parent_node;
00212 int child_idx, parent_idx;
00213
00214
00215 nd->_status = nd->open;
00216
00217 _open_list.push_back(nd);
00218
00219
00220
00221
00222
00223 child_idx = _open_list.size() - 1;
00224 parent_idx = child_idx / 2;
00225 child_node = _open_list[child_idx];
00226 parent_node = _open_list[parent_idx];
00227
00228
00229
00230 while(_open_list[child_idx]->_score <= _open_list[parent_idx]->_score) {
00231
00232 _open_list[parent_idx] = child_node;
00233 _open_list[child_idx] = parent_node;
00234
00235
00236 child_idx = parent_idx;
00237 parent_idx = child_idx / 2;
00238
00239
00240 child_node = _open_list[child_idx];
00241 parent_node = _open_list[parent_idx];
00242 }
00243
00244
00245 }
00246
00252
00254
00255 void PathFinder::remove_from_olist() {
00256
00257 Node *child_node, *child_node_1, *child_node_2;
00258 int child_idx, child_idx_1, child_idx_2;
00259
00260
00261
00262 _open_list.erase(_open_list.begin() + 1);
00263
00264 if(_open_list.size() > 1) {
00265
00266 Node *temp_node = _open_list[_open_list.size() - 1];
00267
00268
00269 for(int i = _open_list.size() - 1; i > 1; --i) {
00270 _open_list[i] = _open_list[i - 1];
00271 }
00272
00273
00274 _open_list[1] = temp_node;
00275
00276
00277 unsigned int k = 1;
00278
00279
00280 while(true) {
00281 if((k * 2 + 1) < _open_list.size()) {
00282
00283 child_idx_1 = k * 2;
00284 child_idx_2 = k * 2 + 1;
00285 child_node_1 = _open_list[child_idx_1];
00286 child_node_2 = _open_list[child_idx_2];
00287
00288 if(_open_list[child_idx_1]->_score < _open_list[child_idx_2]->_score) {
00289 if(_open_list[k]->_score > _open_list[child_idx_1]->_score) {
00290
00291 _open_list[child_idx_1] = _open_list[k];
00292 _open_list[k] = child_node_1;
00293
00294
00295 k = child_idx_1;
00296 }
00297 else {
00298 break;
00299 }
00300 }
00301 else {
00302 if(_open_list[k]->_score > _open_list[child_idx_2]->_score) {
00303
00304 _open_list[child_idx_2] = _open_list[k];
00305 _open_list[k] = child_node_2;
00306
00307
00308 k = child_idx_2;
00309 }
00310 else {
00311 break;
00312 }
00313 }
00314 }
00315 else if((k * 2) < _open_list.size()) {
00316
00317 child_idx = k * 2;
00318 child_node = _open_list[child_idx];
00319
00320 if(_open_list[k]->_score > _open_list[child_idx]->_score) {
00321
00322 _open_list[child_idx] = _open_list[k];
00323 _open_list[k] = child_node;
00324
00325
00326 k = child_idx;
00327 }
00328 else {
00329 break;
00330 }
00331 }
00332 else {
00333
00334 break;
00335 }
00336 }
00337 }
00338
00339
00340 }
00341
00346
00348
00349 void PathFinder::add_to_clist(Node *nd) {
00350
00351 nd->_status = nd->close;
00352
00353 _closed_list.push_back(nd);
00354 }
00355
00360
00362
00363 void PathFinder::remove_from_clist(int r, int c) {
00364 for(unsigned int i = 0; i < _closed_list.size(); ++i) {
00365 if(_closed_list[i]->_grid_x == r && _closed_list[i]->_grid_y == c) {
00366 _closed_list.erase(_closed_list.begin() + i);
00367 break;
00368 }
00369 }
00370 }
00371
00378
00380
00381 Node* find_in_mesh(NavMesh nav_mesh, LVecBase3f pos, int grid_size) {
00382 int size = grid_size;
00383 float x = pos[0];
00384 float y = pos[1];
00385
00386 for(int i = 0; i < size; ++i) {
00387 for(int j = 0; j < size; ++j) {
00388 if(nav_mesh[i][j] != NULL && nav_mesh[i][j]->contains(x, y)) {
00389 return(nav_mesh[i][j]);
00390 }
00391 }
00392 }
00393 return NULL;
00394 }