Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-fcl
Commits
67e2cd13
Commit
67e2cd13
authored
Sep 26, 2019
by
Lucile Remigy
Browse files
delete PolySolver
parent
bc75290c
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/intersect.h
View file @
67e2cd13
...
...
@@ -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
src/intersect.cpp
View file @
67e2cd13
...
...
@@ -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
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment