Merge pull request #6 from pFernbach/rbprm

Rbprm

include/hpp/affordance/affordance-extraction.hh

@@ -173,6 +173,17 @@ namespace hpp {
     std::vector<CollisionObjects_t> getAffordanceObjects
                                              (const SemanticsDataPtr_t& sData);
 
+    /// Free function that, given a semanticsData pointer, creates one
+    /// fcl::collisionObject for every Affordance object.
+    /// The object are reduced of \param reduceSize : each vertice are moved toward the center of this value
+    ///
+    /// \param sData reference to all found Affordance objects.
+    /// NOTE : if there is changes in affordance.impl:: Afford::createOperations (), you should be carefull to the index of the affordance type.
+    /// The order of the values in reduceSizes correspond to the order of the affordance type in affordance.impl:: Afford::createOperations ()
+    /// FIXME : not working with plane that contain the z axis
+    std::vector<CollisionObjects_t> getReducedAffordanceObjects
+                                             (const SemanticsDataPtr_t& sData, std::vector<double> reduceSizes);
+
 		/// \}
   } // namespace affordance
 } // namespace hpp

include/hpp/affordance/fwd.hh

@@ -34,12 +34,14 @@ namespace hpp {
     class OperationBase;
     class SupportOperation;
     class LeanOperation;
+    class Support45Operation;
 
     typedef fcl::BVHModel<fcl::OBBRSS> BVHModelOB;
     typedef boost::shared_ptr<BVHModelOB> BVHModelOB_Ptr_t;
     typedef boost::shared_ptr<const BVHModelOB> BVHModelOBConst_Ptr_t;
     typedef boost::shared_ptr <SupportOperation> SupportOperationPtr_t;
     typedef boost::shared_ptr <LeanOperation> LeanOperationPtr_t;
+    typedef boost::shared_ptr <Support45Operation> Support45OperationPtr_t;
     typedef boost::shared_ptr <fcl::CollisionObject> CollisionObjectPtr_t;
     typedef std::vector<CollisionObjectPtr_t> CollisionObjects_t;
     typedef boost::shared_ptr <OperationBase> OperationBasePtr_t; 

include/hpp/affordance/operations.hh

@@ -121,6 +121,40 @@ namespace hpp {
           return (fabs (normal.dot(zWorld_)) < margin_);
         }
     }; // class LeanOperation
+
+    /// Class that contains the information needed to create affordance
+        /// objects of type Lean. Inherits the OperationBase class.
+    class Support45Operation : public OperationBase
+    {
+      public:
+            /// Constructor that takes in user-defined parameters
+            /// \param margin Margin needed for the evaluation of the requirement function
+            /// \param nbTriMargin Margin between two triangles tested for a single
+      ///  affordance surface. If the angle between two triangles is greater
+      ///  than the provided margin, the triangles cannot be part of the same
+      ///  affordance surface.
+            /// \param minArea Minimum area needed for the formation of an affordance object
+            /// \param affordanceName The name of the affordance type
+      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) {}
+       private:
+        const fcl::Vec3f axis45_ = fcl::Vec3f(1./sqrt(2.),0,1./sqrt(2.));
+
+                /// The implementation of the requirement function for Support45 affordances
+                /// overrides the virtual function in class OperationBase.
+                /// Test if the normal in oriented with 45° up +- margin
+                /// \param nromal Normal vector of the tested triangle.
+        bool requirement (const fcl::Vec3f& normal)
+        {
+          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_);
+        }
+    }; // class LeanOperation
+
+
 		/// \} 
   } // namespace affordance
 } // namespace hpp

src/affordance-extraction.cc

@@ -202,6 +202,185 @@ namespace hpp {
       return affObjs;
     }
 
