geometry.hpp

//
// Copyright (c) 2015-2021 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 pair_id);
 
  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 pair_id);
  
  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__