narrowphase.h 18 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
 *  Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
7
8
9
10
11
12
13
14
15
16
17
18
 *  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.
19
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
 *     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.
 */

37
/** \author Jia Pan, Florent Lamiraux */
38

Joseph Mirabel's avatar
Joseph Mirabel committed
39
40
#ifndef HPP_FCL_NARROWPHASE_H
#define HPP_FCL_NARROWPHASE_H
41

42
#include <hpp/fcl/narrowphase/gjk.h>
43

44
45
namespace hpp
{
46
47
48
namespace fcl
{

49
50


51

52
  /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
53
  struct GJKSolver
54
  {
55
56
57
58
    /// @brief intersection checking between two shapes
    template<typename S1, typename S2>
      bool shapeIntersect(const S1& s1, const Transform3f& tf1,
                          const S2& s2, const Transform3f& tf2,
59
60
61
                          FCL_REAL& distance_lower_bound,
                          bool enable_penetration,
                          Vec3f* contact_points, Vec3f* normal) const
62
63
    {
      Vec3f guess(1, 0, 0);
64
65
66
67
68
69
      support_func_guess_t support_hint;
      if(enable_cached_guess) {
        guess = cached_guess;
        support_hint = support_func_cached_guess;
      } else
        support_hint.setZero();
70
    
71
      details::MinkowskiDiff shape;
72
      shape.set (&s1, &s2, tf1, tf2);
73
  
74
      details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
75
      details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
76
77
78
79
      if(enable_cached_guess) {
        cached_guess = gjk.getGuessFromSimplex();
        support_func_cached_guess = gjk.support_hint;
      }
80
    
81
82
      Vec3f w0, w1;
      switch(gjk_status) {
83
        case details::GJK::Inside:
84
85
          if (!enable_penetration && contact_points == NULL && normal == NULL)
            return true;
86
87
          if (gjk.hasPenetrationInformation(shape)) {
            gjk.getClosestPoints (shape, w0, w1);
88
            distance_lower_bound = gjk.distance;
89
90
91
92
            if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
            if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
            return true;
          } else {
93
94
            details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
            details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
95
96
97
98
            if(epa_status & details::EPA::Valid
                || epa_status == details::EPA::OutOfFaces    // Warnings
                || epa_status == details::EPA::OutOfVertices // Warnings
                )
99
100
            {
              epa.getClosestPoints (shape, w0, w1);
101
              distance_lower_bound = -epa.depth;
102
              if(normal) *normal = tf1.getRotation() * epa.normal;
103
104
105
              if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
              return true;
            }
106
            distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
107
108
            // EPA failed but we know there is a collision so we should
            return true;
109
          }
110
          break;
111
112
113
        case details::GJK::Valid:
          distance_lower_bound = gjk.distance;
          break;
114
115
        default:
          ;
116
      }
117

118
119
      return false;
    }
120

121
    //// @brief intersection checking between one shape and a triangle with transformation
Lucile Remigy's avatar
Lucile Remigy committed
122
    /// @return true if the shape are colliding.
123
124
125
126
127
128
129
    template<typename S>
    bool shapeTriangleInteraction
    (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
     const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
     Vec3f& p1, Vec3f& p2, Vec3f& normal) const
    {
      bool col;
130
131
132
133
134
135
      // Express everything in frame 1
      const Transform3f tf_1M2 (tf1.inverseTimes(tf2));
      TriangleP tri(
          tf_1M2.transform (P1),
          tf_1M2.transform (P2),
          tf_1M2.transform (P3));
136

137
      Vec3f guess(1, 0, 0);
138
139
140
141
142
143
      support_func_guess_t support_hint;
      if(enable_cached_guess) {
        guess = cached_guess;
        support_hint = support_func_cached_guess;
      } else
        support_hint.setZero();
144

145
      details::MinkowskiDiff shape;
146
      shape.set (&s, &tri);
147
  
148
      details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
149
      details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
150
151
152
153
      if(enable_cached_guess) {
        cached_guess = gjk.getGuessFromSimplex();
        support_func_cached_guess = gjk.support_hint;
      }
154

155
156
      Vec3f w0, w1;
      switch(gjk_status) {
157
        case details::GJK::Inside:
158
159
160
161
162
163
164
          col = true;
          if (gjk.hasPenetrationInformation(shape)) {
            gjk.getClosestPoints (shape, w0, w1);
            distance = gjk.distance;
            normal = tf1.getRotation() * (w1 - w0).normalized();
            p1 = p2 = tf1.transform((w0 + w1) / 2);
          } else {
165
166
            details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
            details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
            if(epa_status & details::EPA::Valid
                || epa_status == details::EPA::OutOfFaces    // Warnings
                || epa_status == details::EPA::OutOfVertices // Warnings
                )
            {
              epa.getClosestPoints (shape, w0, w1);
              distance = -epa.depth;
              normal = -epa.normal;
              p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
              assert (distance <= 1e-6);
            } else {
              distance = -std::numeric_limits<FCL_REAL>::max();
              gjk.getClosestPoints (shape, w0, w1);
              p1 = p2 = tf1.transform (w0);
            }
182
          }
183
          break;
184
        case details::GJK::Valid:
185
        case details::GJK::Failed:
186
          col = false;
187

188
189
190
191
192
          gjk.getClosestPoints (shape, p1, p2);
          // TODO On degenerated case, the closest point may be wrong
          // (i.e. an object face normal is colinear to gjk.ray
          // assert (distance == (w0 - w1).norm());
          distance = gjk.distance;
193

194
195
196
          p1 = tf1.transform (p1);
          p2 = tf1.transform (p2);
          assert (distance > 0);
197
198
          break;
        default:
Florent Lamiraux's avatar
Florent Lamiraux committed
199
          assert (false && "should not reach type part.");
200
          return true;
201
202
203
204
205
206
207
208
209
210
        }
      return col;
    }

    /// @brief distance computation between two shapes
    template<typename S1, typename S2>
      bool shapeDistance(const S1& s1, const Transform3f& tf1,
                         const S2& s2, const Transform3f& tf2,
                         FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                         Vec3f& normal) const
211
    {
212
#ifndef NDEBUG
Florent Lamiraux's avatar
Florent Lamiraux committed
213
      FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
214
#endif
215
      Vec3f guess(1, 0, 0);
216
217
218
219
220
221
      support_func_guess_t support_hint;
      if(enable_cached_guess) {
        guess = cached_guess;
        support_hint = support_func_cached_guess;
      } else
        support_hint.setZero();
222
223

      details::MinkowskiDiff shape;
224
      shape.set (&s1, &s2, tf1, tf2);
225
226

      details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
227
      details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
228
229
230
231
      if(enable_cached_guess) {
        cached_guess = gjk.getGuessFromSimplex();
        support_func_cached_guess = gjk.support_hint;
      }
232

Florent Lamiraux's avatar
Florent Lamiraux committed
233
234
235
236
      if(gjk_status == details::GJK::Failed)
      {
        // TODO: understand why GJK fails between cylinder and box
        assert (distance * distance < sqrt (eps));
237
        Vec3f w0, w1;
238
        gjk.getClosestPoints (shape, w0, w1);
Florent Lamiraux's avatar
Florent Lamiraux committed
239
240
241
242
243
244
        distance = 0;
        p1 = p2 = tf1.transform (.5* (w0 + w1));
        normal = Vec3f (0,0,0);
        return false;
      }
      else if(gjk_status == details::GJK::Valid)
245
        {
246
          gjk.getClosestPoints (shape, p1, p2);
247
248
249
250
          // TODO On degenerated case, the closest point may be wrong
          // (i.e. an object face normal is colinear to gjk.ray
          // assert (distance == (w0 - w1).norm());
          distance = gjk.distance;
251

252
          normal = (tf1.getRotation() * gjk.ray).normalized();
253
254
          p1 = tf1.transform (p1);
          p2 = tf1.transform (p2);
255
256
          return true;
        }
Florent Lamiraux's avatar
Florent Lamiraux committed
257
      else
258
        {
Florent Lamiraux's avatar
Florent Lamiraux committed
259
          assert (gjk_status == details::GJK::Inside);
260
261
262
          if (gjk.hasPenetrationInformation (shape)) {
            gjk.getClosestPoints (shape, p1, p2);
            distance = gjk.distance;
Joseph Mirabel's avatar
Joseph Mirabel committed
263
264
265
266
267
            // Return contact points in case of collision
            //p1 = tf1.transform (p1);
            //p2 = tf1.transform (p2);
            normal = (tf1.getRotation() * (p2 - p1)).normalized();
            p1 = p2 = tf1.transform(p1);
268
269
270
271
          } else {
            details::EPA epa(epa_max_face_num, epa_max_vertex_num,
                             epa_max_iterations, epa_tolerance);
            details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
272
273
274
275
            if(epa_status & details::EPA::Valid
                || epa_status == details::EPA::OutOfFaces    // Warnings
                || epa_status == details::EPA::OutOfVertices // Warnings
                )
276
            {
277
278
279
280
281
282
283
284
              Vec3f w0, w1;
              epa.getClosestPoints (shape, w0, w1);
              assert (epa.depth >= -eps);
              distance = std::min (0., -epa.depth);
              // TODO should be
              // normal = tf1.getRotation() * epa.normal;
              normal = tf2.getRotation() * epa.normal;
              p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
285
              return false;
286
            }
287
288
289
            distance = -std::numeric_limits<FCL_REAL>::max();
            gjk.getClosestPoints (shape, p1, p2);
            p1 = p2 = tf1.transform (p1);
290
          }
291
          return false;
292
293
        }
    }
294

295
    /// @brief default setting for GJK algorithm
296
    GJKSolver()
297
    {
298
299
300
301
302
303
304
305
      gjk_max_iterations = 128;
      gjk_tolerance = 1e-6;
      epa_max_face_num = 128;
      epa_max_vertex_num = 64;
      epa_max_iterations = 255;
      epa_tolerance = 1e-6;
      enable_cached_guess = false;
      cached_guess = Vec3f(1, 0, 0);
306
      support_func_cached_guess = support_func_guess_t::Zero();
307
    }
308

309
310
311
    void enableCachedGuess(bool if_enable) const
    {
      enable_cached_guess = if_enable;
312
    }
313
314

    void setCachedGuess(const Vec3f& guess) const
315
    {
316
      cached_guess = guess;
317
    }
318
319

    Vec3f getCachedGuess() const
320
    {
321
      return cached_guess;
322
323
    }

324
325
    /// @brief maximum number of simplex face used in EPA algorithm
    unsigned int epa_max_face_num;
326

327
328
    /// @brief maximum number of simplex vertex used in EPA algorithm
    unsigned int epa_max_vertex_num;
329

330
331
    /// @brief maximum number of iterations used for EPA iterations
    unsigned int epa_max_iterations;
332

333
334
    /// @brief the threshold used in EPA to stop iteration
    FCL_REAL epa_tolerance;
335

336
337
    /// @brief the threshold used in GJK to stop iteration
    FCL_REAL gjk_tolerance;
338

339
340
    /// @brief maximum number of iterations used for GJK iterations
    FCL_REAL gjk_max_iterations;
341

342
343
    /// @brief Whether smart guess can be provided
    mutable bool enable_cached_guess;
344

345
346
    /// @brief smart guess
    mutable Vec3f cached_guess;
347
348
349

    /// @brief smart guess for the support function
    mutable support_func_guess_t support_func_cached_guess;
350
  };
351

Joseph Mirabel's avatar
Joseph Mirabel committed
352
#if __cplusplus < 201103L
Justin Carpentier's avatar
Justin Carpentier committed
353
354
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wc99-extensions"
Joseph Mirabel's avatar
Joseph Mirabel committed
355
#endif
356
357
358
359
  /// \name Shape intersection specializations
  /// \{

// param doc is the doxygen detailled description (should be enclosed in /** */
360
// and contain no dot for some obscure reasons).
361
362
363
364
365
366
367
#define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc)                     \
  /** @brief Fast implementation for Shape1-Shape2 collision. */               \
  doc                                                                          \
  template<>                                                                   \
    bool GJKSolver::shapeIntersect<Shape1, Shape2>                             \
    (const Shape1& s1, const Transform3f& tf1,                                 \
     const Shape2& s2, const Transform3f& tf2,                                 \
368
369
     FCL_REAL& distance_lower_bound, bool enable_penetration,                  \
     Vec3f* contact_points, Vec3f* normal) const
370
371
372
373
374
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc)                        \
  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc)                \
  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc);                          \
  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2,Shape1,doc)
