sphere_sphere.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018-2019, CNRS
5  * Author: Florent Lamiraux
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 #include <cmath>
37 #include <limits>
38 #include <hpp/fcl/math/transform.h>
42 
43 // Note that partial specialization of template functions is not allowed.
44 // Therefore, two implementations with the default narrow phase solvers are
45 // provided. If another narrow phase solver were to be used, the default
46 // template implementation would be called, unless the function is also
47 // specialized for this new type.
48 //
49 // One solution would be to make narrow phase solvers derive from an abstract
50 // class and specialize the template for this abstract class.
51 namespace hpp {
52 namespace fcl {
53 struct GJKSolver;
54 
55 template <>
57  const CollisionGeometry* o1, const Transform3f& tf1,
58  const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
59  const DistanceRequest&, DistanceResult& result) {
60  FCL_REAL epsilon = 1e-7;
61  const Sphere* s1 = static_cast<const Sphere*>(o1);
62  const Sphere* s2 = static_cast<const Sphere*>(o2);
63 
64  // We assume that spheres are centered at the origin of their frame.
65  const fcl::Vec3f& center1 = tf1.getTranslation();
66  const fcl::Vec3f& center2 = tf2.getTranslation();
67  FCL_REAL r1 = s1->radius;
68  FCL_REAL r2 = s2->radius;
69 
70  result.o1 = o1;
71  result.o2 = o2;
72  result.b1 = result.b2 = -1;
73  Vec3f c1c2 = center2 - center1;
74  FCL_REAL dist = c1c2.norm();
75  Vec3f unit(0, 0, 0);
76  if (dist > epsilon) unit = c1c2 / dist;
77  FCL_REAL penetrationDepth;
78  penetrationDepth = r1 + r2 - dist;
79  bool collision = (penetrationDepth >= 0);
80  result.min_distance = -penetrationDepth;
81  if (collision) {
82  // Take contact point at the middle of intersection between each sphere
83  // and segment [c1 c2].
84  FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2);
85  Vec3f contact = center1 + abscissa * unit;
86  result.nearest_points[0] = result.nearest_points[1] = contact;
87  return result.min_distance;
88  } else {
89  FCL_REAL abs1(r1), abs2(dist - r2);
90  result.nearest_points[0] = center1 + abs1 * unit;
91  result.nearest_points[1] = center1 + abs2 * unit;
92  }
93  return result.min_distance;
94 }
95 
97  const CollisionGeometry* o1, const Transform3f& tf1,
98  const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
99  const CollisionRequest& request, CollisionResult& result) {
100  FCL_REAL epsilon = 1e-7;
101  const Sphere* s1 = static_cast<const Sphere*>(o1);
102  const Sphere* s2 = static_cast<const Sphere*>(o2);
103 
104  // We assume that capsules are centered at the origin.
105  const fcl::Vec3f& center1 = tf1.getTranslation();
106  const fcl::Vec3f& center2 = tf2.getTranslation();
107  FCL_REAL r1 = s1->radius;
108  FCL_REAL r2 = s2->radius;
109  FCL_REAL margin = request.security_margin;
110 
111  Vec3f c1c2 = center2 - center1;
112  FCL_REAL dist = c1c2.norm();
113  Vec3f unit(0, 0, 0);
114  if (dist > epsilon) unit = c1c2 / dist;
115  // Unlike in distance computation, we consider the security margin.
116  FCL_REAL distToCollision = dist - (r1 + r2 + margin);
117 
118  internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
119  center1 + unit * r1,
120  center2 - unit * r2);
121  if (distToCollision <= request.collision_distance_threshold) {
122  // Take contact point at the middle of intersection between each sphere
123  // and segment [c1 c2].
124  FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2);
125  Vec3f contactPoint = center1 + abscissa * unit;
126  Contact contact(o1, o2, -1, -1, contactPoint, unit,
127  -(distToCollision + margin));
128  result.addContact(contact);
129  return 1;
130  }
131  return 0;
132 }
133 } // namespace fcl
134 
135 } // namespace hpp
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
request to the distance computation
Main namespace.
static FCL_REAL epsilon
Definition: simple.cpp:12
tuple tf2
void addContact(const Contact &c)
add one contact into result structure
request to the collision algorithm
double FCL_REAL
Definition: data_types.h:65
FCL_REAL collision_distance_threshold
threshold below which a collision is considered.
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const FCL_REAL &distance, const Vec3f &p0, const Vec3f &p1)
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
FCL_REAL radius
Radius of the sphere.
Center at zero point sphere.
r2
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
Contact information returned by collision.
The geometry for the object for collision or distance computation.
FCL_REAL ShapeShapeDistance< Sphere, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
tuple tf1
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:02