collision_checking: collision_checking: intersect.h Source 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 COLLISION_CHECKING_INTERSECT_H
00038 #define COLLISION_CHECKING_INTERSECT_H
00039 
00040 #include "collision_checking/vec_3f.h"
00041 #include "collision_checking/roughly_comp.h"
00042 #include "collision_checking/BVH_defs.h"
00043 
00045 namespace collision_checking
00046 {
00047 
00049 class PolySolver
00050 {
00051 public:
00053   static int solveLinear(BVH_REAL c[2], BVH_REAL s[1]);
00054 
00056   static int solveQuadric(BVH_REAL c[3], BVH_REAL s[2]);
00057 
00059   static int solveCubic(BVH_REAL c[4], BVH_REAL s[3]);
00060 
00061 private:
00063   static inline bool isZero(BVH_REAL v);
00064 
00066   static inline bool cbrt(BVH_REAL v);
00067 
00068   static const BVH_REAL NEAR_ZERO_THRESHOLD;
00069 };
00070 
00071 
00073 class Intersect
00074 {
00075 
00076 public:
00077 
00083   static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00084                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00085                            BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00086 
00092   static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00093                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00094                            BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00095 
00097   static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00098                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
00099                            BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00100 
00102   static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00103                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
00104                            BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
00105 
00107   static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00108                            const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
00109                            const Vec3f& L);
00110 
00112   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00113                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00114                                  Vec3f* contact_points = NULL,
00115                                  unsigned int* num_contact_points = NULL,
00116                                  BVH_REAL* penetration_depth = NULL,
00117                                  Vec3f* normal = NULL);
00118 
00119   static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
00120                                  const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
00121                                  const Vec3f R[3], const Vec3f& T,
00122                                  Vec3f* contact_points = NULL,
00123                                  unsigned int* num_contact_points = NULL,
00124                                  BVH_REAL* penetration_depth = NULL,
00125                                  Vec3f* normal = NULL);
00126 
00127 
00128 private:
00129 
00131   static int project6(const Vec3f& ax,
00132                       const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
00133                       const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
00134 
00136   static inline bool isZero(BVH_REAL v);
00137 
00139   static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00140                                            const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00141                                            BVH_REAL& l, BVH_REAL& r, bool bVF, BVH_REAL coeffs[], Vec3f* data = NULL);
00142 
00144   static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
00145 
00147   static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
00148 
00155   static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
00156                                 Vec3f* pa, Vec3f* pb, BVH_REAL* mua, BVH_REAL* mub);
00157 
00159   static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00160                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00161                                    BVH_REAL t);
00162 
00164   static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00165                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00166                                    BVH_REAL t, Vec3f* q_i = NULL);
00167 
00169   static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00170                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00171                                    BVH_REAL t);
00172 
00174   static bool solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
00175                           const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00176                           const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00177                           bool bVF,
00178                           BVH_REAL* ret);
00179 
00181   static bool solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
00182                           const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00183                           const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
00184 
00188   static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
00189                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
00190                                    BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d);
00191 
00193   static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00194                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
00195                                    BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d);
00196 
00198   static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
00199                                    const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
00200                                    const Vec3f& L,
00201                                    BVH_REAL* a, BVH_REAL* b, BVH_REAL* c);
00202 
00204   static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
00205                            const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
00206 
00207 
00208   static BVH_REAL distanceToPlane(const Vec3f& n, BVH_REAL t, const Vec3f& v);
00209 
00210   static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, BVH_REAL t);
00211 
00212   static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
00213                                                   const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
00214                                                   const Vec3f& tn, BVH_REAL to,
00215                                                   Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
00216 
00217   static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, BVH_REAL* t);
00218 
00219   static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, BVH_REAL* t);
00220 
00221   static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, BVH_REAL t, BVH_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
00222 
00223   static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, BVH_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
00224 
00225   static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, BVH_REAL t, Vec3f* clipped_point);
00226 
00227   static const BVH_REAL EPSILON;
00228   static const BVH_REAL NEAR_ZERO_THRESHOLD;
00229   static const BVH_REAL CCD_RESOLUTION;
00230   static const unsigned int MAX_TRIANGLE_CLIPS = 8;
00231 };
00232 
00233 class TriangleDistance
00234 {
00235 public:
00236 
00243   static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
00244                    Vec3f& VEC, Vec3f& X, Vec3f& Y);
00245 
00252   static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
00253 
00254   static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
00255                               const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
00256                               Vec3f& P, Vec3f& Q);
00257 
00265   static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
00266                               const Vec3f R[3], const Vec3f& Tl,
00267                               Vec3f& P, Vec3f& Q);
00268 
00269   static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
00270                               const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
00271                               const Vec3f R[3], const Vec3f& Tl,
00272                               Vec3f& P, Vec3f& Q);
00273 };
00274 
00275 
00276 }
00277 
00278 
00279 #endif