|
|
@@ -1,19 +1,5 @@
|
|
|
-// Filename: pathFind.cxx
|
|
|
-// Created by: Deepak, John, Navin (12Oct09)
|
|
|
-//
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-//
|
|
|
-// PANDA 3D SOFTWARE
|
|
|
-// Copyright (c) Carnegie Mellon University. All rights reserved.
|
|
|
-//
|
|
|
-// All use of this software is subject to the terms of the revised
|
|
|
-// BSD license. You should have received a copy of this license
|
|
|
-// along with this source code in a file named "LICENSE."
|
|
|
-//
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
#include "pathFind.h"
|
|
|
-#include "pstrtod.h"
|
|
|
|
|
|
PathFind::PathFind(AICharacter *ai_ch) {
|
|
|
_ai_char = ai_ch;
|
|
|
@@ -21,7 +7,7 @@ PathFind::PathFind(AICharacter *ai_ch) {
|
|
|
_parent = new GeomNode("parent");
|
|
|
_ai_char->_window_render.attach_new_node(_parent);
|
|
|
|
|
|
- _pen = new LineSegs("pen");
|
|
|
+ _pen = new LineSegs("pen");
|
|
|
_pen->set_color(1.0, 0.0, 0.0);
|
|
|
_pen->set_thickness(2.0);
|
|
|
|
|
|
@@ -32,11 +18,13 @@ PathFind::PathFind(AICharacter *ai_ch) {
|
|
|
PathFind::~PathFind() {
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: create_nav_mesh
|
|
|
-// Description: This function recreates the navigation mesh from
|
|
|
-// the .csv file.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+/////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : create_nav_mesh
|
|
|
+// Description : This function recreates the navigation mesh from the .csv file
|
|
|
+
|
|
|
+/////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::create_nav_mesh(const char* navmesh_filename) {
|
|
|
// Stage variables.
|
|
|
int grid_x, grid_y;
|
|
|
@@ -46,23 +34,22 @@ void PathFind::create_nav_mesh(const char* navmesh_filename) {
|
|
|
// Variable to hold line data read from file.
|
|
|
string line;
|
|
|
|
|
|
- // Array for storing data members obtained from each line of the
|
|
|
- // file.
|
|
|
+ // Array for storing data members obtained from each line of the file.
|
|
|
string fields[10];
|
|
|
|
|
|
// Open data file for reading.
|
|
|
ifstream nav_mesh_file (navmesh_filename);
|
|
|
|
|
|
- if (nav_mesh_file.is_open()) {
|
|
|
+ if(nav_mesh_file.is_open()) {
|
|
|
// Capture the grid size from the file.
|
|
|
getline(nav_mesh_file, line);
|
|
|
int pos = line.find(",");
|
|
|
_grid_size = atoi((line.substr(pos + 1)).c_str());
|
|
|
|
|
|
// Initialize the stage mesh with NULL nodes.
|
|
|
- for (int r = 0; r < _grid_size; ++r) {
|
|
|
- _nav_mesh.push_back(vector<AINode*>());
|
|
|
- for (int c = 0; c < _grid_size; ++c) {
|
|
|
+ for(int r = 0; r < _grid_size; ++r) {
|
|
|
+ _nav_mesh.push_back(vector<Node*>());
|
|
|
+ for(int c = 0; c < _grid_size; ++c) {
|
|
|
_nav_mesh[r].push_back(NULL);
|
|
|
}
|
|
|
}
|
|
|
@@ -71,51 +58,52 @@ void PathFind::create_nav_mesh(const char* navmesh_filename) {
|
|
|
getline(nav_mesh_file, line);
|
|
|
|
|
|
// Begin reading data from the file.
|
|
|
- while (!nav_mesh_file.eof()) {
|
|
|
+ while(!nav_mesh_file.eof()) {
|
|
|
getline(nav_mesh_file, line);
|
|
|
stringstream linestream (line);
|
|
|
|
|
|
// Stores all the data members in the line to the array.
|
|
|
- // Data structure: NULL,NodeType,GridX,GridY,Length,Width,
|
|
|
- // Height,PosX,PosY,PosZ
|
|
|
- for (int i = 0; i < 10; ++i) {
|
|
|
+ // Data structure: NULL,NodeType,GridX,GridY,Length,Width,Height,PosX,PosY,PosZ
|
|
|
+ for(int i = 0; i < 10; ++i) {
|
|
|
getline(linestream, fields[i], ',');
|
|
|
}
|
|
|
|
|
|
// Populate the main nodes into stage mesh.
|
|
|
- if (fields[0] == "0" && fields[1] == "0") {
|
|
|
+ if(fields[0] == "0" && fields[1] == "0") {
|
|
|
grid_x = atoi(fields[2].c_str());
|
|
|
grid_y = atoi(fields[3].c_str());
|
|
|
- l = patof(fields[4].c_str());
|
|
|
- w = patof(fields[5].c_str());
|
|
|
- h = patof(fields[6].c_str());
|
|
|
- position = LVecBase3f(patof(fields[7].c_str()),
|
|
|
- patof(fields[8].c_str()), patof(fields[9].c_str()));
|
|
|
+ l = atof(fields[4].c_str());
|
|
|
+ w = atof(fields[5].c_str());
|
|
|
+ h = atof(fields[6].c_str());
|
|
|
+ position = LVecBase3f(atof(fields[7].c_str()), atof(fields[8].c_str()), atof(fields[9].c_str()));
|
|
|
+
|
|
|
+ Node *stage_node = new Node(grid_x, grid_y, position, w, l, h);
|
|
|
|
|
|
- AINode *stage_node = new AINode(grid_x, grid_y, position, w, l, h);
|
|
|
|
|
|
_nav_mesh[grid_y][grid_x] = stage_node;
|
|
|
}
|
|
|
- else if (fields[0] == "") {
|
|
|
+ else if(fields[0] == "") {
|
|
|
// End of file reached at this point.
|
|
|
nav_mesh_file.close();
|
|
|
|
|
|
- // Assign the neighbor nodes for each of the main nodes
|
|
|
- // that just got populated into the stage mesh.
|
|
|
+ // Assign the neighbor nodes for each of the main nodes that just got populated into the stage mesh.
|
|
|
assign_neighbor_nodes(navmesh_filename);
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
else {
|
|
|
- cout << "error opening navmesh.csv file!\n";
|
|
|
+ cout<<"error opening navmesh.csv file!"<<endl;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: assign_neighbor_nodes
|
|
|
-// Description: This function assigns the neighbor nodes for each
|
|
|
-// main node present in _nav_mesh.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+/////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : assign_neighbor_nodes
|
|
|
+// Description : This function assigns the neighbor nodes for each main node present in
|
|
|
+// _nav_mesh.
|
|
|
+
|
|
|
+/////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::assign_neighbor_nodes(const char* navmesh_filename){
|
|
|
ifstream nav_mesh_file (navmesh_filename);
|
|
|
|
|
|
@@ -125,70 +113,73 @@ void PathFind::assign_neighbor_nodes(const char* navmesh_filename){
|
|
|
string fields[10];
|
|
|
string fields_n[10];
|
|
|
|
|
|
- if (nav_mesh_file.is_open()) {
|
|
|
+ if(nav_mesh_file.is_open()) {
|
|
|
getline(nav_mesh_file, ln); // Get rid of grid size line.
|
|
|
getline(nav_mesh_file, ln); // Get rid of the header.
|
|
|
|
|
|
- while (!nav_mesh_file.eof()) {
|
|
|
+ while(!nav_mesh_file.eof()) {
|
|
|
getline(nav_mesh_file, ln); // Gets main node data only. No neighbor nodes.
|
|
|
stringstream linestream (ln);
|
|
|
- for (int i = 0; i < 10; ++i) {
|
|
|
+ for(int i = 0; i < 10; ++i) {
|
|
|
getline(linestream, fields[i], ',');
|
|
|
}
|
|
|
- if (fields[0] == "0" && fields[1] == "0") {
|
|
|
+ if(fields[0] == "0" && fields[1] == "0") {
|
|
|
// Usable main node.
|
|
|
gd_x = atoi(fields[2].c_str());
|
|
|
gd_y = atoi(fields[3].c_str());
|
|
|
- for (int i = 0; i < 8; ++i) {
|
|
|
+ for(int i = 0; i < 8; ++i) {
|
|
|
getline(nav_mesh_file, ln); // Gets neighbor node data only. No main nodes.
|
|
|
stringstream linestream_n (ln);
|
|
|
- for (int j = 0; j < 10; ++j) {
|
|
|
+ for(int j = 0; j < 10; ++j) {
|
|
|
getline(linestream_n, fields_n[j], ',');
|
|
|
}
|
|
|
gd_xn = atoi(fields_n[2].c_str());
|
|
|
gd_yn = atoi(fields_n[3].c_str());
|
|
|
|
|
|
- if (fields_n[0] == "0" && fields_n[1] == "1") {
|
|
|
+ if(fields_n[0] == "0" && fields_n[1] == "1") {
|
|
|
// Usable neighbor for main node.
|
|
|
- // Note: The indices of the vector are inverted
|
|
|
- // when compared to the values of the nodes on actual grid.
|
|
|
+ // TODO: The indices of the vector are inverted when compared to the values of the nodes on actual grid. Fix this!
|
|
|
_nav_mesh[gd_y][gd_x]->_neighbours[i] = _nav_mesh[gd_yn][gd_xn];
|
|
|
}
|
|
|
- else if (fields_n[0] == "1" && fields_n[1] == "1") {
|
|
|
+ else if(fields_n[0] == "1" && fields_n[1] == "1") {
|
|
|
// NULL neighbor.
|
|
|
_nav_mesh[gd_y][gd_x]->_neighbours[i] = NULL;
|
|
|
}
|
|
|
else {
|
|
|
- cout << "Warning: Corrupt data!\n";
|
|
|
+ cout<<"Warning: Corrupt data!"<<endl;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
- else if (fields[0] == "") {
|
|
|
+ else if(fields[0] == "") {
|
|
|
// End of file reached at this point.
|
|
|
nav_mesh_file.close();
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
else {
|
|
|
- cout << "error opening navmesh.csv file!\n";
|
|
|
+ cout<<"error opening navmesh.csv file!"<<endl;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: set_path_find
|
|
|
-// Description: This function starts the path finding process after
|
|
|
-// reading the given navigation mesh.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : set_path_find
|
|
|
+// Description : This function starts the path finding process after reading the given
|
|
|
+// navigation mesh.
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
+
|
|
|
void PathFind::set_path_find(const char* navmesh_filename) {
|
|
|
create_nav_mesh(navmesh_filename);
|
|
|
|
|
|
- if (_ai_char->_steering->_path_follow_obj) {
|
|
|
+ if(_ai_char->_steering->_path_follow_obj) {
|
|
|
_ai_char->_steering->remove_ai("pathfollow");
|
|
|
}
|
|
|
|
|
|
_ai_char->_steering->path_follow(1.0f);
|
|
|
|
|
|
- if (_path_finder_obj) {
|
|
|
+ if(_path_finder_obj) {
|
|
|
delete _path_finder_obj;
|
|
|
_path_finder_obj = NULL;
|
|
|
}
|
|
|
@@ -196,17 +187,19 @@ void PathFind::set_path_find(const char* navmesh_filename) {
|
|
|
_path_finder_obj = new PathFinder(_nav_mesh);
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: path_find (for pathfinding towards a static position)
|
|
|
-// Description: This function checks for the source and target in
|
|
|
-// the navigation mesh for its availability and then
|
|
|
-// finds the best path via the A* algorithm Then it
|
|
|
-// calls the path follower to make the object follow
|
|
|
-// the path.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : path_find (for pathfinding towards a static position)
|
|
|
+// Description : This function checks for the source and target in the navigation mesh
|
|
|
+// for its availability and then finds the best path via the A* algorithm
|
|
|
+// Then it calls the path follower to make the object follow the path.
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
+
|
|
|
void PathFind::path_find(LVecBase3f pos, string type) {
|
|
|
- if (type == "addPath") {
|
|
|
- if (_ai_char->_steering->_path_follow_obj) {
|
|
|
+ if(type == "addPath") {
|
|
|
+ if(_ai_char->_steering->_path_follow_obj) {
|
|
|
_ai_char->_steering->remove_ai("pathfollow");
|
|
|
}
|
|
|
|
|
|
@@ -215,41 +208,40 @@ void PathFind::path_find(LVecBase3f pos, string type) {
|
|
|
|
|
|
clear_path();
|
|
|
|
|
|
- AINode* src = find_in_mesh(_nav_mesh,
|
|
|
- _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
|
|
|
+ Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
|
|
|
|
|
|
- if (src == NULL) {
|
|
|
- cout << "couldnt find source\n";
|
|
|
+ if(src == NULL) {
|
|
|
+ cout<<"couldnt find source"<<endl;
|
|
|
}
|
|
|
|
|
|
- AINode* dst = find_in_mesh(_nav_mesh, pos, _grid_size);
|
|
|
+ Node* dst = find_in_mesh(_nav_mesh, pos, _grid_size);
|
|
|
|
|
|
- if (dst == NULL) {
|
|
|
- cout << "couldnt find destination\n";
|
|
|
+ if(dst == NULL) {
|
|
|
+ cout<<"couldnt find destination"<<endl;
|
|
|
}
|
|
|
|
|
|
- if (src != NULL && dst != NULL) {
|
|
|
+ if(src != NULL && dst != NULL) {
|
|
|
_path_finder_obj->find_path(src, dst);
|
|
|
trace_path(src);
|
|
|
}
|
|
|
|
|
|
- if (!_ai_char->_steering->_path_follow_obj->_start) {
|
|
|
+ if(!_ai_char->_steering->_path_follow_obj->_start) {
|
|
|
_ai_char->_steering->start_follow();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: path_find (for pathfinding towards a moving target
|
|
|
-// (a NodePath))
|
|
|
-// Description: This function checks for the source and target in
|
|
|
-// the navigation mesh for its availability and then
|
|
|
-// finds the best path via the A* algorithm
|
|
|
-// Then it calls the path follower to make the object
|
|
|
-// follow the path.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : path_find (for pathfinding towards a moving target (a NodePath))
|
|
|
+// Description : This function checks for the source and target in the navigation mesh
|
|
|
+// for its availability and then finds the best path via the A* algorithm
|
|
|
+// Then it calls the path follower to make the object follow the path.
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::path_find(NodePath target, string type) {
|
|
|
- if (type == "addPath") {
|
|
|
- if (_ai_char->_steering->_path_follow_obj) {
|
|
|
+ if(type == "addPath") {
|
|
|
+ if(_ai_char->_steering->_path_follow_obj) {
|
|
|
_ai_char->_steering->remove_ai("pathfollow");
|
|
|
}
|
|
|
|
|
|
@@ -261,42 +253,43 @@ void PathFind::path_find(NodePath target, string type) {
|
|
|
_path_find_target = target;
|
|
|
_prev_position = target.get_pos(_ai_char->_window_render);
|
|
|
|
|
|
- AINode* src = find_in_mesh(_nav_mesh,
|
|
|
- _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
|
|
|
+ Node* src = find_in_mesh(_nav_mesh, _ai_char->_ai_char_np.get_pos(_ai_char->_window_render), _grid_size);
|
|
|
|
|
|
- if (src == NULL) {
|
|
|
- cout << "couldnt find source\n";
|
|
|
+ if(src == NULL) {
|
|
|
+ cout<<"couldnt find source"<<endl;
|
|
|
}
|
|
|
|
|
|
- AINode* dst = find_in_mesh(_nav_mesh, _prev_position, _grid_size);
|
|
|
+ Node* dst = find_in_mesh(_nav_mesh, _prev_position, _grid_size);
|
|
|
|
|
|
- if (dst == NULL) {
|
|
|
- cout << "couldnt find destination\n";
|
|
|
+ if(dst == NULL) {
|
|
|
+ cout<<"couldnt find destination"<<endl;
|
|
|
}
|
|
|
|
|
|
- if (src != NULL && dst != NULL) {
|
|
|
+ if(src != NULL && dst != NULL) {
|
|
|
_path_finder_obj->find_path(src, dst);
|
|
|
trace_path(src);
|
|
|
}
|
|
|
|
|
|
- if (_ai_char->_steering->_path_follow_obj!=NULL) {
|
|
|
- if (!_ai_char->_steering->_path_follow_obj->_start) {
|
|
|
+ if(_ai_char->_steering->_path_follow_obj!=NULL) {
|
|
|
+ if(!_ai_char->_steering->_path_follow_obj->_start) {
|
|
|
_ai_char->_steering->start_follow("pathfind");
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: clear_path
|
|
|
-// Description: Helper function to restore the path and mesh
|
|
|
-// to its initial state
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : clear_path
|
|
|
+// Description : Helper function to restore the path and mesh to its initial state
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::clear_path() {
|
|
|
// Initialize to zero
|
|
|
- for (int i = 0; i < _grid_size; ++i) {
|
|
|
- for (int j = 0; j < _grid_size; ++j) {
|
|
|
- if (_nav_mesh[i][j] != NULL) {
|
|
|
- _nav_mesh[i][j]->_status = _nav_mesh[i][j]->ST_neutral;
|
|
|
+ for(int i = 0; i < _grid_size; ++i) {
|
|
|
+ for(int j = 0; j < _grid_size; ++j) {
|
|
|
+ if(_nav_mesh[i][j] != NULL) {
|
|
|
+ _nav_mesh[i][j]->_status = _nav_mesh[i][j]->neutral;
|
|
|
_nav_mesh[i][j]->_cost = 0;
|
|
|
_nav_mesh[i][j]->_heuristic = 0;
|
|
|
_nav_mesh[i][j]->_score = 0;
|
|
|
@@ -305,34 +298,35 @@ void PathFind::clear_path() {
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- if (_path_finder_obj) {
|
|
|
+ if(_path_finder_obj) {
|
|
|
_path_finder_obj->_open_list.clear();
|
|
|
_path_finder_obj->_closed_list.clear();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: trace_path
|
|
|
-// Description: This function is the function which sends the path
|
|
|
-// information one by one to the path follower so that
|
|
|
-// it can store the path needed to be
|
|
|
-// traversed by the pathfinding object
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-void PathFind::trace_path(AINode* src) {
|
|
|
- if (_ai_char->_pf_guide) {
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : trace_path
|
|
|
+// Description : This function is the function which sends the path information one by
|
|
|
+// one to the path follower so that it can store the path needed to be
|
|
|
+// traversed by the pathfinding object
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
+void PathFind::trace_path(Node* src) {
|
|
|
+ if(_ai_char->_pf_guide) {
|
|
|
_parent->remove_all_children();
|
|
|
}
|
|
|
else {
|
|
|
_parent->remove_all_children();
|
|
|
}
|
|
|
|
|
|
- if (_path_finder_obj->_closed_list.size() > 0) {
|
|
|
- AINode *traversor = _path_finder_obj->_closed_list[_path_finder_obj->_closed_list.size() - 0.5];
|
|
|
- while (traversor != src) {
|
|
|
- if (_ai_char->_pf_guide) {
|
|
|
+ if(_path_finder_obj->_closed_list.size() > 0) {
|
|
|
+ Node *traversor = _path_finder_obj->_closed_list[_path_finder_obj->_closed_list.size() - 0.5];
|
|
|
+ while(traversor != src) {
|
|
|
+ if(_ai_char->_pf_guide) {
|
|
|
_pen->move_to(traversor->_position.get_x(), traversor->_position.get_y(), 1);
|
|
|
- _pen->draw_to(traversor->_prv_node->_position.get_x(),
|
|
|
- traversor->_prv_node->_position.get_y(), 0.5);
|
|
|
+ _pen->draw_to(traversor->_prv_node->_position.get_x(), traversor->_prv_node->_position.get_y(), 0.5);
|
|
|
PT(GeomNode) gnode = _pen->create();
|
|
|
_parent->add_child(gnode);
|
|
|
}
|
|
|
@@ -342,32 +336,33 @@ void PathFind::trace_path(AINode* src) {
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: add_obstacle_to_mesh
|
|
|
-// Description: This function allows the user to dynamically add
|
|
|
-// obstacles to the game environment. The function
|
|
|
-// will update the nodes within the
|
|
|
-// bounding volume of the obstacle as non-traversable.
|
|
|
-// Hence will not be considered by the pathfinding
|
|
|
-// algorithm.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : add_obstacle_to_mesh
|
|
|
+// Description : This function allows the user to dynamically add obstacles to the
|
|
|
+// game environment. The function will update the nodes within the
|
|
|
+// bounding volume of the obstacle as non-traversable. Hence will not be
|
|
|
+// considered by the pathfinding algorithm.
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::add_obstacle_to_mesh(NodePath obstacle) {
|
|
|
PT(BoundingVolume) np_bounds = obstacle.get_bounds();
|
|
|
CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
|
|
|
|
|
|
- AINode* temp = find_in_mesh(_nav_mesh, obstacle.get_pos(), _grid_size);
|
|
|
+ Node* temp = find_in_mesh(_nav_mesh, obstacle.get_pos(), _grid_size);
|
|
|
|
|
|
- if (temp != NULL) {
|
|
|
+ if(temp != NULL) {
|
|
|
float left = temp->_position.get_x() - np_sphere->get_radius();
|
|
|
float right = temp->_position.get_x() + np_sphere->get_radius();
|
|
|
float top = temp->_position.get_y() + np_sphere->get_radius();
|
|
|
float down = temp->_position.get_y() - np_sphere->get_radius();
|
|
|
|
|
|
- for (int i = 0; i < _grid_size; ++i) {
|
|
|
- for (int j = 0; j < _grid_size; ++j) {
|
|
|
- if (_nav_mesh[i][j] != NULL && _nav_mesh[i][j]->_type == true) {
|
|
|
- if (_nav_mesh[i][j]->_position.get_x() >= left && _nav_mesh[i][j]->_position.get_x() <= right &&
|
|
|
- _nav_mesh[i][j]->_position.get_y() >= down && _nav_mesh[i][j]->_position.get_y() <= top) {
|
|
|
+ for(int i = 0; i < _grid_size; ++i) {
|
|
|
+ for(int j = 0; j < _grid_size; ++j) {
|
|
|
+ if(_nav_mesh[i][j] != NULL && _nav_mesh[i][j]->_type == true) {
|
|
|
+ if(_nav_mesh[i][j]->_position.get_x() >= left && _nav_mesh[i][j]->_position.get_x() <= right &&
|
|
|
+ _nav_mesh[i][j]->_position.get_y() >= down && _nav_mesh[i][j]->_position.get_y() <= top) {
|
|
|
_nav_mesh[i][j]->_type = false;
|
|
|
_previous_obstacles.insert(_previous_obstacles.end(), i);
|
|
|
_previous_obstacles.insert(_previous_obstacles.end(), j);
|
|
|
@@ -378,35 +373,44 @@ void PathFind::add_obstacle_to_mesh(NodePath obstacle) {
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: do_dynamic_avoid()
|
|
|
-// Description: This function does the updation of the collisions to
|
|
|
-// the mesh based on the new positions of the obstacles.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : do_dynamic_avoid()
|
|
|
+// Description : This function does the updation of the collisions to the mesh based
|
|
|
+// on the new positions of the obstacles.
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::do_dynamic_avoid() {
|
|
|
clear_previous_obstacles();
|
|
|
_previous_obstacles.clear();
|
|
|
- for (unsigned int i = 0; i < _dynamic_obstacle.size(); ++i) {
|
|
|
+ for(unsigned int i = 0; i < _dynamic_obstacle.size(); ++i) {
|
|
|
add_obstacle_to_mesh(_dynamic_obstacle[i]);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: clear_previous_obstacles()
|
|
|
-// Description: Helper function to reset the collisions if the
|
|
|
-// obstacle is not on the node anymore.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : clear_previous_obstacles()
|
|
|
+// Description : Helper function to reset the collisions if the obstacle is not on the
|
|
|
+// node anymore
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::clear_previous_obstacles(){
|
|
|
- for (unsigned int i = 0; i < _previous_obstacles.size(); i = i + 2) {
|
|
|
+ for(unsigned int i = 0; i < _previous_obstacles.size(); i = i + 2) {
|
|
|
_nav_mesh[_previous_obstacles[i]][_previous_obstacles[i + 1]]->_type = true;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
-// Function: dynamic_avoid
|
|
|
-// Description: This function starts the pathfinding obstacle
|
|
|
-// navigation for the passed in obstacle.
|
|
|
-////////////////////////////////////////////////////////////////////
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+//
|
|
|
+// Function : dynamic_avoid
|
|
|
+// Description : This function starts the pathfinding obstacle navigation for the
|
|
|
+// passed in obstacle.
|
|
|
+
|
|
|
+///////////////////////////////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
void PathFind::dynamic_avoid(NodePath obstacle) {
|
|
|
_dynamic_avoid = true;
|
|
|
_dynamic_obstacle.insert(_dynamic_obstacle.end(), obstacle);
|