Commit 67e2cd13 authored by Lucile Remigy's avatar Lucile Remigy
Browse files

delete PolySolver

parent bc75290c
......@@ -41,42 +41,19 @@
#include <hpp/fcl/math/transform.h>
#include <boost/math/special_functions/erf.hpp>
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)
......
Markdown is supported
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