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
a4c4369d
Commit
a4c4369d
authored
Sep 25, 2019
by
Lucile Remigy
Browse files
Modif BVTesting -> BVDisjoints or BVDistanceLowerBound/ 0 tests failed
parent
be967892
Changes
8
Hide whitespace changes
Inline
Side-by-side
src/traversal/traversal_node_base.cpp
View file @
a4c4369d
...
...
@@ -97,7 +97,7 @@ DistanceTraversalNodeBase::~DistanceTraversalNodeBase()
{
}
FCL_REAL
DistanceTraversalNodeBase
::
BV
Testing
(
int
/*b1*/
,
int
/*b2*/
)
const
FCL_REAL
DistanceTraversalNodeBase
::
BV
DistanceLowerBound
(
int
/*b1*/
,
int
/*b2*/
)
const
{
return
std
::
numeric_limits
<
FCL_REAL
>::
max
();
}
...
...
src/traversal/traversal_node_base.h
View file @
a4c4369d
...
...
@@ -98,13 +98,13 @@ public:
virtual
~
CollisionTraversalNodeBase
();
/// @brief BV test between b1 and b2
virtual
bool
BV
Testing
(
int
b1
,
int
b2
)
const
=
0
;
virtual
bool
BV
Disjoints
(
int
b1
,
int
b2
)
const
=
0
;
/// BV test between b1 and b2
/// \param b1, b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
virtual
bool
BV
Testing
(
int
b1
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
=
0
;
virtual
bool
BV
Disjoints
(
int
b1
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
=
0
;
/// @brief Leaf test between node b1 and b2, if they are both leafs
virtual
void
leafTesting
(
int
/*b1*/
,
int
/*b2*/
,
FCL_REAL
&
/*sqrDistLowerBound*/
)
const
...
...
@@ -139,7 +139,7 @@ public:
/// @brief BV test between b1 and b2
/// \return a lower bound of the distance between the two BV.
/// \note except for OBB, this method returns the distance.
virtual
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
;
virtual
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
;
/// @brief Leaf test between node b1 and b2, if they are both leafs
virtual
void
leafTesting
(
int
b1
,
int
b2
)
const
=
0
;
...
...
src/traversal/traversal_node_bvh_shape.h
View file @
a4c4369d
...
...
@@ -166,7 +166,7 @@ public:
}
/// @brief BV culling test in one BVTT node
bool
BV
Testing
(
int
b1
,
int
/*b2*/
)
const
bool
BV
Disjoints
(
int
b1
,
int
/*b2*/
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
if
(
RTIsIdentity
)
...
...
@@ -180,7 +180,7 @@ public:
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
/// @brief BV culling test in one BVTT node
bool
BV
Testing
(
int
b1
,
int
/*b2*/
,
FCL_REAL
&
sqrDistLowerBound
)
const
bool
BV
Disjoints
(
int
b1
,
int
/*b2*/
,
FCL_REAL
&
sqrDistLowerBound
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
bool
res
;
...
...
@@ -327,7 +327,7 @@ public:
/// BV test between b1 and b2
/// \param b2 Bounding volumes to test,
bool
BV
Testing
(
int
/*b1*/
,
int
b2
)
const
bool
BV
Disjoints
(
int
/*b1*/
,
int
b2
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
if
(
RTIsIdentity
)
...
...
@@ -340,7 +340,7 @@ public:
/// \param b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
bool
BV
Testing
(
int
/*b1*/
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
bool
BV
Disjoints
(
int
/*b1*/
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
bool
res
;
...
...
@@ -493,7 +493,7 @@ public:
}
/// @brief BV culling test in one BVTT node
FCL_REAL
BV
Testing
(
int
b1
,
int
/*b2*/
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
/*b2*/
)
const
{
return
model1
->
getBV
(
b1
).
bv
.
distance
(
model2_bv
);
}
...
...
@@ -541,7 +541,7 @@ public:
}
/// @brief BV culling test in one BVTT node
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
return
model1_bv
.
distance
(
model2
->
getBV
(
b2
).
bv
);
}
...
...
@@ -700,7 +700,7 @@ public:
{
}
FCL_REAL
BV
Testing
(
int
b1
,
int
/*b2*/
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
/*b2*/
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
return
distance
(
this
->
tf1
.
getRotation
(),
this
->
tf1
.
getTranslation
(),
this
->
model2_bv
,
this
->
model1
->
getBV
(
b1
).
bv
);
...
...
@@ -732,7 +732,7 @@ public:
{
}
FCL_REAL
BV
Testing
(
int
b1
,
int
/*b2*/
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
/*b2*/
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
return
distance
(
this
->
tf1
.
getRotation
(),
this
->
tf1
.
getTranslation
(),
this
->
model2_bv
,
this
->
model1
->
getBV
(
b1
).
bv
);
...
...
@@ -765,7 +765,7 @@ public:
}
FCL_REAL
BV
Testing
(
int
b1
,
int
/*b2*/
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
/*b2*/
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
return
distance
(
this
->
tf1
.
getRotation
(),
this
->
tf1
.
getTranslation
(),
this
->
model2_bv
,
this
->
model1
->
getBV
(
b1
).
bv
);
...
...
@@ -856,7 +856,7 @@ public:
{
}
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
return
distance
(
this
->
tf2
.
getRotation
(),
this
->
tf2
.
getTranslation
(),
this
->
model1_bv
,
this
->
model2
->
getBV
(
b2
).
bv
);
...
...
@@ -888,7 +888,7 @@ public:
{
}
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
return
distance
(
this
->
tf2
.
getRotation
(),
this
->
tf2
.
getTranslation
(),
this
->
model1_bv
,
this
->
model2
->
getBV
(
b2
).
bv
);
...
...
@@ -920,7 +920,7 @@ public:
{
}
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
return
distance
(
this
->
tf2
.
getRotation
(),
this
->
tf2
.
getTranslation
(),
this
->
model1_bv
,
this
->
model2
->
getBV
(
b2
).
bv
);
...
...
src/traversal/traversal_node_bvhs.cpp
View file @
a4c4369d
...
...
@@ -152,7 +152,7 @@ void MeshDistanceTraversalNodeRSS::postprocess()
details
::
distancePostprocessOrientedNode
(
model1
,
model2
,
tf1
,
request
,
*
result
);
}
FCL_REAL
MeshDistanceTraversalNodeRSS
::
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
MeshDistanceTraversalNodeRSS
::
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
if
(
enable_statistics
)
num_bv_tests
++
;
return
distance
(
R
,
T
,
model1
->
getBV
(
b1
).
bv
,
model2
->
getBV
(
b2
).
bv
);
...
...
@@ -180,7 +180,7 @@ void MeshDistanceTraversalNodekIOS::postprocess()
details
::
distancePostprocessOrientedNode
(
model1
,
model2
,
tf1
,
request
,
*
result
);
}
FCL_REAL
MeshDistanceTraversalNodekIOS
::
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
MeshDistanceTraversalNodekIOS
::
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
if
(
enable_statistics
)
num_bv_tests
++
;
return
distance
(
R
,
T
,
model1
->
getBV
(
b1
).
bv
,
model2
->
getBV
(
b2
).
bv
);
...
...
@@ -208,7 +208,7 @@ void MeshDistanceTraversalNodeOBBRSS::postprocess()
details
::
distancePostprocessOrientedNode
(
model1
,
model2
,
tf1
,
request
,
*
result
);
}
FCL_REAL
MeshDistanceTraversalNodeOBBRSS
::
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
MeshDistanceTraversalNodeOBBRSS
::
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
if
(
enable_statistics
)
num_bv_tests
++
;
return
distance
(
R
,
T
,
model1
->
getBV
(
b1
).
bv
,
model2
->
getBV
(
b2
).
bv
);
...
...
src/traversal/traversal_node_bvhs.h
View file @
a4c4369d
...
...
@@ -157,7 +157,7 @@ public:
}
/// @brief BV culling test in one BVTT node
bool
BV
Testing
(
int
b1
,
int
b2
)
const
bool
BV
Disjoints
(
int
b1
,
int
b2
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
if
(
RTIsIdentity
)
...
...
@@ -171,7 +171,7 @@ public:
/// \param b1, b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
bool
BV
Testing
(
int
b1
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
bool
BV
Disjoints
(
int
b1
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
{
if
(
this
->
enable_statistics
)
this
->
num_bv_tests
++
;
if
(
RTIsIdentity
)
...
...
@@ -271,7 +271,7 @@ typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
namespace
details
{
template
<
typename
BV
>
struct
DistanceTraversalBV
Testing
_impl
template
<
typename
BV
>
struct
DistanceTraversalBV
DistanceLowerBound
_impl
{
static
FCL_REAL
run
(
const
BVNode
<
BV
>&
b1
,
const
BVNode
<
BV
>&
b2
)
{
...
...
@@ -279,7 +279,7 @@ namespace details
}
};
template
<
>
struct
DistanceTraversalBV
Testing
_impl
<
OBB
>
template
<
>
struct
DistanceTraversalBV
DistanceLowerBound
_impl
<
OBB
>
{
static
FCL_REAL
run
(
const
BVNode
<
OBB
>&
b1
,
const
BVNode
<
OBB
>&
b2
)
{
...
...
@@ -361,10 +361,10 @@ public:
}
/// @brief BV culling test in one BVTT node
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
{
if
(
enable_statistics
)
num_bv_tests
++
;
return
details
::
DistanceTraversalBV
Testing
_impl
<
BV
>
return
details
::
DistanceTraversalBV
DistanceLowerBound
_impl
<
BV
>
::
run
(
model1
->
getBV
(
b1
),
model2
->
getBV
(
b2
));
}
...
...
@@ -457,7 +457,7 @@ public:
void
postprocess
();
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
;
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
;
void
leafTesting
(
int
b1
,
int
b2
)
const
;
...
...
@@ -475,7 +475,7 @@ public:
void
postprocess
();
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
;
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
;
void
leafTesting
(
int
b1
,
int
b2
)
const
;
...
...
@@ -492,9 +492,9 @@ public:
void
postprocess
();
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
)
const
;
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
)
const
;
FCL_REAL
BV
Testing
(
int
b1
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
;
FCL_REAL
BV
DistanceLowerBound
(
int
b1
,
int
b2
,
FCL_REAL
&
sqrDistLowerBound
)
const
;
void
leafTesting
(
int
b1
,
int
b2
)
const
;
...
...
src/traversal/traversal_node_octree.h
View file @
a4c4369d
...
...
@@ -903,12 +903,12 @@ public:
otsolver
=
NULL
;
}
bool
BV
Testing
(
int
,
int
)
const
bool
BV
Disjoints
(
int
,
int
)
const
{
return
false
;
}
bool
BV
Testing
(
int
,
int
,
FCL_REAL
&
)
const
bool
BV
Disjoints
(
int
,
int
,
FCL_REAL
&
)
const
{
return
false
;
}
...
...
@@ -940,12 +940,12 @@ public:
}
FCL_REAL
BV
Testing
(
int
,
int
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
,
int
)
const
{
return
-
1
;
}
bool
BV
Testing
(
int
,
int
,
FCL_REAL
&
)
const
bool
BV
DistanceLowerBound
(
int
,
int
,
FCL_REAL
&
)
const
{
return
false
;
}
...
...
@@ -975,12 +975,12 @@ public:
otsolver
=
NULL
;
}
bool
BV
Testing
(
int
,
int
)
const
bool
BV
Disjoints
(
int
,
int
)
const
{
return
false
;
}
bool
BV
Testing
(
int
,
int
,
FCL_REAL
&
)
const
bool
BV
Disjoints
(
int
,
int
,
FCL_REAL
&
)
const
{
return
false
;
}
...
...
@@ -1012,12 +1012,12 @@ public:
otsolver
=
NULL
;
}
bool
BV
Testing
(
int
,
int
)
const
bool
BV
Disjoints
(
int
,
int
)
const
{
return
false
;
}
bool
BV
Testing
(
int
,
int
,
fcl
::
FCL_REAL
&
)
const
bool
BV
Disjoints
(
int
,
int
,
fcl
::
FCL_REAL
&
)
const
{
return
false
;
}
...
...
@@ -1048,7 +1048,7 @@ public:
otsolver
=
NULL
;
}
FCL_REAL
BV
Testing
(
int
,
int
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
,
int
)
const
{
return
-
1
;
}
...
...
@@ -1077,7 +1077,7 @@ public:
otsolver
=
NULL
;
}
FCL_REAL
BV
Testing
(
int
,
int
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
,
int
)
const
{
return
-
1
;
}
...
...
@@ -1107,12 +1107,12 @@ public:
otsolver
=
NULL
;
}
bool
BV
Testing
(
int
,
int
)
const
bool
BV
Disjoints
(
int
,
int
)
const
{
return
false
;
}
bool
BV
Testing
(
int
,
int
,
FCL_REAL
&
)
const
bool
BV
Disjoints
(
int
,
int
,
FCL_REAL
&
)
const
{
return
false
;
}
...
...
@@ -1144,12 +1144,12 @@ public:
otsolver
=
NULL
;
}
bool
BV
Testing
(
int
,
int
)
const
bool
BV
Disjoints
(
int
,
int
)
const
{
return
false
;
}
bool
BV
Testing
(
int
,
int
,
FCL_REAL
&
)
const
bool
BV
Disjoints
(
int
,
int
,
FCL_REAL
&
)
const
{
return
false
;
}
...
...
@@ -1180,7 +1180,7 @@ public:
otsolver
=
NULL
;
}
FCL_REAL
BV
Testing
(
int
,
int
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
,
int
)
const
{
return
-
1
;
}
...
...
@@ -1210,7 +1210,7 @@ public:
otsolver
=
NULL
;
}
FCL_REAL
BV
Testing
(
int
,
int
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
,
int
)
const
{
return
-
1
;
}
...
...
src/traversal/traversal_node_shapes.h
View file @
a4c4369d
...
...
@@ -67,13 +67,13 @@ public:
}
/// @brief BV culling test in one BVTT node
bool
BV
Testing
(
int
,
int
)
const
bool
BV
Disjoints
(
int
,
int
)
const
{
return
false
;
}
/// @brief BV culling test in one BVTT node
bool
BV
Testing
(
int
,
int
,
FCL_REAL
&
)
const
bool
BV
Disjoints
(
int
,
int
,
FCL_REAL
&
)
const
{
throw
std
::
runtime_error
(
"Not implemented"
);
}
...
...
@@ -128,7 +128,7 @@ public:
}
/// @brief BV culling test in one BVTT node
FCL_REAL
BV
Testing
(
int
,
int
)
const
FCL_REAL
BV
DistanceLowerBound
(
int
,
int
)
const
{
return
-
1
;
// should not be used
}
...
...
src/traversal/traversal_recurse.cpp
View file @
a4c4369d
...
...
@@ -54,12 +54,12 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
{
updateFrontList
(
front_list
,
b1
,
b2
);
if
(
node
->
BV
Testing
(
b1
,
b2
,
sqrDistLowerBound
))
return
;
if
(
node
->
BV
Disjoints
(
b1
,
b2
,
sqrDistLowerBound
))
return
;
node
->
leafTesting
(
b1
,
b2
,
sqrDistLowerBound
);
return
;
}
if
(
node
->
BV
Testing
(
b1
,
b2
,
sqrDistLowerBound
))
{
if
(
node
->
BV
Disjoints
(
b1
,
b2
,
sqrDistLowerBound
))
{
updateFrontList
(
front_list
,
b1
,
b2
);
return
;
}
...
...
@@ -118,7 +118,7 @@ void collisionNonRecurse(CollisionTraversalNodeBase* node,
updateFrontList
(
front_list
,
a
,
b
);
// TODO should we test the BVs ?
//if(node->BV
Testing
(a, b, sdlb)) {
//if(node->BV
Dijsoints
(a, b, sdlb)) {
//if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
//continue;
//}
...
...
@@ -134,7 +134,7 @@ void collisionNonRecurse(CollisionTraversalNodeBase* node,
// }
// Check the BV
if
(
node
->
BV
Testing
(
a
,
b
,
sdlb
))
{
if
(
node
->
BV
Disjoints
(
a
,
b
,
sdlb
))
{
if
(
sdlb
<
sqrDistLowerBound
)
sqrDistLowerBound
=
sdlb
;
updateFrontList
(
front_list
,
a
,
b
);
continue
;
...
...
@@ -190,8 +190,8 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontLi
c2
=
node
->
getSecondRightChild
(
b2
);
}
FCL_REAL
d1
=
node
->
BV
Testing
(
a1
,
a2
);
FCL_REAL
d2
=
node
->
BV
Testing
(
c1
,
c2
);
FCL_REAL
d1
=
node
->
BV
DistanceLowerBound
(
a1
,
a2
);
FCL_REAL
d2
=
node
->
BV
DistanceLowerBound
(
c1
,
c2
);
if
(
d2
<
d1
)
{
...
...
@@ -317,11 +317,11 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFr
int
c2
=
node
->
getFirstRightChild
(
min_test
.
b1
);
bvt1
.
b1
=
c1
;
bvt1
.
b2
=
min_test
.
b2
;
bvt1
.
d
=
node
->
BV
Testing
(
bvt1
.
b1
,
bvt1
.
b2
);
bvt1
.
d
=
node
->
BV
DistanceLowerBound
(
bvt1
.
b1
,
bvt1
.
b2
);
bvt2
.
b1
=
c2
;
bvt2
.
b2
=
min_test
.
b2
;
bvt2
.
d
=
node
->
BV
Testing
(
bvt2
.
b1
,
bvt2
.
b2
);
bvt2
.
d
=
node
->
BV
DistanceLowerBound
(
bvt2
.
b1
,
bvt2
.
b2
);
}
else
{
...
...
@@ -329,11 +329,11 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFr
int
c2
=
node
->
getSecondRightChild
(
min_test
.
b2
);
bvt1
.
b1
=
min_test
.
b1
;
bvt1
.
b2
=
c1
;
bvt1
.
d
=
node
->
BV
Testing
(
bvt1
.
b1
,
bvt1
.
b2
);
bvt1
.
d
=
node
->
BV
DistanceLowerBound
(
bvt1
.
b1
,
bvt1
.
b2
);
bvt2
.
b1
=
min_test
.
b1
;
bvt2
.
b2
=
c2
;
bvt2
.
d
=
node
->
BV
Testing
(
bvt2
.
b1
,
bvt2
.
b2
);
bvt2
.
d
=
node
->
BV
DistanceLowerBound
(
bvt2
.
b1
,
bvt2
.
b2
);
}
bvtq
.
push
(
bvt1
);
...
...
@@ -378,7 +378,7 @@ void propagateBVHFrontListCollisionRecurse
}
else
{
if
(
!
node
->
BV
Testing
(
b1
,
b2
,
sqrDistLowerBound
))
{
if
(
!
node
->
BV
Disjoints
(
b1
,
b2
,
sqrDistLowerBound
))
{
front_iter
->
valid
=
false
;
if
(
node
->
firstOverSecond
(
b1
,
b2
))
{
int
c1
=
node
->
getFirstLeftChild
(
b1
);
...
...
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