distance_capsule_capsule.cpp 5.52 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
// Geometric Tools, LLC
// Copyright (c) 1998-2011
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
//
// Modified by Florent Lamiraux 2014

#include <cmath>
#include <limits>
11
12
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
13
#include <../src/distance_func_matrix.h>
14

15
16
#define CLAMP(value, l, u) fmax(fmin(value, u), l)

17
18
19
20
21
22
23
24
// 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.
25
26
namespace hpp
{
27
namespace fcl {
28
  class GJKSolver;
29

30
31
32
33

  // 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
34
  template <>
35
  FCL_REAL ShapeShapeDistance <Capsule, Capsule> (const CollisionGeometry* o1, const Transform3f& tf1,
36
   const CollisionGeometry* o2, const Transform3f& tf2,
37
   const GJKSolver*, const DistanceRequest& request,
38
39
   DistanceResult& result)
  {
40
41
42
43
    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;
44
45

    // We assume that capsules are centered at the origin.
46
47
    const fcl::Vec3f& c1 = tf1.getTranslation ();
    const fcl::Vec3f& c2 = tf2.getTranslation ();
48
    // We assume that capsules are oriented along z-axis.
49
50
51
52
53
54
55
56
57
58
    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
    // Matrix3f::ConstColXpr d1 = 2 * halfLength1 * tf1.getRotation().col(2);
    // Matrix3f::ConstColXpr d2 = 2 * halfLength2 * tf2.getRotation().col(2);
    const fcl::Vec3f& d1 = 2 * halfLength1 * tf1.getRotation().col(2);
    const fcl::Vec3f& d2 = 2 * halfLength2 * tf2.getRotation().col(2);
59

60
61
62
63
64
65
66
67
68
69
70
71
72
73
    // Starting point of the segments
    // p1 + d1 is the end point of the segment
    const fcl::Vec3f& p1 = c1 - d1 / 2;
    const fcl::Vec3f& p2 = c2 - d2 / 2;
    const fcl::Vec3f& r = p1-p2;
    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;
74

75
76
77
78
79
80
81
82
83
84
85
86
    if (a <= EPSILON && e <= EPSILON)
    {
      // Check if the segments degenerate into points
      s = t = 0.0;
      FCL_REAL distance = (p1-p2).norm();
      Vec3f normal = (p1 - p2) / distance;
      distance = distance - (radius1 + radius2);
      result.min_distance = distance;
      if (request.enable_nearest_points)
      {
        result.nearest_points[0] = p1 - radius1 * normal;
        result.nearest_points[1] = p2 + radius2 * normal;
87
      }
88
89
90
91
92
93
94
      return distance;
    }
    else if (a <= EPSILON)
    {
      // First segment is degenerated
      s = 0.0;
      t = CLAMP(f / e, 0.0, 1.0);
95
    }
96
97
98
99
100
101
102
103
104
105
106
107
108
109
    else if (e <= EPSILON)
    {
      // Second segment is degenerated
      s = CLAMP(-c / a, 0.0, 1.0);
      t = 0.0;
    }
    else
    {
      // Always non-negative, equal 0 if the segments are parallel
      FCL_REAL denom = a*e-b*b;

      if (denom != 0.0)
      {
        s = CLAMP((b*f-c*e) / denom, 0.0, 1.0);
110
      }
111
112
113
      else
      {
        s = 0.0;
114
115
      }

116
117
118
119
120
121
122
123
124
125
126
      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);
      }
127
128
    }

129
130
131
132
133
    // witness points achieving the distance between the two segments
    const Vec3f& w1 = p1 + s * d1;
    const Vec3f& w2 = p2 + t * d2;
    FCL_REAL distance = (w1 - w2).norm();
    Vec3f normal = (w1 - w2) / distance;
134

135
136
137
138
139
140
141
142
    // 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;
143
    }
144
    return distance;
145
146
147
  }

  template <>
Lucile Remigy's avatar
Lucile Remigy committed
148
  std::size_t ShapeShapeCollide <Capsule, Capsule>
149
150
  (const CollisionGeometry* o1, const Transform3f& tf1,
   const CollisionGeometry* o2, const Transform3f& tf2,
151
   const GJKSolver*, const CollisionRequest& request,
152
153
   CollisionResult& result)
  {
154
    GJKSolver* unused = 0x0;
155
156
157
    DistanceResult distanceResult;
    DistanceRequest distanceRequest (request.enable_contact);

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

161
162
163
164
165
166
167
    if (distance > 0)
    {
      return 0;
    }
    else
    {
      Contact contact (o1, o2, -1, -1);
168
169
      const Vec3f& p1 = distanceResult.nearest_points [0];
      const Vec3f& p2 = distanceResult.nearest_points [1];
170
      contact.pos = 0.5 * (p1+p2);
171
      contact.normal = (p2-p1)/(p2-p1).norm ();
172
      result.addContact(contact);
173
174
175
      return 1;
    }
  }
176

177
} // namespace fcl
178
179

} // namespace hpp