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
5261d92b
Commit
5261d92b
authored
Aug 14, 2014
by
Florent Lamiraux
Browse files
Add a test on distance lower bound.
parent
6e699b0c
Changes
2
Hide whitespace changes
Inline
Side-by-side
test/CMakeLists.txt
View file @
5261d92b
...
...
@@ -15,6 +15,8 @@ include_directories(${Boost_INCLUDE_DIRS})
add_fcl_test
(
test_fcl_collision test_fcl_collision.cpp test_fcl_utility.cpp
)
add_fcl_test
(
test_fcl_distance test_fcl_distance.cpp test_fcl_utility.cpp
)
add_fcl_test
(
test_fcl_distance_lower_bound test_fcl_distance_lower_bound.cpp
test_fcl_utility.cpp
)
add_fcl_test
(
test_fcl_geometric_shapes test_fcl_geometric_shapes.cpp test_fcl_utility.cpp
)
#add_fcl_test(test_fcl_broadphase test_fcl_broadphase.cpp test_fcl_utility.cpp)
#add_fcl_test(test_fcl_shape_mesh_consistency test_fcl_shape_mesh_consistency.cpp test_fcl_utility.cpp)
...
...
test/test_fcl_distance_lower_bound.cpp
0 → 100644
View file @
5261d92b
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, CNRS-LAAS
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of CNRS-LAAS nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#define BOOST_TEST_MODULE "FCL_DISTANCE_LOWER_BOUND"
# include <boost/test/included/unit_test.hpp>
# include <boost/filesystem.hpp>
# include <fcl/fwd.hh>
# include <fcl/data_types.h>
# include <fcl/BV/OBBRSS.h>
# include <fcl/BVH/BVH_model.h>
# include <fcl/narrowphase/narrowphase.h>
# include <fcl/collision.h>
# include <fcl/distance.h>
# include "test_fcl_utility.h"
# include "fcl_resources/config.h"
using
fcl
::
Transform3f
;
using
fcl
::
Vec3f
;
using
fcl
::
Triangle
;
using
fcl
::
OBBRSS
;
using
fcl
::
BVHModel
;
using
fcl
::
CollisionResult
;
using
fcl
::
CollisionRequest
;
using
fcl
::
DistanceRequest
;
using
fcl
::
DistanceResult
;
using
fcl
::
CollisionObject
;
using
fcl
::
CollisionGeometryPtr_t
;
using
fcl
::
FCL_REAL
;
bool
testDistanceLowerBound
(
const
Transform3f
&
tf
,
const
CollisionGeometryPtr_t
&
m1
,
const
CollisionGeometryPtr_t
&
m2
,
FCL_REAL
&
distance
)
{
Transform3f
pose1
(
tf
),
pose2
;
CollisionRequest
request
;
request
.
enable_distance_lower_bound
=
true
;
CollisionResult
result
;
CollisionObject
co1
(
m1
,
pose1
);
CollisionObject
co2
(
m2
,
pose2
);
fcl
::
collide
(
&
co1
,
&
co2
,
request
,
result
);
distance
=
result
.
distance_lower_bound
;
return
result
.
isCollision
();
}
bool
testCollide
(
const
Transform3f
&
tf
,
const
CollisionGeometryPtr_t
&
m1
,
const
CollisionGeometryPtr_t
&
m2
)
{
Transform3f
pose1
(
tf
),
pose2
;
CollisionRequest
request
;
request
.
enable_distance_lower_bound
=
false
;
CollisionResult
result
;
CollisionObject
co1
(
m1
,
pose1
);
CollisionObject
co2
(
m2
,
pose2
);
fcl
::
collide
(
&
co1
,
&
co2
,
request
,
result
);
return
result
.
isCollision
();
}
bool
testDistance
(
const
Transform3f
&
tf
,
const
CollisionGeometryPtr_t
&
m1
,
const
CollisionGeometryPtr_t
&
m2
,
FCL_REAL
&
distance
)
{
Transform3f
pose1
(
tf
),
pose2
;
DistanceRequest
request
;
DistanceResult
result
;
CollisionObject
co1
(
m1
,
pose1
);
CollisionObject
co2
(
m2
,
pose2
);
fcl
::
distance
(
&
co1
,
&
co2
,
request
,
result
);
distance
=
result
.
min_distance
;
if
(
result
.
min_distance
<=
0
)
{
return
true
;
}
else
{
return
false
;
}
}
BOOST_AUTO_TEST_CASE
(
mesh_mesh
)
{
std
::
vector
<
Vec3f
>
p1
,
p2
;
std
::
vector
<
Triangle
>
t1
,
t2
;
boost
::
filesystem
::
path
path
(
TEST_RESOURCES_DIR
);
loadOBJFile
((
path
/
"env.obj"
).
string
().
c_str
(),
p1
,
t1
);
loadOBJFile
((
path
/
"rob.obj"
).
string
().
c_str
(),
p2
,
t2
);
boost
::
shared_ptr
<
BVHModel
<
OBBRSS
>
>
m1
(
new
BVHModel
<
OBBRSS
>
);
boost
::
shared_ptr
<
BVHModel
<
OBBRSS
>
>
m2
(
new
BVHModel
<
OBBRSS
>
);
m1
->
beginModel
();
m1
->
addSubModel
(
p1
,
t1
);
m1
->
endModel
();
m2
->
beginModel
();
m2
->
addSubModel
(
p2
,
t2
);
m2
->
endModel
();
std
::
vector
<
Transform3f
>
transforms
;
FCL_REAL
extents
[]
=
{
-
3000
,
-
3000
,
0
,
3000
,
3000
,
3000
};
std
::
size_t
n
=
100
;
bool
verbose
=
false
;
generateRandomTransforms
(
extents
,
transforms
,
n
);
// collision
for
(
std
::
size_t
i
=
0
;
i
<
transforms
.
size
();
++
i
)
{
FCL_REAL
distanceLowerBound
,
distance
;
bool
col1
,
col2
,
col3
;
col1
=
testDistanceLowerBound
(
transforms
[
i
],
m1
,
m2
,
distanceLowerBound
);
col2
=
testDistance
(
transforms
[
i
],
m1
,
m2
,
distance
);
col3
=
testCollide
(
transforms
[
i
],
m1
,
m2
);
std
::
cout
<<
"col1 = "
<<
col1
<<
", col2 = "
<<
col2
<<
", col3 = "
<<
col3
<<
std
::
endl
;
std
::
cout
<<
"distance lower bound = "
<<
distanceLowerBound
<<
std
::
endl
;
std
::
cout
<<
"distance = "
<<
distance
<<
std
::
endl
;
BOOST_CHECK
(
col1
==
col3
);
if
(
!
col1
)
{
BOOST_CHECK_MESSAGE
(
distanceLowerBound
<=
distance
,
"distance = "
<<
distance
<<
", lower bound = "
<<
distanceLowerBound
);
}
}
}
BOOST_AUTO_TEST_CASE
(
box_mesh
)
{
std
::
vector
<
Vec3f
>
p1
;
std
::
vector
<
Triangle
>
t1
;
boost
::
filesystem
::
path
path
(
TEST_RESOURCES_DIR
);
loadOBJFile
((
path
/
"env.obj"
).
string
().
c_str
(),
p1
,
t1
);
boost
::
shared_ptr
<
BVHModel
<
OBBRSS
>
>
m1
(
new
BVHModel
<
OBBRSS
>
);
boost
::
shared_ptr
<
fcl
::
Box
>
m2
(
new
fcl
::
Box
(
500
,
200
,
150
));
m1
->
beginModel
();
m1
->
addSubModel
(
p1
,
t1
);
m1
->
endModel
();
std
::
vector
<
Transform3f
>
transforms
;
FCL_REAL
extents
[]
=
{
-
3000
,
-
3000
,
0
,
3000
,
3000
,
3000
};
std
::
size_t
n
=
100
;
bool
verbose
=
false
;
generateRandomTransforms
(
extents
,
transforms
,
n
);
// collision
for
(
std
::
size_t
i
=
0
;
i
<
transforms
.
size
();
++
i
)
{
FCL_REAL
distanceLowerBound
,
distance
;
bool
col1
,
col2
;
col1
=
testDistanceLowerBound
(
transforms
[
i
],
m1
,
m2
,
distanceLowerBound
);
col2
=
testDistance
(
transforms
[
i
],
m1
,
m2
,
distance
);
BOOST_CHECK
(
col1
==
col2
);
if
(
!
col1
)
{
BOOST_CHECK_MESSAGE
(
distanceLowerBound
<=
distance
,
"distance = "
<<
distance
<<
", lower bound = "
<<
distanceLowerBound
);
}
}
}
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