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
6ccc894f
Commit
6ccc894f
authored
Oct 23, 2019
by
Lucile Remigy
Browse files
traits collision and distance work, 0 test fail
parent
8c8de223
Changes
4
Hide whitespace changes
Inline
Side-by-side
src/collision_func_matrix.cpp
View file @
6ccc894f
...
...
@@ -59,15 +59,15 @@ std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, const C
if
(
request
.
isSatisfied
(
result
))
return
result
.
numContacts
();
typename
TraversalTraitsCollision
<
TypeA
,
TypeB
>::
CollisionTraversal_t
node
(
request
);
const
TypeA
*
obj1
=
stat
ic_cast
<
const
TypeA
*>
(
o1
);
const
TypeB
*
obj2
=
stat
ic_cast
<
const
TypeB
*>
(
o2
);
const
TypeA
*
obj1
=
dynam
ic_cast
<
const
TypeA
*>
(
o1
);
const
TypeB
*
obj2
=
dynam
ic_cast
<
const
TypeB
*>
(
o2
);
OcTreeSolver
otsolver
(
nsolver
);
initialize
(
node
,
*
obj1
,
tf1
,
*
obj2
,
tf2
,
&
otsolver
,
result
);
collide
(
&
node
,
request
,
result
);
return
result
.
numContacts
();
}
#endif
template
<
typename
T_SH1
,
typename
T_SH2
>
...
...
@@ -434,43 +434,43 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix
[
BV_OBBRSS
][
BV_OBBRSS
]
=
&
BVHCollide
<
OBBRSS
>
;
#ifdef HPP_FCL_HAVE_OCTOMAP
collision_matrix
[
GEOM_OCTREE
][
GEOM_BOX
]
=
&
Collide
<
Box
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_SPHERE
]
=
&
Collide
<
Sphere
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CAPSULE
]
=
&
Collide
<
Capsule
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CONE
]
=
&
Collide
<
Cone
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CYLINDER
]
=
&
Collide
<
Cylinder
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CONVEX
]
=
&
Collide
<
ConvexBase
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_PLANE
]
=
&
Collide
<
Plane
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_HALFSPACE
]
=
&
Collide
<
Halfspace
>
;
collision_matrix
[
GEOM_BOX
][
GEOM_OCTREE
]
=
&
Collide
<
Box
>
;
collision_matrix
[
GEOM_SPHERE
][
GEOM_OCTREE
]
=
&
Collide
<
Sphere
>
;
collision_matrix
[
GEOM_CAPSULE
][
GEOM_OCTREE
]
=
&
Collide
<
Capsule
>
;
collision_matrix
[
GEOM_CONE
][
GEOM_OCTREE
]
=
&
Collide
<
Cone
>
;
collision_matrix
[
GEOM_CYLINDER
][
GEOM_OCTREE
]
=
&
Collide
<
Cylinder
>
;
collision_matrix
[
GEOM_CONVEX
][
GEOM_OCTREE
]
=
&
Collide
<
ConvexBase
>
;
collision_matrix
[
GEOM_PLANE
][
GEOM_OCTREE
]
=
&
Collide
<
Plane
>
;
collision_matrix
[
GEOM_HALFSPACE
][
GEOM_OCTREE
]
=
&
Collide
<
Halfspace
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_OCTREE
]
=
&
Collide
;
collision_matrix
[
GEOM_OCTREE
][
BV_AABB
]
=
&
Collide
<
AABB
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_OBB
]
=
&
Collide
<
O
BB
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_RSS
]
=
&
Collide
<
RSS
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_OBBRSS
]
=
&
Collide
<
O
BBRSS
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_kIOS
]
=
&
Collide
<
kIOS
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_KDOP16
]
=
&
Collide
<
KDOP
<
16
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_KDOP18
]
=
&
Collide
<
KDOP
<
18
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_KDOP24
]
=
&
Collide
<
KDOP
<
24
>
>
;
collision_matrix
[
BV_AABB
][
GEOM_OCTREE
]
=
&
Collide
<
AABB
>
;
collision_matrix
[
BV_OBB
][
GEOM_OCTREE
]
=
&
Collide
<
OBB
>
;
collision_matrix
[
BV_RSS
][
GEOM_OCTREE
]
=
&
Collide
<
RSS
>
;
collision_matrix
[
BV_OBBRSS
][
GEOM_OCTREE
]
=
&
Collide
<
OBBRSS
>
;
collision_matrix
[
BV_kIOS
][
GEOM_OCTREE
]
=
&
Collide
<
kIOS
>
;
collision_matrix
[
BV_KDOP16
][
GEOM_OCTREE
]
=
&
Collide
<
KDOP
<
16
>
>
;
collision_matrix
[
BV_KDOP18
][
GEOM_OCTREE
]
=
&
Collide
<
KDOP
<
18
>
>
;
collision_matrix
[
BV_KDOP24
][
GEOM_OCTREE
]
=
&
Collide
<
KDOP
<
24
>
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_BOX
]
=
&
Collide
<
OcTree
,
Box
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_SPHERE
]
=
&
Collide
<
OcTree
,
Sphere
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CAPSULE
]
=
&
Collide
<
OcTree
,
Capsule
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CONE
]
=
&
Collide
<
OcTree
,
Cone
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CYLINDER
]
=
&
Collide
<
OcTree
,
Cylinder
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_CONVEX
]
=
&
Collide
<
OcTree
,
ConvexBase
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_PLANE
]
=
&
Collide
<
OcTree
,
Plane
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_HALFSPACE
]
=
&
Collide
<
OcTree
,
Halfspace
>
;
collision_matrix
[
GEOM_BOX
][
GEOM_OCTREE
]
=
&
Collide
<
Box
,
OcTree
>
;
collision_matrix
[
GEOM_SPHERE
][
GEOM_OCTREE
]
=
&
Collide
<
Sphere
,
OcTree
>
;
collision_matrix
[
GEOM_CAPSULE
][
GEOM_OCTREE
]
=
&
Collide
<
Capsule
,
OcTree
>
;
collision_matrix
[
GEOM_CONE
][
GEOM_OCTREE
]
=
&
Collide
<
Cone
,
OcTree
>
;
collision_matrix
[
GEOM_CYLINDER
][
GEOM_OCTREE
]
=
&
Collide
<
Cylinder
,
OcTree
>
;
collision_matrix
[
GEOM_CONVEX
][
GEOM_OCTREE
]
=
&
Collide
<
ConvexBase
,
OcTree
>
;
collision_matrix
[
GEOM_PLANE
][
GEOM_OCTREE
]
=
&
Collide
<
Plane
,
OcTree
>
;
collision_matrix
[
GEOM_HALFSPACE
][
GEOM_OCTREE
]
=
&
Collide
<
Halfspace
,
OcTree
>
;
collision_matrix
[
GEOM_OCTREE
][
GEOM_OCTREE
]
=
&
Collide
<
OcTree
,
OcTree
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_AABB
]
=
&
Collide
<
OcTree
,
BVHModel
<
AABB
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_OBB
]
=
&
Collide
<
O
cTree
,
BVHModel
<
OBB
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_RSS
]
=
&
Collide
<
OcTree
,
BVHModel
<
RSS
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_OBBRSS
]
=
&
Collide
<
O
cTree
,
BVHModel
<
OBBRSS
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_kIOS
]
=
&
Collide
<
OcTree
,
BVHModel
<
kIOS
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_KDOP16
]
=
&
Collide
<
OcTree
,
BVHModel
<
KDOP
<
16
>
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_KDOP18
]
=
&
Collide
<
OcTree
,
BVHModel
<
KDOP
<
18
>
>
>
;
collision_matrix
[
GEOM_OCTREE
][
BV_KDOP24
]
=
&
Collide
<
OcTree
,
BVHModel
<
KDOP
<
24
>
>
>
;
collision_matrix
[
BV_AABB
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
AABB
>
,
OcTree
>
;
collision_matrix
[
BV_OBB
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
OBB
>
,
OcTree
>
;
collision_matrix
[
BV_RSS
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
RSS
>
,
OcTree
>
;
collision_matrix
[
BV_OBBRSS
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
OBBRSS
>
,
OcTree
>
;
collision_matrix
[
BV_kIOS
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
kIOS
>
,
OcTree
>
;
collision_matrix
[
BV_KDOP16
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
KDOP
<
16
>
>
,
OcTree
>
;
collision_matrix
[
BV_KDOP18
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
KDOP
<
18
>
>
,
OcTree
>
;
collision_matrix
[
BV_KDOP24
][
GEOM_OCTREE
]
=
&
Collide
<
BVHModel
<
KDOP
<
24
>
>
,
OcTree
>
;
#endif
}
//template struct CollisionFunctionMatrix;
...
...
src/distance_func_matrix.cpp
View file @
6ccc894f
...
...
@@ -390,43 +390,43 @@ DistanceFunctionMatrix::DistanceFunctionMatrix()
distance_matrix
[
BV_OBBRSS
][
BV_OBBRSS
]
=
&
BVHDistance
<
OBBRSS
>
;
#ifdef HPP_FCL_HAVE_OCTOMAP
distance_matrix
[
GEOM_OCTREE
][
GEOM_BOX
]
=
&
Distance
<
Box
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_SPHERE
]
=
&
Distance
<
Sphere
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CAPSULE
]
=
&
Distance
<
Capsule
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CONE
]
=
&
Distance
<
Cone
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CYLINDER
]
=
&
Distance
<
Cylinder
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CONVEX
]
=
&
Distance
<
ConvexBase
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_PLANE
]
=
&
Distance
<
Plane
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_HALFSPACE
]
=
&
Distance
<
Halfspace
>
;
distance_matrix
[
GEOM_BOX
][
GEOM_OCTREE
]
=
&
Distance
<
Box
>
;
distance_matrix
[
GEOM_SPHERE
][
GEOM_OCTREE
]
=
&
Distance
<
Sphere
>
;
distance_matrix
[
GEOM_CAPSULE
][
GEOM_OCTREE
]
=
&
Distance
<
Capsule
>
;
distance_matrix
[
GEOM_CONE
][
GEOM_OCTREE
]
=
&
Distance
<
Cone
>
;
distance_matrix
[
GEOM_CYLINDER
][
GEOM_OCTREE
]
=
&
Distance
<
Cylinder
>
;
distance_matrix
[
GEOM_CONVEX
][
GEOM_OCTREE
]
=
&
Distance
<
ConvexBase
>
;
distance_matrix
[
GEOM_PLANE
][
GEOM_OCTREE
]
=
&
Distance
<
Plane
>
;
distance_matrix
[
GEOM_HALFSPACE
][
GEOM_OCTREE
]
=
&
Distance
<
Halfspace
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_OCTREE
]
=
&
Distance
;
distance_matrix
[
GEOM_OCTREE
][
BV_AABB
]
=
&
Distance
<
AABB
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_OBB
]
=
&
Distance
<
O
BB
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_RSS
]
=
&
Distance
<
RSS
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_OBBRSS
]
=
&
Distance
<
OBBRSS
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_kIOS
]
=
&
Distance
<
kIOS
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_KDOP16
]
=
&
Distance
<
KDOP
<
16
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_KDOP18
]
=
&
Distance
<
KDOP
<
18
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_KDOP24
]
=
&
Distance
<
KDOP
<
24
>
>
;
distance_matrix
[
BV_AABB
][
GEOM_OCTREE
]
=
&
Distance
<
AABB
>
;
distance_matrix
[
BV_OBB
][
GEOM_OCTREE
]
=
&
Distance
<
OBB
>
;
distance_matrix
[
BV_RSS
][
GEOM_OCTREE
]
=
&
Distance
<
RSS
>
;
distance_matrix
[
BV_OBBRSS
][
GEOM_OCTREE
]
=
&
Distance
<
OBBRSS
>
;
distance_matrix
[
BV_kIOS
][
GEOM_OCTREE
]
=
&
Distance
<
kIOS
>
;
distance_matrix
[
BV_KDOP16
][
GEOM_OCTREE
]
=
&
Distance
<
KDOP
<
16
>
>
;
distance_matrix
[
BV_KDOP18
][
GEOM_OCTREE
]
=
&
Distance
<
KDOP
<
18
>
>
;
distance_matrix
[
BV_KDOP24
][
GEOM_OCTREE
]
=
&
Distance
<
KDOP
<
24
>
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_BOX
]
=
&
Distance
<
OcTree
,
Box
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_SPHERE
]
=
&
Distance
<
OcTree
,
Sphere
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CAPSULE
]
=
&
Distance
<
OcTree
,
Capsule
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CONE
]
=
&
Distance
<
OcTree
,
Cone
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CYLINDER
]
=
&
Distance
<
OcTree
,
Cylinder
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_CONVEX
]
=
&
Distance
<
OcTree
,
ConvexBase
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_PLANE
]
=
&
Distance
<
OcTree
,
Plane
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_HALFSPACE
]
=
&
Distance
<
OcTree
,
Halfspace
>
;
distance_matrix
[
GEOM_BOX
][
GEOM_OCTREE
]
=
&
Distance
<
Box
,
OcTree
>
;
distance_matrix
[
GEOM_SPHERE
][
GEOM_OCTREE
]
=
&
Distance
<
Sphere
,
OcTree
>
;
distance_matrix
[
GEOM_CAPSULE
][
GEOM_OCTREE
]
=
&
Distance
<
Capsule
,
OcTree
>
;
distance_matrix
[
GEOM_CONE
][
GEOM_OCTREE
]
=
&
Distance
<
Cone
,
OcTree
>
;
distance_matrix
[
GEOM_CYLINDER
][
GEOM_OCTREE
]
=
&
Distance
<
Cylinder
,
OcTree
>
;
distance_matrix
[
GEOM_CONVEX
][
GEOM_OCTREE
]
=
&
Distance
<
ConvexBase
,
OcTree
>
;
distance_matrix
[
GEOM_PLANE
][
GEOM_OCTREE
]
=
&
Distance
<
Plane
,
OcTree
>
;
distance_matrix
[
GEOM_HALFSPACE
][
GEOM_OCTREE
]
=
&
Distance
<
Halfspace
,
OcTree
>
;
distance_matrix
[
GEOM_OCTREE
][
GEOM_OCTREE
]
=
&
Distance
<
OcTree
,
OcTree
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_AABB
]
=
&
Distance
<
OcTree
,
BVHModel
<
AABB
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_OBB
]
=
&
Distance
<
O
cTree
,
BVHModel
<
OBB
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_RSS
]
=
&
Distance
<
OcTree
,
BVHModel
<
RSS
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_OBBRSS
]
=
&
Distance
<
OcTree
,
BVHModel
<
OBBRSS
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_kIOS
]
=
&
Distance
<
OcTree
,
BVHModel
<
kIOS
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_KDOP16
]
=
&
Distance
<
OcTree
,
BVHModel
<
KDOP
<
16
>
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_KDOP18
]
=
&
Distance
<
OcTree
,
BVHModel
<
KDOP
<
18
>
>
>
;
distance_matrix
[
GEOM_OCTREE
][
BV_KDOP24
]
=
&
Distance
<
OcTree
,
BVHModel
<
KDOP
<
24
>
>
>
;
distance_matrix
[
BV_AABB
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
AABB
>
,
OcTree
>
;
distance_matrix
[
BV_OBB
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
OBB
>
,
OcTree
>
;
distance_matrix
[
BV_RSS
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
RSS
>
,
OcTree
>
;
distance_matrix
[
BV_OBBRSS
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
OBBRSS
>
,
OcTree
>
;
distance_matrix
[
BV_kIOS
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
kIOS
>
,
OcTree
>
;
distance_matrix
[
BV_KDOP16
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
KDOP
<
16
>>
,
OcTree
>
;
distance_matrix
[
BV_KDOP18
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
KDOP
<
18
>>
,
OcTree
>
;
distance_matrix
[
BV_KDOP24
][
GEOM_OCTREE
]
=
&
Distance
<
BVHModel
<
KDOP
<
24
>>
,
OcTree
>
;
#endif
...
...
src/traits_traversal.h
View file @
6ccc894f
...
...
@@ -34,7 +34,7 @@ struct TraversalTraitsCollision <OcTree, T_SH>
template
<
>
struct
TraversalTraitsCollision
<
OcTree
,
OcTree
>
{
typedef
OcTreeCollisionTraversalNode
<>
CollisionTraversal_t
;
typedef
OcTreeCollisionTraversalNode
CollisionTraversal_t
;
};
template
<
typename
T_BVH
>
...
...
test/geometric_shapes.cpp
View file @
6ccc894f
...
...
@@ -791,20 +791,27 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle)
normal
);
BOOST_CHECK
(
res
);
// These tests fail because of numerical precision errors. The points t[1] and t[2]
// lies on the border of the half-space.
// The normals should be good, when computed (i.e. res == true)
res
=
solver1
.
shapeTriangleInteraction
(
hs
,
transform
,
t
[
0
],
t
[
1
],
t
[
2
],
transform
,
distance
,
c1
,
c2
,
normal
);
BOOST_CHECK
(
res
);
// BOOST_CHECK(res);
if
(
res
)
BOOST_CHECK
(
isEqual
(
normal
,
transform
.
getRotation
()
*
Vec3f
(
1
,
0
,
0
),
1e-9
));
res
=
solver1
.
shapeTriangleInteraction
(
hs
,
Transform3f
(),
t
[
0
],
t
[
1
],
t
[
2
],
Transform3f
(),
distance
,
c1
,
c2
,
normal
);
BOOST_CHECK
(
res
);
BOOST_CHECK
(
isEqual
(
normal
,
Vec3f
(
1
,
0
,
0
),
1e-9
));
// BOOST_CHECK(res);
if
(
res
)
BOOST_CHECK
(
isEqual
(
normal
,
Vec3f
(
1
,
0
,
0
),
1e-9
));
res
=
solver1
.
shapeTriangleInteraction
(
hs
,
transform
,
t
[
0
],
t
[
1
],
t
[
2
],
transform
,
distance
,
c1
,
c2
,
normal
);
BOOST_CHECK
(
res
);
BOOST_CHECK
(
isEqual
(
normal
,
transform
.
getRotation
()
*
Vec3f
(
1
,
0
,
0
),
1e-9
));
// BOOST_CHECK(res);
if
(
res
)
BOOST_CHECK
(
isEqual
(
normal
,
transform
.
getRotation
()
*
Vec3f
(
1
,
0
,
0
),
1e-9
));
}
BOOST_AUTO_TEST_CASE
(
shapeIntersection_planetriangle
)
...
...
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