+
+
+    /// isLeft(): tests if a point is Left|On|Right of an infinite line.
+    /// \param lA 1st point of the line
+    /// \param lB 2nd point of the line
+    /// \param p2 point to test
+    /// \return: >0 for p2 left of the line through p0 and p1
+    ///          =0 for p2 on the line
+    ///          <0 for p2 right of the line
+    /// See: the January 2001 Algorithm "Area of 2D and 3D Triangles and Polygons"
+    double isLeft(fcl::Vec3f lA, fcl::Vec3f lB, fcl::Vec3f p2)
+    {
+      return (lB[0] - lA[0]) * (p2[1] - lA[1]) - (p2[0] - lA[0]) * (lB[1] - lA[1]);
+    }
+
+    inline bool isTop(fcl::Vec3f A,fcl::Vec3f B){
+        return A[1]>B[1];
+    }
+
+
+
+    // return the index of the left most points of points
+    size_t leftTopMost(std::vector<fcl::Vec3f> points)
+    {
+      size_t res=0;
+      for(size_t id=1;id<points.size();++id)
+      {
+          if(points[id][0] < points[res][0]){
+              res=id;
+          }
+          else if (points[id][0] == points[res][0]){ // take the top most
+            if(points[id][1] > points[res][1]){
+                res=id;
+            }
+          }
+      }
+      return res;
+    }
+
+
+    // return a vector containing the order of the index of points, such that the vertices are in clockwise order
+    std::vector<size_t> convexHull(const std::vector<fcl::Vec3f> points)
+    {
+
+        std::vector<size_t> res;
+        size_t id_pointOnHull = leftTopMost(points);
+        size_t id_lastPoint = 0;
+       // std::cout<<"leftTopMost : "<<points[id_pointOnHull]<<" id = "<<id_pointOnHull<<std::endl;
+        do {
+            id_lastPoint = 0;
+            //std::cout<<"On Hull : "<<points[id_pointOnHull]<<" id = "<<id_pointOnHull<<std::endl;
+           // std::cout<<"last point : "<<points[id_lastPoint]<<" id = "<<id_lastPoint<<std::endl;
+            for(size_t id_current = 1 ; id_current < points.size();++id_current)
+            {
+                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
+                    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;
+                }
+            }
+
+            //std::cout<<"new last point : "<<points[id_lastPoint]<<" id = "<<id_lastPoint<<std::endl;
+            res.push_back(id_pointOnHull);
+            id_pointOnHull = id_lastPoint;
+        } while(id_lastPoint != res[0]);
+        //res.insert(res.end(), lastPoint);
+       // std::cout<<"points size = "<<points.size()<<" orderedlist size = "<<res.size()<<std::endl;
+        if(points.size() != res.size()){
+            std::ostringstream oss("Error while sorting vertices of affordances");
+            throw std::runtime_error (oss.str ());
+        }
+        return res;
+    }
+
+
+
+    // same as getAffordanceObject but apply a reduction of a given size on the mesh
+    std::vector<CollisionObjects_t> getReducedAffordanceObjects
+    (const SemanticsDataPtr_t& sData,std::vector<double> reduceSizes)
+    {
+        if(reduceSizes.size() != sData->affordances_.size ()){
+            std::ostringstream oss("Error : the vector 'reduceSizes' must have one element per affordance types");
+            throw std::runtime_error (oss.str ());
+        }
+        std::vector<CollisionObjects_t> affObjs;
+        affObjs.clear();
+        for (unsigned int affIdx = 0; affIdx < sData->affordances_.size (); affIdx ++) { // for all affordance type
+            std::vector<fcl::CollisionObjectPtr_t> objVec;
+            objVec.clear();
+            affObjs.push_back (objVec);
+            // get number of affordances of specific type (lean OR support etc)
+            // this corresponds to number of objects to be created for specific aff type
+            long unsigned int len = sData->affordances_[affIdx].size ();
+            for (unsigned int idx = 0; idx < len; idx++) { // for all affordances of a given type
+                std::vector<fcl::Vec3f> vertices;
+                std::vector<fcl::Triangle> triangles;
+                std::vector<std::size_t> triIndices;
+                triIndices.clear ();
+                hpp::affordance::AffordancePtr_t affPtr = sData->affordances_[affIdx][idx];   // current affordance object to copy
+                BVHModelOBConst_Ptr_t model =  GetModel (affPtr->colObj_);
+                for (unsigned int triIdx = 0; triIdx <  affPtr->indices_.size (); triIdx++) {
+                    // give triangles of new object new vertex indices (start from 0
+                    // and go up to 3*nbTris - 1 [all tris have 3 unique indices])
+                    std::vector<std::size_t> triPoints;
+                    const fcl::Triangle& refTri = model->tri_indices[affPtr->indices_[triIdx]];
+                    for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
+                        std::vector<std::size_t>::iterator it = std::find (triIndices.begin (), triIndices.end (), refTri[vertIdx]);
+                        if (it == triIndices.end ()) {
+                            triIndices.push_back (refTri[vertIdx]);
+                            vertices.push_back (model->vertices [refTri[vertIdx]]);
+                            triPoints.push_back (std::size_t (vertices.size () -1));
+                        } else {
+                            triPoints.push_back (it - triIndices.begin ());
+                        }
+                    }
+                    if (triPoints.size () != 3) {
+                        std::ostringstream oss("wrong number of vertex indices in triangle!!");
+                        throw std::runtime_error (oss.str ());
+                    }
+                    triangles.push_back (fcl::Triangle (triPoints[0], triPoints[1], triPoints[2]));
+                }
+
+                if(reduceSizes[affIdx]>std::numeric_limits<double>::epsilon()){
+
+                    //std::cout<<"For points of affordance : "<<std::endl;
+                    for(unsigned int id=0;id<vertices.size();++id){
+                        std::cout<<"( "<<vertices[id][0]<<" , "<<vertices[id][1]<<" , "<<vertices[id][2]<<" ) "<<std::endl;
+                    }
+
+                    //first we need to sort the vertices in clockwise order.
+                    // but as we cannot change the order in the 'vertices' vector we use another vector for correspondance of index
+                    std::vector<size_t> orderedIndex = convexHull(vertices); // this vector contain the index of 'vertices' vertice in a counter-clockwise order.
+
+                    // shift each vertice of 'reduceSize' along the vector defined by : unit(next-current)+unit(prev-current)
+                    fcl::Vec3f prev,next,A,B,dir;
+                    std::vector<fcl::Vec3f> shiftedVertices(vertices.size());
+                    for(unsigned int id=0;id<orderedIndex.size();++id){
+                        if(id==0)
+                            prev=vertices[orderedIndex[orderedIndex.size()-1]];
+                        else
+                            prev=vertices[orderedIndex[id-1]];
+                        if(id==(orderedIndex.size()-1))
+                            next=vertices[orderedIndex[0]];
+                        else
+                            next=vertices[orderedIndex[id+1]];
+
+                        A=(next-vertices[orderedIndex[id]]).normalize();
+                        B=(prev-vertices[orderedIndex[id]]).normalize();
+                        dir=(A+B).normalize();
+                     /*   if(dir.length() < std::numeric_limits<double>::epsilon()){ // this mean that prev, current and next are aligned, we make a 90° angle
+                            dir=
+                        }*/
+                        fcl::Vec3f shiftedVertice = vertices[orderedIndex[id]] + ((dir)*reduceSizes[affIdx]);
+                        shiftedVertices[orderedIndex[id]] = shiftedVertice;
+                    }
+                    vertices=shiftedVertices;
+                }
+
+
+
+                BVHModelOB_Ptr_t model1 (new BVHModelOB ());
+                // add the mesh data into the BVHModel structure
+                model1->beginModel ();
+                model1->addSubModel (vertices, triangles);
+                model1->endModel ();
+
+
+
+                // create affordance collision object from created affModel and
+                // tranformation of corresponding reference collision object.
+                fcl::CollisionObjectPtr_t obj (new fcl::CollisionObject(model1,affPtr->colObj_->getTransform ()));
+                affObjs[affIdx].push_back (obj);
+            }
+        }
+        return affObjs;
+    }
+
   } // namespace affordance
 } // namespace hpp