collision_data.h 12.3 KB
Newer Older
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */


sachinc's avatar
sachinc committed
39
40
41
#ifndef FCL_COLLISION_DATA_H
#define FCL_COLLISION_DATA_H

42
#include <hpp/fcl/collision_object.h>
panjia1983's avatar
panjia1983 committed
43

44
#include <hpp/fcl/math/vec_3f.h>
sachinc's avatar
sachinc committed
45
#include <vector>
46
#include <set>
jpan's avatar
jpan committed
47
#include <limits>
sachinc's avatar
sachinc committed
48
49
50
51
52


namespace fcl
{

53
/// @brief Type of narrow phase GJK solver
54
enum GJKSolverType {GST_INDEP};
55

jpan's avatar
jpan committed
56
/// @brief Contact information returned by collision
sachinc's avatar
sachinc committed
57
58
struct Contact
{
jpan's avatar
jpan committed
59
  /// @brief collision object 1
60
  const CollisionGeometry* o1;
jpan's avatar
jpan committed
61
62

  /// @brief collision object 2
63
  const CollisionGeometry* o2;
jpan's avatar
jpan committed
64
65
66
67
68

  /// @brief contact primitive in object 1
  /// if object 1 is mesh or point cloud, it is the triangle or point id
  /// if object 1 is geometry shape, it is NONE (-1),
  /// if object 1 is octree, it is the id of the cell
sachinc's avatar
sachinc committed
69
  int b1;
jpan's avatar
jpan committed
70
71
72
73
74
75


  /// @brief contact primitive in object 2
  /// if object 2 is mesh or point cloud, it is the triangle or point id
  /// if object 2 is geometry shape, it is NONE (-1),
  /// if object 2 is octree, it is the id of the cell
sachinc's avatar
sachinc committed
76
  int b2;
jpan's avatar
jpan committed
77
78
79
80
81
82
83
84
85
86
87
88
 
  /// @brief contact normal, pointing from o1 to o2
  Vec3f normal;

  /// @brief contact position, in world space
  Vec3f pos;

  /// @brief penetration depth
  FCL_REAL penetration_depth;

 
  /// @brief invalid contact primitive information
jpan's avatar
   
jpan committed
89
  static const int NONE = -1;
sachinc's avatar
sachinc committed
90

jpan's avatar
jpan committed
91
92
93
94
95
  Contact() : o1(NULL),
              o2(NULL),
              b1(NONE),
              b2(NONE)
  {}
sachinc's avatar
sachinc committed
96

jpan's avatar
jpan committed
97
98
99
100
101
  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
                                                                                          o2(o2_),
                                                                                          b1(b1_),
                                                                                          b2(b2_)
  {}
sachinc's avatar
sachinc committed
102

103
  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
jpan's avatar
jpan committed
104
105
106
107
108
109
110
111
          const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
                                                                      o2(o2_),
                                                                      b1(b1_),
                                                                      b2(b2_),
                                                                      normal(normal_),
                                                                      pos(pos_),
                                                                      penetration_depth(depth_)
  {}
jpan's avatar
   
jpan committed
112
113
114
115
116
117
118

  bool operator < (const Contact& other) const
  {
    if(b1 == other.b1)
      return b2 < other.b2;
    return b1 < other.b1;
  }
119
120
121
122
123
124
125
126
127
128
129