375

376
377
378
379
  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,);
380

381
382
383
384
385
386
387
388
389
390
391
#ifdef IS_DOXYGEN // for doxygen only
  /** \todo currently disabled and to re-enable it, API of function
   *  \ref obbDisjointAndLowerBoundDistance should be modified.
   *  */
  template<> bool GJKSolver::shapeIntersect<Box, Box>
    (const Box& s1, const Transform3f& tf1,
     const Box& s2, const Transform3f& tf2,
     FCL_REAL& distance_lower_bound, bool enable_penetration,
     Vec3f* contact_points, Vec3f* normal) const;
#endif
  //HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane,);

  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane,);

  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane,);

  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane,);

  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace,);

  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane,);
  HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace,);
408

409
410
411
412
413
414
415
416
417
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR

  /// \}

  /// \name Shape triangle interaction specializations
  /// \{

418
419
// param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no dot for some obscure reasons).
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc)                              \
  /** @brief Fast implementation for Shape-Triangle interaction. */            \
  doc                                                                          \
  template<> bool GJKSolver::shapeTriangleInteraction<Shape>                   \
    (const Shape& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, \
     const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,              \
     Vec3f& p1, Vec3f& p2, Vec3f& normal) const

  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere,);
  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace,);
  HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane,);

