geometry.hpp

//
// Copyright (c) 2015-2020 CNRS INRIA
//

#ifndef __pinocchio_algo_geometry_hpp__
#define __pinocchio_algo_geometry_hpp__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"

#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/multibody/geometry.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                       const GeometryModel & geom_model,
                                       GeometryData & geom_data,
                                       const Eigen::MatrixBase<ConfigVectorType> & q);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                       const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                       const GeometryModel & geom_model,
                                       GeometryData & geom_data);

  template<typename Vector3Like>
  PINOCCHIO_DEPRECATED
  inline void setGeometryMeshScales(GeometryModel & geom_model, const Eigen::MatrixBase<Vector3Like> & meshScale)
  {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
    for(GeomIndex index=0; index<geom_model.ngeoms; index++)
      geom_model.geometryObjects[index].meshScale = meshScale;
  }

  PINOCCHIO_DEPRECATED
  inline void setGeometryMeshScales(GeometryModel & geom_model, const double meshScale)
  {
    for(GeomIndex index=0; index<geom_model.ngeoms; index++)
      geom_model.geometryObjects[index].meshScale.setConstant(meshScale);
  }

#ifdef PINOCCHIO_WITH_HPP_FCL

  bool computeCollision(const GeometryModel & geom_model,
                        GeometryData & geom_data,
                        const PairIndex & pairId);

  inline bool computeCollisions(const GeometryModel & geom_model,
                                GeometryData & geom_data,
                                const bool stopAtFirstCollision = false);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline bool computeCollisions(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const GeometryModel & geom_model,
                                GeometryData & geom_data,
                                const Eigen::MatrixBase<ConfigVectorType> & q,
                                const bool stopAtFirstCollision = false);

  fcl::DistanceResult & computeDistance(const GeometryModel & geom_model,
                                        GeometryData & geom_data,
                                        const PairIndex & pairId);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const GeometryModel & geom_model,
                                      GeometryData & geom_data,
                                      const Eigen::MatrixBase<ConfigVectorType> & q);
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const GeometryModel & geom_model,
                                      GeometryData & geom_data);

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void computeBodyRadius(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const GeometryModel & geom_model,
                                GeometryData & geom_data);
#endif // PINOCCHIO_WITH_HPP_FCL

  inline void appendGeometryModel(GeometryModel & geom_model1,
                                  const GeometryModel & geom_model2);

} // namespace pinocchio 

/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/geometry.hxx"

#endif // ifndef __pinocchio_algo_geometry_hpp__