Drake
ProximityEngine< T > Class Template Reference

The underlying engine for performing geometric proximity queries. More...

#include <geometry/proximity_engine.h>

Public Member Functions

 ProximityEngine ()
 
 ~ProximityEngine ()
 
 ProximityEngine (const ProximityEngine &other)
 Construct a deep copy of the provided other engine. More...
 
ProximityEngineoperator= (const ProximityEngine &other)
 Set this engine to be a deep copy of the other engine. More...
 
 ProximityEngine (ProximityEngine &&other) noexcept
 Construct an engine by moving the data of a source engine. More...
 
ProximityEngineoperator= (ProximityEngine &&other) noexcept
 Move assign a source engine to this engine. More...
 
std::unique_ptr< ProximityEngine< AutoDiffXd > > ToAutoDiffXd () const
 Returns an independent copy of this engine templated on the AutoDiffXd scalar type. More...
 
void UpdateWorldPoses (const std::vector< Isometry3< T >> &X_WG)
 Updates the poses for all of the dynamic geometries in the engine. More...
 
template<typename T >
 ProximityEngine (ProximityEngine< T >::Impl *impl)
 
Topology management
GeometryIndex AddDynamicGeometry (const Shape &shape)
 Adds the given shape to the engine's dynamic geometry. More...
 
AnchoredGeometryIndex AddAnchoredGeometry (const Shape &shape, const Isometry3< double > &X_WG)
 Adds the given shape to the engine's anchored geometry at the fixed pose given by X_WG (in the world frame W). More...
 
int num_geometries () const
 Reports the total number of geometries in the engine – dynamic and anchored (spanning all sources). More...
 
int num_dynamic () const
 Reports the number of dynamic geometries (spanning all sources). More...
 
int num_anchored () const
 Reports the number of anchored geometries (spanning all sources). More...
 
Collision Queries

These queries detect collisions between geometry.

Two geometries collide if they overlap each other and are not explicitly excluded through collision filtering. These algorithms find those colliding cases, characterize them, and report the essential characteristics of that collision.

std::vector< PenetrationAsPointPair< double > > ComputePointPairPenetration (const std::vector< GeometryId > &dynamic_map, const std::vector< GeometryId > &anchored_map) const
 Computes the penetrations across all pairs of geometries in the world. More...
 

Friends

template<typename >
class ProximityEngine
 
class ProximityEngineTester
 

Detailed Description

template<typename T>
class drake::geometry::internal::ProximityEngine< T >

The underlying engine for performing geometric proximity queries.

It owns the geometry instances and, once it has been provided with the poses of the geometry, it provides geometric queries on that geometry.

Proximity queries span a range of types, including:

  • penetration
  • distance
  • ray-intersection
Template Parameters
TThe scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided:

  • double
  • AutoDiffXd

They are already available to link against in the containing library. No other values for T are currently supported.

Constructor & Destructor Documentation

ProximityEngine ( const ProximityEngine< T > &  other)

Construct a deep copy of the provided other engine.

ProximityEngine ( ProximityEngine< T > &&  other)
noexcept

Construct an engine by moving the data of a source engine.

The source engine will be returned to its default-initialized state.

ProximityEngine ( ProximityEngine< T >::Impl *  impl)

Member Function Documentation

AnchoredGeometryIndex AddAnchoredGeometry ( const Shape shape,
const Isometry3< double > &  X_WG 
)

Adds the given shape to the engine's anchored geometry at the fixed pose given by X_WG (in the world frame W).

GeometryIndex AddDynamicGeometry ( const Shape shape)

Adds the given shape to the engine's dynamic geometry.

std::vector< PenetrationAsPointPair< double > > ComputePointPairPenetration ( const std::vector< GeometryId > &  dynamic_map,
const std::vector< GeometryId > &  anchored_map 
) const

Computes the penetrations across all pairs of geometries in the world.

Only reports results for penetrating geometries; if two geometries are separated, there will be no result for that pair.

The penetrations are characterized by pairs of points (providing some measure of the penetration "depth" of the two objects – but not the overlapping volume.

Parameters
[in]dynamic_mapA map from geometry index to the corresponding global geometry identifier for dynamic geometries.
[in]anchored_mapA map from geometry index to the corresponding global geometry identifier for anchored geometries.
Returns
A vector populated with all detected penetrations characterized as point pairs.
int num_anchored ( ) const

Reports the number of anchored geometries (spanning all sources).

int num_dynamic ( ) const

Reports the number of dynamic geometries (spanning all sources).

int num_geometries ( ) const

Reports the total number of geometries in the engine – dynamic and anchored (spanning all sources).

ProximityEngine< T > & operator= ( const ProximityEngine< T > &  other)

Set this engine to be a deep copy of the other engine.

ProximityEngine< T > & operator= ( ProximityEngine< T > &&  other)
noexcept

Move assign a source engine to this engine.

The source engine will be returned to its default-initialized state.

std::unique_ptr< ProximityEngine< AutoDiffXd > > ToAutoDiffXd ( ) const

Returns an independent copy of this engine templated on the AutoDiffXd scalar type.

If the engine is already an AutoDiffXd engine, it is equivalent to using the copy constructor to create a duplicate on the heap.

void UpdateWorldPoses ( const std::vector< Isometry3< T >> &  X_WG)

Updates the poses for all of the dynamic geometries in the engine.

It is an invariant that every registered dynamic geometry, across all geometry sources, has a unique index that lies in the range [0, num_dynamic() - 1]. Therefore, X_WG should have size equal to num_dynamics() and any other length will cause program failure. The iᵗʰ entry contains the pose for the geometry whose GeometryIndex value is i.

Parameters
X_WGThe poses of each geometry G measured and expressed in the world frame W.

Friends And Related Function Documentation

friend class ProximityEngine
friend
friend class ProximityEngineTester
friend

The documentation for this class was generated from the following files: