sphere_box-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Toyota Research Institute.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Open Source Robotics Foundation nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #ifndef FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H
38 #define FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H
39 
41 
42 namespace fcl {
43 namespace detail {
44 
45 extern template FCL_EXPORT bool
46 sphereBoxIntersect(const Sphere<double>& sphere, const Transform3<double>& X_FS,
47  const Box<double>& box, const Transform3<double>& X_FB,
48  std::vector<ContactPoint<double>>* contacts);
49 
50 //==============================================================================
51 
52 extern template FCL_EXPORT bool
53 sphereBoxDistance(const Sphere<double>& sphere, const Transform3<double>& X_FS,
54  const Box<double>& box, const Transform3<double>& X_FB,
55  double* distance, Vector3<double>* p_FSb,
56  Vector3<double>* p_FBs);
57 
58 //==============================================================================
59 
60 // Helper function for box-sphere queries. Given a box defined in its canonical
61 // frame B (i.e., aligned to the axes and centered on the origin) and a query
62 // point Q, determines point N, the point *inside* the box nearest to Q. Note:
63 // this is *not* the nearest point on the surface of the box; if Q is inside
64 // the box, then the nearest point is Q itself.
65 // @param size The size of the box to query against.
66 // @param p_BQ The position vector from frame B's origin to the query
67 // point Q measured and expressed in frame B.
68 // @param[out] p_BN_ptr A position vector from frame B's origin to the point N
69 // measured and expressed in frame B.
70 // @returns true if the nearest point is a different point than the query point.
71 // @pre P_BN_ptr must point to a valid Vector3<S> instance.
72 template <typename S>
73 bool nearestPointInBox(const Vector3<S>& size, const Vector3<S>& p_BQ,
74  Vector3<S>* p_BN_ptr) {
75  assert(p_BN_ptr != nullptr);
76  Vector3<S>& p_BN = *p_BN_ptr;
77  // Clamp the point to the box. If we do *any* clamping we know the center was
78  // outside. If we did *no* clamping, the point is inside the box.
79  const Vector3<S> half_size = size / 2;
80  // The nearest point on the box (N) to Q measured in frame B.
81  bool clamped = false;
82  for (int i = 0; i < 3; ++i) {
83  p_BN(i) = p_BQ(i);
84  if (p_BQ(i) < -half_size(i)) {
85  clamped = true;
86  p_BN(i) = -half_size(i);
87  }
88  if (p_BQ(i) > half_size(i)) {
89  clamped = true;
90  p_BN(i) = half_size(i);
91  }
92  }
93  return clamped;
94 }
95 //==============================================================================
96 
97 template <typename S>
98 FCL_EXPORT bool sphereBoxIntersect(const Sphere<S>& sphere,
99  const Transform3<S>& X_FS, const Box<S>& box,
100  const Transform3<S>& X_FB,
101  std::vector<ContactPoint<S>>* contacts) {
102  const S r = sphere.radius;
103  // Find the sphere center C in the box's frame.
104  const Transform3<S> X_BS = X_FB.inverse() * X_FS;
105  const Vector3<S> p_BC = X_BS.translation();
106 
107  // Find N, the nearest point *inside* the box to the sphere center C (measured
108  // and expressed in frame B)
109  Vector3<S> p_BN;
110  bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN);
111 
112  // Compute the position vector from the nearest point N to the sphere center
113  // C in the common box frame B. If the center is inside the box, this will be
114  // the zero vector.
115  Vector3<S> p_CN_B = p_BN - p_BC;
116  S squared_distance = p_CN_B.squaredNorm();
117  // The nearest point to the sphere is *farther* than radius, they are *not*
118  // penetrating.
119  if (squared_distance > r * r)
120  return false;
121 
122  // Now we know they are colliding.
123 
124  if (contacts != nullptr) {
125  // Return values have been requested.
126  S depth{0};
127  Vector3<S> n_SB_B; // Normal pointing from sphere into box (in box's frame)
128  Vector3<S> p_BP; // Contact position (P) in the box frame.
129  // We want to make sure that differences exceed machine precision -- we
130  // don't want normal and contact position dominated by noise. However,
131  // because we apply an arbitrary rigid transform to the sphere's center, we
132  // lose bits of precision. For an arbitrary non-identity transform, 4 bits
133  // is the maximum possible precision loss. So, we only consider a point to
134  // be outside the box if it's distance is at least that epsilon.
135  // Furthermore, in finding the *near* face, a better candidate must be more
136  // than this epsilon closer to the sphere center (see the test in the
137  // else branch).
138  constexpr auto eps = 16 * constants<S>::eps();
139  if (N_is_not_C && squared_distance > eps * eps) {
140  // The center is on the outside. Normal direction is from C to N (computed
141  // above) and penetration depth is r - |p_BN - p_BC|. The contact position
142  // is 1/2 the penetration distance in the normal direction from p_BN.
143  S distance = sqrt(squared_distance);
144  n_SB_B = p_CN_B / distance;
145  depth = r - distance;
146  p_BP = p_BN + n_SB_B * (depth * 0.5);
147  } else {
148  // The center is inside. The sphere center projects onto all faces. The
149  // face that is closest defines the normal direction. The penetration
150  // depth is the distance to that face + radius. The position is the point
151  // midway between the projection point, and the point opposite the sphere
152  // center in the *negative* normal direction.
153  Vector3<S> half_size = box.side / 2;
154  S min_distance =
155  std::numeric_limits<typename constants<S>::Real>::infinity();
156  int min_axis = -1;
157  for (int i = 0; i < 3; ++i) {
158  S dist = p_BC(i) >= 0 ? half_size(i) - p_BC(i) : p_BC(i) + half_size(i);
159  // To be closer, the face has to be more than epsilon closer.
160  if (dist + eps < min_distance) {
161  min_distance = dist;
162  min_axis = i;
163  }
164  }
165  n_SB_B << 0, 0, 0;
166  // NOTE: This sign *may* seem counter-intuitive. A center nearest the +z
167  // face produces a normal in the -z direction; this is because the normal
168  // points from the sphere and into the box; and the penetration is *into*
169  // the +z face (so points in the -z direction). The same logic applies to
170  // all other directions.
171  n_SB_B(min_axis) = p_BC(min_axis) >= 0 ? -1 : 1;
172  depth = min_distance + r;
173  p_BP = p_BC + n_SB_B * ((r - min_distance) / 2);
174  }
175  contacts->emplace_back(X_FB.linear() * n_SB_B, X_FB * p_BP, depth);
176  }
177  return true;
178 }
179 
180 //==============================================================================
181 
182 template <typename S>
183 FCL_EXPORT bool sphereBoxDistance(const Sphere<S>& sphere,
184  const Transform3<S>& X_FS, const Box<S>& box,
185  const Transform3<S>& X_FB, S* distance,
186  Vector3<S>* p_FSb, Vector3<S>* p_FBs) {
187  // Find the sphere center C in the box's frame.
188  const Transform3<S> X_BS = X_FB.inverse() * X_FS;
189  const Vector3<S> p_BC = X_BS.translation();
190  const S r = sphere.radius;
191 
192  // Find N, the nearest point *inside* the box to the sphere center C (measured
193  // and expressed in frame B)
194  Vector3<S> p_BN;
195  bool N_is_not_C = nearestPointInBox(box.side, p_BC, &p_BN);
196 
197  if (N_is_not_C) {
198  // If N is not C, we know the sphere center is *outside* the box (but we
199  // don't know yet if the they are completely separated).
200 
201  // Compute the position vector from the nearest point N to the sphere center
202  // C in the frame B.
203  Vector3<S> p_NC_B = p_BC - p_BN;
204  S squared_distance = p_NC_B.squaredNorm();
205  if (squared_distance > r * r) {
206  // The distance to the nearest point is greater than the radius, we have
207  // proven separation.
208  S d{-1};
209  if (distance || p_FBs || p_FSb)
210  d = sqrt(squared_distance);
211  if (distance != nullptr)
212  *distance = d - r;
213  if (p_FBs != nullptr)
214  *p_FBs = X_FB * p_BN;
215  if (p_FSb != nullptr) {
216  const Vector3<S> p_BSb = (p_NC_B / d) * (d - r) + p_BN;
217  *p_FSb = X_FB * p_BSb;
218  }
219  return true;
220  }
221  }
222 
223  // We didn't *prove* separation, so we must be in penetration.
224  if (distance != nullptr) *distance = -1;
225  return false;
226 }
227 
228 } // namespace detail
229 } // namespace fcl
230 
231 #endif // FCL_NARROWPHASE_DETAIL_SPHEREBOX_INL_H
fcl::Sphere< double >
template class FCL_EXPORT Sphere< double >
fcl::detail::sphereBoxDistance
template FCL_EXPORT bool sphereBoxDistance(const Sphere< double > &sphere, const Transform3< double > &X_FS, const Box< double > &box, const Transform3< double > &X_FB, double *distance, Vector3< double > *p_FSb, Vector3< double > *p_FBs)
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::sphereBoxIntersect
template FCL_EXPORT bool sphereBoxIntersect(const Sphere< double > &sphere, const Transform3< double > &X_FS, const Box< double > &box, const Transform3< double > &X_FB, std::vector< ContactPoint< double >> *contacts)
fcl::Sphere::radius
S radius
Radius of the sphere.
Definition: sphere.h:59
r
S r
Definition: test_sphere_box.cpp:171
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
half_size
Vector3< S > half_size
Definition: test_sphere_box.cpp:169
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:50
sphere_box.h
fcl::detail::nearestPointInBox
bool nearestPointInBox(const Vector3< S > &size, const Vector3< S > &p_BQ, Vector3< S > *p_BN_ptr)
Definition: sphere_box-inl.h:73
fcl::Box< double >
template class FCL_EXPORT Box< double >
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:66


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:38:01