FCL  0.6.0
Flexible Collision Library
sphere_sphere-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
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 
38 #ifndef FCL_NARROWPHASE_DETAIL_SPHERESPHERE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_SPHERESPHERE_INL_H
40 
41 #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h"
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 bool sphereSphereIntersect(const Sphere<double>& s1, const Transform3<double>& tf1,
52  const Sphere<double>& s2, const Transform3<double>& tf2,
53  std::vector<ContactPoint<double>>* contacts);
54 
55 //==============================================================================
56 extern template
57 bool sphereSphereDistance(const Sphere<double>& s1, const Transform3<double>& tf1,
58  const Sphere<double>& s2, const Transform3<double>& tf2,
59  double* dist, Vector3<double>* p1, Vector3<double>* p2);
60 
61 //==============================================================================
62 template <typename S>
63 bool sphereSphereIntersect(const Sphere<S>& s1, const Transform3<S>& tf1,
64  const Sphere<S>& s2, const Transform3<S>& tf2,
65  std::vector<ContactPoint<S>>* contacts)
66 {
67  Vector3<S> diff = tf2.translation() - tf1.translation();
68  S len = diff.norm();
69  if(len > s1.radius + s2.radius)
70  return false;
71 
72  if(contacts)
73  {
74  // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
75  // Otherwise, normal is pointing from center of object 1 to center of object 2
76  const Vector3<S> normal = len > 0 ? (diff / len).eval() : diff;
77  const Vector3<S> point = tf1.translation() + diff * s1.radius / (s1.radius + s2.radius);
78  const S penetration_depth = s1.radius + s2.radius - len;
79  contacts->emplace_back(normal, point, penetration_depth);
80  }
81 
82  return true;
83 }
84 
85 //==============================================================================
86 template <typename S>
87 bool sphereSphereDistance(const Sphere<S>& s1, const Transform3<S>& tf1,
88  const Sphere<S>& s2, const Transform3<S>& tf2,
89  S* dist, Vector3<S>* p1, Vector3<S>* p2)
90 {
91  Vector3<S> o1 = tf1.translation();
92  Vector3<S> o2 = tf2.translation();
93  Vector3<S> diff = o1 - o2;
94  S len = diff.norm();
95  if(len > s1.radius + s2.radius)
96  {
97  if(dist) *dist = len - (s1.radius + s2.radius);
98  if(p1) *p1 = tf1.inverse(Eigen::Isometry) * (o1 - diff * (s1.radius / len));
99  if(p2) *p2 = tf2.inverse(Eigen::Isometry) * (o2 + diff * (s2.radius / len));
100  return true;
101  }
102 
103  if(dist) *dist = -1;
104  return false;
105 }
106 
107 } // namespace detail
108 } // namespace fcl
109 
110 #endif
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: time.h:51