sphere_triangle-inl.h
Go to the documentation of this file.
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_SPHERETRIANGLE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_SPHERETRIANGLE_INL_H
40 
42 
44 
45 namespace fcl
46 {
47 
48 namespace detail
49 {
50 
51 //==============================================================================
52 extern template
53 double segmentSqrDistance(const Vector3<double>& from, const Vector3<double>& to,const Vector3<double>& p, Vector3<double>& nearest);
54 
55 //==============================================================================
56 extern template
57 bool projectInTriangle(const Vector3<double>& p1, const Vector3<double>& p2, const Vector3<double>& p3, const Vector3<double>& normal, const Vector3<double>& p);
58 
59 //==============================================================================
60 extern template
61 bool sphereTriangleIntersect(const Sphere<double>& s, const Transform3<double>& tf,
62  const Vector3<double>& P1, const Vector3<double>& P2, const Vector3<double>& P3, Vector3<double>* contact_points, double* penetration_depth, Vector3<double>* normal_);
63 
64 //==============================================================================
65 extern template
66 bool sphereTriangleDistance(const Sphere<double>& sp, const Transform3<double>& tf,
67  const Vector3<double>& P1, const Vector3<double>& P2, const Vector3<double>& P3,
68  double* dist);
69 
70 //==============================================================================
71 extern template
72 bool sphereTriangleDistance(const Sphere<double>& sp, const Transform3<double>& tf,
73  const Vector3<double>& P1, const Vector3<double>& P2, const Vector3<double>& P3,
74  double* dist, Vector3<double>* p1, Vector3<double>* p2);
75 
76 //==============================================================================
77 extern template
78 bool sphereTriangleDistance(const Sphere<double>& sp, const Transform3<double>& tf1,
79  const Vector3<double>& P1, const Vector3<double>& P2, const Vector3<double>& P3, const Transform3<double>& tf2,
80  double* dist, Vector3<double>* p1, Vector3<double>* p2);
81 
82 //==============================================================================
83 template <typename S>
84 S segmentSqrDistance(const Vector3<S>& from, const Vector3<S>& to,const Vector3<S>& p, Vector3<S>& nearest)
85 {
86  Vector3<S> diff = p - from;
87  Vector3<S> v = to - from;
88  S t = v.dot(diff);
89 
90  if(t > 0)
91  {
92  S dotVV = v.dot(v);
93  if(t < dotVV)
94  {
95  t /= dotVV;
96  diff -= v * t;
97  }
98  else
99  {
100  t = 1;
101  diff -= v;
102  }
103  }
104  else
105  t = 0;
106 
107  nearest = from + v * t;
108  return diff.dot(diff);
109 }
110 
111 //==============================================================================
112 template <typename S>
113 bool projectInTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3, const Vector3<S>& normal, const Vector3<S>& p)
114 {
115  Vector3<S> edge1(p2 - p1);
116  Vector3<S> edge2(p3 - p2);
117  Vector3<S> edge3(p1 - p3);
118 
119  Vector3<S> p1_to_p(p - p1);
120  Vector3<S> p2_to_p(p - p2);
121  Vector3<S> p3_to_p(p - p3);
122 
123  Vector3<S> edge1_normal(edge1.cross(normal));
124  Vector3<S> edge2_normal(edge2.cross(normal));
125  Vector3<S> edge3_normal(edge3.cross(normal));
126 
127  S r1, r2, r3;
128  r1 = edge1_normal.dot(p1_to_p);
129  r2 = edge2_normal.dot(p2_to_p);
130  r3 = edge3_normal.dot(p3_to_p);
131  if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
132  ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
133  return true;
134  return false;
135 }
136 
137 //==============================================================================
138 template <typename S>
140  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3, Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal_)
141 {
142  Vector3<S> normal = (P2 - P1).cross(P3 - P1);
143  normal.normalize();
144  const Vector3<S>& center = tf.translation();
145  const S& radius = s.radius;
146  S radius_with_threshold = radius + std::numeric_limits<S>::epsilon();
147  Vector3<S> p1_to_center = center - P1;
148  S distance_from_plane = p1_to_center.dot(normal);
149 
150  if(distance_from_plane < 0)
151  {
152  distance_from_plane *= -1;
153  normal *= -1;
154  }
155 
156  bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold);
157 
158  bool has_contact = false;
159  Vector3<S> contact_point;
160  if(is_inside_contact_plane)
161  {
162  if(projectInTriangle(P1, P2, P3, normal, center))
163  {
164  has_contact = true;
165  contact_point = center - normal * distance_from_plane;
166  }
167  else
168  {
169  S contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
170  Vector3<S> nearest_on_edge;
171  S distance_sqr;
172  distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge);
173  if(distance_sqr < contact_capsule_radius_sqr)
174  {
175  has_contact = true;
176  contact_point = nearest_on_edge;
177  }
178 
179  distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
180  if(distance_sqr < contact_capsule_radius_sqr)
181  {
182  has_contact = true;
183  contact_point = nearest_on_edge;
184  }
185 
186  distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
187  if(distance_sqr < contact_capsule_radius_sqr)
188  {
189  has_contact = true;
190  contact_point = nearest_on_edge;
191  }
192  }
193  }
194 
195  if(has_contact)
196  {
197  Vector3<S> contact_to_center = contact_point - center;
198  S distance_sqr = contact_to_center.squaredNorm();
199 
200  if(distance_sqr < radius_with_threshold * radius_with_threshold)
201  {
202  if(distance_sqr > 0)
203  {
204  S distance = std::sqrt(distance_sqr);
205  if(normal_) *normal_ = contact_to_center.normalized();
206  if(contact_points) *contact_points = contact_point;
207  if(penetration_depth) *penetration_depth = -(radius - distance);
208  }
209  else
210  {
211  if(normal_) *normal_ = -normal;
212  if(contact_points) *contact_points = contact_point;
213  if(penetration_depth) *penetration_depth = -radius;
214  }
215 
216  return true;
217  }
218  }
219 
220  return false;
221 }
222 
223 //==============================================================================
224 template <typename S>
226  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3,
227  S* dist)
228 {
229  // from geometric tools, very different from the collision code.
230 
231  const Vector3<S>& center = tf.translation();
232  S radius = sp.radius;
233  Vector3<S> diff = P1 - center;
234  Vector3<S> edge0 = P2 - P1;
235  Vector3<S> edge1 = P3 - P1;
236  S a00 = edge0.squaredNorm();
237  S a01 = edge0.dot(edge1);
238  S a11 = edge1.squaredNorm();
239  S b0 = diff.dot(edge0);
240  S b1 = diff.dot(edge1);
241  S c = diff.squaredNorm();
242  S det = fabs(a00*a11 - a01*a01);
243  S s = a01*b1 - a11*b0;
244  S t = a01*b0 - a00*b1;
245 
246  S sqr_dist;
247 
248  if(s + t <= det)
249  {
250  if(s < 0)
251  {
252  if(t < 0) // region 4
253  {
254  if(b0 < 0)
255  {
256  t = 0;
257  if(-b0 >= a00)
258  {
259  s = 1;
260  sqr_dist = a00 + 2*b0 + c;
261  }
262  else
263  {
264  s = -b0/a00;
265  sqr_dist = b0*s + c;
266  }
267  }
268  else
269  {
270  s = 0;
271  if(b1 >= 0)
272  {
273  t = 0;
274  sqr_dist = c;
275  }
276  else if(-b1 >= a11)
277  {
278  t = 1;
279  sqr_dist = a11 + 2*b1 + c;
280  }
281  else
282  {
283  t = -b1/a11;
284  sqr_dist = b1*t + c;
285  }
286  }
287  }
288  else // region 3
289  {
290  s = 0;
291  if(b1 >= 0)
292  {
293  t = 0;
294  sqr_dist = c;
295  }
296  else if(-b1 >= a11)
297  {
298  t = 1;
299  sqr_dist = a11 + 2*b1 + c;
300  }
301  else
302  {
303  t = -b1/a11;
304  sqr_dist = b1*t + c;
305  }
306  }
307  }
308  else if(t < 0) // region 5
309  {
310  t = 0;
311  if(b0 >= 0)
312  {
313  s = 0;
314  sqr_dist = c;
315  }
316  else if(-b0 >= a00)
317  {
318  s = 1;
319  sqr_dist = a00 + 2*b0 + c;
320  }
321  else
322  {
323  s = -b0/a00;
324  sqr_dist = b0*s + c;
325  }
326  }
327  else // region 0
328  {
329  // minimum at interior point
330  S inv_det = (1)/det;
331  s *= inv_det;
332  t *= inv_det;
333  sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
334  }
335  }
336  else
337  {
338  S tmp0, tmp1, numer, denom;
339 
340  if(s < 0) // region 2
341  {
342  tmp0 = a01 + b0;
343  tmp1 = a11 + b1;
344  if(tmp1 > tmp0)
345  {
346  numer = tmp1 - tmp0;
347  denom = a00 - 2*a01 + a11;
348  if(numer >= denom)
349  {
350  s = 1;
351  t = 0;
352  sqr_dist = a00 + 2*b0 + c;
353  }
354  else
355  {
356  s = numer/denom;
357  t = 1 - s;
358  sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
359  }
360  }
361  else
362  {
363  s = 0;
364  if(tmp1 <= 0)
365  {
366  t = 1;
367  sqr_dist = a11 + 2*b1 + c;
368  }
369  else if(b1 >= 0)
370  {
371  t = 0;
372  sqr_dist = c;
373  }
374  else
375  {
376  t = -b1/a11;
377  sqr_dist = b1*t + c;
378  }
379  }
380  }
381  else if(t < 0) // region 6
382  {
383  tmp0 = a01 + b1;
384  tmp1 = a00 + b0;
385  if(tmp1 > tmp0)
386  {
387  numer = tmp1 - tmp0;
388  denom = a00 - 2*a01 + a11;
389  if(numer >= denom)
390  {
391  t = 1;
392  s = 0;
393  sqr_dist = a11 + 2*b1 + c;
394  }
395  else
396  {
397  t = numer/denom;
398  s = 1 - t;
399  sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
400  }
401  }
402  else
403  {
404  t = 0;
405  if(tmp1 <= 0)
406  {
407  s = 1;
408  sqr_dist = a00 + 2*b0 + c;
409  }
410  else if(b0 >= 0)
411  {
412  s = 0;
413  sqr_dist = c;
414  }
415  else
416  {
417  s = -b0/a00;
418  sqr_dist = b0*s + c;
419  }
420  }
421  }
422  else // region 1
423  {
424  numer = a11 + b1 - a01 - b0;
425  if(numer <= 0)
426  {
427  s = 0;
428  t = 1;
429  sqr_dist = a11 + 2*b1 + c;
430  }
431  else
432  {
433  denom = a00 - 2*a01 + a11;
434  if(numer >= denom)
435  {
436  s = 1;
437  t = 0;
438  sqr_dist = a00 + 2*b0 + c;
439  }
440  else
441  {
442  s = numer/denom;
443  t = 1 - s;
444  sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
445  }
446  }
447  }
448  }
449 
450  // Account for numerical round-off error.
451  if(sqr_dist < 0)
452  sqr_dist = 0;
453 
454  if(sqr_dist > radius * radius)
455  {
456  if(dist) *dist = std::sqrt(sqr_dist) - radius;
457  return true;
458  }
459  else
460  {
461  if(dist) *dist = -1;
462  return false;
463  }
464 }
465 
466 //==============================================================================
467 template <typename S>
469  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3,
470  S* dist, Vector3<S>* p1, Vector3<S>* p2)
471 {
472  if(p1 || p2)
473  {
474  Vector3<S> o = tf.translation();
475  typename Project<S>::ProjectResult result;
476  result = Project<S>::projectTriangle(P1, P2, P3, o);
477  if(result.sqr_distance > sp.radius * sp.radius)
478  {
479  if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius;
480  Vector3<S> project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
481  Vector3<S> dir = o - project_p;
482  dir.normalize();
483  if(p1) { *p1 = o - dir * sp.radius; *p1 = tf.inverse(Eigen::Isometry) * (*p1); }
484  if(p2) *p2 = project_p;
485  return true;
486  }
487  else
488  return false;
489  }
490  else
491  {
492  return sphereTriangleDistance(sp, tf, P1, P2, P3, dist);
493  }
494 }
495 
496 //==============================================================================
497 template <typename S>
498 bool sphereTriangleDistance(const Sphere<S>& sp, const Transform3<S>& tf1,
499  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3, const Transform3<S>& tf2,
500  S* dist, Vector3<S>* p1, Vector3<S>* p2)
501 {
502  bool res = detail::sphereTriangleDistance(sp, tf1, tf2 * P1, tf2 * P2, tf2 * P3, dist, p1, p2);
503  if(p2) *p2 = tf2.inverse(Eigen::Isometry) * (*p2);
504 
505  return res;
506 }
507 
508 } // namespace detail
509 } // namespace fcl
510 
511 #endif
fcl::Sphere< double >
template class FCL_EXPORT Sphere< double >
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
project.h
fcl::detail::projectInTriangle
template bool projectInTriangle(const Vector3< double > &p1, const Vector3< double > &p2, const Vector3< double > &p3, const Vector3< double > &normal, const Vector3< double > &p)
fcl::detail::sphereTriangleIntersect
template bool sphereTriangleIntersect(const Sphere< double > &s, const Transform3< double > &tf, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal_)
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::Project::projectTriangle
static ProjectResult projectTriangle(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &p)
Project point p onto triangle a-b-c.
Definition: project-inl.h:77
fcl::detail::sphereTriangleDistance
template bool sphereTriangleDistance(const Sphere< double > &sp, const Transform3< double > &tf, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, double *dist)
fcl::detail::Project::ProjectResult
Definition: project.h:55
fcl::Sphere::radius
S radius
Radius of the sphere.
Definition: sphere.h:60
fcl::detail::Project::ProjectResult::parameterization
S parameterization[4]
Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the...
Definition: project.h:58
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
fcl::detail::Project::ProjectResult::sqr_distance
S sqr_distance
square distance from the query point to the projected simplex
Definition: project.h:61
fcl::detail::segmentSqrDistance
template double segmentSqrDistance(const Vector3< double > &from, const Vector3< double > &to, const Vector3< double > &p, Vector3< double > &nearest)
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
sphere_triangle.h


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49