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
0aa5da29
Commit
0aa5da29
authored
Sep 17, 2020
by
Joseph Mirabel
Browse files
Add collision and distance between ConvexBase and Halfspace.
parent
a843f920
Changes
5
Hide whitespace changes
Inline
Side-by-side
src/CMakeLists.txt
View file @
0aa5da29
...
...
@@ -65,6 +65,8 @@ set(${LIBRARY_NAME}_SOURCES
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/convex_halfspace.cpp
0 → 100644
View file @
0aa5da29
/*
* 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/triangle_halfspace.cpp
0 → 100644
View file @
0aa5da29
/*
* 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 @
0aa5da29
...
...
@@ -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 @
0aa5da29
...
...
@@ -1942,6 +1942,29 @@ 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
);
dist
=
p2
.
dot
(
n_2
)
-
h
.
d
;
p1
=
p2
-
dist
*
n_2
;
p1
=
tf2
.
transform
(
p1
);
p2
=
tf2
.
transform
(
p2
);
normal
=
n_w
;
return
dist
<=
0
;
}
template
<
typename
T
>
inline
T
planeIntersectTolerance
()
{
...
...
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