[ContinuousValidation] Split pairs into model and data

include/hpp/core/continuous-validation/body-pair-collision.hh

@@ -66,7 +66,13 @@ namespace hpp {
         // Get pairs checked for collision
         const CollisionPairs_t& pairs () const
         {
-          return pairs_;
+          return m_->pairs;
+        }
+
+        // Get pairs checked for collision
+        CollisionPairs_t& pairs ()
+        {
+          return m_->pairs;
         }
 
         // Get maximal velocity
@@ -87,19 +93,17 @@ namespace hpp {
         virtual std::ostream& print (std::ostream& os) const = 0;
 
       protected:
-        mutable vector_t Vb_;
-        CollisionPairs_t pairs_;
         /// Constructor of body pair collision
         ///
         /// \param tolerance allowed penetration should be positive
         BodyPairCollision (value_type tolerance):
-          IntervalValidation(tolerance), pairs_ (), maximalVelocity_(0)
+          IntervalValidation(tolerance), m_ (new Model), maximalVelocity_(0)
         {
         }
 
         virtual void setReport (CollisionValidationReportPtr_t& report,
                             fcl::CollisionResult result,
-                            CollisionPair_t _pair)
+                            CollisionPair_t _pair) const
         {
           report = CollisionValidationReportPtr_t
             (new CollisionValidationReport);
@@ -109,6 +113,12 @@ namespace hpp {
         }
 
       private:
+        struct Model {
+          CollisionPairs_t pairs;
+        };
+        boost::shared_ptr<Model> m_;
+
+        mutable vector_t Vb_;
         value_type maximalVelocity_;
 
         /// Compute maximal velocity for a given velocity bound
@@ -133,7 +143,7 @@ namespace hpp {
         /// \retval report the collision validation report
         /// \return true if the bodies are not in collision, else false
         bool computeDistanceLowerBound(value_type &distanceLowerBound,
-          CollisionValidationReportPtr_t& report, pinocchio::DeviceData& data);
+          CollisionValidationReportPtr_t& report, pinocchio::DeviceData& data) const;
 
       }; // class BodyPairCollision
 

include/hpp/core/continuous-validation/solid-solid-collision.hh

@@ -85,29 +85,29 @@ namespace hpp {
         // Get coefficients and joints
         const CoefficientVelocities_t& coefficients () const
         {
-          return coefficients_;
+          return m_->coefficients;
         }
 
         /// Get joint a
         const JointPtr_t& joint_a () const
         {
-          return joint_a_;
+          return m_->joint_a;
         }
         /// Get joint b
         const JointPtr_t& joint_b () const
         {
-          return joint_b_;
+          return m_->joint_b;
         }
 
         /// Returns joint A index or -1 if no such joint exists.
         size_type indexJointA () const
         {
-          return (joint_a_ ? joint_a_->index() : 0);
+          return (m_->joint_a ? m_->joint_a->index() : 0);
         }
         /// Returns joint B index or -1 if no such joint exists.
         size_type indexJointB () const
         {
-          return (joint_b_ ? joint_b_->index() : 0);
+          return (m_->joint_b ? m_->joint_b->index() : 0);
         }
 
       protected:
@@ -133,11 +133,15 @@ namespace hpp {
       private:
         typedef pinocchio::JointIndex JointIndex;
         typedef std::vector<JointIndex> JointIndices_t;
-        JointPtr_t joint_a_;
-        JointPtr_t joint_b_;
-        CoefficientVelocities_t coefficients_;
-        JointIndices_t computeSequenceOfJoints () const;
-        void computeCoefficients (const JointIndices_t& joints);
+
+        struct Model {
+          JointPtr_t joint_a;
+          JointPtr_t joint_b;
+          CoefficientVelocities_t coefficients;
+          JointIndices_t computeSequenceOfJoints () const;
+          void computeCoefficients (const JointIndices_t& joints);
+        };
+        boost::shared_ptr<Model> m_;
       }; // class SolidSolidCollision
     } // namespace continuousValidation
   } // namespace core

src/continuous-validation/body-pair-collision.cc

