multibody/geometry.hpp

//
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio 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.
//
// Pinocchio 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
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_geom_hpp__
#define __se3_geom_hpp__


#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/container/aligned-vector.hpp"

#include <iostream>

#include <boost/foreach.hpp>
#include <map>
#include <list>
#include <utility>
#include <assert.h>


namespace se3
{
  
  struct GeometryModel
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    Index ngeoms;

    container::aligned_vector<GeometryObject> geometryObjects;
    std::vector<CollisionPair> collisionPairs;
  
    GeometryModel()
      : ngeoms(0)
      , geometryObjects()
      , collisionPairs()
    { 
      const std::size_t num_max_collision_pairs = (ngeoms * (ngeoms-1))/2;
      collisionPairs.reserve(num_max_collision_pairs);
    }

    ~GeometryModel() {};

    inline GeomIndex addGeometryObject(GeometryObject object,
                                       const Model & model,
                                       const bool autofillJointParent = false);

    GeomIndex getGeometryId(const std::string & name) const;

    
    bool existGeometryName(const std::string & name) const;


    PINOCCHIO_DEPRECATED const std::string & getGeometryName(const GeomIndex index) const;

#ifdef WITH_HPP_FCL
    void addCollisionPair (const CollisionPair & pair);
    
    void addAllCollisionPairs();
   
    void removeCollisionPair (const CollisionPair& pair);
    
    void removeAllCollisionPairs ();
   
    bool existCollisionPair (const CollisionPair & pair) const;
    
    PairIndex findCollisionPair (const CollisionPair & pair) const;
    
#endif // WITH_HPP_FCL

    friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
  }; // struct GeometryModel

  struct GeometryData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    container::aligned_vector<se3::SE3> oMg;

#ifdef WITH_HPP_FCL
    std::vector<fcl::CollisionObject> collisionObjects;

    std::vector<bool> activeCollisionPairs;

    fcl::DistanceRequest distanceRequest;

    std::vector <fcl::DistanceResult> distanceResults;
    
    fcl::CollisionRequest collisionRequest;

    std::vector <fcl::CollisionResult> collisionResults;

    std::vector<double> radius;

    PairIndex collisionPairIndex;

    typedef std::vector<GeomIndex> GeomIndexList;

    std::map < JointIndex, GeomIndexList >  innerObjects;

    std::map < JointIndex, GeomIndexList >  outerObjects;
#endif // WITH_HPP_FCL   

    GeometryData(const GeometryModel & geomModel);
    ~GeometryData() {};

#ifdef WITH_HPP_FCL

    void fillInnerOuterObjectMaps(const GeometryModel & geomModel);

    void activateCollisionPair(const PairIndex pairId,const bool flag=true);

    void deactivateCollisionPair(const PairIndex pairId);

#endif //WITH_HPP_FCL
    friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
    
  }; // struct GeometryData

} // namespace se3

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

#endif // ifndef __se3_geom_hpp__