Commit 67e2cd13 by Lucile Remigy

### delete PolySolver

parent bc75290c
 ... ... @@ -41,42 +41,19 @@ #include #include namespace hpp { namespace hpp { namespace fcl { /// @brief A class solves polynomial degree (1,2,3) equations class PolySolver /// @brief CCD intersect kernel among primitives class Intersect { public: /// @brief Solve a linear equation with coefficients c, return roots s and number of roots static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]); /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]); /// @brief Solve a cubic function with coefficients c, return roots s and number of roots static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]); private: /// @brief Check whether v is zero static inline bool isZero(FCL_REAL v); static bool buildTrianglePlane (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t); }; // class Intersect /// @brief Compute v^{1/3} static inline bool cbrt(FCL_REAL v); static const FCL_REAL NEAR_ZERO_THRESHOLD; }; /// @brief CCD intersect kernel among primitives class Intersect { public: static bool buildTrianglePlane (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t); }; // class Intersect /// @brief Project functions class Project { ... ... @@ -213,6 +190,6 @@ public: } } // namespace hpp } // namespace hpp #endif
 ... ... @@ -46,134 +46,6 @@ namespace hpp { namespace fcl { const FCL_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9; bool PolySolver::isZero(FCL_REAL v) { return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD); } bool PolySolver::cbrt(FCL_REAL v) { return powf((float) v, (float) (1.0 / 3.0)); } int PolySolver::solveLinear(FCL_REAL c[2], FCL_REAL s[1]) { if(isZero(c[1])) return 0; s[0] = - c[0] / c[1]; return 1; } int PolySolver::solveQuadric(FCL_REAL c[3], FCL_REAL s[2]) { FCL_REAL p, q, D; // make sure we have a d2 equation if(isZero(c[2])) return solveLinear(c, s); // normal for: x^2 + px + q p = c[1] / (2.0 * c[2]); q = c[0] / c[2]; D = p * p - q; if(isZero(D)) { // one FCL_REAL root s[0] = s[1] = -p; return 1; } if(D < 0.0) // no real root return 0; else { // two real roots FCL_REAL sqrt_D = sqrt(D); s[0] = sqrt_D - p; s[1] = -sqrt_D - p; return 2; } } int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3]) { int i, num; FCL_REAL sub, A, B, C, sq_A, p, q, cb_p, D; const FCL_REAL ONE_OVER_THREE = 1 / 3.0; const FCL_REAL PI = 3.14159265358979323846; // make sure we have a d2 equation if(isZero(c[3])) return solveQuadric(c, s); // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 A = c[2] / c[3]; B = c[1] / c[3]; C = c[0] / c[3]; // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 sq_A = A * A; p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); // use Cardano's formula cb_p = p * p * p; D = q * q + cb_p; if(isZero(D)) { if(isZero(q)) { // one triple solution s[0] = 0.0; num = 1; } else { // one single and one FCL_REAL solution FCL_REAL u = cbrt(-q); s[0] = 2.0 * u; s[1] = -u; num = 2; } } else { if(D < 0.0) { // three real solutions FCL_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); FCL_REAL t = 2.0 * sqrt(-p); s[0] = t * cos(phi); s[1] = -t * cos(phi + PI / 3.0); s[2] = -t * cos(phi - PI / 3.0); num = 3; } else { // one real solution FCL_REAL sqrt_D = sqrt(D); FCL_REAL u = cbrt(sqrt_D + fabs(q)); if(q > 0.0) s[0] = - u + p / u ; else s[0] = u - p / u; num = 1; } } // re-substitute sub = ONE_OVER_THREE * A; for(i = 0; i < num; i++) s[i] -= sub; return num; } bool Intersect::buildTrianglePlane (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t) ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment