affordance runs with hpp-pinocchio

.gitignore

 *~
 /build/*
 *.swp
+*.user
+/build-rel/*

include/hpp/affordance/operations.hh

@@ -93,7 +93,7 @@ namespace hpp {
 				/// \param nromal Normal vector of the tested triangle.
         bool requirement (const fcl::Vec3f& normal) 
       {
-        return ((zWorld_ - normal).sqrLength() < margin_);
+        return ((zWorld_ - normal).squaredNorm() < margin_);
       }
     }; // class SupportOperation
     /// Class that contains the information needed to create affordance
@@ -138,9 +138,10 @@ namespace hpp {
       explicit Support45Operation (const double margin = 0.3, const double nbTriMargin = 0.3,
                                                                 const double minArea = 0.05,
                                 const char* affordanceName = "Support45"):
-                                OperationBase(margin, 0.05, minArea, affordanceName) {}
+                                OperationBase(margin, 0.05, minArea, affordanceName)
+                                , axis45_ (fcl::Vec3f(1./sqrt(2.),0,1./sqrt(2.))) {}
        private:
-        const fcl::Vec3f axis45_ = fcl::Vec3f(1./sqrt(2.),0,1./sqrt(2.));
+        const fcl::Vec3f axis45_;
 
                 /// The implementation of the requirement function for Support45 affordances
                 /// overrides the virtual function in class OperationBase.
@@ -150,7 +151,7 @@ namespace hpp {
         {
           fcl::Vec3f projectedNormal(0,0,normal[2]); // project normal in x,z plan
           projectedNormal[0] = sqrt(normal[0]*normal[0] + normal[1]*normal[1]);
-          return ((axis45_ - projectedNormal).sqrLength() < margin_);
+          return ((axis45_ - projectedNormal).squaredNorm() < margin_);
         }
     }; // class LeanOperation
 

src/affordance-extraction.cc

@@ -57,11 +57,11 @@ namespace hpp {
           refPoints.push_back(refTri.points.p3);
         for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
           const Triangle& searchTri = allTris [searchIdx];
-          if ((refPoints[vertIdx] - searchTri.points.p1).sqrLength () < margin
-              || (refPoints[vertIdx] - searchTri.points.p2).sqrLength () < margin
-              || (refPoints[vertIdx] - searchTri.points.p3).sqrLength () < margin) {
+          if ((refPoints[vertIdx] - searchTri.points.p1).squaredNorm () < margin
+              || (refPoints[vertIdx] - searchTri.points.p2).squaredNorm () < margin
+              || (refPoints[vertIdx] - searchTri.points.p3).squaredNorm () < margin) {
             if (refOp->requirement (searchTri.normal)) {
-              if ((searchTri.normal - refTri.normal).sqrLength () < refOp->neighbouringTriangleMargin_) {
+              if ((searchTri.normal - refTri.normal).squaredNorm () < refOp->neighbouringTriangleMargin_) {
                 area += searchTri.area;
                 listPotential.push_back (searchIdx);
                 searchLinkedTriangles (listPotential, refOp, allTris,
@@ -259,7 +259,7 @@ namespace hpp {
                 if((id_lastPoint == id_pointOnHull) || (isLeft(points[id_pointOnHull], points[id_lastPoint],points[id_current]) > 0)
                         || ((id_current!=id_pointOnHull) && (id_current!= id_lastPoint) &&
                             (isLeft(points[id_pointOnHull], points[id_lastPoint],points[id_current]) == 0 )
-                            && ((points[id_pointOnHull]-points[id_current]).sqrLength() < (points[id_pointOnHull]-points[id_lastPoint]).sqrLength()))){ // if on the same line, take the closest one from ptsOnHull
+                            && ((points[id_pointOnHull]-points[id_current]).squaredNorm() < (points[id_pointOnHull]-points[id_lastPoint]).squaredNorm()))){ // if on the same line, take the closest one from ptsOnHull
                     if( ( std::find(res.begin(),res.end(),id_current) == res.end() ) || ((res.size()>0) && (id_current == res[0])))// only selected it if not on the list (or the first)
                         id_lastPoint = id_current;
                 }
@@ -350,9 +350,9 @@ namespace hpp {
                         else
                             next=vertices[orderedIndex[id+1]];
 
-                        A=(next-vertices[orderedIndex[id]]).normalize();
-                        B=(prev-vertices[orderedIndex[id]]).normalize();
-                        dir=(A+B).normalize();
+                        A=(next-vertices[orderedIndex[id]]).normalized();
+                        B=(prev-vertices[orderedIndex[id]]).normalized();
+                        dir=(A+B).normalized();
                      /*   if(dir.length() < std::numeric_limits<double>::epsilon()){ // this mean that prev, current and next are aligned, we make a 90° angle
                             dir=
                         }*/

tests/test-one-triangle2.cc

@@ -53,7 +53,7 @@ BOOST_AUTO_TEST_CASE (one_triangle2)
 
   fcl::Matrix3f R;
   fcl::Quaternion3f quat (1,0,0,0);
-	quat.toRotation(R);
+  R = quat.matrix();
   fcl::Vec3f T (0,0,0);
 
   fcl::Transform3f pose (R, T);

tests/test-oriented-triangles1.cc

@@ -58,10 +58,8 @@ BOOST_AUTO_TEST_CASE (oriented_triangles1)
 	  boost::shared_ptr<Model> model (new Model ());
 		fcl::Triangle tri (0,1,2);
 		triangles.push_back (tri); 
-	
-		fcl::Quaternion3f quat;
-		quat.fromAxisAngle(axis,3.1416*float(5*i)/180.0);
-		quat.toRotation(R);
+
+        R =  Eigen::AngleAxisd(3.1416*double(5*i)/180.0, axis);
 	
 	  fcl::Transform3f pose (R, T);
 	

tests/test-oriented-triangles2.cc

@@ -57,11 +57,9 @@ BOOST_AUTO_TEST_CASE (oriented_triangles2)
 	for (int i = 0; i < size_; ++i) {
 	  boost::shared_ptr<Model> model (new Model ());
 		fcl::Triangle tri (0,1,2);
-		triangles.push_back (tri); 
-	
-		fcl::Quaternion3f quat;
-		quat.fromAxisAngle(axis,3.1416*float(i)/180.0);
-		quat.toRotation(R);
+        triangles.push_back (tri);
+
+        R =  Eigen::AngleAxisd(3.1416*double(i)/180.0, axis);
 	
 	  fcl::Transform3f pose (R, T);