collision_data.h 14.8 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
};

jpan's avatar
jpan committed
132
/// @brief Cost source describes an area with a cost. The area is described by an AABB region.
133
134
struct CostSource
{
jpan's avatar
jpan committed
135
  /// @brief aabb lower bound
136
  Vec3f aabb_min;
jpan's avatar
jpan committed
137
138

  /// @brief aabb upper bound
139
  Vec3f aabb_max;
jpan's avatar
jpan committed
140
141

  /// @brief cost density in the AABB region
jpan's avatar
jpan committed
142
143
  FCL_REAL cost_density;

isucan's avatar
isucan committed
144
145
  FCL_REAL total_cost;

jpan's avatar
jpan committed
146
147
148
149
  CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_),
                                                                                       aabb_max(aabb_max_),
                                                                                       cost_density(cost_density_)
  {
isucan's avatar
isucan committed
150
    total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
jpan's avatar
jpan committed
151
152
  }

153
154
155
156
  CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_),
                                                         aabb_max(aabb.max_),
                                                         cost_density(cost_density_)
  {
isucan's avatar
isucan committed
157
    total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
158
159
  }

jpan's avatar
jpan committed
160
  CostSource() {}
jpan's avatar
   
jpan committed
161
162
163

  bool operator < (const CostSource& other) const
  {
isucan's avatar
isucan committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
    if(total_cost < other.total_cost) 
      return false;
    if(total_cost > other.total_cost)
      return true;
    
    if(cost_density < other.cost_density)
      return false;
    if(cost_density > other.cost_density)
      return true;
  
    for(size_t i = 0; i < 3; ++i)
      if(aabb_min[i] != other.aabb_min[i])
	return aabb_min[i] < other.aabb_min[i];
 
    return false;
jpan's avatar
   
jpan committed
179
  }
180
181
182
183
184
185
186
187

  bool operator == (const CostSource& other) const
  {
    return aabb_min == other.aabb_min
            && aabb_max == other.aabb_max
            && cost_density == other.cost_density
            && total_cost == other.total_cost;
  }
188
189
};

190
191
struct CollisionResult;

jpan's avatar
jpan committed
192
/// @brief request to the collision algorithm
193
struct CollisionRequest
194
{  
jpan's avatar
jpan committed
195
  /// @brief The maximum number of contacts will return
jpan's avatar
jpan committed
196
  size_t num_max_contacts;
jpan's avatar
jpan committed
197
198

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

201
202
203
  /// Whether a lower bound on distance is returned when objects are disjoint
  bool enable_distance_lower_bound;

jpan's avatar
jpan committed
204
  /// @brief The maximum number of cost sources will return
jpan's avatar
jpan committed
205
  size_t num_max_cost_sources;
jpan's avatar
jpan committed
206
207

  /// @brief whether the cost sources will be computed
208
209
  bool enable_cost;

jpan's avatar
   
jpan committed
210
211
212
  /// @brief whether the cost computation is approximated
  bool use_approximate_cost;

213
214
215
216
217
218
219
220
221
  /// @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;

jpan's avatar
jpan committed
222
  CollisionRequest(size_t num_max_contacts_ = 1,
223
                   bool enable_contact_ = false,
224
		   bool enable_distance_lower_bound_ = false,
jpan's avatar
jpan committed
225
                   size_t num_max_cost_sources_ = 1,
jpan's avatar
   
jpan committed
226
                   bool enable_cost_ = false,
227
                   bool use_approximate_cost_ = true,
228
                   GJKSolverType gjk_solver_type_ = GST_INDEP) :
229
230
231
232
233
234
235
  num_max_contacts(num_max_contacts_),
    enable_contact(enable_contact_),
    enable_distance_lower_bound (enable_distance_lower_bound_),
    num_max_cost_sources(num_max_cost_sources_),
    enable_cost(enable_cost_),
    use_approximate_cost(use_approximate_cost_),
    gjk_solver_type(gjk_solver_type_)
236
  {
237
238
    enable_cached_gjk_guess = false;
    cached_gjk_guess = Vec3f(1, 0, 0);
239
240
  }

241
  bool isSatisfied(const CollisionResult& result) const;
242
243
};

jpan's avatar
jpan committed
244
/// @brief collision result
245
246
struct CollisionResult
{
247
private:
jpan's avatar
jpan committed
248
  /// @brief contact information
249
  std::vector<Contact> contacts;
250

jpan's avatar
jpan committed
251
  /// @brief cost sources
252
253
  std::set<CostSource> cost_sources;

254
255
256
public:
  Vec3f cached_gjk_guess;

257
258
259
260
  /// Lower bound on distance between objects if they are disjoint
  /// \note computed only on request.
  FCL_REAL distance_lower_bound;

261
public:
jpan's avatar
   
jpan committed
262
263
  CollisionResult()
  {
264
265
  }

jpan's avatar
   
jpan committed
266

jpan's avatar
jpan committed
267
  /// @brief add one contact into result structure
jpan's avatar
   
jpan committed
268
269
270
271
272
  inline void addContact(const Contact& c) 
  {
    contacts.push_back(c);
  }

jpan's avatar
jpan committed
273
  /// @brief add one cost source into result structure
274
  inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources)
jpan's avatar
   
