distance_capsule_capsule.cpp 6.43 KB
Newer Older
Justin Carpentier's avatar
Justin Carpentier committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
/*
 * Software License Agreement (BSD License)
 *  Copyright (c) 2015-2019, CNRS - LAAS INRIA
 *  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.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     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.
 */
33
34
35

#include <cmath>
#include <limits>
36
37
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
38
#include <../src/distance_func_matrix.h>
39

40
41
#define CLAMP(value, l, u) fmax(fmin(value, u), l)

42
43
44
45
46
47
48
49
// Note that partial specialization of template functions is not allowed.
// Therefore, two implementations with the default narrow phase solvers are
// provided. If another narrow phase solver were to be used, the default
// template implementation would be called, unless the function is also
// specialized for this new type.
//
// One solution would be to make narrow phase solvers derive from an abstract
// class and specialize the template for this abstract class.
50
51
namespace hpp
{
52
namespace fcl {
53
  class GJKSolver;
54

55
56
57
58

  // Compute the distance between C1 and C2 by computing the distance
  // between the two segments supporting the capsules.
  // Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest Point of Two Line Segments
59
  template <>
60
  FCL_REAL ShapeShapeDistance <Capsule, Capsule> (const CollisionGeometry* o1, const Transform3f& tf1,
61
   const CollisionGeometry* o2, const Transform3f& tf2,
62
   const GJKSolver*, const DistanceRequest& request,
63
64
   DistanceResult& result)
  {
65
66
67
68
    const Capsule* capsule1 = static_cast <const Capsule*> (o1);
    const Capsule* capsule2 = static_cast <const Capsule*> (o2);

    FCL_REAL EPSILON = std::numeric_limits<FCL_REAL>::epsilon () * 100;
69
70

    // We assume that capsules are centered at the origin.
71
72
    const fcl::Vec3f& c1 = tf1.getTranslation ();
    const fcl::Vec3f& c2 = tf2.getTranslation ();
73
    // We assume that capsules are oriented along z-axis.
74
75
76
77
78
79
    FCL_REAL halfLength1 = capsule1->halfLength;
    FCL_REAL halfLength2 = capsule2->halfLength;
    FCL_REAL radius1 = capsule1->radius;
    FCL_REAL radius2 = capsule2->radius;
    // direction of capsules
    // ||d1|| = 2 * halfLength1
80
81
    const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2);
    const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2);
82

83
84
    // Starting point of the segments
    // p1 + d1 is the end point of the segment
85
86
87
    const fcl::Vec3f p1 = c1 - d1 / 2;
    const fcl::Vec3f p2 = c2 - d2 / 2;
    const fcl::Vec3f r = p1-p2;
88
89
90
91
92
93
94
95
96
    FCL_REAL a = d1.dot(d1);
    FCL_REAL b = d1.dot(d2);
    FCL_REAL c = d1.dot(r);
    FCL_REAL e = d2.dot(d2);
    FCL_REAL f = d2.dot(r);
    // S1 is parametrized by the equation p1 + s * d1
    // S2 is parametrized by the equation p2 + t * d2
    FCL_REAL s = 0.0;
    FCL_REAL t = 0.0;
97

98
99
100
101
102
103
104
105
106
107
    if (a <= EPSILON && e <= EPSILON)
    {
      // Check if the segments degenerate into points
      s = t = 0.0;
    }
    else if (a <= EPSILON)
    {
      // First segment is degenerated
      s = 0.0;
      t = CLAMP(f / e, 0.0, 1.0);
108
    }
109
110
111
112
113
114
115
116
    else if (e <= EPSILON)
    {
      // Second segment is degenerated
      s = CLAMP(-c / a, 0.0, 1.0);
      t = 0.0;
    }
    else
    {
Robin Strudel's avatar
Robin Strudel committed
117
      // Always non-negative, equal 0 if the segments are colinear
118
      FCL_REAL denom = fmax(a*e-b*b, 0);
119

120
      if (denom > EPSILON)
121
122
      {
        s = CLAMP((b*f-c*e) / denom, 0.0, 1.0);
123
      }
124
125
126
      else
      {
        s = 0.0;
127
128
      }

129
130
131
132
133
134
135
136
137
138
139
      t = (b*s + f) / e;
      if (t < 0.0)
      {
        t = 0.0;
        s = CLAMP(-c / a, 0.0, 1.0);
      }
      else if (t > 1.0)
      {
        t = 1.0;
        s = CLAMP((b - c)/a, 0.0, 1.0);
      }
140
141
    }

142
    // witness points achieving the distance between the two segments
143
144
    const Vec3f w1 = p1 + s * d1;
    const Vec3f w2 = p2 + t * d2;
145
146
    FCL_REAL distance = (w1 - w2).norm();
    Vec3f normal = (w1 - w2) / distance;
147
    result.normal = normal;
148

149
150
151
152
153
154
155
156
    // capsule spcecific distance computation
    distance = distance - (radius1 + radius2);
    result.min_distance = distance;
    // witness points for the capsules
    if (request.enable_nearest_points)
    {
      result.nearest_points[0] = w1 - radius1 * normal;
      result.nearest_points[1] = w2 + radius2 * normal;
157
    }
158
    return distance;
159
160
161
  }

  template <>
Lucile Remigy's avatar
Lucile Remigy committed
162
  std::size_t ShapeShapeCollide <Capsule, Capsule>
163
164
  (const CollisionGeometry* o1, const Transform3f& tf1,
   const CollisionGeometry* o2, const Transform3f& tf2,
165
   const GJKSolver*, const CollisionRequest& request,
166
167
   CollisionResult& result)
  {
168
    GJKSolver* unused = 0x0;
169
170
171
    DistanceResult distanceResult;
    DistanceRequest distanceRequest (request.enable_contact);

Lucile Remigy's avatar
Lucile Remigy committed
172
    FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule>
173
174
      (o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);

175
176
177
178
179
180
181
    if (distance > 0)
    {
      return 0;
    }
    else
    {
      Contact contact (o1, o2, -1, -1);
182
183
      const Vec3f& p1 = distanceResult.nearest_points [0];
      const Vec3f& p2 = distanceResult.nearest_points [1];
184
      contact.pos = 0.5 * (p1+p2);
185
      contact.normal = distanceResult.normal;
186
      result.addContact(contact);
187
188
189
      return 1;
    }
  }
190

191
} // namespace fcl
192
193

} // namespace hpp