added matrix to convert coordinate system

panda/src/navigation/navMesh.cxx

@@ -16,6 +16,7 @@
 #include "geom.h"
 #include "geomTrifans.h"
 #include "config_navigation.h"
+#include "lvecBase3.h"
 #include <iostream>
 
 TypeHandle NavMesh::_type_handle;
@@ -134,12 +135,15 @@ PT(GeomNode) NavMesh::draw_nav_mesh_geom() {
     const float x = orig[0] + v[0] * cs;
     const float y = orig[1] + v[1] * ch;
     const float z = orig[2] + v[2] * cs;
+    
 
-    vertex.add_data3(x, -z, y); //if origingally model is z-up
+    LVecBase3 vec = mat_from_y.xform_point({x, y, z});
+    vertex.add_data3(vec);
+
+    //vertex.add_data3(x, -z, y); //if origingally model is z-up
     //vertex.add_data3(x, y, z); //if originally model is y-up
+    
     colour.add_data4((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX, 1);
-    //std::cout << "index: " << i / 3 << "\t" << x << "\t" << y << "\t" << z << "\n";
-
   }
 
   PT(GeomNode) node;
@@ -150,7 +154,6 @@ PT(GeomNode) NavMesh::draw_nav_mesh_geom() {
 
   for (int i = 0; i < _params.polyCount; ++i) {
 
-
     const unsigned short* p = &_params.polys[i*nvp * 2];
 
     // Iterate the vertices.
@@ -168,8 +171,6 @@ PT(GeomNode) NavMesh::draw_nav_mesh_geom() {
         // The edge beginning with this vertex connects to 
         // polygon p[j + nvp].
       }
-      //std::cout << "p[j]: " << p[j] << std::endl;
-
     }
     prim->close_primitive();
 
@@ -286,7 +287,6 @@ make_from_bam(const FactoryParams &params) {
  */
 void NavMesh::
 fillin(DatagramIterator &scan, BamReader *manager) {
-  //NavMesh::fillin(scan, manager);
   
   int vert_count = scan.get_int32();
   int poly_count = scan.get_int32();
@@ -382,6 +382,4 @@ fillin(DatagramIterator &scan, BamReader *manager) {
   _params.buildBvTree = build_bv_tree;
 
   init_nav_mesh();
-
-
 }

panda/src/navigation/navMesh.h

@@ -20,6 +20,7 @@
 #include "typedWritableReferenceCount.h"
 #include "pandaFramework.h"
 #include "pandaSystem.h"
+#include "lmatrix.h"
 
 class EXPCL_NAVIGATION NavMeshParams {
 PUBLISHED:
@@ -44,11 +45,14 @@ PUBLISHED:
   float *detail_verts;          // size: detail_vert_count * 3
   unsigned char *detail_tris;   // size: detail_tri_count * 4
   int RC_MESH_NULL_IDX = 65535;
+  int input_model_coordinate_system = 0;
+
 };
 
 class EXPCL_NAVIGATION NavMesh: public TypedWritableReferenceCount
 {
 PUBLISHED:
+
   NavMesh(dtNavMesh *nav_mesh);
   NavMesh(NavMeshParams mesh_params);
   void set_nav_mesh(dtNavMesh *m) { _nav_mesh = m; }
@@ -62,6 +66,10 @@ PUBLISHED:
 private:
   dtNavMesh *_nav_mesh;
   dtNavMeshCreateParams _params;
+  LMatrix4 mat_from_y = LMatrix4::convert_mat(CS_yup_right, CS_default);
+  LMatrix4 mat_from_z = LMatrix4::convert_mat(CS_zup_right, CS_default);
+  LMatrix4 mat_to_y = LMatrix4::convert_mat(CS_default, CS_yup_right);
+  LMatrix4 mat_to_z = LMatrix4::convert_mat(CS_default, CS_zup_right);
   int RC_MESH_NULL_IDX;
   
 public:

panda/src/navmeshgen/navMeshBuilder.cxx

@@ -16,6 +16,7 @@
 #include "DetourNavMesh.h"
 #include "DetourNavMeshBuilder.h"
 #include "pta_LVecBase3.h"
+#include "lvecBase3.h"
 #include "config_navmeshgen.h"
 
 #define _USE_MATH_DEFINES
@@ -120,7 +121,11 @@ void NavMeshBuilder::add_polygon(LPoint3 a, LPoint3 b, LPoint3 c) {
 
   LVector3 v = {a[0], a[1], a[2]};
   if (_vertex_map.find(v) == _vertex_map.end()) {
-    add_vertex(v[0], v[2], -v[1], _vert_capacity); //if input model is originally z-up
+
+    LVecBase3 vec = mat_to_y.xform_point(v);
+    add_vertex(vec[0], vec[1], vec[2], _vert_capacity);
+  
+    //add_vertex(v[0], v[2], -v[1], _vert_capacity); //if input model is originally z-up
     //add_vertex(v[0], v[1], v[2], _vert_capacity); //if input model is originally y-up
     _vertex_map[v] = index_temp++;
     _vertex_vector.push_back(v);
@@ -130,7 +135,10 @@ void NavMeshBuilder::add_polygon(LPoint3 a, LPoint3 b, LPoint3 c) {
 
   v = {b[0], b[1], b[2]};
   if (_vertex_map.find(v) == _vertex_map.end()) {
-    add_vertex(v[0], v[2], -v[1], _vert_capacity); //if input model is originally z-up
+    LVecBase3 vec = mat_to_y.xform_point(v);
+    add_vertex(vec[0], vec[1], vec[2], _vert_capacity);
+
+    //add_vertex(v[0], v[2], -v[1], _vert_capacity); //if input model is originally z-up
     //add_vertex(v[0], v[1], v[2], _vert_capacity); //if input model is originally y-up
     _vertex_map[v] = index_temp++;
     _vertex_vector.push_back(v);
@@ -140,7 +148,10 @@ void NavMeshBuilder::add_polygon(LPoint3 a, LPoint3 b, LPoint3 c) {
 
   v = {c[0], c[1], c[2]};
   if (_vertex_map.find(v) == _vertex_map.end()) {
-    add_vertex(v[0], v[2], -v[1], _vert_capacity); //if input model is originally z-up
+    LVecBase3 vec = mat_to_y.xform_point(v);
+    add_vertex(vec[0], vec[1], vec[2], _vert_capacity);
+
+    //add_vertex(v[0], v[2], -v[1], _vert_capacity); //if input model is originally z-up
     //add_vertex(v[0], v[1], v[2], _vert_capacity); //if input model is originally y-up
     _vertex_map[v] = index_temp++;
     _vertex_vector.push_back(v);
@@ -282,7 +293,10 @@ void NavMeshBuilder::process_vertex_data(const GeomVertexData *vdata, int &vcap)
     y = v[1];
     z = v[2];
     if (_vertex_map.find(v) == _vertex_map.end()) {
-      add_vertex(x, z, -y, vcap); //if input model is originally z-up
+      LVecBase3 vec = mat_to_y.xform_point(v);
+      add_vertex(vec[0], vec[1], vec[2], vcap);
+      
+      //add_vertex(x, z, -y, vcap); //if input model is originally z-up
       //add_vertex(x, y, z, vcap); //if input model is originally y-up
 
       //_vertex_map[v] = index_temp++;
@@ -686,7 +700,7 @@ PT(NavMesh) NavMeshBuilder::build() {
     mesh_params.build_bv_tree = true;
 
     mesh_params.RC_MESH_NULL_IDX = RC_MESH_NULL_IDX;
-
+    
     _nav_mesh_obj = new NavMesh(mesh_params);
 
   }
@@ -726,8 +740,11 @@ PT(GeomNode) NavMeshBuilder::draw_poly_mesh_geom() {
     const float x = orig[0] + v[0] * cs;
     const float y = orig[1] + v[1] * ch;
     const float z = orig[2] + v[2] * cs;
+    
+    LVecBase3 vec = mat_from_y.xform_point({x, y, z});
+    vertex.add_data3(vec);
 
-    vertex.add_data3(x, -z, y); //if origingally model is z-up
+    //vertex.add_data3(x, -z, y); //if origingally model is z-up
     //vertex.add_data3(x, y, z); //if originally model is y-up
     colour.add_data4((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX, 1);
     
@@ -768,6 +785,6 @@ PT(GeomNode) NavMeshBuilder::draw_poly_mesh_geom() {
   polymeshgeom->add_primitive(prim);
 
   node->add_geom(polymeshgeom);
-  std::cout << "Number of Polygons: " << _pmesh->npolys << std::endl;
+  navmeshgen_cat.info() << "Number of Polygons: " << _pmesh->npolys << std::endl;
   return node;
 }

panda/src/navmeshgen/navMeshBuilder.h

@@ -25,6 +25,7 @@
 #include "navMesh.h"
 #include "lpoint3.h"
 #include "pta_LVecBase3.h"
+#include "lmatrix.h"
 
 enum SamplePolyAreas {
   SAMPLE_POLYAREA_GROUND,
@@ -53,6 +54,7 @@ PUBLISHED:
     SAMPLE_PARTITION_MONOTONE,
     SAMPLE_PARTITION_LAYERS,
   };
+  
   float get_actor_radius() { return _agent_radius; }
   float get_actor_height() { return _agent_height; }
   float get_actor_max_climb() { return _agent_max_climb; }
@@ -65,7 +67,7 @@ PUBLISHED:
   float get_cell_size() { return _cell_size; }
   float get_cell_height() { return _cell_height; }
   PartitionType get_partition_type() { return _partition_type; }
-
+  
   void set_actor_height(float height) { _agent_height = height; }
   void set_actor_radius(float radius) { _agent_radius = radius; }
   void set_actor_max_climb(float climb) { _agent_max_climb = climb; }
@@ -78,7 +80,7 @@ PUBLISHED:
   void set_cell_size(float cs) { _cell_size = cs; }
   void set_cell_height(float ch) { _cell_height = ch; }
   void set_partition_type(PartitionType partition) { _partition_type = partition; }
-
+  
   MAKE_PROPERTY(_agent_radius, get_actor_radius, set_actor_radius);
   MAKE_PROPERTY(_agent_height, get_actor_height, set_actor_height);
   MAKE_PROPERTY(_agent_max_climb, get_actor_max_climb, set_actor_max_climb);
@@ -131,6 +133,11 @@ private:
   bool _loaded;
   int index_temp;
 
+  LMatrix4 mat_from_y = LMatrix4::convert_mat(CS_yup_right, CS_default);
+  LMatrix4 mat_from_z = LMatrix4::convert_mat(CS_zup_right, CS_default);
+  LMatrix4 mat_to_y = LMatrix4::convert_mat(CS_default, CS_yup_right);
+  LMatrix4 mat_to_z = LMatrix4::convert_mat(CS_default, CS_zup_right);
+
 protected:
   PT(NavMesh) _nav_mesh_obj;
   class dtNavMesh *_nav_mesh;
@@ -151,7 +158,7 @@ protected:
   float _detail_sample_dist;
   float _detail_sample_max_error;
   PartitionType _partition_type;
-
+  
   bool _filter_low_hanging_obstacles;
   bool _filter_ledge_spans;
   bool _filter_walkable_low_height_spans;