solid-solid-collision.hh

Go to the documentation of this file.

//
// Copyright (c) 2014,2015,2016,2018 CNRS
// Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH
# define HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH

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

namespace hpp {
  namespace core {
    namespace continuousValidation {
      struct CoefficientVelocity
      {
          CoefficientVelocity () : value_ (0)
          {
        }
        JointPtr_t joint_;
        value_type value_;
      }; // struct CoefficientVelocity
      typedef std::vector <CoefficientVelocity> CoefficientVelocities_t;

      class HPP_CORE_DLLAPI SolidSolidCollision : public BodyPairCollision
      {
      public:
        static SolidSolidCollisionPtr_t create (const JointPtr_t& joint_a,
                          const ConstObjectStdVector_t& objects_b,
                          value_type tolerance);

        static SolidSolidCollisionPtr_t create (const JointPtr_t& joint_a,
                          const JointPtr_t& joint_b,
                          value_type tolerance);

        static SolidSolidCollisionPtr_t createCopy
          (const SolidSolidCollisionPtr_t& other);

        value_type computeMaximalVelocity(vector_t& Vb) const;

        bool removeObjectTo_b (const CollisionObjectConstPtr_t& object);

    std::string name () const;

    std::ostream& print (std::ostream& os) const;

        void addCollisionPair (const CollisionObjectConstPtr_t& left,
            const CollisionObjectConstPtr_t &right);

        // Get coefficients and joints
        const CoefficientVelocities_t& coefficients () const
        {
          return m_->coefficients;
        }

        const JointPtr_t& joint_a () const
        {
          return m_->joint_a;
        }
        const JointPtr_t& joint_b () const
        {
          return m_->joint_b;
        }

        size_type indexJointA () const
        {
          return (m_->joint_a ? m_->joint_a->index() : 0);
        }
        size_type indexJointB () const
        {
          return (m_->joint_b ? m_->joint_b->index() : 0);
        }

        IntervalValidationPtr_t copy () const;

      protected:
        SolidSolidCollision (const JointPtr_t& joint_a,
              const JointPtr_t& joint_b,
              value_type tolerance);

        SolidSolidCollision (const JointPtr_t& joint_a,
              const ConstObjectStdVector_t& objects_b,
              value_type tolerance);

        SolidSolidCollision (const SolidSolidCollision& other)
          : BodyPairCollision (other), m_ (other.m_)
        {}

        void init(const SolidSolidCollisionWkPtr_t& weak);
      private:
        typedef pinocchio::JointIndex JointIndex;
        typedef std::vector<JointIndex> JointIndices_t;

        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_;
        SolidSolidCollisionWkPtr_t weak_;
      }; // class SolidSolidCollision
    } // namespace continuousValidation
  } // namespace core
} // namespace hpp
#endif // HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH