geometry.hxx 8.57 KB
Newer Older
1
//
2
// Copyright (c) 2015-2017 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// 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/>.

jcarpent's avatar
jcarpent committed
18
19
#ifndef __se3_geometry_hxx__
#define __se3_geometry_hxx__
20
21
22
23
24
25

#include <iostream>
#include <map>
#include <list>
#include <utility>

jcarpent's avatar
jcarpent committed
26
/// @cond DEV
27
28
29

namespace se3
{
30
  inline GeometryData::GeometryData(const GeometryModel & modelGeom)
31
    : oMg(modelGeom.ngeoms)
32

33
#ifdef WITH_HPP_FCL
34
35
    , activeCollisionPairs(modelGeom.collisionPairs.size(), true)
    , distanceRequest (true, 0, 0, fcl::GST_INDEP)
36
    , distanceResults(modelGeom.collisionPairs.size())
37
    , collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP)
38
    , collisionResults(modelGeom.collisionPairs.size())
39
    , radius()
40
    , collisionPairIndex(0)
41
42
43
44
45
46
    , innerObjects()
    , outerObjects()
  {
    collisionObjects.reserve(modelGeom.geometryObjects.size());
    BOOST_FOREACH( const GeometryObject & geom, modelGeom.geometryObjects)
      { collisionObjects.push_back
47
          (fcl::CollisionObject(geom.fcl)); }
48
    fillInnerOuterObjectMaps(modelGeom);
49
50
51
52
  }
#else
  {}
#endif // WITH_HPP_FCL   
53

54
  inline GeomIndex GeometryModel::addGeometryObject(GeometryObject object,
55
56
                                                    const Model & model,
                                                    const bool autofillJointParent)
57
  {
58
    // TODO reenable when relevant: assert( (object.parentFrame != -1) || (object.parentJoint != -1) );
59

60
61
    if( autofillJointParent )
      // TODO: this might be automatically done for some default value of parentJoint (eg ==-1)
62
63
      object.parentJoint = model.frames[object.parentFrame].parent; 

64
65
66
67
    assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
           (model.frames[object.parentFrame].type == se3::BODY)  );
    assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
           (model.frames[object.parentFrame].parent == object.parentJoint) );
68

69
    GeomIndex idx = (GeomIndex) (ngeoms ++);
70
71
72
73
    geometryObjects.push_back(object);
    return idx;
  }

74
  inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const
75
  {
76

77
    container::aligned_vector<GeometryObject>::const_iterator it = std::find_if(geometryObjects.begin(),
78
                                                                  geometryObjects.end(),
79
80
                                                                  boost::bind(&GeometryObject::name, _1) == name
                                                                  );
81
    return GeomIndex(it - geometryObjects.begin());
82
83
  }

84

85

86
  inline bool GeometryModel::existGeometryName(const std::string & name) const
87
  {
88
89
90
    return std::find_if(geometryObjects.begin(),
                        geometryObjects.end(),
                        boost::bind(&GeometryObject::name, _1) == name) != geometryObjects.end();
91
92
93
  }


94
  inline const std::string& GeometryModel::getGeometryName(const GeomIndex index) const
95
  {
96
97
    assert( index < (GeomIndex)geometryObjects.size() );
    return geometryObjects[index].name;
98
99
  }

100

101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
    /**
     * @brief      Associate a GeometryObject of type COLLISION to a joint's inner objects list
     *
     * @param[in]  joint         Index of the joint
     * @param[in]  inner_object  Index of the GeometryObject that will be an inner object
     */
  // inline void GeometryModel::addInnerObject(const JointIndex joint_id, const GeomIndex inner_object)
  // {
  //   if (std::find(innerObjects[joint_id].begin(),
  //                 innerObjects[joint_id].end(),
  //                 inner_object) == innerObjects[joint_id].end())
  //     innerObjects[joint_id].push_back(inner_object);
  //   else
  //     std::cout << "inner object already added" << std::endl;
  // }

    /**
     * @brief      Associate a GeometryObject of type COLLISION to a joint's outer objects list
     *
     * @param[in]  joint         Index of the joint
     * @param[in]  inner_object  Index of the GeometryObject that will be an outer object
     */
  // inline void GeometryModel::addOutterObject (const JointIndex joint, const GeomIndex outer_object)
  // {
  //   if (std::find(outerObjects[joint].begin(),
  //                 outerObjects[joint].end(),
  //                 outer_object) == outerObjects[joint].end())
  //     outerObjects[joint].push_back(outer_object);
  //   else
  //     std::cout << "outer object already added" << std::endl;
  // }

133
#ifdef WITH_HPP_FCL
134
  inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel)
