clean up code

include/hpp/affordance/affordance-extraction.hh

@@ -61,7 +61,7 @@ namespace hpp {
     };
 
 		// helper function to extract mesh model of an fcl::collisionObstacle
-    BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object);
+    BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t& object);
 
     /// \addtogroup affordance
     /// \{
@@ -94,17 +94,11 @@ namespace hpp {
     {
       public:
 
-      SemanticsData (): SemanticsData (0) {}
-			/// Constructor that adjust the affordances_ vector to
-			/// the amount of defined affordance types but leaves the
-			/// vectors of affordances empty.
-			///
-			/// \param affordanceCount number of defined affordance types.
-			SemanticsData (const long unsigned int& affordanceCount)
-      {
-        affordances_.resize(affordanceCount);
-      }
+      SemanticsData () {}
       std::vector<std::vector<AffordancePtr_t> > affordances_;
+			private:
+			SemanticsData(const SemanticsData&); // Prevent copy-construction
+			SemanticsData& operator=(const SemanticsData&);
     };
 
 
@@ -131,7 +125,7 @@ namespace hpp {
 		///				 the global requirement set by the affordance type, it is deleted
 		/// 			 and will not be tested again in subsequent recursive steps.
 		/// \param refTriIdx index corresponding to the last found triangle that
-		///				 fullfils bot the local and the global requirement. It is then
+		///				 fullfils both the local and the global requirement. It is then
 		///				 used as reference in the following recursive step.
 		/// \param area total area of all triangles that are part of the potential
 		///				 affordance object. Every time a triangle fulfilling all set

include/hpp/affordance/operations.hh

@@ -29,10 +29,10 @@ namespace hpp {
       public:
         OperationBase (): zWorld_(0,0,1), margin_(0.3), minArea_(0.05), 
                           affordance_("noAffordance") {}
-        explicit OperationBase (const double & margin = 0.3, const double minArea = 0.05, 
-                                const char* affordanceName = "noAffordance"): zWorld_(0,0,1), 
+        explicit OperationBase (const double margin = 0.3, const double minArea = 0.05,
+                                const char* affordanceName = "noAffordance"): zWorld_(0,0,1),
                                 margin_(margin), minArea_(minArea), affordance_(affordanceName) {}
-        virtual bool requirement (fcl::Vec3f normal) =0;
+        virtual bool requirement (const fcl::Vec3f& normal) =0;
 
         const fcl::Vec3f zWorld_;
         const double margin_;
@@ -43,31 +43,25 @@ namespace hpp {
     class SupportOperation : public OperationBase
     {
       public:
-	 explicit SupportOperation (const double & margin = 0.3, const double minArea = 0.05,  
-                                    const char* affordanceName = "Support"): 
+	 explicit SupportOperation (const double margin = 0.3, const double minArea = 0.05, 
+                                    const char* affordanceName = "Support"):
                                     OperationBase(margin, minArea, affordanceName) {}
 
-        bool requirement (fcl::Vec3f normal) 
+        bool requirement (const fcl::Vec3f& normal) 
       {
-        if ((zWorld_ - normal).sqrLength() < margin_) {
-         return true;
-        }
-	return false;
+        return ((zWorld_ - normal).sqrLength() < margin_);
       }
     }; // class SupportOperation
    
     class LeanOperation : public OperationBase
     {
       public:
-        explicit LeanOperation (const double & margin = 0.3, const double minArea = 0.05,  
-                                const char* affordanceName = "Lean"): 
+        explicit LeanOperation (const double margin = 0.3, const double minArea = 0.05,
+                                const char* affordanceName = "Lean"):
                                 OperationBase(margin, minArea, affordanceName) {}
-        bool requirement (fcl::Vec3f normal) 
+        bool requirement (const fcl::Vec3f& normal) 
         {
-          if (fabs (normal.dot(zWorld_)) < margin_) {
-            return true;
-          }
-	  return false;
+          return (fabs (normal.dot(zWorld_)) < margin_);
         }
     }; // class LeanOperation
    

src/affordance-extraction.cc

@@ -27,7 +27,7 @@
 namespace hpp {
   namespace affordance {
 
-    BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object)
+    BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t& object)
     {
         assert (object->collisionGeometry ()->getNodeType () == fcl::BV_OBBRSS);
         const BVHModelOBConst_Ptr_t model = boost::static_pointer_cast<const BVHModelOB>
@@ -43,10 +43,9 @@ namespace hpp {
 			// TODO: think of a better way of declaring margins?
       const double marginRad = 0.3;
       const double margin = 1e-15;
-      Triangle refTri = allTris[refTriIdx];
-      // TODO: find a cleaner way of removing & resizing the searchableTriangels vector?
-      std::remove (searchableTris.begin (), searchableTris.end (), refTriIdx);
-      searchableTris.pop_back ();
+			const Triangle& refTri = allTris[refTriIdx];
+			searchableTris.erase (std::remove(searchableTris.begin (),
+				searchableTris.end (), refTriIdx), searchableTris.end ());
       for (unsigned int searchIdx = 0; searchIdx < allTris.size (); searchIdx++) {
         std::vector<unsigned int>::iterator it = std::find (searchableTris.begin (),
                                                             searchableTris.end (), searchIdx);
@@ -58,7 +57,7 @@ namespace hpp {
           refPoints.push_back(refTri.points.p2);
           refPoints.push_back(refTri.points.p3);
         for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
-          Triangle searchTri = allTris [searchIdx];
+          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) {
@@ -71,8 +70,8 @@ namespace hpp {
               }
             } else {
               // if linked face does not fulfil global requirement, discard
-              std::remove (searchableTris.begin (), searchableTris.end (), searchIdx);
-              searchableTris.pop_back ();
+							searchableTris.erase(std::remove(searchableTris.begin(),
+								searchableTris.end(), searchIdx), searchableTris.end());
             }
             break; // jump out of vertex loop if triangle already tested for affordance
           }
@@ -89,12 +88,10 @@ namespace hpp {
       std::vector <unsigned int> unsetTriangles;
       double totArea = .0;
       std::vector<std::vector<unsigned int> > potentialAffordances (opVec.size ());
-      SemanticsDataPtr_t foundAffordances(new SemanticsData(opVec.size ()));
+      SemanticsDataPtr_t foundAffordances(new SemanticsData());
 
-      for(unsigned int i = 0; i < model->num_tris; ++i)
+      for(int i = 0; i < model->num_tris; ++i)
       {
-        // TODO: make sure triagle points are correct in world frame after every 
-        // change!! 
         TrianglePoints tri;
         fcl::Triangle fcltri = model->tri_indices [i];
         tri.p1 = colObj->getRotation () *

tests/test-operations.cc

@@ -22,6 +22,19 @@
 #define BOOST_TEST_MODULE test-operations
 #include <boost/test/included/unit_test.hpp>
 
+const double epsilon = 10e-6;
+
+bool compDouble(const double a, const double b)
+{
+	return a-b < epsilon;
+}
+
+bool compVec(const fcl::Vec3f& a, const fcl::Vec3f& b)
+{
+	return (a-b).norm() < epsilon;
+}
+
+
 BOOST_AUTO_TEST_SUITE (test_affordance)
 
 BOOST_AUTO_TEST_CASE (operations)
@@ -35,10 +48,10 @@ BOOST_AUTO_TEST_CASE (operations)
 
   const fcl::Vec3f normal1(0, 0, 1);
 
-	BOOST_CHECK_MESSAGE (support->zWorld_ == normal1, 
+	BOOST_CHECK_MESSAGE (compVec(support->zWorld_,normal1),
 		"default value for zWorld should be " << normal1 << " but is " << support->zWorld_);
 
-	BOOST_CHECK_MESSAGE (lean->margin_ == 0.1, 
+	BOOST_CHECK_MESSAGE (compDouble(lean->margin_,0.1),
 		"margin should match the one given when creating operation");
 
 	BOOST_CHECK_MESSAGE (operations.size () == 2, 

tests/test-oriented-triangles2.cc

@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE (oriented_triangles2)
 	fcl::Vec3f axis (1,0,0);
 	fcl::Matrix3f R;
 
-	for (unsigned int i = 0; i < size_; ++i) {
+	for (int i = 0; i < size_; ++i) {
 	  boost::shared_ptr<Model> model (new Model ());
 		fcl::Triangle tri (0,1,2);
 		triangles.push_back (tri);