jpan committed
275
  {
276
    cost_sources.insert(c);
isucan's avatar
isucan committed
277
278
    while (cost_sources.size() > num_max_cost_sources)
      cost_sources.erase(--cost_sources.end());
jpan's avatar
   
jpan committed
279
280
  }

281
282
283
284
285
286
287
288
  /// @brief whether two CollisionResult are the same or not
  inline bool operator ==(const CollisionResult& other) const
  {
    return contacts == other.contacts 
            && cost_sources == other.cost_sources
            && distance_lower_bound == other.distance_lower_bound;
  }

jpan's avatar
jpan committed
289
  /// @brief return binary collision result
jpan's avatar
   
jpan committed
290
291
292
293
294
  bool isCollision() const
  {
    return contacts.size() > 0;
  }

jpan's avatar
jpan committed
295
  /// @brief number of contacts found
jpan's avatar
   
jpan committed
296
297
298
299
  size_t numContacts() const
  {
    return contacts.size();
  }
300

jpan's avatar
jpan committed
301
  /// @brief number of cost sources found
jpan's avatar
   
jpan committed
302
  size_t numCostSources() const
303
  {
jpan's avatar
   
jpan committed
304
    return cost_sources.size();
305
306
  }

jpan's avatar
jpan committed
307
  /// @brief get the i-th contact calculated
308
309
310
311
312
313
314
315
  const Contact& getContact(size_t i) const
  {
    if(i < contacts.size()) 
      return contacts[i];
    else
      return contacts.back();
  }

jpan's avatar
jpan committed
316
  /// @brief get all the contacts
317
318
319
320
321
322
  void getContacts(std::vector<Contact>& contacts_)
  {
    contacts_.resize(contacts.size());
    std::copy(contacts.begin(), contacts.end(), contacts_.begin());
  }

jpan's avatar
jpan committed
323
  /// @brief get all the cost sources 
324
325
326
327
328
329
  void getCostSources(std::vector<CostSource>& cost_sources_)
  {
    cost_sources_.resize(cost_sources.size());
    std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin());
  }

jpan's avatar
jpan committed
330
  /// @brief clear the results obtained
331
332
333
334
335
  void clear()
  {
    contacts.clear();
    cost_sources.clear();
  }
336
337
338
339

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

342

343
struct DistanceResult;
sachinc's avatar
sachinc committed
344

jpan's avatar
jpan committed
345
/// @brief request to the distance computation
346
347
struct DistanceRequest
{
jpan's avatar
jpan committed
348
  /// @brief whether to return the nearest points
349
  bool enable_nearest_points;
350

351
352
353
354
355
356
357
358
359
360
361
362
  /// @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,
363
                  GJKSolverType gjk_solver_type_ = GST_INDEP) : enable_nearest_points(enable_nearest_points_),
364
365
366
                                                                rel_err(rel_err_),
                                                                abs_err(abs_err_),
                                                                gjk_solver_type(gjk_solver_type_)
367
368
  {
  }
369
370

  bool isSatisfied(const DistanceResult& result) const;
371
372
};

373

jpan's avatar
jpan committed
374
/// @brief distance result
375
376
struct DistanceResult
{
377
378

public:
379

380
  /// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0.
381
382
  FCL_REAL min_distance;

jpan's avatar
jpan committed
383
  /// @brief nearest points
384
  Vec3f nearest_points[2];
385

jpan's avatar
jpan committed
386
  /// @brief collision object 1
387
  const CollisionGeometry* o1;
jpan's avatar
jpan committed
388
389

  /// @brief collision object 2
390
  const CollisionGeometry* o2;
jpan's avatar
jpan committed
391
392
393
394
395

  /// @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
396
  int b1;
jpan's avatar
jpan committed
397
398
399
400
401

  /// @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
402
403
  int b2;

jpan's avatar
jpan committed
404
  /// @brief invalid contact primitive information
405
  static const int NONE = -1;
406
  
407
408
409
  DistanceResult(FCL_REAL min_distance_ = std::numeric_limits<FCL_REAL>::max()) : min_distance(min_distance_), 
                                                                                  o1(NULL),
                                                                                  o2(NULL),
jpan's avatar
jpan committed
410
411
                                                                                  b1(NONE),
                                                                                  b2(NONE)
412
  {
413
414
  }

415

jpan's avatar
jpan committed
416
  /// @brief add distance information into the result
417
418
419
420
421
422
423
424
425
426
427
428
  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
429
  /// @brief add distance information into the result
430
431
432
433
434
435
436
437
438
439
440
441
442
443
  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
444
  /// @brief add distance information into the result
445
446
447
448
449
450
451
452
453
454
455
456
457
458
  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
459
  /// @brief clear the result
460
461
462
  void clear()
  {
    min_distance = std::numeric_limits<FCL_REAL>::max();
463
464
    o1 = NULL;
    o2 = NULL;
jpan's avatar
jpan committed
465
466
    b1 = NONE;
    b2 = NONE;
467
  }
468
469
470
471

  /// @brief whether two DistanceResult are the same or not
  inline bool operator ==(const DistanceResult& other) const
  {
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
    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.
    if (o1 != NULL xor other.o1 != NULL) return false;
    is_same &= (o1 == other.o1);
//    else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1;

    if (o2 != NULL xor other.o2 != NULL) return false;
    is_same &= (o2 == other.o2);
//    else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2;
  
    return is_same;
490
  }
491

sachinc's avatar
sachinc committed
492
493
494
495
496
};

}

#endif