@@ -144,14 +144,15 @@ namespace hpp {
 
       bool BodyPairCollision::computeDistanceLowerBound(value_type &distanceLowerBound,
         CollisionValidationReportPtr_t& report,
-        pinocchio::DeviceData& data)
+        pinocchio::DeviceData& data) const
       {
         using std::numeric_limits;
         distanceLowerBound = numeric_limits <value_type>::infinity ();
         static const fcl::CollisionRequest request (fcl::DISTANCE_LOWER_BOUND, 1);
         assert (request.enable_distance_lower_bound == true);
-        for (CollisionPairs_t::const_iterator _pair = pairs_.begin();
-            _pair != pairs_.end(); ++_pair) {
+        const CollisionPairs_t& prs (pairs());
+        for (CollisionPairs_t::const_iterator _pair = prs.begin();
+            _pair != prs.end(); ++_pair) {
           pinocchio::FclConstCollisionObjectPtr_t object_a = _pair->first ->fcl (data);
           pinocchio::FclConstCollisionObjectPtr_t object_b = _pair->second->fcl (data);
           fcl::CollisionResult result;

src/continuous-validation/solid-solid-collision.cc

@@ -54,7 +54,7 @@ namespace hpp {
       {
         value_type maximalVelocity = 0;
         for (CoefficientVelocities_t::const_iterator itCoef =
-        coefficients_.begin (); itCoef != coefficients_.end (); ++itCoef) {
+        m_->coefficients.begin (); itCoef != m_->coefficients.end (); ++itCoef) {
           const JointPtr_t& joint = itCoef->joint_;
           const value_type& value = itCoef->value_;
           maximalVelocity += value * Vb.segment(joint->rankInVelocity(), joint->numberDof()).norm();
@@ -64,30 +64,31 @@ namespace hpp {
 
       bool SolidSolidCollision::removeObjectTo_b (const CollisionObjectConstPtr_t& object)
       {
-        const std::size_t s = pairs_.size();
-        for (CollisionPairs_t::iterator _pair = pairs_.begin ();
-          _pair != pairs_.end ();) {
+        CollisionPairs_t& prs (pairs());
+        const std::size_t s = prs.size();
+        for (CollisionPairs_t::iterator _pair = prs.begin ();
+          _pair != prs.end ();) {
           if (object == _pair->second)
-            _pair = pairs_.erase (_pair);
+            _pair = prs.erase (_pair);
           else
             ++_pair;
         }
-        return pairs_.size() < s;
+        return prs.size() < s;
       }
 
       void SolidSolidCollision::addCollisionPair (const CollisionObjectConstPtr_t& left,
           const CollisionObjectConstPtr_t& right)
       {
-        // std::cout << "size = " << pairs_.size() << std::endl;
-        // std::cout << "capacity = " << pairs_.capacity() << std::endl;
-        pairs_.push_back (CollisionPair_t (left, right));
+        // std::cout << "size = " << pairs().size() << std::endl;
+        // std::cout << "capacity = " << pairs().capacity() << std::endl;
+        pairs().push_back (CollisionPair_t (left, right));
       }
 
       std::string SolidSolidCollision::name () const
       {
         std::ostringstream oss;
-        oss << "(" << joint_a_->name () << ",";
-        if (joint_b_) oss << joint_b_->name ();
+        oss << "(" << m_->joint_a->name () << ",";
+        if (m_->joint_b) oss << m_->joint_b->name ();
         else oss << "obstacles";
         oss << ")";
         return oss.str ();
@@ -95,25 +96,25 @@ namespace hpp {
 
       std::ostream& SolidSolidCollision::print (std::ostream& os) const
       {
-        os << "SolidSolidCollision: " << joint_a_->name()
-          << " - " << (joint_b_ ? joint_b_->name() : "World") << '\n';
+        os << "SolidSolidCollision: " << m_->joint_a->name()
+          << " - " << (m_->joint_b ? m_->joint_b->name() : "World") << '\n';
         const pinocchio::Model& model = joint_a_->robot ()->model();
-        JointIndices_t joints = computeSequenceOfJoints ();
+        JointIndices_t joints = m_->computeSequenceOfJoints ();
         for (std::size_t i = 0; i < joints.size (); ++i) {
           if (i > 0) os << model.names[i] << ',';
           else       os << "World"        << ',';
         }
         os << '\n';
-        for (std::size_t i = 0; i < coefficients_.size(); ++i)
-          os << coefficients_[i].value_ << ", ";
+        for (std::size_t i = 0; i < m_->coefficients.size(); ++i)
+          os << m_->coefficients[i].value_ << ", ";
         return os;
       }
 
-      SolidSolidCollision::JointIndices_t SolidSolidCollision::computeSequenceOfJoints () const
+      SolidSolidCollision::JointIndices_t SolidSolidCollision::Model::computeSequenceOfJoints () const
       {
         JointIndices_t joints;
 
-        const pinocchio::Model& model = joint_a_->robot ()->model();
+        const pinocchio::Model& model = joint_a->robot ()->model();
         const JointIndex id_a = indexJointA(),
                          id_b = indexJointB();
         JointIndex ia = id_a, ib = id_b;
@@ -145,16 +146,16 @@ namespace hpp {
         return joints;
       }
 
-      void SolidSolidCollision::computeCoefficients (const JointIndices_t& joints)
+      void SolidSolidCollision::Model::computeCoefficients (const JointIndices_t& joints)
       {
-        const pinocchio::Model& model = joint_a_->robot ()->model();
+        const pinocchio::Model& model = joint_a->robot ()->model();
 
         JointPtr_t child;
         assert (joints.size () > 1);
-        coefficients_.resize (joints.size () - 1);
-        pinocchio::DevicePtr_t robot =joint_a_->robot ();
+        coefficients.resize (joints.size () - 1);
+        pinocchio::DevicePtr_t robot =joint_a->robot ();
         // Store r0 + sum of T_{i/i+1} in a variable
-        value_type cumulativeLength = joint_a_->linkedBody ()->radius ();
+        value_type cumulativeLength = joint_a->linkedBody ()->radius ();
         value_type distance;
         std::size_t i = 0;
         while (i + 1 < joints.size()) {
@@ -165,11 +166,11 @@ namespace hpp {
           else
             abort ();
           assert(child);
-          coefficients_ [i].joint_ = child;
+          coefficients [i].joint_ = child;
           // Go through all known types of joints
           //  TODO: REPLACE THESE FUNCTIONS WITH NEW API
           distance = child->maximalDistanceToParent ();
-          coefficients_ [i].value_ =
+          coefficients [i].value_ =
             child->upperBoundLinearVelocity () +
             cumulativeLength * child->upperBoundAngularVelocity ();
           cumulativeLength += distance;
@@ -181,11 +182,13 @@ namespace hpp {
       SolidSolidCollision::SolidSolidCollision (const JointPtr_t& joint_a,
             const JointPtr_t& joint_b,
             value_type tolerance) :
-        BodyPairCollision(tolerance), joint_a_ (joint_a), joint_b_ (joint_b),
-        coefficients_ ()
+        BodyPairCollision(tolerance), m_ (new Model)
       {
+        m_->joint_a = joint_a;
+        m_->joint_b = joint_b;
+
         assert (joint_a);
-        if (joint_b && joint_b_->robot () != joint_a_->robot ()) {
+        if (joint_b && joint_b->robot () != joint_a->robot ()) {
           throw std::runtime_error
             ("Joints do not belong to the same device.");
         }
@@ -196,34 +199,35 @@ namespace hpp {
           throw std::runtime_error ("tolerance should be non-negative.");
         }
 
-        if (joint_a_) { assert(joint_a_->linkedBody ()); }
-        if (joint_b_) { assert(joint_b_->linkedBody ()); }
+        if (joint_a_) { assert(joint_a->linkedBody ()); }
+        if (joint_b_) { assert(joint_b->linkedBody ()); }
         // Find sequence of joints
-        JointIndices_t joints (computeSequenceOfJoints ());
-        computeCoefficients (joints);
+        JointIndices_t joints (m_->computeSequenceOfJoints ());
+        m_->computeCoefficients (joints);
       }
 
       SolidSolidCollision::SolidSolidCollision (const JointPtr_t& joint_a,
             const ConstObjectStdVector_t& objects_b,
             value_type tolerance) :
-        BodyPairCollision(tolerance), joint_a_ (joint_a), joint_b_ (),
-        coefficients_ ()
+        BodyPairCollision(tolerance), m_ (new Model)
       {
+        m_->joint_a = joint_a;
+
         assert (joint_a);
-        BodyPtr_t body_a = joint_a_->linkedBody ();
+        BodyPtr_t body_a = joint_a->linkedBody ();
         assert (body_a);
         for (size_type i = 0; i < body_a->nbInnerObjects(); ++i) {
 	      CollisionObjectConstPtr_t obj = body_a->innerObjectAt(i);
           for (ConstObjectStdVector_t::const_iterator it = objects_b.begin ();
               it != objects_b.end (); ++it) {
             assert (!(*it)->joint () ||
-                (*it)->joint ()->robot () != joint_a_->robot ());
-            pairs_.push_back (CollisionPair_t(obj, *it));
+                (*it)->joint ()->robot () != joint_a->robot ());
+            pairs().push_back (CollisionPair_t(obj, *it));
           }
         }
         // Find sequence of joints
-        JointIndices_t joints (computeSequenceOfJoints ());
-        computeCoefficients (joints);
+        JointIndices_t joints (m_->computeSequenceOfJoints ());
+        m_->computeCoefficients (joints);
       }
 
     } // namespace continuousValidation