fcl » include » fcl
intersect.h

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 
00042 namespace fcl
00043 {
00044 
00046 class PolySolver
00047 {
00048 public:
00050   static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
00051 
00053   static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
00054 
00056   static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
00057 
00058 private:
00060   static inline bool isZero(FCL_REAL v);
00061 
00063   static inline bool cbrt(FCL_REAL v);
00064 
00065   static const FCL_REAL NEAR_ZERO_THRESHOLD;
00066 };
00067 
00068 
00070 class Intersect
00071 {
00072 
00073 public:
00074 
00079   static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00080                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00081                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00082 
00087   static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00088                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00089                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00090 
00092   static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00093                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00094                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00095 
00097   static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00098                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00099                            FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00100 
00102   static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00103                            const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
00104                            const Vec3f& L);
00105 
00107   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00108                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00109                                  Vec3f* contact_points = NULL,
00110                                  unsigned int* num_contact_points = NULL,
00111                                  FCL_REAL* penetration_depth = NULL,
00112                                  Vec3f* normal = NULL);
00113 
00114   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00115                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00116                                  const Matrix3f& R, const Vec3f& T,
00117                                  Vec3f* contact_points = NULL,
00118                                  unsigned int* num_contact_points = NULL,
00119                                  FCL_REAL* penetration_depth = NULL,
00120                                  Vec3f* normal = NULL);
00121 
00122   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00123                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00124                                  const Transform3f& tf,
00125                                  Vec3f* contact_points = NULL,
00126                                  unsigned int* num_contact_points = NULL,
00127                                  FCL_REAL* penetration_depth = NULL,
00128                                  Vec3f* normal = NULL);
00129   
00130 private:
00131 
00133   static int project6(const Vec3f& ax,
00134                       const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
00135                       const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
00136 
00138   static inline bool isZero(FCL_REAL v);
00139 
00141   static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00142                                            const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00143                                            FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
00144 
00146   static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
00147 
00149   static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
00150 
00156   static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
00157                                 Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
00158 
00160   static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00161                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00162                                    FCL_REAL t);
00163 
00165   static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00166                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00167                                    FCL_REAL t, Vec3f* q_i = NULL);
00168 
00170   static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00171                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00172                                    FCL_REAL t);
00173 
00175   static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
00176                           const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00177                           const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00178                           bool bVF,
00179                           FCL_REAL* ret);
00180 
00182   static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
00183                           const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00184                           const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
00185 
00188    
00189   static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00190                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00191                                    FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
00192 
00194   static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00195                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00196                                    FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
00197 
00199   static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00200                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00201                                    const Vec3f& L,
00202                                    FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
00203 
00205   static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00206                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
00207 
00209   static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
00210 
00212   static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
00213 
00215   static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
00216                                                   const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00217                                                   const Vec3f& tn, FCL_REAL to,
00218                                                   Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
00219 
00221   static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
00222 
00224   static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
00225 
00227   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);
00228 
00230   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);
00231 
00233   static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
00234 
00236   static FCL_REAL gaussianCDF(FCL_REAL x)
00237   {
00238     return 0.5 * erfc(-x / sqrt(2.0));
00239   }
00240 
00241 
00242   static const FCL_REAL EPSILON;
00243   static const FCL_REAL NEAR_ZERO_THRESHOLD;
00244   static const FCL_REAL CCD_RESOLUTION;
00245   static const unsigned int MAX_TRIANGLE_CLIPS = 8;
00246 };
00247 
00248 class TriangleDistance
00249 {
00250 public:
00251 
00257   static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
00258                    Vec3f& VEC, Vec3f& X, Vec3f& Y);
00259 
00265   static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
00266 
00267   static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
00268                               const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
00269                               Vec3f& P, Vec3f& Q);
00270 
00277   static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
00278                               const Matrix3f& R, const Vec3f& Tl,
00279                               Vec3f& P, Vec3f& Q);
00280 
00281   static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
00282                               const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
00283                               const Matrix3f& R, const Vec3f& Tl,
00284                               Vec3f& P, Vec3f& Q);
00285 };
00286 
00287 
00288 }
00289 
00290 
00291 #endif