Merge pull request #164 from jmirabel/devel

[Python] Add constructor for class Convex.

include/hpp/fcl/narrowphase/narrowphase.h

@@ -225,6 +225,7 @@ namespace fcl
           // assert (distance == (w0 - w1).norm());
           distance = gjk.distance;
 
+          normal = (tf1.getRotation() * gjk.ray).normalized();
           p1 = tf1.transform (p1);
           p2 = tf1.transform (p2);
           return true;

python/collision-geometries.cc

@@ -73,6 +73,9 @@ namespace dv = doxygen::visitor;
 using boost::shared_ptr;
 using boost::noncopyable;
 
+typedef std::vector<Vec3f> Vec3fs;
+typedef std::vector<Triangle> Triangles;
+
 struct BVHModelBaseWrapper
 {
   static Vec3f vertices (const BVHModelBase& bvh, int i)
@@ -129,6 +132,16 @@ struct ConvexWrapper
     if (i >= convex.num_polygons) throw std::out_of_range("index is out of range");
     return convex.polygons[i];
   }
+
+  static shared_ptr<Convex_t> constructor (const Vec3fs& _points, const Triangles& _tris)
+  {
+    Vec3f* points = new Vec3f[_points.size()];
+    for (std::size_t i = 0; i < _points.size(); ++i) points[i] = _points[i];
+    Triangle* tris = new Triangle[_tris.size()];
+    for (std::size_t i = 0; i < _tris.size(); ++i) tris[i] = _tris[i];
+    return shared_ptr<Convex_t>(new Convex_t(true, points, (int)_points.size(),
+          tris, (int)_tris.size()));
+  }
 };
 
 void exposeShapes ()
@@ -170,6 +183,7 @@ void exposeShapes ()
 
   class_ <Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle> >, noncopyable>
     ("Convex", doxygen::class_doc< Convex<Triangle> >(), no_init)
+    .def("__init__", make_constructor(&ConvexWrapper<Triangle>::constructor))
     .DEF_RO_CLASS_ATTRIB (Convex<Triangle>, num_polygons)
     .def ("polygons", &ConvexWrapper<Triangle>::polygons)
     ;
@@ -360,9 +374,6 @@ void exposeCollisionGeometries ()
 
   exposeShapes();
 
-  typedef std::vector<Vec3f> Vec3fs;
-  typedef std::vector<Triangle> Triangles;
-
   class_ <BVHModelBase, bases<CollisionGeometry>, BVHModelPtr_t, noncopyable>
     ("BVHModelBase", no_init)
     .def ("vertices", &BVHModelBaseWrapper::vertices,

test/python_unit/geometric_shapes.py

@@ -149,5 +149,12 @@ class TestGeometricShapes(TestCase):
         Ic_ref = np.diag([Icx_ref,Icx_ref,Iz_ref])
         self.assertApprox(Ic, Ic_ref)
 
+    def test_convex(self):
+        verts = hppfcl.StdVec_Vec3f ()
+        faces = hppfcl.StdVec_Triangle ()
+        verts.extend( [ np.array([0, 0, 0]), np.array([0, 1, 0]), np.array([1, 0, 0]), ])
+        faces.append(hppfcl.Triangle(0,1,2))
+        convex = hppfcl.Convex(verts, faces)
+
 if __name__ == '__main__':
     unittest.main()