From c3c1f3c69b2b75da037e2a6c3d533eb82e84d87c Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 5 Dec 2018 14:15:35 +0100
Subject: [PATCH] Remove code that has become useless since 94c5713.
---
include/hpp/fcl/intersect.h | 182 -----
src/intersect.cpp | 1000 -------------------------
src/traversal/traversal_node_bvhs.cpp | 76 --
test/test_fcl_math.cpp | 68 --
4 files changed, 1326 deletions(-)
diff --git a/include/hpp/fcl/intersect.h b/include/hpp/fcl/intersect.h
index 12102ebd..28ca972a 100644
--- a/include/hpp/fcl/intersect.h
+++ b/include/hpp/fcl/intersect.h
@@ -67,188 +67,6 @@ private:
static const FCL_REAL NEAR_ZERO_THRESHOLD;
};
-
-/// @brief CCD intersect kernel among primitives
-class Intersect
-{
-
-public:
-
- /// @brief CCD intersect between one vertex and one face
- /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1
- /// p0 and p1 are points for vertex in time t0 and t1
- /// p_i returns the coordinate of the collision point
- static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /// @brief CCD intersect between two edges
- /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
- /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1
- /// p_i returns the coordinate of the collision point
- static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /// @brief CCD intersect between one vertex and one face, using additional filter
- static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /// @brief CCD intersect between two edges, using additional filter
- static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
-
- /// @brief CCD intersect between one vertex and and one edge
- static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
- const Vec3f& L);
-
- /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3]
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- FCL_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- FCL_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
- static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Transform3f& tf,
- Vec3f* contact_points = NULL,
- unsigned int* num_contact_points = NULL,
- FCL_REAL* penetration_depth = NULL,
- Vec3f* normal = NULL);
-
- /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to
- static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
- const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
- const Vec3f& tn, FCL_REAL to,
- Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
-
- /// @brief build a plane passed through triangle v1 v2 v3
- static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
-
-private:
-
- /// @brief Project function used in intersect_Triangle()
- static int project6(const Vec3f& ax,
- const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
- const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
-
- /// @brief Check whether one value is zero
- static inline bool isZero(FCL_REAL v);
-
- /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction
- static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
-
- /// @brief Check whether one point p is within triangle [a, b, c]
- static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
-
- /// @brief Check whether one point p is within a line segment [a, b]
- static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
-
- /// @brief Calculate the line segment papb that is the shortest route between
- /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where
- /// pa = p1 + mua (p2 - p1)
- /// pb = p3 + mub (p4 - p3)
- /// return FALSE if no solution exists.
- static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
-
- /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t
- static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL t);
-
- /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time
- static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL t, Vec3f* q_i = NULL);
-
- /// @brief Check whether a root for VE intersection is valid
- static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- FCL_REAL t);
-
- /// @brief Solve a square function for EE intersection (with interval restriction)
- static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- bool bVF,
- FCL_REAL* ret);
-
- /// @brief Solve a square function for VE intersection (with interval restriction)
- static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
-
- /// @brief Compute the cubic coefficients for VF intersection
- /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
-
- static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
-
- /// @brief Compute the cubic coefficients for EE intersection
- static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
-
- /// @brief Compute the cubic coefficients for VE intersection
- static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- const Vec3f& L,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
-
- /// @brief filter for intersection, works for both VF and EE
- static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
-
- /// @brief distance of point v to a plane n * x - t = 0
- static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
-
- /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0
- static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
-
-
-
- /// @brief build a plane pass through edge v1 and v2, normal is tn
- static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
-
- /// @brief compute the points which has deepest penetration depth
- 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);
-
- /// @brief clip polygon by plane
- 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);
-
- /// @brief clip a line segment by plane
- static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
-
- /// @brief compute the cdf(x)
- static FCL_REAL gaussianCDF(FCL_REAL x)
- {
- return 0.5 * boost::math::erfc(-x / sqrt(2.0));
- }
-
-
- static const FCL_REAL EPSILON;
- static const FCL_REAL NEAR_ZERO_THRESHOLD;
- static const FCL_REAL CCD_RESOLUTION;
- static const unsigned int MAX_TRIANGLE_CLIPS = 8;
-};
-
/// @brief Project functions
class Project
{
diff --git a/src/intersect.cpp b/src/intersect.cpp
index e27e03d7..f35a94a3 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -172,1006 +172,6 @@ int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3])
return num;
}
-
-
-const FCL_REAL Intersect::EPSILON = 1e-5;
-const FCL_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7;
-const FCL_REAL Intersect::CCD_RESOLUTION = 1e-7;
-
-
-bool Intersect::isZero(FCL_REAL v)
-{
- return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
-}
-
-/// @brief data: only used for EE, return the intersect point
-bool Intersect::solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data)
-{
- FCL_REAL v2[2]= {l*l,r*r};
- FCL_REAL v[2]= {l,r};
- FCL_REAL r_backup;
-
- unsigned char min3, min2, min1, max3, max2, max1;
-
- min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1;
- min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1;
- min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1;
-
- // bound the cubic
-
- FCL_REAL minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0];
- FCL_REAL major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0];
-
- if(major<0) return false;
- if(minor>0) return false;
-
- // starting here, the bounds have opposite values
- FCL_REAL m = 0.5 * (r + l);
-
- // bound the derivative
- FCL_REAL dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1];
- FCL_REAL dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1];
-
- if((dminor > 0)||(dmajor < 0)) // we can use Newton
- {
- FCL_REAL m2 = m*m;
- FCL_REAL fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0];
- FCL_REAL nl = m;
- FCL_REAL nu = m;
- if(fm>0)
- {
- nl-=(fm/dminor);
- nu-=(fm/dmajor);
- }
- else
- {
- nu-=(fm/dminor);
- nl-=(fm/dmajor);
- }
-
- //intersect with [l,r]
-
- if(nl>r) return false;
- if(nu<l) return false;
- if(nl>l)
- {
- if(nu<r) { l=nl; r=nu; m=0.5*(l+r); }
- else { l=nl; m=0.5*(l+r); }
- }
- else
- {
- if(nu<r) { r=nu; m=0.5*(l+r); }
- }
- }
-
- // sufficient temporal resolution, check root validity
- if((r-l)< CCD_RESOLUTION)
- {
- if(bVF)
- return checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r);
- else
- return checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, data);
- }
-
- r_backup = r, r = m;
- if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data))
- return true;
-
- l = m, r = r_backup;
- return solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data);
-}
-
-
-
-bool Intersect::insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p)
-{
- Vec3f ab = b - a;
- Vec3f ac = c - a;
- Vec3f n = ab.cross(ac);
-
- Vec3f pa = a - p;
- Vec3f pb = b - p;
- Vec3f pc = c - p;
-
- if((pb.cross(pc)).dot(n) < -EPSILON) return false;
- if((pc.cross(pa)).dot(n) < -EPSILON) return false;
- if((pa.cross(pb)).dot(n) < -EPSILON) return false;
-
- return true;
-}
-
-bool Intersect::insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p)
-{
- return (p - a).dot(p - b) <= 0;
-}
-
-/// @brief Calculate the line segment papb that is the shortest route between
-/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where
-/// pa = p1 + mua (p2 - p1)
-/// pb = p3 + mub (p4 - p3)
-/// Return FALSE if no solution exists.
-bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub)
-{
- Vec3f p31 = p1 - p3;
- Vec3f p34 = p4 - p3;
- if(fabs(p34[0]) < EPSILON && fabs(p34[1]) < EPSILON && fabs(p34[2]) < EPSILON)
- return false;
-
- Vec3f p12 = p2 - p1;
- if(fabs(p12[0]) < EPSILON && fabs(p12[1]) < EPSILON && fabs(p12[2]) < EPSILON)
- return false;
-
- FCL_REAL d3134 = p31.dot(p34);
- FCL_REAL d3412 = p34.dot(p12);
- FCL_REAL d3112 = p31.dot(p12);
- FCL_REAL d3434 = p34.dot(p34);
- FCL_REAL d1212 = p12.dot(p12);
-
- FCL_REAL denom = d1212 * d3434 - d3412 * d3412;
- if(fabs(denom) < EPSILON)
- return false;
- FCL_REAL numer = d3134 * d3412 - d3112 * d3434;
-
- *mua = numer / denom;
- if(*mua < 0 || *mua > 1)
- return false;
-
- *mub = (d3134 + d3412 * (*mua)) / d3434;
- if(*mub < 0 || *mub > 1)
- return false;
-
- *pa = p1 + p12 * (*mua);
- *pb = p3 + p34 * (*mub);
- return true;
-}
-
-bool Intersect::checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL t)
-{
- return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t);
-}
-
-bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL t, Vec3f* q_i)
-{
- Vec3f a = a0 + va * t;
- Vec3f b = b0 + vb * t;
- Vec3f c = c0 + vc * t;
- Vec3f d = d0 + vd * t;
- Vec3f p1, p2;
- FCL_REAL t_ab, t_cd;
- if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd))
- {
- if(q_i) *q_i = p1;
- return true;
- }
-
- return false;
-}
-
-bool Intersect::checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- FCL_REAL t)
-{
- return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t);
-}
-
-bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- bool bVF,
- FCL_REAL* ret)
-{
- FCL_REAL discriminant = b * b - 4 * a * c;
- if(discriminant < 0)
- return false;
-
- FCL_REAL sqrt_dis = sqrt(discriminant);
- FCL_REAL r1 = (-b + sqrt_dis) / (2 * a);
- bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false;
-
- FCL_REAL r2 = (-b - sqrt_dis) / (2 * a);
- bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false;
-
- if(v1 && v2)
- {
- *ret = (r1 > r2) ? r2 : r1;
- return true;
- }
- if(v1)
- {
- *ret = r1;
- return true;
- }
- if(v2)
- {
- *ret = r2;
- return true;
- }
-
- return false;
-}
-
-bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp)
-{
- if(isZero(a))
- {
- FCL_REAL t = -c/b;
- return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false;
- }
-
- FCL_REAL discriminant = b*b-4*a*c;
- if(discriminant < 0)
- return false;
-
- FCL_REAL sqrt_dis = sqrt(discriminant);
-
- FCL_REAL r1 = (-b+sqrt_dis) / (2 * a);
- bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false;
- if(v1) return true;
-
- FCL_REAL r2 = (-b-sqrt_dis) / (2 * a);
- bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false;
- return v2;
-}
-
-
-
-/// @brief Compute the cubic coefficients for VF case
-/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
-void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
-{
- Vec3f vavb = vb - va;
- Vec3f vavc = vc - va;
- Vec3f vavp = vp - va;
- Vec3f a0b0 = b0 - a0;
- Vec3f a0c0 = c0 - a0;
- Vec3f a0p0 = p0 - a0;
-
- Vec3f vavb_cross_vavc = vavb.cross(vavc);
- Vec3f vavb_cross_a0c0 = vavb.cross(a0c0);
- Vec3f a0b0_cross_vavc = a0b0.cross(vavc);
- Vec3f a0b0_cross_a0c0 = a0b0.cross(a0c0);
-
- *a = vavp.dot(vavb_cross_vavc);
- *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
- *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
- *d = a0p0.dot(a0b0_cross_a0c0);
-}
-
-void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
-{
- Vec3f vavb = vb - va;
- Vec3f vcvd = vd - vc;
- Vec3f vavc = vc - va;
- Vec3f c0d0 = d0 - c0;
- Vec3f a0b0 = b0 - a0;
- Vec3f a0c0 = c0 - a0;
- Vec3f vavb_cross_vcvd = vavb.cross(vcvd);
- Vec3f vavb_cross_c0d0 = vavb.cross(c0d0);
- Vec3f a0b0_cross_vcvd = a0b0.cross(vcvd);
- Vec3f a0b0_cross_c0d0 = a0b0.cross(c0d0);
-
- *a = vavc.dot(vavb_cross_vcvd);
- *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
- *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
- *d = a0c0.dot(a0b0_cross_c0d0);
-}
-
-void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- const Vec3f& L,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c)
-{
- Vec3f vbva = va - vb;
- Vec3f vbvp = vp - vb;
- Vec3f b0a0 = a0 - b0;
- Vec3f b0p0 = p0 - b0;
-
- Vec3f L_cross_vbvp = L.cross(vbvp);
- Vec3f L_cross_b0p0 = L.cross(b0p0);
-
- *a = L_cross_vbvp.dot(vbva);
- *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva);
- *c = L_cross_b0p0.dot(b0a0);
-}
-
-
-bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- *collision_time = 2.0;
-
- Vec3f vp, va, vb, vc;
- vp = p1 - p0;
- va = a1 - a0;
- vb = b1 - b0;
- vc = c1 - c0;
-
- FCL_REAL a, b, c, d;
- computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d);
-
- if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
- {
- return false;
- }
-
-
- /// if(isZero(a))
- /// {
- /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time);
- /// }
-
- FCL_REAL coeffs[4];
- coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
-
- if(useNewton)
- {
- FCL_REAL l = 0;
- FCL_REAL r = 1;
-
- if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs))
- {
- *collision_time = 0.5 * (l + r);
- }
- }
- else
- {
- FCL_REAL roots[3];
- int num = PolySolver::solveCubic(coeffs, roots);
- for(int i = 0; i < num; ++i)
- {
- FCL_REAL r = roots[i];
- if(r < 0 || r > 1) continue;
- if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r))
- {
- *collision_time = r;
- break;
- }
- }
- }
-
- if(*collision_time > 1)
- {
- return false;
- }
-
- *p_i = vp * (*collision_time) + p0;
- return true;
-}
-
-bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- *collision_time = 2.0;
-
- Vec3f va, vb, vc, vd;
- va = a1 - a0;
- vb = b1 - b0;
- vc = c1 - c0;
- vd = d1 - d0;
-
- FCL_REAL a, b, c, d;
- computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d);
-
- if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
- {
- return false;
- }
-
- /// if(isZero(a))
- /// {
- /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false);
- /// }
-
-
- FCL_REAL coeffs[4];
- coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
-
- if(useNewton)
- {
- FCL_REAL l = 0;
- FCL_REAL r = 1;
-
- if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i))
- {
- *collision_time = (l + r) * 0.5;
- }
- }
- else
- {
- FCL_REAL roots[3];
- int num = PolySolver::solveCubic(coeffs, roots);
- for(int i = 0; i < num; ++i)
- {
- FCL_REAL r = roots[i];
- if(r < 0 || r > 1) continue;
-
- if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i))
- {
- *collision_time = r;
- break;
- }
- }
- }
-
- if(*collision_time > 1)
- {
- return false;
- }
-
- return true;
-}
-
-
-bool Intersect::intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
- const Vec3f& L)
-{
- Vec3f va, vb, vp;
- va = a1 - a0;
- vb = b1 - b0;
- vp = p1 - p0;
-
- FCL_REAL a, b, c;
- computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c);
-
- if(isZero(a) && isZero(b) && isZero(c))
- return true;
-
- return solveSquare(a, b, c, a0, b0, p0, va, vb, vp);
-
-}
-
-
-/// @brief Prefilter for intersection, works for both VF and EE
-bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1)
-{
- Vec3f n0 = (b0 - a0).cross(c0 - a0);
- Vec3f n1 = (b1 - a1).cross(c1 - a1);
- Vec3f a0a1 = a1 - a0;
- Vec3f b0b1 = b1 - b0;
- Vec3f c0c1 = c1 - c0;
- Vec3f delta = (b0b1 - a0a1).cross(c0c1 - a0a1);
- Vec3f nx = (n0 + n1 - delta) * 0.5;
-
- Vec3f a0d0 = d0 - a0;
- Vec3f a1d1 = d1 - a1;
-
- FCL_REAL A = n0.dot(a0d0);
- FCL_REAL B = n1.dot(a1d1);
- FCL_REAL C = nx.dot(a0d0);
- FCL_REAL D = nx.dot(a1d1);
- FCL_REAL E = n1.dot(a0d0);
- FCL_REAL F = n0.dot(a1d1);
-
- if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0)
- return false;
- if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0)
- return false;
-
- return true;
-}
-
-bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1))
- {
- return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton);
- }
- else
- return false;
-}
-
-bool Intersect::intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
-{
- if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1))
- {
- return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton);
- }
- else
- return false;
-}
-
-bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points,
- unsigned int* num_contact_points,
- FCL_REAL* penetration_depth,
- Vec3f* normal)
-{
- Vec3f Q1_ = R * Q1 + T;
- Vec3f Q2_ = R * Q2 + T;
- Vec3f Q3_ = R * Q3 + T;
-
- return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
-}
-
-bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Transform3f& tf,
- Vec3f* contact_points,
- unsigned int* num_contact_points,
- FCL_REAL* penetration_depth,
- Vec3f* normal)
-{
- Vec3f Q1_ = tf.transform(Q1);
- Vec3f Q2_ = tf.transform(Q2);
- Vec3f Q3_ = tf.transform(Q3);
-
- return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
-}
-
-
-#if ODE_STYLE
-bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- Vec3f* contact_points,
- unsigned int* num_contact_points,
- FCL_REAL* penetration_depth,
- Vec3f* normal)
-{
-
-
- Vec3f n1;
- FCL_REAL t1;
- bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1);
- if(!b1) return false;
-
- Vec3f n2;
- FCL_REAL t2;
- bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
- if(!b2) return false;
-
- if(sameSideOfPlane(P1, P2, P3, n2, t2))
- return false;
-
- if(sameSideOfPlane(Q1, Q2, Q3, n1, t1))
- return false;
-
- Vec3f clipped_points1[MAX_TRIANGLE_CLIPS];
- unsigned int num_clipped_points1 = 0;
- Vec3f clipped_points2[MAX_TRIANGLE_CLIPS];
- unsigned int num_clipped_points2 = 0;
-
- Vec3f deepest_points1[MAX_TRIANGLE_CLIPS];
- unsigned int num_deepest_points1 = 0;
- Vec3f deepest_points2[MAX_TRIANGLE_CLIPS];
- unsigned int num_deepest_points2 = 0;
- FCL_REAL penetration_depth1 = -1, penetration_depth2 = -1;
-
- clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2);
-
- if(num_clipped_points2 == 0)
- return false;
-
- computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
- if(num_deepest_points2 == 0)
- return false;
-
- clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1);
- if(num_clipped_points1 == 0)
- return false;
-
- computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
- if(num_deepest_points1 == 0)
- return false;
-
-
- /// Return contact information
- if(contact_points && num_contact_points && penetration_depth && normal)
- {
- if(penetration_depth1 > penetration_depth2)
- {
- *num_contact_points = num_deepest_points2;
- for(unsigned int i = 0; i < num_deepest_points2; ++i)
- {
- contact_points[i] = deepest_points2[i];
- }
-
- *normal = n1;
- *penetration_depth = penetration_depth2;
- }
- else
- {
- *num_contact_points = num_deepest_points1;
- for(unsigned int i = 0; i < num_deepest_points1; ++i)
- {
- contact_points[i] = deepest_points1[i];
- }
-
- *normal = -n2;
- *penetration_depth = penetration_depth1;
- }
- }
-
- return true;
-}
-#else
-bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- Vec3f* contact_points,
- unsigned int* num_contact_points,
- FCL_REAL* penetration_depth,
- Vec3f* normal)
-{
- Vec3f p1 = P1 - P1;
- Vec3f p2 = P2 - P1;
- Vec3f p3 = P3 - P1;
- Vec3f q1 = Q1 - P1;
- Vec3f q2 = Q2 - P1;
- Vec3f q3 = Q3 - P1;
-
- Vec3f e1 = p2 - p1;
- Vec3f e2 = p3 - p2;
- Vec3f n1 = e1.cross(e2);
- if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f f1 = q2 - q1;
- Vec3f f2 = q3 - q2;
- Vec3f m1 = f1.cross(f2);
- if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f ef11 = e1.cross(f1);
- if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f ef12 = e1.cross(f2);
- if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f f3 = q1 - q3;
- Vec3f ef13 = e1.cross(f3);
- if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f ef21 = e2.cross(f1);
- if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f ef22 = e2.cross(f2);
- if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f ef23 = e2.cross(f3);
- if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f e3 = p1 - p3;
- Vec3f ef31 = e3.cross(f1);
- if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f ef32 = e3.cross(f2);
- if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f ef33 = e3.cross(f3);
- if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f g1 = e1.cross(n1);
- if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f g2 = e2.cross(n1);
- if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f g3 = e3.cross(n1);
- if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f h1 = f1.cross(m1);
- if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f h2 = f2.cross(m1);
- if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false;
-
- Vec3f h3 = f3.cross(m1);
- if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false;
-
- if(contact_points && num_contact_points && penetration_depth && normal)
- {
- Vec3f n1, n2;
- FCL_REAL t1, t2;
- bool n1defined = buildTrianglePlane(P1, P2, P3, &n1, &t1);
- bool n2defined = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
- assert (n1defined && n2defined);
-
- Vec3f deepest_points1[3];
- unsigned int num_deepest_points1 = 0;
- Vec3f deepest_points2[3];
- unsigned int num_deepest_points2 = 0;
- FCL_REAL penetration_depth1, penetration_depth2;
-
- Vec3f P[3] = {P1, P2, P3};
- Vec3f Q[3] = {Q1, Q2, Q3};
-
- computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
- computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
-
-
- if(penetration_depth1 > penetration_depth2)
- {
- *num_contact_points = std::min(num_deepest_points2, (unsigned int)2);
- for(unsigned int i = 0; i < *num_contact_points; ++i)
- {
- contact_points[i] = deepest_points2[i];
- }
-
- *normal = n1;
- *penetration_depth = penetration_depth2;
- }
- else
- {
- *num_contact_points = std::min(num_deepest_points1, (unsigned int)2);
- for(unsigned int i = 0; i < *num_contact_points; ++i)
- {
- contact_points[i] = deepest_points1[i];
- }
-
- *normal = -n2;
- *penetration_depth = penetration_depth1;
- }
- assert(*num_contact_points > 0);
- }
-
- return true;
-}
-#endif
-
-
-void Intersect::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)
-{
- *num_deepest_points = 0;
- FCL_REAL max_depth = -std::numeric_limits<FCL_REAL>::max();
- unsigned int num_deepest_points_ = 0;
- unsigned int num_neg = 0;
- unsigned int num_pos = 0;
- unsigned int num_zero = 0;
-
- for(unsigned int i = 0; i < num_clipped_points; ++i)
- {
- FCL_REAL dist = -distanceToPlane(n, t, clipped_points[i]);
- if(dist > EPSILON) num_pos++;
- else if(dist < -EPSILON) num_neg++;
- else num_zero++;
- if(dist > max_depth)
- {
- max_depth = dist;
- num_deepest_points_ = 1;
- deepest_points[num_deepest_points_ - 1] = clipped_points[i];
- }
- else if(dist + 1e-6 >= max_depth)
- {
- num_deepest_points_++;
- deepest_points[num_deepest_points_ - 1] = clipped_points[i];
- }
- }
-
- if(max_depth < -EPSILON)
- num_deepest_points_ = 0;
-
- if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0)))
- num_deepest_points_ = 0;
-
- *penetration_depth = max_depth;
- *num_deepest_points = num_deepest_points_;
-}
-
-void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
- const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
- const Vec3f& tn, FCL_REAL to,
- Vec3f clipped_points[], unsigned int* num_clipped_points,
- bool clip_triangle)
-{
- *num_clipped_points = 0;
- Vec3f temp_clip[MAX_TRIANGLE_CLIPS];
- Vec3f temp_clip2[MAX_TRIANGLE_CLIPS];
- unsigned int num_temp_clip = 0;
- unsigned int num_temp_clip2 = 0;
- Vec3f v[3] = {v1, v2, v3};
-
- Vec3f plane_n;
- FCL_REAL plane_dist;
-
- if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist))
- {
- clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip);
- if(num_temp_clip > 0)
- {
- if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist))
- {
- clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2);
- if(num_temp_clip2 > 0)
- {
- if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist))
- {
- if(clip_triangle)
- {
- num_temp_clip = 0;
- clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip);
- if(num_temp_clip > 0)
- {
- clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points);
- }
- }
- else
- {
- clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points);
- }
- }
- }
- }
- }
- }
-}
-
-void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points)
-{
- *num_clipped_points = 0;
-
- unsigned int num_clipped_points_ = 0;
- unsigned int vi;
- unsigned int prev_classify = 2;
- unsigned int classify;
- for(unsigned int i = 0; i <= num_polygon_points; ++i)
- {
- vi = (i % num_polygon_points);
- FCL_REAL d = distanceToPlane(n, t, polygon_points[i]);
- classify = ((d > EPSILON) ? 1 : 0);
- if(classify == 0)
- {
- if(prev_classify == 1)
- {
- if(num_clipped_points_ < MAX_TRIANGLE_CLIPS)
- {
- Vec3f tmp;
- clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
- if(num_clipped_points_ > 0)
- {
- if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > EPSILON)
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- else
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- }
-
- if(num_clipped_points_ < MAX_TRIANGLE_CLIPS && i < num_polygon_points)
- {
- clipped_points[num_clipped_points_] = polygon_points[vi];
- num_clipped_points_++;
- }
- }
- else
- {
- if(prev_classify == 0)
- {
- if(num_clipped_points_ < MAX_TRIANGLE_CLIPS)
- {
- Vec3f tmp;
- clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
- if(num_clipped_points_ > 0)
- {
- if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > EPSILON)
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- else
- {
- clipped_points[num_clipped_points_] = tmp;
- num_clipped_points_++;
- }
- }
- }
- }
-
- prev_classify = classify;
- }
-
- if(num_clipped_points_ > 2)
- {
- if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < EPSILON)
- {
- num_clipped_points_--;
- }
- }
-
- *num_clipped_points = num_clipped_points_;
-}
-
-void Intersect::clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point)
-{
- FCL_REAL dist1 = distanceToPlane(n, t, v1);
- Vec3f tmp = v2 - v1;
- FCL_REAL dist2 = tmp.dot(n);
- *clipped_point = tmp * (-dist1 / dist2) + v1;
-}
-
-FCL_REAL Intersect::distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v)
-{
- return n.dot(v) - t;
-}
-
-bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t)
-{
- Vec3f n_ = (v2 - v1).cross(v3 - v1);
- FCL_REAL norm2 = n_.squaredNorm();
- if (norm2 > 0)
- {
- *n = n_ / sqrt(norm2);
- *t = n->dot(v1);
- return true;
- }
-
- return false;
-}
-
-bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t)
-{
- Vec3f n_ = (v2 - v1).cross(tn);
- FCL_REAL norm2 = n_.squaredNorm();
- if (norm2 > 0)
- {
- *n = n_ / sqrt(norm2);
- *t = n->dot(v1);
- return true;
- }
-
- return false;
-}
-
-bool Intersect::sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t)
-{
- FCL_REAL dist1 = distanceToPlane(n, t, v1);
- FCL_REAL dist2 = dist1 * distanceToPlane(n, t, v2);
- FCL_REAL dist3 = dist1 * distanceToPlane(n, t, v3);
- if((dist2 > 0) && (dist3 > 0))
- return true;
- return false;
-}
-
-int Intersect::project6(const Vec3f& ax,
- const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
- const Vec3f& q1, const Vec3f& q2, const Vec3f& q3)
-{
- FCL_REAL P1 = ax.dot(p1);
- FCL_REAL P2 = ax.dot(p2);
- FCL_REAL P3 = ax.dot(p3);
- FCL_REAL Q1 = ax.dot(q1);
- FCL_REAL Q2 = ax.dot(q2);
- FCL_REAL Q3 = ax.dot(q3);
-
- FCL_REAL mn1 = std::min(P1, std::min(P2, P3));
- FCL_REAL mx2 = std::max(Q1, std::max(Q2, Q3));
- if(mn1 > mx2) return 0;
-
- FCL_REAL mx1 = std::max(P1, std::max(P2, P3));
- FCL_REAL mn2 = std::min(Q1, std::min(Q2, Q3));
-
- if(mn2 > mx1) return 0;
- return 1;
-}
-
-
-
void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
Vec3f& VEC, Vec3f& X, Vec3f& Y)
{
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index 88989ab8..ebc3b72f 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -43,82 +43,6 @@ namespace fcl
namespace details
{
-template<typename BV>
-static inline void meshCollisionOrientedNodeLeafTesting
-(int b1, int b2, const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- Vec3f* vertices1, Vec3f* vertices2, Triangle* tri_indices1,
- Triangle* tri_indices2, const Matrix3f& R, const Vec3f& T,
- const Transform3f& tf1, const Transform3f& tf2, bool enable_statistics,
- int& num_leaf_tests, const CollisionRequest& request,
- CollisionResult& result, FCL_REAL& sqrDistLowerBound)
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<BV>& node1 = model1->getBV(b1);
- const BVNode<BV>& node2 = model2->getBV(b2);
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- const Vec3f& p1 = vertices1[tri_id1[0]];
- const Vec3f& p2 = vertices1[tri_id1[1]];
- const Vec3f& p3 = vertices1[tri_id1[2]];
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- bool is_intersect = false;
-
- if(!request.enable_contact) // only interested in collision or not
- {
- if (request.enable_distance_lower_bound) {
- Vec3f P, Q;
- sqrDistLowerBound = TriangleDistance::sqrTriDistance
- (p1, p2, p3, q1, q2, q3, R, T, P, Q);
- if (sqrDistLowerBound == 0) {
- is_intersect = true;
- if(result.numContacts() < request.num_max_contacts)
- result.addContact(Contact(model1, model2, primitive_id1,
- primitive_id2));
- }
- } else {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) {
- is_intersect = true;
- if(result.numContacts() < request.num_max_contacts)
- result.addContact(Contact(model1, model2, primitive_id1,
- primitive_id2));
- }
- }
- }
- else // need compute the contact information
- {
- FCL_REAL penetration;
- Vec3f normal;
- unsigned int n_contacts;
- Vec3f contacts[2];
-
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T, contacts,
- &n_contacts, &penetration, &normal))
- {
- is_intersect = true;
-
- if(request.num_max_contacts < result.numContacts() + n_contacts)
- n_contacts = (request.num_max_contacts > result.numContacts()) ?
- (request.num_max_contacts - result.numContacts()) : 0;
-
- for(unsigned int i = 0; i < n_contacts; ++i)
- {
- result.addContact(Contact(model1, model2, primitive_id1, primitive_id2,
- tf1.transform(contacts[i]),
- tf1.getQuatRotation()* normal, penetration));
- }
- }
- }
-}
-
template<typename BV>
static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
index 74b9665e..2c22d2bf 100644
--- a/test/test_fcl_math.cpp
+++ b/test/test_fcl_math.cpp
@@ -147,71 +147,3 @@ BOOST_AUTO_TEST_CASE(transform)
// std::cout << output.lhs() << std::endl;
BOOST_CHECK(isEqual(rv + T, tf.transform(v)));
}
-
-BOOST_AUTO_TEST_CASE(intersect_triangle)
-{
- std::vector< Vec3f > points(3 * 6);
- points[0] = Vec3f(0,0,0);
- points[1] = Vec3f(1,0,0);
- points[2] = Vec3f(0,1,0);
-
- // FCL_REAL eps = +1e-16;
- FCL_REAL eps = 0;
- points[3] = Vec3f(0.5,0,eps);
- points[4] = Vec3f(0.5,0,1);
- points[5] = Vec3f(0.5,1,eps);
-
- eps = -1e-3;
- points[6] = Vec3f(0.5,0,eps);
- points[7] = Vec3f(0.5,0,1);
- points[8] = Vec3f(0.5,1,eps);
-
- eps = -1e-9;
- points[9] = Vec3f(0.5,0,eps);
- points[10] = Vec3f(0.5,0,1);
- points[11] = Vec3f(0.5,1,eps);
-
- points[12] = Vec3f(0.43977451324462891,0.047868609428405762,-0.074923992156982422);
- points[13] = Vec3f(0.409393310546875,0.048755228519439697,-0.083331555128097534);
- points[14] = Vec3f(0.41051089763641357,0.059760168194770813,-0.071275442838668823);
-
- points[15] = Vec3f(0.43746706770940053,0.04866138334047334,-0.075818714863365125);
- points[16] = Vec3f(0.44251195980451652,0.043831023891018804,-0.074980982849817135);
- points[17] = Vec3f(0.4213840328819074,0.076059133343436849,-0.07361578194185768);
-
- std::vector < int > pairs(2 * 4);
- pairs[0] = 0;
- pairs[1] = 3;
- pairs[2] = 0;
- pairs[3] = 6;
- pairs[4] = 0;
- pairs[5] = 9;
- pairs[6] = 12;
- pairs[7] = 15;
-
- for (std::size_t ip = 6; ip < pairs.size(); ip += 2) {
- Vec3f& p1 = points[pairs[ip + 0] ];
- Vec3f& p2 = points[pairs[ip + 0] + 1];
- Vec3f& p3 = points[pairs[ip + 0] + 2];
-
- Vec3f& q1 = points[pairs[ip + 1] ];
- Vec3f& q2 = points[pairs[ip + 1] + 1];
- Vec3f& q3 = points[pairs[ip + 1] + 2];
-
- FCL_REAL penetration;
- Vec3f normal;
- unsigned int n_contacts;
- Vec3f contacts[2];
-
- bool intersect =
- Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- contacts, &n_contacts, &penetration, &normal);
-
- if (intersect) {
- std::cout << ip << " intersect" << std::endl;
- BOOST_CHECK_MESSAGE (n_contacts > 0, "There shoud be at least 1 contact: " << n_contacts);
- } else {
- BOOST_CHECK_MESSAGE (n_contacts == 0, "There shoud be 0 contact: " << n_contacts);
- }
- }
-}
--
GitLab