  bool operator == (const Contact& other) const
  {
    return o1 == other.o1
            && o2 == other.o2
            && b1 == other.b1
            && b2 == other.b2
            && normal == other.normal
            && pos == other.pos
            && penetration_depth == other.penetration_depth;
  }
sachinc's avatar
sachinc committed
130
131
};

132
133
struct CollisionResult;

jpan's avatar
jpan committed
134
/// @brief request to the collision algorithm
135
struct CollisionRequest
136
{  
jpan's avatar
jpan committed
137
  /// @brief The maximum number of contacts will return
jpan's avatar
jpan committed
138
  size_t num_max_contacts;
jpan's avatar
jpan committed
139
140

  /// @brief whether the contact information (normal, penetration depth and contact position) will return
141
  bool enable_contact;
jpan's avatar
jpan committed
142

143
144
145
  /// Whether a lower bound on distance is returned when objects are disjoint
  bool enable_distance_lower_bound;

146
147
148
149
150
151
152
153
154
  /// @brief narrow phase solver
  GJKSolverType gjk_solver_type;

  /// @brief whether enable gjk intial guess
  bool enable_cached_gjk_guess;
  
  /// @brief the gjk intial guess set by user
  Vec3f cached_gjk_guess;

155
156
157
158
159
160
  /// @brief Distance below which objects are considered in collision
  FCL_REAL security_margin;

  /// @brief Distance below which bounding volumes are break down
  FCL_REAL break_distance;

jpan's avatar
jpan committed
161
  CollisionRequest(size_t num_max_contacts_ = 1,
162
                   bool enable_contact_ = false,
163
		   bool enable_distance_lower_bound_ = false,
jpan's avatar
jpan committed
164
                   size_t num_max_cost_sources_ = 1,
jpan's avatar
   
jpan committed
165
                   bool enable_cost_ = false,
166
                   bool use_approximate_cost_ = true,
167
168
169
170
171
172
                   GJKSolverType gjk_solver_type_ = GST_INDEP)
  HPP_FCL_DEPRECATED;

  CollisionRequest(bool enable_contact_, size_t num_max_contacts_,
		   bool enable_distance_lower_bound_) :
    num_max_contacts(num_max_contacts_),
173
174
    enable_contact(enable_contact_),
    enable_distance_lower_bound (enable_distance_lower_bound_),
175
    gjk_solver_type(GST_INDEP),
176
177
    security_margin (0),
    break_distance (1e-3)
178
  {
179
180
    enable_cached_gjk_guess = false;
    cached_gjk_guess = Vec3f(1, 0, 0);
181
182
  }

183
  bool isSatisfied(const CollisionResult& result) const;
184
185
};

jpan's avatar
jpan committed
186
/// @brief collision result
187
188
struct CollisionResult
{
189
private:
jpan's avatar
jpan committed
190
  /// @brief contact information
191
  std::vector<Contact> contacts;
192

193
194
195
public:
  Vec3f cached_gjk_guess;

196
197
198
199
  /// Lower bound on distance between objects if they are disjoint
  /// \note computed only on request.
  FCL_REAL distance_lower_bound;

200
public:
jpan's avatar
   
jpan committed
201
202
  CollisionResult()
  {
203
204
  }

jpan's avatar
   
jpan committed
205

jpan's avatar
jpan committed
206
  /// @brief add one contact into result structure
jpan's avatar
   
jpan committed
207
208
209
210
211
  inline void addContact(const Contact& c) 
  {
    contacts.push_back(c);
  }

212
213
214
215
216
217
218
  /// @brief whether two CollisionResult are the same or not
  inline bool operator ==(const CollisionResult& other) const
  {
    return contacts == other.contacts 
            && distance_lower_bound == other.distance_lower_bound;
  }

jpan's avatar
jpan committed
219
  /// @brief return binary collision result
jpan's avatar
   
jpan committed
220
221
222
223
224
  bool isCollision() const
  {
    return contacts.size() > 0;
  }

jpan's avatar
jpan committed
225
  /// @brief number of contacts found
jpan's avatar
   
jpan committed
226
227
228
229
  size_t numContacts() const
  {
    return contacts.size();
  }
230

jpan's avatar
jpan committed
231
  /// @brief get the i-th contact calculated
232
233
234
235
236
237
238
239
  const Contact& getContact(size_t i) const
  {
    if(i < contacts.size()) 
      return contacts[i];
    else
      return contacts.back();
  }

jpan's avatar
jpan committed
240
  /// @brief get all the contacts
241
242
243
244
245
246
  void getContacts(std::vector<Contact>& contacts_)
  {
    contacts_.resize(contacts.size());
    std::copy(contacts.begin(), contacts.end(), contacts_.begin());
  }

jpan's avatar
jpan committed
247
  /// @brief clear the results obtained
248
249
250
251
  void clear()
  {
    contacts.clear();
  }
252
253
254
255

  /// @brief reposition Contact objects when fcl inverts them
  /// during their construction.
  friend void invertResults(CollisionResult& result);
256
257
};

258

259
struct DistanceResult;
sachinc's avatar
sachinc committed
260

jpan's avatar
jpan committed
261
/// @brief request to the distance computation
262
263
struct DistanceRequest
{
jpan's avatar
jpan committed
264
  /// @brief whether to return the nearest points
265
  bool enable_nearest_points;
266

267
268
269
270
271
272
273
274
275
276
277
278
  /// @brief error threshold for approximate distance
  FCL_REAL rel_err; // relative error, between 0 and 1
  FCL_REAL abs_err; // absoluate error

  /// @brief narrow phase solver type
  GJKSolverType gjk_solver_type;



