narrowphase.h 15.9 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
64
    {
      Vec3f guess(1, 0, 0);
      if(enable_cached_guess) guess = cached_guess;
65
    
66
      details::MinkowskiDiff shape;
67
      shape.set (&s1, &s2, tf1, tf2);
68
  
69
70
71
      details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
72
    
73
74
      Vec3f w0, w1;
      switch(gjk_status) {
75
        case details::GJK::Inside:
76
77
          if (!enable_penetration && contact_points == NULL && normal == NULL)
            return true;
78
79
          if (gjk.hasPenetrationInformation(shape)) {
            gjk.getClosestPoints (shape, w0, w1);
80
            distance_lower_bound = gjk.distance;
81
82
83
84
            if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
            if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
            return true;
          } else {
85
86
            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);
87
88
89
90
            if(epa_status & details::EPA::Valid
                || epa_status == details::EPA::OutOfFaces    // Warnings
                || epa_status == details::EPA::OutOfVertices // Warnings
                )
91
92
            {
              epa.getClosestPoints (shape, w0, w1);
93
              distance_lower_bound = -epa.depth;
94
              if(normal) *normal = tf1.getRotation() * epa.normal;
95
96
97
              if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
              return true;
            }
98
            distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
99
100
            // EPA failed but we know there is a collision so we should
            return true;
101
          }
102
          break;
103
104
105
        case details::GJK::Valid:
          distance_lower_bound = gjk.distance;
          break;
106
107
        default:
          ;
108
      }
109

110
111
      return false;
    }
112

113
    //// @brief intersection checking between one shape and a triangle with transformation
Lucile Remigy's avatar
Lucile Remigy committed
114
    /// @return true if the shape are colliding.
115
116
117
118
119
120
121
    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;
122
123
124
125
126
127
      // 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));
128

129
130
      Vec3f guess(1, 0, 0);
      if(enable_cached_guess) guess = cached_guess;
131

132
      details::MinkowskiDiff shape;
133
      shape.set (&s, &tri);
134
  
135
136
137
      details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
138

139
140
      Vec3f w0, w1;
      switch(gjk_status) {
141
        case details::GJK::Inside:
142
143
144
145
146
147
148
          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 {
149
150
            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);
151
            assert (epa_status & details::EPA::Valid); (void) epa_status;
152

153
            epa.getClosestPoints (shape, w0, w1);
154
155
156
            distance = -epa.depth;
            normal = -epa.normal;
            p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
Florent Lamiraux's avatar
Florent Lamiraux committed
157
            assert (distance <= 1e-6);
158
          }
159
          break;
160
        case details::GJK::Valid:
161
        case details::GJK::Failed:
162
          col = false;
163

164
165
166
167
168
          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;
169

170
171
172
          p1 = tf1.transform (p1);
          p2 = tf1.transform (p2);
          assert (distance > 0);
173
174
          break;
        default:
Florent Lamiraux's avatar
Florent Lamiraux committed
175
          assert (false && "should not reach type part.");
176
          return true;
177
178
179
180
181
182
183
184
185
186
        }
      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
