A convex polytope. More...
#include <convex.h>
Public Types | |
using | S = S_ |
Public Types inherited from fcl::ShapeBase< S_ > | |
using | S = S_ |
Public Member Functions | |
Vector3< S > | computeCOM () const override |
void | computeLocalAABB () override |
Computes AABB | |
Matrix3< S > | computeMomentofInertia () const override |
S | computeVolume () const override |
Convex (const Convex &other) | |
Copy constructor. More... | |
Convex (const std::shared_ptr< const std::vector< Vector3< S >>> &vertices, int num_faces, const std::shared_ptr< const std::vector< int >> &faces, bool throw_if_invalid=false) | |
Constructor. More... | |
const Vector3< S > & | findExtremeVertex (const Vector3< S > &v_C) const |
Reports a vertex in this convex polytope that lies farthest in the given direction v_C. The direction vector must be expressed in the same frame C as the vertices of this Convex polytope. More... | |
std::vector< Vector3< S > > | getBoundVertices (const Transform3< S > &tf) const |
Gets the vertices of some convex shape which can bound this shape in a specific configuration. More... | |
int | getFaceCount () const |
Gets the total number of faces in the convex mesh. More... | |
const std::vector< int > & | getFaces () const |
Gets the representation of the faces of the convex hull. More... | |
const Vector3< S > & | getInteriorPoint () const |
A point guaranteed to be on the interior of the convex polytope, used for collision. More... | |
NODE_TYPE | getNodeType () const override |
Gets node type: a convex polytope. More... | |
const std::vector< Vector3< S > > & | getVertices () const |
Gets the vertex positions in the geometry's frame G. More... | |
std::string | representation (int precision=20) const |
Create a string that should be sufficient to recreate this shape. This is akin to the repr() implementation in python. More... | |
~Convex ()=default | |
Public Member Functions inherited from fcl::ShapeBase< S_ > | |
OBJECT_TYPE | getObjectType () const |
Get object type: a geometric shape. More... | |
ShapeBase () | |
Public Member Functions inherited from fcl::CollisionGeometry< S_ > | |
CollisionGeometry () | |
virtual Vector3< S_ > | computeCOM () const |
compute center of mass More... | |
virtual Matrix3< S_ > | computeMomentofInertia () const |
compute the inertia matrix, related to the origin More... | |
virtual Matrix3< S_ > | computeMomentofInertiaRelatedToCOM () const |
compute the inertia matrix, related to the com More... | |
virtual S_ | computeVolume () const |
compute the volume More... | |
virtual NODE_TYPE | getNodeType () const |
get the node type More... | |
virtual OBJECT_TYPE | getObjectType () const |
get the type of the object More... | |
void * | getUserData () const |
get user data in geometry More... | |
bool | isFree () const |
whether the object is completely free More... | |
bool | isOccupied () const |
whether the object is completely occupied More... | |
bool | isUncertain () const |
whether the object has some uncertainty More... | |
void | setUserData (void *data) |
set user data in geometry More... | |
virtual | ~CollisionGeometry () |
Private Member Functions | |
void | FindVertexNeighbors () |
void | ValidateMesh (bool throw_on_error) |
void | ValidateTopology (bool throw_on_error) |
Private Attributes | |
const std::shared_ptr< const std::vector< int > > | faces_ |
bool | find_extreme_via_neighbors_ {false} |
Vector3< S > | interior_point_ |
std::vector< int > | neighbors_ |
const int | num_faces_ |
const bool | throw_if_invalid_ {} |
const std::shared_ptr< const std::vector< Vector3< S > > > | vertices_ |
Static Private Attributes | |
static constexpr int | kMinVertCountForEdgeWalking = 32 |
Friends | |
class | ConvexTester |
std::ostream & | operator<< (std::ostream &out, const Convex &convex) |
Additional Inherited Members | |
Public Attributes inherited from fcl::CollisionGeometry< S_ > | |
Vector3< S_ > | aabb_center |
AABB center in local coordinate. More... | |
AABB< S_ > | aabb_local |
AABB in local coordinate, used for tight AABB when only translation transform. More... | |
S_ | aabb_radius |
AABB radius. More... | |
S_ | cost_density |
collision cost for unit volume More... | |
S_ | threshold_free |
threshold for free (<= is free) More... | |
S_ | threshold_occupied |
threshold for occupied ( >= is occupied) More... | |
void * | user_data |
pointer to user defined data specific to this object More... | |
A convex polytope.
The Convex class represents a subset of general meshes: convex meshes. The class represents its underlying mesh quite simply: an ordered list of vertices, V = [v₀, v₁, ..., vₙ₋₁]
, and an ordered list of faces, F = [f₀, f₁, ..., fₘ₋₁]
, built on those vertices (via vertex indices).
A mesh is only compatible with Convex if it satisfies the following properties:
F = [f₀, f₁, ..., fₙ₋₁]
and Π = [π₀, π₁, ..., πₙ₋₁]
are defined such that face fᵢ has supporting plane πᵢ.fᵢ = [iᵢ₀, iᵢ₁, ..., iᵢₖ₋₁]
(for a face with k vertices and k edges). The ordering of the vertex indices must visit the face's vertices in a counter-clockwise order when viewed from outside the mesh and the vertices must all lie on the face's supporting plane.πᵢ(x)
which report the signed distance from point x
to plane πᵢ
. To be convex, it must be true that π(v) ≤ 0, ∀ π ∈ Π ∧ v ∈ V
.If those requirements are satisfied, for a given convex region, the mesh with the smallest number of faces may have non-triangular faces. In fact, they could be polygons with an arbitrary number of edges/vertices. The Convex class supports compact representation. But it also supports representations that decompose a large n-gon into a set of smaller polygons or triangles. In this case, each of the triangles supporting planes would be the same and the requirements listed above would still be satisfied.
S_ | The scalar type; must be a valid Eigen scalar. |
using fcl::Convex< S_ >::S = S_ |
fcl::Convex< S >::Convex | ( | const std::shared_ptr< const std::vector< Vector3< S >>> & | vertices, |
int | num_faces, | ||
const std::shared_ptr< const std::vector< int >> & | faces, | ||
bool | throw_if_invalid = false |
||
) |
Constructor.
throw_if_invalid
is set to true
.vertices | The positions of the polytope vertices. |
num_faces | The number of faces defined in faces . |
faces | Encoding of the polytope faces. Must encode num_faces number of faces. See member documentation for details on encoding. |
throw_if_invalid | If true , failed attempts to validate the mesh will throw an exception. |
Definition at line 60 of file convex-inl.h.
|
default |
Copy constructor.
|
default |
|
override |
Definition at line 166 of file convex-inl.h.
|
overridevirtual |
Computes AABB in the geometry's canonical frame.
Implements fcl::CollisionGeometry< S_ >.
Definition at line 86 of file convex-inl.h.
|
override |
Definition at line 111 of file convex-inl.h.
|
override |
Definition at line 205 of file convex-inl.h.
const Vector3< S > & fcl::Convex< S >::findExtremeVertex | ( | const Vector3< S > & | v_C | ) | const |
Reports a vertex in this convex polytope that lies farthest in the given direction v_C. The direction vector must be expressed in the same frame C as the vertices of this
Convex polytope.
p_CE | the position vector from Frame C's origin to the extreme vertex E. It is guaranteed that v_C⋅p_CE ≥ v_C⋅p_CF for all F ≠ E in the Convex polytope's set of vertices. |
Definition at line 266 of file convex-inl.h.
|
private |
Definition at line 440 of file convex-inl.h.
std::vector< Vector3< S > > fcl::Convex< S >::getBoundVertices | ( | const Transform3< S > & | tf | ) | const |
Gets the vertices of some convex shape which can bound this shape in a specific configuration.
Definition at line 252 of file convex-inl.h.
|
inline |
|
inline |
Gets the representation of the faces of the convex hull.
The array is the concatenation of an integer-based representations of each face. A single face is encoded as a sub-array of ints where the first int is the number n of vertices in the face, and the following n values are ordered indices into vertices
which visit the vertex values in a counter-clockwise order (viewed from the outside).
For a well-formed face f
consisting of indices [i₀, i₁, ..., iₘ₋₁], it should be the case that:
eⱼ × eⱼ₊₁ · n̂ₚ = |eⱼ × eⱼ₊₁|, ∀ 0 ≤ j < m, j ∈ ℤ
, where n̂ₚ
is the normal for plane π
on which face f
lies. eⱼ = vertices[iⱼ] - vertices[iⱼ₋₁]
is the edge vector of face f
defined by adjacent vertex indices at jᵗʰ vertex (wrapping around such that j - 1 = m - 1
for j = 0
).
Satisfying this condition implies the following:
|
inline |
|
override |
Gets node type: a convex polytope.
Definition at line 100 of file convex-inl.h.
|
inline |
std::string fcl::Convex< S >::representation | ( | int | precision = 20 | ) | const |
Create a string that should be sufficient to recreate this shape. This is akin to the repr() implementation in python.
precision | The requested digits of precision for the numerical measures (same semantics as std::setprecision()). |
Definition at line 318 of file convex-inl.h.
|
private |
Definition at line 352 of file convex-inl.h.
|
private |
Definition at line 360 of file convex-inl.h.
|
friend |
|
friend |
|
private |
|
private |
|
private |
|
staticconstexprprivate |
|
private |
|
private |
|
private |
|
private |