#undef HPP_FCL_DECLARE_SHAPE_TRIANGLE

  /// \}

  /// \name Shape distance specializations
  /// \{

// param doc is the doxygen detailled description (should be enclosed in /** */
440
// and contain no dot for some obscure reasons).
441
#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc)                      \
442
443
  /** @brief Fast implementation for Shape1-Shape2 distance. */                \
  doc                                                                          \
444
445
446
447
448
  template<>                                                                   \
    bool GJKSolver::shapeDistance<Shape1, Shape2>                              \
    (const Shape1& s1, const Transform3f& tf1,                                 \
     const Shape2& s2, const Transform3f& tf2,                                 \
     FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
449
450
451
452
453
454
455
456
457
458
459
460
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape,doc)                         \
  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape,Shape,doc)
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1,Shape2,doc)                 \
  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc);                           \
  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2,Shape1,doc)

  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box,);
  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule,);
  HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder,);
  HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere,);

  HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Capsule,
461
462
      /** Closest points are based on two line-segments. */
      );
463
464

  HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(TriangleP,
465
466
      /** Do not run EPA algorithm to compute penetration depth. Use a dedicated method. */
      );
467
468
469
470

#undef HPP_FCL_DECLARE_SHAPE_DISTANCE
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
471
472

  /// \}
Joseph Mirabel's avatar
Joseph Mirabel committed
473
#if __cplusplus < 201103L
Justin Carpentier's avatar
Justin Carpentier committed
474
#pragma GCC diagnostic pop
Joseph Mirabel's avatar
Joseph Mirabel committed
475
#endif
476
477
}

478
479
} // namespace hpp

480
#endif