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
Humanoid Path Planner
hpp-fcl
Commits
cc9744cf
Unverified
Commit
cc9744cf
authored
Sep 22, 2020
by
Justin Carpentier
Committed by
GitHub
Sep 22, 2020
Browse files
Merge pull request #191 from jmirabel/devel
Add collision and distance between ConvexBase and Halfspace.
parents
c3d3a30c
5d20d8c9
Changes
22
Hide whitespace changes
Inline
Side-by-side
python/collision.cc
View file @
cc9744cf
...
...
@@ -66,6 +66,9 @@ using namespace hpp::fcl;
namespace
dv
=
doxygen
::
visitor
;
template
<
int
index
>
const
CollisionGeometry
*
geto
(
const
Contact
&
c
)
{
return
index
==
1
?
c
.
o1
:
c
.
o2
;
}
void
exposeCollisionAPI
()
{
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
CollisionRequestFlag
>
())
...
...
@@ -115,8 +118,12 @@ void exposeCollisionAPI ()
doxygen
::
class_doc
<
Contact
>
(),
init
<>
(
arg
(
"self"
),
"Default constructor"
))
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>())
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>())
.
DEF_RO_CLASS_ATTRIB
(
Contact
,
o1
)
.
DEF_RO_CLASS_ATTRIB
(
Contact
,
o2
)
.
add_property
(
"o1"
,
make_function
(
&
geto
<
1
>
,
return_value_policy
<
reference_existing_object
>
()),
doxygen
::
class_attrib_doc
<
Contact
>
(
"o1"
))
.
add_property
(
"o2"
,
make_function
(
&
geto
<
2
>
,
return_value_policy
<
reference_existing_object
>
()),
doxygen
::
class_attrib_doc
<
Contact
>
(
"o2"
))
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
b1
)
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
b2
)
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
normal
)
...
...
src/CMakeLists.txt
View file @
cc9744cf
...
...
@@ -51,20 +51,22 @@ set(${LIBRARY_NAME}_SOURCES
shape/convex.cpp
shape/geometric_shapes.cpp
shape/geometric_shapes_utility.cpp
distance_box_halfspace.cpp
distance_box_plane.cpp
distance_box_sphere.cpp
distance_capsule_capsule.cpp
distance_capsule_halfspace.cpp
distance_capsule_plane.cpp
distance_cone_halfspace.cpp
distance_cone_plane.cpp
distance_cylinder_halfspace.cpp
distance_cylinder_plane.cpp
distance_sphere_sphere.cpp
distance_sphere_cylinder.cpp
distance_sphere_halfspace.cpp
distance_sphere_plane.cpp
distance/box_halfspace.cpp
distance/box_plane.cpp
distance/box_sphere.cpp
distance/capsule_capsule.cpp
distance/capsule_halfspace.cpp
distance/capsule_plane.cpp
distance/cone_halfspace.cpp
distance/cone_plane.cpp
distance/cylinder_halfspace.cpp
distance/cylinder_plane.cpp
distance/sphere_sphere.cpp
distance/sphere_cylinder.cpp
distance/sphere_halfspace.cpp
distance/sphere_plane.cpp
distance/convex_halfspace.cpp
distance/triangle_halfspace.cpp
intersect.cpp
math/transform.cpp
traversal/traversal_recurse.cpp
...
...
src/distance
_
box_halfspace.cpp
→
src/distance
/
box_halfspace.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
box_plane.cpp
→
src/distance
/
box_plane.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
box_sphere.cpp
→
src/distance
/
box_sphere.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
capsule_capsule.cpp
→
src/distance
/
capsule_capsule.cpp
View file @
cc9744cf
File moved
src/distance
_
capsule_halfspace.cpp
→
src/distance
/
capsule_halfspace.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
capsule_plane.cpp
→
src/distance
/
capsule_plane.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
cone_halfspace.cpp
→
src/distance
/
cone_halfspace.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
cone_plane.cpp
→
src/distance
/
cone_plane.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance/convex_halfspace.cpp
0 → 100644
View file @
cc9744cf
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Center National de la Recherche Scientifique
* 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 Open Source Robotics Foundation 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.
*/
/** \author Joseph Mirabel */
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
namespace
fcl
{
template
<
>
FCL_REAL
ShapeShapeDistance
<
ConvexBase
,
Halfspace
>
(
const
CollisionGeometry
*
o1
,
const
Transform3f
&
tf1
,
const
CollisionGeometry
*
o2
,
const
Transform3f
&
tf2
,
const
GJKSolver
*
,
const
DistanceRequest
&
,
DistanceResult
&
result
)
{
const
ConvexBase
&
s1
=
static_cast
<
const
ConvexBase
&>
(
*
o1
);
const
Halfspace
&
s2
=
static_cast
<
const
Halfspace
&>
(
*
o2
);
details
::
halfspaceDistance
(
s2
,
tf2
,
s1
,
tf1
,
result
.
min_distance
,
result
.
nearest_points
[
1
],
result
.
nearest_points
[
0
],
result
.
normal
);
result
.
o1
=
o1
;
result
.
o2
=
o2
;
result
.
b1
=
-
1
;
result
.
b2
=
-
1
;
result
.
normal
=
-
result
.
normal
;
return
result
.
min_distance
;
}
template
<
>
FCL_REAL
ShapeShapeDistance
<
Halfspace
,
ConvexBase
>
(
const
CollisionGeometry
*
o1
,
const
Transform3f
&
tf1
,
const
CollisionGeometry
*
o2
,
const
Transform3f
&
tf2
,
const
GJKSolver
*
,
const
DistanceRequest
&
,
DistanceResult
&
result
)
{
const
Halfspace
&
s1
=
static_cast
<
const
Halfspace
&>
(
*
o1
);
const
ConvexBase
&
s2
=
static_cast
<
const
ConvexBase
&>
(
*
o2
);
details
::
halfspaceDistance
(
s1
,
tf1
,
s2
,
tf2
,
result
.
min_distance
,
result
.
nearest_points
[
0
],
result
.
nearest_points
[
1
],
result
.
normal
);
result
.
o1
=
o1
;
result
.
o2
=
o2
;
result
.
b1
=
-
1
;
result
.
b2
=
-
1
;
return
result
.
min_distance
;
}
}
// namespace fcl
}
// namespace hpp
src/distance
_
cylinder_halfspace.cpp
→
src/distance
/
cylinder_halfspace.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
cylinder_plane.cpp
→
src/distance
/
cylinder_plane.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
sphere_cylinder.cpp
→
src/distance
/
sphere_cylinder.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
sphere_halfspace.cpp
→
src/distance
/
sphere_halfspace.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
sphere_plane.cpp
→
src/distance
/
sphere_plane.cpp
View file @
cc9744cf
...
...
@@ -40,8 +40,9 @@
#include
<limits>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#include
"narrowphase/details.h"
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
...
...
src/distance
_
sphere_sphere.cpp
→
src/distance
/
sphere_sphere.cpp
View file @
cc9744cf
File moved
src/distance/triangle_halfspace.cpp
0 → 100644
View file @
cc9744cf
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Center National de la Recherche Scientifique
* 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 Open Source Robotics Foundation 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.
*/
/** \author Joseph Mirabel */
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
"../distance_func_matrix.h"
#include
"../narrowphase/details.h"
namespace
hpp
{
namespace
fcl
{
template
<
>
FCL_REAL
ShapeShapeDistance
<
TriangleP
,
Halfspace
>
(
const
CollisionGeometry
*
o1
,
const
Transform3f
&
tf1
,
const
CollisionGeometry
*
o2
,
const
Transform3f
&
tf2
,
const
GJKSolver
*
,
const
DistanceRequest
&
,
DistanceResult
&
result
)
{
const
TriangleP
&
s1
=
static_cast
<
const
TriangleP
&>
(
*
o1
);
const
Halfspace
&
s2
=
static_cast
<
const
Halfspace
&>
(
*
o2
);
details
::
halfspaceDistance
(
s2
,
tf2
,
s1
,
tf1
,
result
.
min_distance
,
result
.
nearest_points
[
1
],
result
.
nearest_points
[
0
],
result
.
normal
);
result
.
o1
=
o1
;
result
.
o2
=
o2
;
result
.
b1
=
-
1
;
result
.
b2
=
-
1
;
result
.
normal
=
-
result
.
normal
;
return
result
.
min_distance
;
}
template
<
>
FCL_REAL
ShapeShapeDistance
<
Halfspace
,
TriangleP
>
(
const
CollisionGeometry
*
o1
,
const
Transform3f
&
tf1
,
const
CollisionGeometry
*
o2
,
const
Transform3f
&
tf2
,
const
GJKSolver
*
,
const
DistanceRequest
&
,
DistanceResult
&
result
)
{
const
Halfspace
&
s1
=
static_cast
<
const
Halfspace
&>
(
*
o1
);
const
TriangleP
&
s2
=
static_cast
<
const
TriangleP
&>
(
*
o2
);
details
::
halfspaceDistance
(
s1
,
tf1
,
s2
,
tf2
,
result
.
min_distance
,
result
.
nearest_points
[
0
],
result
.
nearest_points
[
1
],
result
.
normal
);
result
.
o1
=
o1
;
result
.
o2
=
o2
;
result
.
b1
=
-
1
;
result
.
b2
=
-
1
;
return
result
.
min_distance
;
}
}
// namespace fcl
}
// namespace hpp
src/distance_func_matrix.h
View file @
cc9744cf
...
...
@@ -89,6 +89,9 @@ HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance<T2,T1> \
SHAPE_SHAPE_DISTANCE_SPECIALIZATION
(
Sphere
,
Sphere
);
SHAPE_SHAPE_DISTANCE_SPECIALIZATION
(
Sphere
,
Cylinder
);
SHAPE_SHAPE_DISTANCE_SPECIALIZATION
(
ConvexBase
,
Halfspace
);
SHAPE_SHAPE_DISTANCE_SPECIALIZATION
(
TriangleP
,
Halfspace
);
#undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1,T2) \
...
...
src/narrowphase/details.h
View file @
cc9744cf
...
...
@@ -1942,6 +1942,28 @@ namespace fcl {
return
true
;
}
/// @param p1 closest (or most penetrating) point on the Halfspace,
/// @param p2 closest (or most penetrating) point on the shape,
/// @param normal the halfspace normal.
/// @return true if the distance is negative (the shape overlaps).
inline
bool
halfspaceDistance
(
const
Halfspace
&
h
,
const
Transform3f
&
tf1
,
const
ShapeBase
&
s
,
const
Transform3f
&
tf2
,
FCL_REAL
&
dist
,
Vec3f
&
p1
,
Vec3f
&
p2
,
Vec3f
&
normal
)
{
Vec3f
n_w
=
tf1
.
getRotation
()
*
h
.
n
;
Vec3f
n_2
(
tf2
.
getRotation
().
transpose
()
*
n_w
);
int
hint
=
0
;
p2
=
getSupport
(
&
s
,
-
n_2
,
true
,
hint
);
p2
=
tf2
.
transform
(
p2
);
dist
=
(
p2
-
tf1
.
getTranslation
()).
dot
(
n_w
)
-
h
.
d
;
p1
=
p2
-
dist
*
n_w
;
normal
=
n_w
;
return
dist
<=
0
;
}
template
<
typename
T
>
inline
T
planeIntersectTolerance
()
{
...
...
Prev
1
2
Next
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