fcl: intersect.h Source File

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #ifndef FCL_INTERSECT_H
00038 #define FCL_INTERSECT_H
00039 
00040 #include "fcl/math/transform.h"
00041 #include <boost/math/special_functions/erf.hpp>
00042 
00043 namespace fcl
00044 {
00045 
00047 class PolySolver
00048 {
00049 public:
00051   static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
00052 
00054   static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
00055 
00057   static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
00058 
00059 private:
00061   static inline bool isZero(FCL_REAL v);
00062 
00064   static inline bool cbrt(FCL_REAL v);
00065 
00066   static const FCL_REAL NEAR_ZERO_THRESHOLD;
00067 };
00068 
00069 
00071 class Intersect
00072 {
00073 
00074 public:
00075 
00080   static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00081                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00082                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00083 
00088   static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00089                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00090                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00091 
00093   static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00094                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00095                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00096 
00098   static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00099                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00100                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00101 
00103   static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00104                            const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
00105                            const Vec3f& L);
00106 
00108   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00109                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00110                                  Vec3f* contact_points = NULL,
00111                                  unsigned int* num_contact_points = NULL,
00112                                  FCL_REAL* penetration_depth = NULL,
00113                                  Vec3f* normal = NULL);
00114 
00115   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00116                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00117                                  const Matrix3f& R, const Vec3f& T,
00118                                  Vec3f* contact_points = NULL,
00119                                  unsigned int* num_contact_points = NULL,
00120                                  FCL_REAL* penetration_depth = NULL,
00121                                  Vec3f* normal = NULL);
00122 
00123   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00124                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00125                                  const Transform3f& tf,
00126                                  Vec3f* contact_points = NULL,
00127                                  unsigned int* num_contact_points = NULL,
00128                                  FCL_REAL* penetration_depth = NULL,
00129                                  Vec3f* normal = NULL);
00130   
00131 private:
00132 
00134   static int project6(const Vec3f& ax,
00135                       const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
00136                       const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
00137 
00139   static inline bool isZero(FCL_REAL v);
00140 
00142   static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00143                                            const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00144                                            FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
00145 
00147   static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
00148 
00150   static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
00151 
00157   static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
00158                                 Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
00159 
00161   static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00162                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00163                                    FCL_REAL t);
00164 
00166   static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00167                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00168                                    FCL_REAL t, Vec3f* q_i = NULL);
00169 
00171   static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00172                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00173                                    FCL_REAL t);
00174 
00176   static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
00177                           const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00178                           const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00179                           bool bVF,
00180                           FCL_REAL* ret);
00181 
00183   static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
00184                           const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00185                           const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
00186 
00189    
00190   static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00191                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00192                                    FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
00193 
00195   static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00196                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00197                                    FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
00198 
00200   static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00201                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00202                                    const Vec3f& L,
00203                                    FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
00204 
00206   static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00207                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
00208 
00210   static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
00211 
00213   static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
00214 
00216   static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
00217                                                   const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00218                                                   const Vec3f& tn, FCL_REAL to,
00219                                                   Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
00220 
00222   static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
00223 
00225   static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
00226 
00228   static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
00229 
00231   static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
00232 
00234   static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
00235 
00237   static FCL_REAL gaussianCDF(FCL_REAL x)
00238   {
00239     return 0.5 * boost::math::erfc(-x / sqrt(2.0));
00240   }
00241 
00242 
00243   static const FCL_REAL EPSILON;
00244   static const FCL_REAL NEAR_ZERO_THRESHOLD;
00245   static const FCL_REAL CCD_RESOLUTION;
00246   static const unsigned int MAX_TRIANGLE_CLIPS = 8;
00247 };
00248 
00249 class TriangleDistance
00250 {
00251 public:
00252 
00258   static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
00259                    Vec3f& VEC, Vec3f& X, Vec3f& Y);
00260 
00266   static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
00267 
00268   static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
00269                               const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
00270                               Vec3f& P, Vec3f& Q);
00271 
00278   static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
00279                               const Matrix3f& R, const Vec3f& Tl,
00280                               Vec3f& P, Vec3f& Q);
00281 
00282   static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
00283                               const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
00284                               const Matrix3f& R, const Vec3f& Tl,
00285                               Vec3f& P, Vec3f& Q);
00286 };
00287 
00288 
00289 }
00290 
00291 
00292 #endif