removed unnecessary comments, added navigation_cat.error() and added add_plygon with PTA_LVecBase3f as input

panda/src/navigation/config_navigation.cxx

@@ -43,17 +43,14 @@ ConfigVariableInt navigation_sample_config_variable
 void
 init_libnavigation() {
   
-  std::cout<<"\nCalled init_libnavigation\n";
   static bool initialized = false;
   if (initialized) {
     return;
   }
   initialized = true;
-  std::cout<<"\nCalling init_type\n";
   NavMesh::init_type();
   NavMeshNode::init_type();
   
-  std::cout<<"\nCalling register_with_read_factory\n";
   // Register factory functions for constructing objects from .bam files
   NavMeshNode::register_with_read_factory();
   NavMesh::register_with_read_factory();

panda/src/navigation/navMesh.cxx

@@ -15,10 +15,14 @@
 #include "navMesh.h"
 #include "geom.h"
 #include "geomTrifans.h"
+#include "config_navigation.h"
 #include <iostream>
 
 TypeHandle NavMesh::_type_handle;
 
+/**
+ * NavMesh constructor to initialize member variables.
+ */
 NavMesh::NavMesh():
   _nav_mesh(0) {}
 
@@ -39,7 +43,8 @@ NavMesh::NavMesh(dtNavMesh *nav_mesh) {
  */
 NavMesh::NavMesh(NavMeshParams mesh_params):
   _nav_mesh(0) {
-  memset(&(_params), 0, sizeof(_params));
+  //memset(&(_params), 0, sizeof(_params));
+  _params = {};
 
   _params.verts = mesh_params.verts;
   _params.vertCount = mesh_params.vert_count;
@@ -79,11 +84,11 @@ NavMesh::NavMesh(NavMeshParams mesh_params):
  * Function to build navigation mesh from the parameters.
  */
 bool NavMesh::init_nav_mesh() {
-	unsigned char *nav_data = 0;
+  unsigned char *nav_data = 0;
   int nav_data_size = 0;
 
   if (!dtCreateNavMeshData(&_params, &nav_data, &nav_data_size)) {
-    std::cout<<"\nCould not build Detour navmesh.\n";
+    navigation_cat.error() << "\nCould not build Detour navmesh.\n";
     return false;
   }
 
@@ -91,7 +96,7 @@ bool NavMesh::init_nav_mesh() {
 
   if (!_nav_mesh) {
     dtFree(nav_data);
-    std::cout<<"\nCould not create Detour navmesh\n";
+    navigation_cat.error() << "\nCould not create Detour navmesh\n";
     return false;
   }
 
@@ -100,7 +105,7 @@ bool NavMesh::init_nav_mesh() {
   status = _nav_mesh->init(nav_data, nav_data_size, DT_TILE_FREE_DATA);
   if (dtStatusFailed(status)) {
     dtFree(nav_data);
-    std::cout<<"\nCould not init Detour navmesh\n";
+    navigation_cat.error() << "\nCould not init Detour navmesh\n";
     return false;
   }
 }
@@ -112,26 +117,17 @@ PT(GeomNode) NavMesh::draw_nav_mesh_geom() {
 
   PT(GeomVertexData) vdata;
   vdata = new GeomVertexData("vertexInfo", GeomVertexFormat::get_v3c4(), Geom::UH_static);
-  std::cout << "\nJust degbugging for navMesh: navmesh vert count: " << _params.vertCount << "\n";
   vdata->set_num_rows(_params.vertCount);
 
   GeomVertexWriter vertex(vdata, "vertex");
   GeomVertexWriter colour(vdata, "color");
 
   const int nvp = _params.nvp;
-  std::cout << "nvp: " << nvp << std::endl;
   const float cs = _params.cs;
-  std::cout << "cs: " << cs << std::endl;
   const float ch = _params.ch;
-  std::cout << "ch: " << ch << std::endl;
   const float* orig = _params.bmin;
-  std::cout << "orig: " << orig[0] << "\t" << orig[1] << "\t" << orig[2] << std::endl;
-
-  std::cout << "NavMesh poly count: " << _params.polyCount << std::endl;
-  std::cout << "NavMesh vert count: " << _params.vertCount << std::endl;
-
+  
   for (int i = 0;i < _params.vertCount * 3;i += 3) {
-
     const unsigned short* v = &_params.verts[i];
 
     //convert to world space
@@ -183,8 +179,6 @@ PT(GeomNode) NavMesh::draw_nav_mesh_geom() {
   polymeshgeom->add_primitive(prim);
 
   node->add_geom(polymeshgeom);
-  std::cout << "Number of Polygons: " << _params.polyCount << std::endl;
-
 
   return node;
 }
@@ -194,7 +188,6 @@ PT(GeomNode) NavMesh::draw_nav_mesh_geom() {
  */
 void NavMesh::
 register_with_read_factory() {
-  std::cout<<"\nCalled NavMesh::register_with_read_factory()\n";
   BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
 }
 

panda/src/navigation/navMeshNode.cxx

@@ -36,7 +36,6 @@ NavMeshNode::~NavMeshNode() {}
  */
 void NavMeshNode::
 register_with_read_factory() {
-  std::cout<<"\nCalled NavMeshNode::register_with_read_factory()\n";
   BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
 }
 

panda/src/navigation/navMeshQuery.cxx

@@ -15,6 +15,7 @@
 #include "navMeshNode.h"
 #include "DetourNavMeshQuery.h"
 #include "lvecBase3.h"
+#include "config_navigation.h"
 #include <iostream>
 
 /**
@@ -65,7 +66,7 @@ bool NavMeshQuery::set_nav_query(PT(NavMesh) nav_mesh) {
   dtStatus status = _nav_query->init(nav_mesh->get_nav_mesh(), MAX_NODES);
 
   if (dtStatusFailed(status)) {
-    std::cout<<"\nCannot set nav query!\n";
+    navigation_cat.error() << "\nCannot set nav query!\n";
     return false;
   }
 
@@ -78,7 +79,8 @@ bool NavMeshQuery::set_nav_query(PT(NavMesh) nav_mesh) {
  * and can be used multiple times.
  */
 bool NavMeshQuery::set_nav_query(NodePath nav_mesh_node_path) {
-  NavMeshNode *node = dynamic_cast<NavMeshNode*>(nav_mesh_node_path.node());
+  // NavMeshNode *node = dynamic_cast<NavMeshNode*>(nav_mesh_node_path.node());
+  NavMeshNode *node = DCAST(NavMeshNode, nav_mesh_node_path.node());
   return set_nav_query(node->get_nav_mesh());
 }
 
@@ -88,7 +90,7 @@ bool NavMeshQuery::set_nav_query(NodePath nav_mesh_node_path) {
  */
 bool NavMeshQuery::nearest_point(LPoint3 &p) {
   if (!_nav_query) {
-    std::cout << "\nNavMeshQuery not created!\n";
+    navigation_cat.error() << "\nNavMeshQuery not created!\n";
     return false;
   }
 
@@ -103,7 +105,7 @@ bool NavMeshQuery::nearest_point(LPoint3 &p) {
   dtStatus status = _nav_query->findNearestPoly(center, extents, &filter, &nearest_poly_ref_id, nearest_p);
 
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find nearest point on polymesh.\n";
+    navigation_cat.error() << "\nCannot find nearest point on polymesh.\n";
     return false;
   }
   p = LPoint3(nearest_p[0], -nearest_p[2], nearest_p[1]); // convert back from y-up system to z-up system
@@ -133,26 +135,21 @@ PTA_LVecBase3 NavMeshQuery::find_path(LPoint3 &start, LPoint3 &end) {
 
   dtStatus status = _nav_query->findNearestPoly(start_pos, extents, &filter, &start_ref, nearest_start);
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find nearest point on polymesh for start point.\n";
+    navigation_cat.error() << "\nCannot find nearest point on polymesh for start point.\n";
     return path_array;
   }
-  std::cout<<"\nStart_ref: "<<start_ref<<"\n";
-  std::cout<<"\nStarting point:\t"<<nearest_start[0]<<"\t"<<-nearest_start[2]<<"\t"<<nearest_start[1]<<"\n";
-
+  
   status = _nav_query->findNearestPoly(end_pos, extents, &filter, &end_ref, nearest_end);
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find nearest point on polymesh for end point.\n";
+    navigation_cat.error() << "\nCannot find nearest point on polymesh for end point.\n";
     return path_array;
   }
-  std::cout<<"\nEnd_ref: "<<end_ref<<"\n";
-  std::cout<<"\nEnding point:\t"<<nearest_end[0]<<"\t"<<-nearest_end[2]<<"\t"<<nearest_end[1]<<"\n";
   
   status = _nav_query->findPath(start_ref, end_ref, nearest_start, nearest_end, &filter, path, &path_count, MAX_POLYS);
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find the path.\n";
+    navigation_cat.error() << "\nCannot find the path.\n";
     return path_array;
   }
-  std::cout<<"\nNumber of polygons included in path: "<<path_count<<"\n";
   
   float pos[3] = {start_pos[0], start_pos[1], start_pos[2]};
 
@@ -162,7 +159,7 @@ PTA_LVecBase3 NavMeshQuery::find_path(LPoint3 &start, LPoint3 &end) {
     
     status = _nav_query->closestPointOnPolyBoundary(path[i], pos, closest);
     if (dtStatusFailed(status)) {
-      std::cout << "\nCannot find the nearest point on polygon.\n";
+      navigation_cat.error() << "\nCannot find the nearest point on polygon.\n";
       return path_array;
     }
 
@@ -199,27 +196,22 @@ PTA_LVecBase3 NavMeshQuery::find_straight_path(LPoint3 &start, LPoint3 &end, int
 
   dtStatus status = _nav_query->findNearestPoly(start_pos, extents, &filter, &start_ref, nearest_start);
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find nearest point on polymesh for start point.\n";
+    navigation_cat.error() << "\nCannot find nearest point on polymesh for start point.\n";
     return straight_path_array;
   }
-  std::cout<<"\nStart_ref: "<<start_ref<<"\n";
-  std::cout<<"\nStarting point:\t"<<nearest_start[0]<<"\t"<<-nearest_start[2]<<"\t"<<nearest_start[1]<<"\n";
-
+  
   status = _nav_query->findNearestPoly(end_pos, extents, &filter, &end_ref, nearest_end);
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find nearest point on polymesh for end point.\n";
+    navigation_cat.error() << "\nCannot find nearest point on polymesh for end point.\n";
     return straight_path_array;
   }
-  std::cout<<"\nEnd_ref: "<<end_ref<<"\n";
-  std::cout<<"\nEnding point:\t"<<nearest_end[0]<<"\t"<<-nearest_end[2]<<"\t"<<nearest_end[1]<<"\n";
   
   status = _nav_query->findPath(start_ref, end_ref, nearest_start, nearest_end, &filter, path, &path_count, MAX_POLYS);
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find the path.\n";
+    navigation_cat.error() << "\nCannot find the path.\n";
     return straight_path_array;
   }
-  std::cout<<"\nNumber of polygons included in path: "<<path_count<<"\n";
-
+  
   float straight_path[MAX_POLYS*3];
   unsigned char straight_path_flags[MAX_POLYS];
   dtPolyRef straight_path_refs[MAX_POLYS];
@@ -227,11 +219,10 @@ PTA_LVecBase3 NavMeshQuery::find_straight_path(LPoint3 &start, LPoint3 &end, int
 
   status = _nav_query->findStraightPath(nearest_start, nearest_end, path, path_count, straight_path, straight_path_flags, straight_path_refs, &straight_path_count, MAX_POLYS, opt);
   if (dtStatusFailed(status)) {
-    std::cout << "\nCannot find the straight path.\n";
+    navigation_cat.error() << "\nCannot find the straight path.\n";
     return straight_path_array;
   }
-  std::cout<<"\nNumber of points included in straight path: "<<straight_path_count<<"\n";
-
+  
   for(int i=0;i<straight_path_count*3;i+=3) {
     
     LVecBase3 point = LVecBase3( straight_path[i], -straight_path[i+2], straight_path[i+1]);  // convert back from y-up system to z-up system

panda/src/navigation/navMeshQuery.h

@@ -45,7 +45,6 @@ public:
   
   ~NavMeshQuery();
   
-  
 };
 
 #endif // NAVMESHQUERY_H

panda/src/navmeshgen/navMeshBuilder.cxx

@@ -15,6 +15,7 @@
 #include "Recast.h"
 #include "DetourNavMesh.h"
 #include "DetourNavMeshBuilder.h"
+#include "pta_LVecBase3.h"
 
 #define _USE_MATH_DEFINES
 #include <math.h>
@@ -158,11 +159,11 @@ void NavMeshBuilder::add_polygon(LPoint3 a, LPoint3 b, LPoint3 c) {
 /**
  * This function adds a custom polygon with equal to or more than three vertices to the input geometry.
  */
-// void NavMeshBuilder::add_polygon(std::vector<LVector3> p) {
-//   for (int i = 2; i < p.size(); ++i) {
-//     add_polygon(p[i-2], p[i-1], p[i]);
-//   }
-// }
+void NavMeshBuilder::add_polygon(PTA_LVecBase3f vec) {
+  for (int i = 2; i < vec.size(); ++i) {
+    add_polygon(LPoint3(vec[i-2]), LPoint3(vec[i-1]), LPoint3(vec[i]));
+  }
+}
 
 /**
  * Function to build vertex array and triangles array from a geom.

panda/src/navmeshgen/navMeshBuilder.h

@@ -24,6 +24,7 @@
 #include "pandaSystem.h"
 #include "navMesh.h"
 #include "lpoint3.h"
+#include "pta_LVecBase3.h"
 
 enum SamplePolyAreas {
   SAMPLE_POLYAREA_GROUND,
@@ -93,7 +94,7 @@ PUBLISHED:
   int get_pmesh_max_poly_count() { return _pmesh->maxpolys; }
 
   void add_polygon(LPoint3 a, LPoint3 b, LPoint3 c);
-  // void add_polygon(std::vector<LVector3> p);
+  void add_polygon(PTA_LVecBase3f vec);
 
 private:
   float _scale;