  DistanceRequest(bool enable_nearest_points_ = false,
                  FCL_REAL rel_err_ = 0.0,
                  FCL_REAL abs_err_ = 0.0,
279
                  GJKSolverType gjk_solver_type_ = GST_INDEP) : enable_nearest_points(enable_nearest_points_),
280
281
282
                                                                rel_err(rel_err_),
                                                                abs_err(abs_err_),
                                                                gjk_solver_type(gjk_solver_type_)
283
284
  {
  }
285
286

  bool isSatisfied(const DistanceResult& result) const;
287
288
};

289

jpan's avatar
jpan committed
290
/// @brief distance result
291
292
struct DistanceResult
{
293
294

public:
295

296
  /// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0.
297
298
  FCL_REAL min_distance;

jpan's avatar
jpan committed
299
  /// @brief nearest points
300
  Vec3f nearest_points[2];
301

jpan's avatar
jpan committed
302
  /// @brief collision object 1
303
  const CollisionGeometry* o1;
jpan's avatar
jpan committed
304
305

  /// @brief collision object 2
306
  const CollisionGeometry* o2;
jpan's avatar
jpan committed
307
308
309
310
311

  /// @brief information about the nearest point in object 1
  /// if object 1 is mesh or point cloud, it is the triangle or point id
  /// if object 1 is geometry shape, it is NONE (-1),
  /// if object 1 is octree, it is the id of the cell
312
  int b1;
jpan's avatar
jpan committed
313
314
315
316
317

  /// @brief information about the nearest point in object 2
  /// if object 2 is mesh or point cloud, it is the triangle or point id
  /// if object 2 is geometry shape, it is NONE (-1),
  /// if object 2 is octree, it is the id of the cell
318
319
  int b2;

jpan's avatar
jpan committed
320
  /// @brief invalid contact primitive information
321
  static const int NONE = -1;
322
  
323
324
325
  DistanceResult(FCL_REAL min_distance_ = std::numeric_limits<FCL_REAL>::max()) : min_distance(min_distance_), 
                                                                                  o1(NULL),
                                                                                  o2(NULL),
jpan's avatar
jpan committed
326
327
                                                                                  b1(NONE),
                                                                                  b2(NONE)
328
  {
329
330
  }

331

jpan's avatar
jpan committed
332
  /// @brief add distance information into the result
333
334
335
336
337
338
339
340
341
342
343
344
  void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
  {
    if(min_distance > distance)
    {
      min_distance = distance;
      o1 = o1_;
      o2 = o2_;
      b1 = b1_;
      b2 = b2_;
    }
  }

jpan's avatar
jpan committed
345
  /// @brief add distance information into the result
346
347
348
349
350
351
352
353
354
355
356
357
358
359
  void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2)
  {
    if(min_distance > distance)
    {
      min_distance = distance;
      o1 = o1_;
      o2 = o2_;
      b1 = b1_;
      b2 = b2_;
      nearest_points[0] = p1;
      nearest_points[1] = p2;
    }
  }

jpan's avatar
jpan committed
360
  /// @brief add distance information into the result
361
362
363
364
365
366
367
368
369
370
371
372
373
374
  void update(const DistanceResult& other_result)
  {
    if(min_distance > other_result.min_distance)
    {
      min_distance = other_result.min_distance;
      o1 = other_result.o1;
      o2 = other_result.o2;
      b1 = other_result.b1;
      b2 = other_result.b2;
      nearest_points[0] = other_result.nearest_points[0];
      nearest_points[1] = other_result.nearest_points[1];
    }
  }

jpan's avatar
jpan committed
375
  /// @brief clear the result
376
377
378
  void clear()
  {
    min_distance = std::numeric_limits<FCL_REAL>::max();
379
380
    o1 = NULL;
    o2 = NULL;
jpan's avatar
jpan committed
381
382
    b1 = NONE;
    b2 = NONE;
383
  }
384
385
386
387

  /// @brief whether two DistanceResult are the same or not
  inline bool operator ==(const DistanceResult& other) const
  {
388
389
390
391
392
393
394
395
396
    bool is_same = min_distance == other.min_distance
                  && nearest_points[0] == other.nearest_points[0]
                  && nearest_points[1] == other.nearest_points[1]
                  && o1 == other.o1
                  && o2 == other.o2
                  && b1 == other.b1
                  && b2 == other.b2;

// TODO: check also that two GeometryObject are indeed equal.
397
    if ((o1 != NULL) xor (other.o1 != NULL)) return false;
398
399
400
    is_same &= (o1 == other.o1);
//    else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1;

401
    if ((o2 != NULL) xor (other.o2 != NULL)) return false;
402
403
404
405
    is_same &= (o2 == other.o2);
//    else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2;
  
    return is_same;
406
  }
407

sachinc's avatar
sachinc committed
408
409
410
411
412
};

}

#endif