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
e8a5f65d
Commit
e8a5f65d
authored
Sep 09, 2019
by
Joseph Mirabel
Browse files
Update class Convex to enable faster support retrieval.
parent
dbf93557
Changes
5
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/shape/geometric_shapes.h
View file @
e8a5f65d
...
...
@@ -275,40 +275,39 @@ public:
Convex
(
Vec3f
*
points_
,
int
num_points_
,
int
*
polygons_
,
int
num_polygons_
)
:
ShapeBase
()
int
num_polygons_
)
:
ShapeBase
(),
points
(
points_
),
num_points
(
num_points_
),
polygons
(
polygons_
),
num_polygons
(
num_polygons_
)
{
num_polygons
=
num_polygons_
;
points
=
points_
;
num_points
=
num_points_
;
polygons
=
polygons_
;
edges
=
NULL
;
Vec3f
sum
(
0
,
0
,
0
);
for
(
int
i
=
0
;
i
<
num_points
;
++
i
)
{
sum
+=
points
[
i
];
}
center
=
sum
*
(
FCL_REAL
)(
1.0
/
num_points
);
fillEdges
();
initialize
();
}
/// @brief Copy constructor
Convex
(
const
Convex
&
other
)
:
ShapeBase
(
other
)
/// Only the list of neighbors is copied.
Convex
(
const
Convex
&
other
)
:
ShapeBase
(
other
),
points
(
other
.
points
),
num_points
(
other
.
num_points
),
polygons
(
other
.
polygons
),
num_polygons
(
other
.
num_polygons
),
center
(
other
.
center
)
{
n
um_polygons
=
other
.
num_polygons
;
points
=
other
.
points
;
num_points
=
other
.
num_points
;
polygons
=
other
.
polygons
;
num_edge
s
=
other
.
num_edges
;
edges
=
new
Edge
[
num_edge
s
];
memcpy
(
edges
,
other
.
edges
,
sizeof
(
Edge
)
*
num_edge
s
);
n
eighbors
=
new
Neighbors
[
num_points
]
;
memcpy
(
neighbors
,
other
.
neighbors
,
sizeof
(
Neighbors
)
*
num_
points
)
;
int
c_nneighbors
=
0
;
for
(
int
i
=
0
;
i
<
num_points
;
++
i
)
c_nneighbor
s
+
=
neighbors
[
i
].
count
()
;
nneighbors_
=
new
unsigned
int
[
c_nneighbor
s
];
memcpy
(
nneighbors_
,
other
.
nneighbors_
,
sizeof
(
unsigned
int
)
*
c_nneighbor
s
);
}
~
Convex
()
{
delete
[]
edges
;
delete
[]
neighbors
;
delete
[]
nneighbors_
;
}
/// @brief Compute AABB
...
...
@@ -317,21 +316,31 @@ public:
/// @brief Get node type: a conex polytope
NODE_TYPE
getNodeType
()
const
{
return
GEOM_CONVEX
;
}
/// @brief An array of indices to the points of each polygon, it should be the number of vertices
/// followed by that amount of indices to "points" in counter clockwise order
/// @brief An array of indices to the points of each polygon.
/// It should be the number of vertices
/// followed by that amount of indices to "points" in counter clockwise order.
int
*
polygons
;
int
num_polygons
;
/// @brief An array of the points of the polygon.
Vec3f
*
points
;
int
num_points
;
int
num_edges
;
int
num_polygons
;
struct
Edge
class
Neighbors
{
int
first
,
second
;
private:
unsigned
char
count_
;
unsigned
int
*
n_
;
friend
class
Convex
;
public:
unsigned
char
const
&
count
()
const
{
return
count_
;
}
unsigned
int
&
operator
[]
(
int
i
)
{
assert
(
i
<
count_
);
return
n_
[
i
];
}
unsigned
int
const
&
operator
[]
(
int
i
)
const
{
assert
(
i
<
count_
);
return
n_
[
i
];
}
};
Edge
*
edges
;
/// Neighbors of each vertex.
/// It is an array of size num_points. For each vertex, it contains the number
/// of neighbors and a list of indices to them.
Neighbors
*
neighbors
;
/// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex)
Vec3f
center
;
...
...
@@ -445,13 +454,12 @@ public:
return
vol
/
6
;
}
protected:
/// @brief Get edge information
void
fillEdges
();
};
void
initialize
();
private:
unsigned
int
*
nneighbors_
;
};
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
...
...
src/narrowphase/gjk.cpp
View file @
e8a5f65d
...
...
@@ -169,17 +169,28 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
void
getShapeSupport
(
const
Convex
*
convex
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
FCL_REAL
maxdot
=
-
std
::
numeric_limits
<
FCL_REAL
>::
max
();
Vec3f
*
curp
=
convex
->
points
;
for
(
int
i
=
0
;
i
<
convex
->
num_points
;
++
i
,
curp
+=
1
)
const
Vec3f
*
pts
=
convex
->
points
;
const
Convex
::
Neighbors
*
nn
=
convex
->
neighbors
;
int
i
=
0
;
FCL_REAL
maxdot
=
pts
[
i
].
dot
(
dir
);
FCL_REAL
dot
;
bool
found
=
true
;
while
(
found
)
{
FCL_REAL
dot
=
dir
.
dot
(
*
curp
);
if
(
dot
>
maxdot
)
{
support
=
*
curp
;
maxdot
=
dot
;
const
Convex
::
Neighbors
&
n
=
nn
[
i
];
found
=
false
;
for
(
int
in
=
0
;
in
<
n
.
count
();
++
in
)
{
dot
=
pts
[
n
[
in
]].
dot
(
dir
);
if
(
dot
>
maxdot
)
{
maxdot
=
dot
;
i
=
n
[
in
];
found
=
true
;
}
}
}
support
=
pts
[
i
];
}
#define CALL_GET_SHAPE_SUPPORT(ShapeType) \
...
...
src/shape/geometric_shapes.cpp
View file @
e8a5f65d
...
...
@@ -39,65 +39,61 @@
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<hpp/fcl/shape/geometric_shapes_utility.h>
#include
<set>
namespace
hpp
{
namespace
fcl
{
void
Convex
::
fillEdges
()
void
Convex
::
initialize
()
{
int
*
points_in_poly
=
polygons
;
if
(
edges
)
delete
[]
edges
;
center
.
setZero
();
for
(
int
i
=
0
;
i
<
num_points
;
++
i
)
center
+=
points
[
i
];
center
/=
num_points
;
int
num_edges_alloc
=
0
;
for
(
int
i
=
0
;
i
<
num_polygons
;
++
i
)
{
num_edges_alloc
+=
*
points_in_poly
;
points_in_poly
+=
(
*
points_in_poly
+
1
);
}
int
*
points_in_poly
=
polygons
;
neighbors
=
new
Neighbors
[
num_points
];
edges
=
new
Edge
[
num_edges_alloc
];
std
::
vector
<
std
::
set
<
unsigned
int
>
>
nneighbors
(
num_points
);
unsigned
int
c_nneighbors
=
0
;
points_in_poly
=
polygons
;
int
*
index
=
polygons
+
1
;
num_edges
=
0
;
Edge
e
;
bool
isinset
;
for
(
int
i
=
0
;
i
<
num_polygons
;
++
i
)
for
(
int
l
=
0
;
l
<
num_polygons
;
++
l
)
{
for
(
int
j
=
0
;
j
<
*
points_in_poly
;
++
j
)
{
e
.
first
=
std
::
min
(
index
[
j
],
index
[(
j
+
1
)
%*
points_in_poly
]);
e
.
second
=
std
::
max
(
index
[
j
],
index
[(
j
+
1
)
%*
points_in_poly
])
;
i
sinset
=
false
;
for
(
int
k
=
0
;
k
<
num_edges
;
++
k
)
{
if
((
edges
[
k
].
first
==
e
.
first
)
&&
(
edges
[
k
].
second
==
e
.
second
))
{
isinset
=
true
;
break
;
}
int
n
=
*
points_in_poly
;
for
(
int
j
=
0
;
j
<
n
;
++
j
)
{
int
i
=
(
j
==
0
)
?
n
-
1
:
j
-
1
;
i
nt
k
=
(
j
==
n
-
1
)
?
0
:
j
+
1
;
int
pi
=
points_in_poly
[
i
+
1
];
int
pj
=
points_in_poly
[
j
+
1
];
int
pk
=
points_in_poly
[
k
+
1
];
// Update neighbors of pj;
if
(
nneighbors
[
pj
].
count
(
pi
)
==
0
)
{
c_nneighbors
++
;
nneighbors
[
pj
].
insert
(
pi
);
}
if
(
!
isinset
)
{
edges
[
num_edges
].
first
=
e
.
first
;
edges
[
num_edges
].
second
=
e
.
second
;
++
num_edges
;
if
(
nneighbors
[
pj
].
count
(
pk
)
==
0
)
{
c_nneighbors
++
;
nneighbors
[
pj
].
insert
(
pk
);
}
}
points_in_poly
+=
(
*
points_in_poly
+
1
);
index
=
points_in_poly
+
1
;
points_in_poly
+=
n
+
1
;
}
if
(
num_edges
<
num_edges_alloc
)
{
Edge
*
tmp
=
new
Edge
[
num_edges
];
memcpy
(
tmp
,
edges
,
num_edges
*
sizeof
(
Edge
));
delete
[]
edges
;
edges
=
tmp
;
nneighbors_
=
new
unsigned
int
[
c_nneighbors
];
unsigned
int
*
p_nneighbors
=
nneighbors_
;
for
(
int
i
=
0
;
i
<
num_points
;
++
i
)
{
Neighbors
&
n
=
neighbors
[
i
];
if
(
nneighbors
[
i
].
size
()
>=
std
::
numeric_limits
<
unsigned
char
>::
max
())
throw
std
::
logic_error
(
"Too many neighbors."
);
n
.
count_
=
(
unsigned
char
)
nneighbors
[
i
].
size
();
n
.
n_
=
p_nneighbors
;
p_nneighbors
=
std
::
copy
(
nneighbors
[
i
].
begin
(),
nneighbors
[
i
].
end
(),
p_nneighbors
);
}
assert
(
p_nneighbors
==
nneighbors_
+
c_nneighbors
);
}
void
Halfspace
::
unitNormalTest
()
...
...
test/CMakeLists.txt
View file @
e8a5f65d
...
...
@@ -42,6 +42,7 @@ add_fcl_test(simple simple.cpp)
add_fcl_test
(
capsule_box_1 capsule_box_1.cpp
)
add_fcl_test
(
capsule_box_2 capsule_box_2.cpp
)
add_fcl_test
(
obb obb.cpp
)
add_fcl_test
(
convex convex.cpp
)
add_fcl_test
(
bvh_models bvh_models.cpp
)
...
...
test/convex.cpp
0 → 100644
View file @
e8a5f65d
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, LAAS-CNRS
* 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 */
#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
#define BOOST_TEST_DYN_LINK
#include
<boost/test/unit_test.hpp>
#include
<boost/utility/binary.hpp>
#include
<hpp/fcl/shape/geometric_shapes.h>
using
namespace
hpp
::
fcl
;
BOOST_AUTO_TEST_CASE
(
convex
)
{
FCL_REAL
l
=
1
,
w
=
1
,
d
=
1
;
Vec3f
pts
[
8
];
pts
[
0
]
=
Vec3f
(
l
,
w
,
d
);
pts
[
1
]
=
Vec3f
(
l
,
w
,
-
d
);
pts
[
2
]
=
Vec3f
(
l
,
-
w
,
d
);
pts
[
3
]
=
Vec3f
(
l
,
-
w
,
-
d
);
pts
[
4
]
=
Vec3f
(
-
l
,
w
,
d
);
pts
[
5
]
=
Vec3f
(
-
l
,
w
,
-
d
);
pts
[
6
]
=
Vec3f
(
-
l
,
-
w
,
d
);
pts
[
7
]
=
Vec3f
(
-
l
,
-
w
,
-
d
);
std
::
vector
<
int
>
polygons
;
polygons
.
push_back
(
4
);
polygons
.
push_back
(
0
);
polygons
.
push_back
(
2
);
polygons
.
push_back
(
3
);
polygons
.
push_back
(
1
);
polygons
.
push_back
(
4
);
polygons
.
push_back
(
2
);
polygons
.
push_back
(
6
);
polygons
.
push_back
(
7
);
polygons
.
push_back
(
3
);
polygons
.
push_back
(
4
);
polygons
.
push_back
(
4
);
polygons
.
push_back
(
5
);
polygons
.
push_back
(
7
);
polygons
.
push_back
(
6
);
polygons
.
push_back
(
4
);
polygons
.
push_back
(
0
);
polygons
.
push_back
(
1
);
polygons
.
push_back
(
5
);
polygons
.
push_back
(
4
);
polygons
.
push_back
(
4
);
polygons
.
push_back
(
1
);
polygons
.
push_back
(
3
);
polygons
.
push_back
(
7
);
polygons
.
push_back
(
5
);
polygons
.
push_back
(
4
);
polygons
.
push_back
(
0
);
polygons
.
push_back
(
2
);
polygons
.
push_back
(
6
);
polygons
.
push_back
(
4
);
Convex
box
(
pts
,
// points
8
,
// num points
polygons
.
data
(),
6
// number of polygons
);
// Check neighbors
for
(
int
i
=
0
;
i
<
8
;
++
i
)
{
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
i
].
count
(),
3
);
}
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
0
][
0
],
1
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
0
][
1
],
2
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
0
][
2
],
4
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
1
][
0
],
0
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
1
][
1
],
3
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
1
][
2
],
5
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
2
][
0
],
0
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
2
][
1
],
3
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
2
][
2
],
6
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
3
][
0
],
1
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
3
][
1
],
2
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
3
][
2
],
7
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
4
][
0
],
0
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
4
][
1
],
5
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
4
][
2
],
6
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
5
][
0
],
1
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
5
][
1
],
4
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
5
][
2
],
7
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
6
][
0
],
2
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
6
][
1
],
4
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
6
][
2
],
7
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
7
][
0
],
3
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
7
][
1
],
5
);
BOOST_CHECK_EQUAL
(
box
.
neighbors
[
7
][
2
],
6
);
}
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