src
extra
src/extra/reachable-workspace.cpp
Go to the documentation of this file.
1
//
2
// Copyright (c) 2016-2023 CNRS INRIA
3
//
4
5
#include "
pinocchio/extra/reachable-workspace.hpp
"
6
7
#include <
libqhullcpp/QhullError.h
>
8
#include <
libqhullcpp/QhullFacet.h
>
9
#include <
libqhullcpp/QhullVertex.h
>
10
#include <
libqhullcpp/QhullVertexSet.h
>
11
#include <
libqhullcpp/Qhull.h
>
12
13
namespace
pinocchio
14
{
15
namespace
internal
16
{
17
void
buildConvexHull
(
ReachableSetResults
&
res
)
18
{
19
using namespace
orgQhull
;
20
21
Qhull
qh
;
22
qh
.runQhull(
23
""
,
static_cast<
int
>
(
res
.vertex.rows()),
static_cast<
int
>
(
res
.vertex.cols()),
24
res
.vertex.data(),
"QJ"
);
25
if
(
qh
.qhullStatus() !=
qh_ERRnone
)
26
throw
(
qh
.qhullMessage());
27
28
QhullFacetList
facets =
qh
.facetList();
29
res
.faces.resize(
static_cast<
int
>
(facets.
count
()), 3);
30
QhullFacetListIterator
j(facets);
31
QhullFacet
f
;
32
int
count
= 0;
33
34
while
(j.
hasNext
())
35
{
36
f
= j.
next
();
37
if
(!
f
.isGood())
38
{
39
// ignore facet
40
}
41
else
42
{
43
QhullVertexSet
vs =
f
.vertices();
44
for
(
int
i
= 0; i < static_cast<int>(vs.
size
());
i
++)
45
res
.faces(
count
,
i
) =
static_cast<
int
>
(vs[
i
].point().id());
46
count
++;
47
}
48
}
49
}
50
}
// namespace internal
51
}
// namespace pinocchio
QhullVertexSet.h
orgQhull::QhullVertexSet
reachable-workspace.hpp
orgQhull
Qhull.h
inverse-kinematics.i
int i
Definition:
inverse-kinematics.py:17
QhullFacet.h
pinocchio::res
ReturnType res
Definition:
spatial/classic-acceleration.hpp:57
pinocchio::ReachableSetResults
Structure containing the return value for the reachable algorithm.
Definition:
reachable-workspace.hpp:25
autodiff-rnea.f
f
Definition:
autodiff-rnea.py:24
count
unsigned char const & count() const
orgQhull::QhullLinkedListIterator::hasNext
bool hasNext() const
pinocchio::internal::buildConvexHull
PINOCCHIO_EXTRA_DLLAPI void buildConvexHull(ReachableSetResults &res)
Computes the convex hull using qhull associated with the vertex stored in res.
Definition:
src/extra/reachable-workspace.cpp:17
orgQhull::QhullFacetList
orgQhull::Qhull
qh_ERRnone
#define qh_ERRnone
orgQhull::QhullSetBase::size
size_t size() const
QhullVertex.h
qh
#define qh
orgQhull::QhullLinkedListIterator::next
T next()
QhullError.h
orgQhull::QhullFacet
orgQhull::QhullLinkedListIterator
orgQhull::QhullFacetList::count
countT count() const
pinocchio
Main pinocchio namespace.
Definition:
timings.cpp:27
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:12