187
    {
188
#ifndef NDEBUG
Florent Lamiraux's avatar
Florent Lamiraux committed
189
      FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
190
#endif
191
192
193
194
      Vec3f guess(1, 0, 0);
      if(enable_cached_guess) guess = cached_guess;

      details::MinkowskiDiff shape;
195
      shape.set (&s1, &s2, tf1, tf2);
196
197
198
199
200

      details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();

Florent Lamiraux's avatar
Florent Lamiraux committed
201
202
203
204
      if(gjk_status == details::GJK::Failed)
      {
        // TODO: understand why GJK fails between cylinder and box
        assert (distance * distance < sqrt (eps));
205
        Vec3f w0, w1;
206
        gjk.getClosestPoints (shape, w0, w1);
Florent Lamiraux's avatar
Florent Lamiraux committed
207
208
209
210
211
212
        distance = 0;
        p1 = p2 = tf1.transform (.5* (w0 + w1));
        normal = Vec3f (0,0,0);
        return false;
      }
      else if(gjk_status == details::GJK::Valid)
213
        {
214
          gjk.getClosestPoints (shape, p1, p2);
215
216
217
218
          // 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;
219

220
221
          p1 = tf1.transform (p1);
          p2 = tf1.transform (p2);
222
223
          return true;
        }
Florent Lamiraux's avatar
Florent Lamiraux committed
224
      else
225
        {
Florent Lamiraux's avatar
Florent Lamiraux committed
226
          assert (gjk_status == details::GJK::Inside);
227
228
229
          if (gjk.hasPenetrationInformation (shape)) {
            gjk.getClosestPoints (shape, p1, p2);
            distance = gjk.distance;
Joseph Mirabel's avatar
Joseph Mirabel committed
230
231
232
233
234
            // 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);
235
236
237
238
          } 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);
239
240
241
242
            if(epa_status & details::EPA::Valid
                || epa_status == details::EPA::OutOfFaces    // Warnings
                || epa_status == details::EPA::OutOfVertices // Warnings
                )
243
            {
244
245
246
247
248
249
250
251
              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));
252
            }
253
          }
254
          return false;
255
256
        }
    }
257

258
    /// @brief default setting for GJK algorithm
259
    GJKSolver()
260
    {
261
262
263
264
265
266
267
268
      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);
269
    }
270

271
272
273
    void enableCachedGuess(bool if_enable) const
    {
      enable_cached_guess = if_enable;
274
    }
275
276

    void setCachedGuess(const Vec3f& guess) const
277
    {
278
      cached_guess = guess;
279
    }
280
281

    Vec3f getCachedGuess() const
282
    {
283
      return cached_guess;
284
285
    }

286
287
    /// @brief maximum number of simplex face used in EPA algorithm
    unsigned int epa_max_face_num;
288

289
290
    /// @brief maximum number of simplex vertex used in EPA algorithm
    unsigned int epa_max_vertex_num;
291

292
293
    /// @brief maximum number of iterations used for EPA iterations
    unsigned int epa_max_iterations;
294

295
296
    /// @brief the threshold used in EPA to stop iteration
    FCL_REAL epa_tolerance;
297

298
299
    /// @brief the threshold used in GJK to stop iteration
    FCL_REAL gjk_tolerance;
300

301
302
    /// @brief maximum number of iterations used for GJK iterations
    FCL_REAL gjk_max_iterations;
303

304
305
    /// @brief Whether smart guess can be provided
    mutable bool enable_cached_guess;
306

307
308
309
    /// @brief smart guess
    mutable Vec3f cached_guess;
  };
310

Justin Carpentier's avatar
Justin Carpentier committed
311
312
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wc99-extensions"
313
314
315
316
317
318
319
320
321
322
323
324
  /// \name Shape intersection specializations
  /// \{

// param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no space.
#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,                                 \
325
326
     FCL_REAL& distance_lower_bound, bool enable_penetration,                  \
     Vec3f* contact_points, Vec3f* normal) const
327
328
329
330
331
#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)
332

333
334
335
336
  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,);
337

338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
  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,);
355

356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
#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
  /// \{

#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 /** */
// and contain no space.
386
#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc)                      \
387
388
  /** @brief Fast implementation for Shape1-Shape2 distance. */                \
  doc                                                                          \
389
390
391
392
393
  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
394
395
396
397
398
399
400
401
402
403
404
405
#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,
406
407
      /** Closest points are based on two line-segments. */
      );
408
409

  HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(TriangleP,
410
411
      /** Do not run EPA algorithm to compute penetration depth. Use a dedicated method. */
      );
412
413
414
415

#undef HPP_FCL_DECLARE_SHAPE_DISTANCE
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
416
417

  /// \}
Justin Carpentier's avatar
Justin Carpentier committed
418
#pragma GCC diagnostic pop
419
420
}

421
422
} // namespace hpp

423
#endif