FCL  0.6.0
Flexible Collision Library
intersect.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_INTERSECT_H
39 #define FCL_NARROWPHASE_DETAIL_INTERSECT_H
40 
41 #include <limits>
42 #include "fcl/common/types.h"
43 #include "fcl/math/geometry.h"
44 #include "fcl/math/detail/polysolver.h"
45 
46 namespace fcl
47 {
48 
49 namespace detail
50 {
51 
53 template <typename S>
54 class Intersect
55 {
56 
57 public:
58 
63  static bool intersect_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
64  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& p1,
65  S* collision_time, Vector3<S>* p_i, bool useNewton = true);
66 
71  static bool intersect_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
72  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1,
73  S* collision_time, Vector3<S>* p_i, bool useNewton = true);
74 
76  static bool intersect_VF_filtered(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
77  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& p1,
78  S* collision_time, Vector3<S>* p_i, bool useNewton = true);
79 
81  static bool intersect_EE_filtered(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
82  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1,
83  S* collision_time, Vector3<S>* p_i, bool useNewton = true);
84 
86  static bool intersect_VE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
87  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& p1,
88  const Vector3<S>& L);
89 
91  static bool intersect_Triangle(
92  const Vector3<S>& P1,
93  const Vector3<S>& P2,
94  const Vector3<S>& P3,
95  const Vector3<S>& Q1,
96  const Vector3<S>& Q2,
97  const Vector3<S>& Q3,
98  Vector3<S>* contact_points = nullptr,
99  unsigned int* num_contact_points = nullptr,
100  S* penetration_depth = nullptr,
101  Vector3<S>* normal = nullptr);
102 
104  static bool intersect_Triangle_ODE_style(
105  const Vector3<S>& P1,
106  const Vector3<S>& P2,
107  const Vector3<S>& P3,
108  const Vector3<S>& Q1,
109  const Vector3<S>& Q2,
110  const Vector3<S>& Q3,
111  Vector3<S>* contact_points = nullptr,
112  unsigned int* num_contact_points = nullptr,
113  S* penetration_depth = nullptr,
114  Vector3<S>* normal = nullptr);
115 
116  static bool intersect_Triangle(
117  const Vector3<S>& P1,
118  const Vector3<S>& P2,
119  const Vector3<S>& P3,
120  const Vector3<S>& Q1,
121  const Vector3<S>& Q2,
122  const Vector3<S>& Q3,
123  const Matrix3<S>& R,
124  const Vector3<S>& T,
125  Vector3<S>* contact_points = nullptr,
126  unsigned int* num_contact_points = nullptr,
127  S* penetration_depth = nullptr,
128  Vector3<S>* normal = nullptr);
129 
130  static bool intersect_Triangle(
131  const Vector3<S>& P1,
132  const Vector3<S>& P2,
133  const Vector3<S>& P3,
134  const Vector3<S>& Q1,
135  const Vector3<S>& Q2,
136  const Vector3<S>& Q3,
137  const Transform3<S>& tf,
138  Vector3<S>* contact_points = nullptr,
139  unsigned int* num_contact_points = nullptr,
140  S* penetration_depth = nullptr,
141  Vector3<S>* normal = nullptr);
142 
143 private:
144 
146  static int project6(const Vector3<S>& ax,
147  const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3,
148  const Vector3<S>& q1, const Vector3<S>& q2, const Vector3<S>& q3);
149 
151  static bool isZero(S v);
152 
154  static bool solveCubicWithIntervalNewton(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
155  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
156  S& l, S& r, bool bVF, S coeffs[], Vector3<S>* data = nullptr);
157 
159  static bool insideTriangle(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& c, const Vector3<S>&p);
160 
162  static bool insideLineSegment(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& p);
163 
169  static bool linelineIntersect(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3, const Vector3<S>& p4,
170  Vector3<S>* pa, Vector3<S>* pb, S* mua, S* mub);
171 
173  static bool checkRootValidity_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
174  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vp,
175  S t);
176 
178  static bool checkRootValidity_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
179  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
180  S t, Vector3<S>* q_i = nullptr);
181 
183  static bool checkRootValidity_VE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
184  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp,
185  S t);
186 
188  static bool solveSquare(S a, S b, S c,
189  const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
190  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
191  bool bVF,
192  S* ret);
193 
195  static bool solveSquare(S a, S b, S c,
196  const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
197  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp);
198 
201 
202  static void computeCubicCoeff_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
203  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vp,
204  S* a, S* b, S* c, S* d);
205 
207  static void computeCubicCoeff_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
208  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
209  S* a, S* b, S* c, S* d);
210 
212  static void computeCubicCoeff_VE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
213  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp,
214  const Vector3<S>& L,
215  S* a, S* b, S* c);
216 
218  static bool intersectPreFiltering(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
219  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1);
220 
222  static S distanceToPlane(const Vector3<S>& n, S t, const Vector3<S>& v);
223 
225  static bool sameSideOfPlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3, const Vector3<S>& n, S t);
226 
228  static void clipTriangleByTriangleAndEdgePlanes(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3,
229  const Vector3<S>& t1, const Vector3<S>& t2, const Vector3<S>& t3,
230  const Vector3<S>& tn, S to,
231  Vector3<S> clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
232 
234  static bool buildTrianglePlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3, Vector3<S>* n, S* t);
235 
237  static bool buildEdgePlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& tn, Vector3<S>* n, S* t);
238 
240  static void computeDeepestPoints(Vector3<S>* clipped_points, unsigned int num_clipped_points, const Vector3<S>& n, S t, S* penetration_depth, Vector3<S>* deepest_points, unsigned int* num_deepest_points);
241 
243  static void clipPolygonByPlane(Vector3<S>* polygon_points, unsigned int num_polygon_points, const Vector3<S>& n, S t, Vector3<S> clipped_points[], unsigned int* num_clipped_points);
244 
246  static void clipSegmentByPlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& n, S t, Vector3<S>* clipped_point);
247 
249  static S gaussianCDF(S x);
250 
251  static constexpr S getEpsilon();
252  static constexpr S getNearZeroThreshold();
253  static constexpr S getCcdResolution();
254  static constexpr unsigned int getMaxTriangleClips();
255 };
256 
259 
260 } // namespace detail
261 } // namespace fcl
262 
263 #include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h"
264 
265 #endif
static bool intersect_Triangle_ODE_style(const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Vector3< S > &Q1, const Vector3< S > &Q2, const Vector3< S > &Q3, Vector3< S > *contact_points=nullptr, unsigned int *num_contact_points=nullptr, S *penetration_depth=nullptr, Vector3< S > *normal=nullptr)
CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3].
Definition: intersect-inl.h:643
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
static bool intersect_EE_filtered(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &d1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between two edges, using additional filter.
Definition: intersect-inl.h:584
static bool intersect_VF(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &p0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &p1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between one vertex and one face [a0, b0, c0] and [a1, b1, c1] are points for the triang...
Definition: intersect-inl.h:385
static bool intersect_EE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &d1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between two edges [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 [c0...
Definition: intersect-inl.h:451
static bool intersect_Triangle(const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Vector3< S > &Q1, const Vector3< S > &Q2, const Vector3< S > &Q3, Vector3< S > *contact_points=nullptr, unsigned int *num_contact_points=nullptr, S *penetration_depth=nullptr, Vector3< S > *normal=nullptr)
CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3].
Definition: intersect-inl.h:728
static bool intersect_VE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &p0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &p1, const Vector3< S > &L)
CCD intersect between one vertex and and one edge.
Definition: intersect-inl.h:517
static bool intersect_VF_filtered(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &p0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &p1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between one vertex and one face, using additional filter.
Definition: intersect-inl.h:570
CCD intersect kernel among primitives.
Definition: intersect.h:54