135
  {
136
137
    innerObjects.clear();
    outerObjects.clear();
138

139
140
    for( GeomIndex gid = 0; gid<geomModel.geometryObjects.size(); gid++)
      innerObjects[geomModel.geometryObjects[gid].parentJoint].push_back(gid);
141

142
    BOOST_FOREACH( const CollisionPair & pair, geomModel.collisionPairs )
143
      {
144
        outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second);
145
      }
146
  }
147
#endif
148

149
  inline std::ostream & operator<< (std::ostream & os, const GeometryModel & geomModel)
150
  {
151
    os << "Nb geometry objects = " << geomModel.ngeoms << std::endl;
152
    
153
    for(GeomIndex i=0;i<(GeomIndex)(geomModel.ngeoms);++i)
154
    {
155
      os  << geomModel.geometryObjects[i] <<std::endl;
156
157
158
159
160
    }

    return os;
  }

161
  inline std::ostream & operator<< (std::ostream & os, const GeometryData & geomData)
162
  {
163
#ifdef WITH_HPP_FCL
164
    os << "Number of collision pairs = " << geomData.activeCollisionPairs.size() << std::endl;
165
    
166
    for(PairIndex i=0;i<(PairIndex)(geomData.activeCollisionPairs.size());++i)
167
    {
168
      os << "Pairs " << i << (geomData.activeCollisionPairs[i]?" active":" inactive") << std::endl;
169
    }
170
#else
171
172
    os << "WARNING** Without fcl library, no collision checking or distance computations are possible. Only geometry placements can be computed." << std::endl;
    os << "Number of geometry objects = " << geomData.oMg.size() << std::endl;
173
#endif
174
175
176
177

    return os;
  }

178
179
#ifdef WITH_HPP_FCL
  
180
  inline void GeometryModel::addCollisionPair (const CollisionPair & pair)
181
  {
182
183
    assert( (pair.first < ngeoms) && (pair.second < ngeoms) );
    if (!existCollisionPair(pair)) { collisionPairs.push_back(pair); }
184
  }
185
  
186
  inline void GeometryModel::addAllCollisionPairs()
187
188
  {
    removeAllCollisionPairs();
189
    for (GeomIndex i = 0; i < ngeoms; ++i)
190
    {
191
      const JointIndex& joint_i = geometryObjects[i].parentJoint;
192
      for (GeomIndex j = i+1; j < ngeoms; ++j)
193
      {
194
195
        const JointIndex& joint_j = geometryObjects[j].parentJoint;
        if (joint_i != joint_j)
196
197
198
          addCollisionPair(CollisionPair(i,j));
      }
    }
199
200
  }
  
201
  inline void GeometryModel::removeCollisionPair (const CollisionPair & pair)
202
  {
203
    assert( (pair.first < ngeoms) && (pair.second < ngeoms) );
204

205
206
    CollisionPairsVector_t::iterator it = std::find(collisionPairs.begin(),
                                                    collisionPairs.end(),
207
                                                    pair);
208
    if (it != collisionPairs.end()) { collisionPairs.erase(it); }
209
  }
210
  
211
  inline void GeometryModel::removeAllCollisionPairs () { collisionPairs.clear(); }
212

213
  inline bool GeometryModel::existCollisionPair (const CollisionPair & pair) const
214
  {
215
216
217
    return (std::find(collisionPairs.begin(),
                      collisionPairs.end(),
                      pair) != collisionPairs.end());
jcarpent's avatar
jcarpent committed
218
219
  }
  
220
  inline PairIndex GeometryModel::findCollisionPair (const CollisionPair & pair) const
jcarpent's avatar
jcarpent committed
221
  {
222
223
    CollisionPairsVector_t::const_iterator it = std::find(collisionPairs.begin(),
                                                          collisionPairs.end(),
224
                                                          pair);
jcarpent's avatar
jcarpent committed
225
    
226
    return (PairIndex) std::distance(collisionPairs.begin(), it);
227
228
  }

229
  inline void GeometryData::activateCollisionPair(const PairIndex pairId,const bool flag)
230
  {
231
232
    assert( pairId < activeCollisionPairs.size() );
    activeCollisionPairs[pairId] = flag;
233
234
  }

235
  inline void GeometryData::deactivateCollisionPair(const PairIndex pairId)
236
  {
237
238
    assert( pairId < activeCollisionPairs.size() );
    activeCollisionPairs[pairId] = false;
239
  }
240

241
#endif //WITH_HPP_FCL
242
243
} // namespace se3

jcarpent's avatar
jcarpent committed
244
245
/// @endcond

jcarpent's avatar
jcarpent committed
246
#endif // ifndef __se3_geometry_hxx__