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
47fb8b34
Commit
47fb8b34
authored
May 18, 2017
by
Joseph Mirabel
Committed by
Joseph Mirabel
May 18, 2017
Browse files
Rename BVHIntersection into BVHExtract and add method generic extract
parent
f3a8710f
Changes
8
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
47fb8b34
...
...
@@ -137,6 +137,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/intersect.h
include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h
include/hpp/fcl/octree.h
include/hpp/fcl/fwd.hh
include/hpp/fcl/mesh_loader/assimp.h
...
...
include/hpp/fcl/BVH/BVH_utility.h
View file @
47fb8b34
...
...
@@ -77,9 +77,10 @@ void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS
void
BVHExpand
(
BVHModel
<
RSS
>&
model
,
const
Variance3f
*
ucs
,
FCL_REAL
r
);
/// @brief Return the intersection of a BVHModel and a AABB.
/// @brief Extract the part of the BVHModel that is inside an AABB.
/// A triangle in collision with the AABB is considered inside.
template
<
typename
BV
>
BVHModel
<
BV
>*
BVH
Intersection
(
const
BVHModel
<
BV
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
BVHModel
<
BV
>*
BVH
Extract
(
const
BVHModel
<
BV
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
/// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles
void
getCovariance
(
Vec3f
*
ps
,
Vec3f
*
ps2
,
Triangle
*
ts
,
unsigned
int
*
indices
,
int
n
,
Matrix3f
&
M
);
...
...
include/hpp/fcl/collision_utility.h
0 → 100644
View file @
47fb8b34
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-fcl.
// hpp-fcl is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-fcl is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
#ifndef FCL_COLLISION_UTILITY_H
#define FCL_COLLISION_UTILITY_H
#include
<hpp/fcl/collision_object.h>
namespace
fcl
{
CollisionGeometry
*
extract
(
const
CollisionGeometry
*
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
}
#endif // FCL_COLLISION_UTILITY_H
src/BVH/BVH_utility.cpp
View file @
47fb8b34
...
...
@@ -37,6 +37,8 @@
#include
<hpp/fcl/BVH/BVH_utility.h>
#include
<hpp/fcl/narrowphase/narrowphase.h>
#include
<hpp/fcl/shape/geometric_shapes_utility.h>
namespace
fcl
{
...
...
@@ -104,12 +106,18 @@ void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
}
template
<
typename
BV
>
BVHModel
<
BV
>*
BVH
Intersection
(
const
BVHModel
<
BV
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
_aabb
)
BVHModel
<
BV
>*
BVH
Extract
(
const
BVHModel
<
BV
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
_aabb
)
{
assert
(
model
.
getModelType
()
==
BVH_MODEL_TRIANGLES
);
const
Quaternion3f
&
q
=
pose
.
getQuatRotation
();
AABB
aabb
=
translate
(
_aabb
,
-
pose
.
getTranslation
());
Transform3f
box_pose
;
Box
box
;
constructBox
(
_aabb
,
box
,
box_pose
);
box_pose
=
pose
.
inverseTimes
(
box_pose
);
GJKSolver_indep
gjk
;
// Check what triangles should be kept.
// TODO use the BV hierarchy
std
::
vector
<
bool
>
keep_vertex
(
model
.
num_vertices
,
false
);
...
...
@@ -120,9 +128,6 @@ BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose
bool
keep_this_tri
=
keep_vertex
[
t
[
0
]]
||
keep_vertex
[
t
[
1
]]
||
keep_vertex
[
t
[
2
]];
// const Vec3f& p0 = vertices[t[0]];
// const Vec3f& p1 = vertices[t[1]];
// const Vec3f& p2 = vertices[t[2]];
if
(
!
keep_this_tri
)
{
for
(
std
::
size_t
j
=
0
;
j
<
3
;
++
j
)
{
if
(
aabb
.
contain
(
q
*
model
.
vertices
[
t
[
j
]]))
{
...
...
@@ -130,6 +135,12 @@ BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose
break
;
}
}
const
Vec3f
&
p0
=
model
.
vertices
[
t
[
0
]];
const
Vec3f
&
p1
=
model
.
vertices
[
t
[
1
]];
const
Vec3f
&
p2
=
model
.
vertices
[
t
[
2
]];
if
(
!
keep_this_tri
&&
gjk
.
shapeTriangleIntersect
(
box
,
box_pose
,
p0
,
p1
,
p2
,
NULL
,
NULL
,
NULL
))
{
keep_this_tri
=
true
;
}
}
if
(
keep_this_tri
)
{
keep_vertex
[
t
[
0
]]
=
keep_vertex
[
t
[
1
]]
=
keep_vertex
[
t
[
2
]]
=
true
;
...
...
@@ -168,14 +179,14 @@ BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose
}
return
new_model
;
}
template
BVHModel
<
OBB
>
*
BVH
Intersection
(
const
BVHModel
<
OBB
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
AABB
>
*
BVH
Intersection
(
const
BVHModel
<
AABB
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
RSS
>
*
BVH
Intersection
(
const
BVHModel
<
RSS
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
kIOS
>
*
BVH
Intersection
(
const
BVHModel
<
kIOS
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
OBBRSS
>
*
BVH
Intersection
(
const
BVHModel
<
OBBRSS
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
KDOP
<
16
>
>*
BVH
Intersection
(
const
BVHModel
<
KDOP
<
16
>
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
KDOP
<
18
>
>*
BVH
Intersection
(
const
BVHModel
<
KDOP
<
18
>
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
KDOP
<
24
>
>*
BVH
Intersection
(
const
BVHModel
<
KDOP
<
24
>
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
OBB
>
*
BVH
Extract
(
const
BVHModel
<
OBB
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
AABB
>
*
BVH
Extract
(
const
BVHModel
<
AABB
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
RSS
>
*
BVH
Extract
(
const
BVHModel
<
RSS
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
kIOS
>
*
BVH
Extract
(
const
BVHModel
<
kIOS
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
OBBRSS
>
*
BVH
Extract
(
const
BVHModel
<
OBBRSS
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
KDOP
<
16
>
>*
BVH
Extract
(
const
BVHModel
<
KDOP
<
16
>
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
KDOP
<
18
>
>*
BVH
Extract
(
const
BVHModel
<
KDOP
<
18
>
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
template
BVHModel
<
KDOP
<
24
>
>*
BVH
Extract
(
const
BVHModel
<
KDOP
<
24
>
>&
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
);
void
getCovariance
(
Vec3f
*
ps
,
Vec3f
*
ps2
,
Triangle
*
ts
,
unsigned
int
*
indices
,
int
n
,
Matrix3f
&
M
)
{
...
...
src/CMakeLists.txt
View file @
47fb8b34
...
...
@@ -71,6 +71,7 @@ set(${LIBRARY_NAME}_SOURCES
BVH/BVH_model.cpp
BVH/BV_splitter.cpp
collision_func_matrix.cpp
collision_utility.cpp
)
# Declare boost include directories
...
...
src/collision_utility.cpp
0 → 100644
View file @
47fb8b34
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-fcl.
// hpp-fcl is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-fcl is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
#include
<hpp/fcl/collision_utility.h>
#include
<hpp/fcl/BVH/BVH_utility.h>
namespace
fcl
{
namespace
details
{
template
<
typename
NT
>
inline
CollisionGeometry
*
extractBVHtpl
(
const
CollisionGeometry
*
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
)
{
const
BVHModel
<
NT
>*
m
=
static_cast
<
const
BVHModel
<
NT
>*>
(
model
);
return
BVHExtract
(
*
m
,
pose
,
aabb
);
}
CollisionGeometry
*
extractBVH
(
const
CollisionGeometry
*
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
)
{
switch
(
model
->
getNodeType
())
{
case
BV_AABB
:
return
extractBVHtpl
<
AABB
>
(
model
,
pose
,
aabb
);
case
BV_OBB
:
return
extractBVHtpl
<
OBB
>
(
model
,
pose
,
aabb
);
case
BV_RSS
:
return
extractBVHtpl
<
RSS
>
(
model
,
pose
,
aabb
);
case
BV_kIOS
:
return
extractBVHtpl
<
kIOS
>
(
model
,
pose
,
aabb
);
case
BV_OBBRSS
:
return
extractBVHtpl
<
OBBRSS
>
(
model
,
pose
,
aabb
);
case
BV_KDOP16
:
return
extractBVHtpl
<
KDOP
<
16
>
>
(
model
,
pose
,
aabb
);
case
BV_KDOP18
:
return
extractBVHtpl
<
KDOP
<
18
>
>
(
model
,
pose
,
aabb
);
case
BV_KDOP24
:
return
extractBVHtpl
<
KDOP
<
24
>
>
(
model
,
pose
,
aabb
);
default:
throw
std
::
runtime_error
(
"Unknown type of bounding volume"
);
}
}
}
CollisionGeometry
*
extract
(
const
CollisionGeometry
*
model
,
const
Transform3f
&
pose
,
const
AABB
&
aabb
)
{
switch
(
model
->
getObjectType
())
{
case
OT_BVH
:
return
details
::
extractBVH
(
model
,
pose
,
aabb
);
// case OT_GEOM: return model;
default:
throw
std
::
runtime_error
(
"Extraction is not implemented for this type of object"
);
}
}
}
test/test_fcl_bvh_models.cpp
View file @
47fb8b34
...
...
@@ -155,13 +155,15 @@ void testBVHModelTriangles()
BOOST_CHECK_EQUAL
(
model
->
build_state
,
BVH_BUILD_STATE_PROCESSED
);
Transform3f
pose
;
boost
::
shared_ptr
<
BVHModel
<
BV
>
>
cropped
(
BVHIntersection
(
*
model
,
pose
,
aabb
));
boost
::
shared_ptr
<
BVHModel
<
BV
>
>
cropped
(
BVHExtract
(
*
model
,
pose
,
aabb
));
BOOST_REQUIRE
(
cropped
);
BOOST_CHECK
(
cropped
->
build_state
==
BVH_BUILD_STATE_PROCESSED
);
BOOST_CHECK_EQUAL
(
cropped
->
num_vertices
,
model
->
num_vertices
-
6
);
BOOST_CHECK_EQUAL
(
cropped
->
num_tris
,
model
->
num_tris
-
2
);
pose
.
setTranslation
(
Vec3f
(
0
,
1
,
0
));
cropped
.
reset
(
BVHIntersection
(
*
model
,
pose
,
aabb
));
cropped
.
reset
(
BVHExtract
(
*
model
,
pose
,
aabb
));
BOOST_REQUIRE
(
cropped
);
BOOST_CHECK
(
cropped
->
build_state
==
BVH_BUILD_STATE_PROCESSED
);
BOOST_CHECK_EQUAL
(
cropped
->
num_vertices
,
model
->
num_vertices
-
6
);
BOOST_CHECK_EQUAL
(
cropped
->
num_tris
,
model
->
num_tris
-
2
);
...
...
@@ -169,15 +171,23 @@ void testBVHModelTriangles()
pose
.
setTranslation
(
Vec3f
(
0
,
0
,
0
));
FCL_REAL
sqrt2_2
=
std
::
sqrt
(
2
)
/
2
;
pose
.
setQuatRotation
(
Quaternion3f
(
sqrt2_2
,
sqrt2_2
,
0
,
0
));
cropped
.
reset
(
BVHIntersection
(
*
model
,
pose
,
aabb
));
cropped
.
reset
(
BVHExtract
(
*
model
,
pose
,
aabb
));
BOOST_REQUIRE
(
cropped
);
BOOST_CHECK
(
cropped
->
build_state
==
BVH_BUILD_STATE_PROCESSED
);
BOOST_CHECK_EQUAL
(
cropped
->
num_vertices
,
model
->
num_vertices
-
6
);
BOOST_CHECK_EQUAL
(
cropped
->
num_tris
,
model
->
num_tris
-
2
);
pose
.
setTranslation
(
-
Vec3f
(
1
,
1
,
1
));
pose
.
setQuatRotation
(
Quaternion3f
::
Identity
());
cropped
.
reset
(
BVH
Intersection
(
*
model
,
pose
,
aabb
));
cropped
.
reset
(
BVH
Extract
(
*
model
,
pose
,
aabb
));
BOOST_CHECK
(
!
cropped
);
aabb
=
AABB
(
Vec3f
(
-
0.1
,
-
0.1
,
-
0.1
),
Vec3f
(
0.1
,
0.1
,
0.1
));
pose
.
setTranslation
(
Vec3f
(
-
0.5
,
-
0.5
,
0
));
cropped
.
reset
(
BVHExtract
(
*
model
,
pose
,
aabb
));
BOOST_REQUIRE
(
cropped
);
BOOST_CHECK_EQUAL
(
cropped
->
num_tris
,
2
);
BOOST_CHECK_EQUAL
(
cropped
->
num_vertices
,
6
);
}
template
<
typename
BV
>
...
...
test/test_fcl_profiling.cpp
View file @
47fb8b34
...
...
@@ -21,6 +21,7 @@
#include
<hpp/fcl/fwd.hh>
#include
<hpp/fcl/collision.h>
#include
<hpp/fcl/BVH/BVH_model.h>
#include
<hpp/fcl/collision_utility.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<hpp/fcl/collision_func_matrix.h>
#include
<hpp/fcl/narrowphase/narrowphase.h>
...
...
@@ -121,7 +122,9 @@ void printResults (const Geometry& g1, const Geometry& g2, const Results& rs)
int
Ntransform
=
100
;
FCL_REAL
limit
=
20
;
bool
verbose
=
false
;
#define OUT(x) if (verbose) std::cout << x << std::endl
#define CHECK_PARAM_NB(NB, NAME) \
if (iarg + NB >= argc) throw std::invalid_argument(#NAME " requires " #NB " numbers")
void
handleParam
(
int
&
iarg
,
const
int
&
argc
,
char
**
argv
,
CollisionRequest
&
request
)
...
...
@@ -131,6 +134,7 @@ void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& req
if
(
a
==
"-nb_transform"
)
{
CHECK_PARAM_NB
(
1
,
nb_transform
);
Ntransform
=
atoi
(
argv
[
iarg
+
1
]);
OUT
(
"nb_transform = "
<<
Ntransform
);
iarg
+=
2
;
}
else
if
(
a
==
"-enable_distance_lower_bound"
)
{
CHECK_PARAM_NB
(
1
,
enable_distance_lower_bound
);
...
...
@@ -140,6 +144,9 @@ void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& req
CHECK_PARAM_NB
(
1
,
limit
);
limit
=
atof
(
argv
[
iarg
+
1
]);
iarg
+=
2
;
}
else
if
(
a
==
"-verbose"
)
{
verbose
=
true
;
iarg
+=
1
;
}
else
{
break
;
}
...
...
@@ -169,17 +176,38 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv)
type
=
"sphere"
;
}
else
if
(
a
==
"-mesh"
)
{
CHECK_PARAM_NB
(
2
,
Mesh
);
OUT
(
"Loading "
<<
argv
[
iarg
+
2
]
<<
" as BVHModel<"
<<
argv
[
iarg
+
1
]
<<
">..."
);
if
(
strcmp
(
argv
[
iarg
+
1
],
"obb"
)
==
0
)
{
o
=
meshToGeom
<
OBB
>
(
argv
[
iarg
+
2
]);
OUT
(
"Mesh has "
<<
boost
::
dynamic_pointer_cast
<
BVHModel
<
OBB
>
>
(
o
)
->
num_tris
<<
" triangles"
);
type
=
"mesh_obb"
;
}
else
if
(
strcmp
(
argv
[
iarg
+
1
],
"obbrss"
)
==
0
)
{
o
=
meshToGeom
<
OBBRSS
>
(
argv
[
iarg
+
2
]);
OUT
(
"Mesh has "
<<
boost
::
dynamic_pointer_cast
<
BVHModel
<
OBBRSS
>
>
(
o
)
->
num_tris
<<
" triangles"
);
type
=
"mesh_obbrss"
;
}
else
throw
std
::
invalid_argument
(
"BV type must be obb or obbrss"
);
OUT
(
"done."
);
iarg
+=
3
;
if
(
iarg
<
argc
&&
strcmp
(
argv
[
iarg
],
"crop"
)
==
0
)
{
CHECK_PARAM_NB
(
6
,
Crop
);
fcl
::
AABB
aabb
(
Vec3f
(
atof
(
argv
[
iarg
+
1
]),
atof
(
argv
[
iarg
+
2
]),
atof
(
argv
[
iarg
+
3
])),
Vec3f
(
atof
(
argv
[
iarg
+
4
]),
atof
(
argv
[
iarg
+
5
]),
atof
(
argv
[
iarg
+
6
])));
OUT
(
"Cropping "
<<
aabb
.
min_
.
transpose
()
<<
" ---- "
<<
aabb
.
max_
.
transpose
()
<<
" ..."
);
o
->
computeLocalAABB
();
OUT
(
"Mesh AABB is "
<<
o
->
aabb_local
.
min_
.
transpose
()
<<
" ---- "
<<
o
->
aabb_local
.
max_
.
transpose
()
<<
" ..."
);
o
.
reset
(
extract
(
o
.
get
(),
Transform3f
(),
aabb
));
if
(
!
o
)
throw
std
::
invalid_argument
(
"Failed to crop."
);
OUT
(
"Crop has "
<<
boost
::
dynamic_pointer_cast
<
BVHModel
<
OBB
>
>
(
o
)
->
num_tris
<<
" triangles"
);
iarg
+=
7
;
}
}
else
if
(
a
==
"-capsule"
)
{
CREATE_SHAPE_2
(
o
,
Capsule
);
type
=
"capsule"
;
...
...
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