hpp-rbprm  4.14.0
Implementation of RB-PRM planner using hpp.
geom Namespace Reference

Classes

struct  TrianglePoints
 

Typedefs

typedef Eigen::Vector3d Point
 
typedef std::vector< PointT_Point
 
typedef T_Point::const_iterator CIT_Point
 
typedef T_Point::iterator IT_Point
 
typedef const Eigen::Ref< const Point > & CPointRef
 
typedef std::vector< Eigen::Vector2d > T_Point2D
 
typedef T_Point2D::const_iterator CIT_Point2D
 
typedef hpp::fcl::BVHModel< hpp::fcl::OBBRSS > BVHModelOB
 
typedef hpp::fcl::shared_ptr< const BVHModelOBBVHModelOBConst_Ptr_t
 

Functions

BVHModelOBConst_Ptr_t GetModel (const hpp::pinocchio::CollisionObjectConstPtr_t object, hpp::pinocchio::DeviceData &deviceData)
 
void projectZ (IT_Point pointsBegin, IT_Point pointsEnd)
 
T_Point convexHull (CIT_Point pointsBegin, CIT_Point pointsEnd)
 
bool containsHull (T_Point hull, CPointRef aPoint, const double epsilon=10e-6)
 
bool contains (T_Point points, CPointRef aPoint, const double epsilon=10e-6)
 
T_Point compute2DIntersection (CIT_Point aPointsBegin, CIT_Point aPointsEnd, CIT_Point bPointsBegin, CIT_Point bPointsEnd)
 
T_Point compute2DIntersection (T_Point subPolygon, T_Point clipPolygon)
 
T_Point compute3DIntersection (T_Point subPolygon, T_Point clipPolygon)
 
double isLeft (CPointRef lA, CPointRef lB, CPointRef p2)
 
CIT_Point leftMost (CIT_Point pointsBegin, CIT_Point pointsEnd)
 
double area (CIT_Point pointsBegin, CIT_Point pointsEnd)
 area compute the area of a 2D polygon More...
 
Point centerPlanar (T_Point points, const hpp::fcl::Vec3f &n, double t)
 center compute the center of the polygon (planar) More...
 
Point center (CIT_Point pointsBegin, CIT_Point pointsEnd)
 center compute the center using average method More...
 
Point centroid (CIT_Point pointsBegin, CIT_Point pointsEnd, double &area)
 
double distanceToPlane (const hpp::fcl::Vec3f &n, double t, const hpp::fcl::Vec3f &v)
 distanceToPlane Distance point plan More...
 
double distanceToPlane (CPointRef point, CPointRef Pn, CPointRef P0)
 distanceToPlane compute the min distance from a point to an (infinite) plan More...
 
Point projectPointOnPlane (CPointRef point, CPointRef Pn, CPointRef P0)
 projectPointOnPlane othrogonal projection of a given point on the plan More...
 
double projectPointInsidePlan (T_Point plan, CPointRef point, CPointRef Pn, CPointRef P0, Eigen::Ref< Point > res)
 projectPointInsidePlan project a point inside on the given plan, inside the given convex hull More...
 
void computeTrianglePlaneDistance (hpp::fcl::Vec3f *tri_point, const hpp::fcl::Vec3f &n, double t, hpp::fcl::Vec3f *distance, unsigned int *num_penetrating_points)
 computeTrianglePlaneDistance compute distance between each vertice of the triangle and a plane More...
 
bool insideTriangle (const hpp::fcl::Vec3f &a, const hpp::fcl::Vec3f &b, const hpp::fcl::Vec3f &c, const hpp::fcl::Vec3f &p)
 insideTriangle check if a point is inside a triangle More...
 
void intersect3DGeoms (BVHModelOBConst_Ptr_t model1, BVHModelOBConst_Ptr_t model2, hpp::fcl::CollisionResult result)
 intersectGeoms compute intersection between 2 OBBRSS geometries More...
 
T_Point intersectTriangles (hpp::fcl::Vec3f *tri, hpp::fcl::Vec3f *tri2, std::ostringstream *ss=0)
 intersectPolygonePlane Compute the intersection of a polygon and a plane The returned point belongs to the surfaces of More...
 
T_Point intersectSegmentPlane (Point s0, Point s1, Eigen::Vector3d pn, Point p0)
 intersectSegmentPlane compute the intersection between a segment and a plane (infinite) More...
 
T_Point intersectPolygonePlane (BVHModelOBConst_Ptr_t polygone, BVHModelOBConst_Ptr_t plane, Eigen::Ref< Point > Pn)
 intersectPolygonePlane compute the intersection between a polygone and an (infinite) plane More...
 
T_Point convertBVH (BVHModelOBConst_Ptr_t obj)
 
void computePlanEquation (BVHModelOBConst_Ptr_t plane, Eigen::Ref< Point > Pn, Eigen::Ref< Point > P0)
 computePlanEquation compute a plan normal and a point in the plan from the first triangle of a BVHModel More...
 

Variables

const double EPSILON = 1e-5
 
const double ZJUMP = 0.001
 

Typedef Documentation

◆ BVHModelOB

typedef hpp::fcl::BVHModel<hpp::fcl::OBBRSS> geom::BVHModelOB

◆ BVHModelOBConst_Ptr_t

typedef hpp::fcl::shared_ptr<const BVHModelOB> geom::BVHModelOBConst_Ptr_t

◆ CIT_Point

typedef T_Point::const_iterator geom::CIT_Point

◆ CIT_Point2D

typedef T_Point2D::const_iterator geom::CIT_Point2D

◆ CPointRef

typedef const Eigen::Ref<const Point>& geom::CPointRef

◆ IT_Point

typedef T_Point::iterator geom::IT_Point

◆ Point

typedef Eigen::Vector3d geom::Point

◆ T_Point

typedef std::vector<Point> geom::T_Point

◆ T_Point2D

typedef std::vector<Eigen::Vector2d> geom::T_Point2D

Function Documentation

◆ area()

double geom::area ( CIT_Point  pointsBegin,
CIT_Point  pointsEnd 
)

area compute the area of a 2D polygon

Parameters
pointsBegin
pointsEnd
Returns

◆ center()

Point geom::center ( CIT_Point  pointsBegin,
CIT_Point  pointsEnd 
)

center compute the center using average method

Parameters
pointslist of points of the polygon
Returns
the center of the polygon

◆ centerPlanar()

Point geom::centerPlanar ( T_Point  points,
const hpp::fcl::Vec3f &  n,
double  t 
)

center compute the center of the polygon (planar)

Parameters
pointsBegin
PointsEnd
nnormal of the plan which contain the polygon
t
Returns

◆ centroid()

Point geom::centroid ( CIT_Point  pointsBegin,
CIT_Point  pointsEnd,
double &  area 
)

◆ compute2DIntersection() [1/2]

T_Point geom::compute2DIntersection ( CIT_Point  aPointsBegin,
CIT_Point  aPointsEnd,
CIT_Point  bPointsBegin,
CIT_Point  bPointsEnd 
)

Computes whether two convex polygons intersect

Parameters
aPointsBegin,aPointsEnditerators to first and last points of the first polygon
bPointsBegin,bPointsEnditerators to first and last points of the second polygon
Returns
the convex polygon resulting from the intersection

◆ compute2DIntersection() [2/2]

T_Point geom::compute2DIntersection ( T_Point  subPolygon,
T_Point  clipPolygon 
)

Computes whether two convex polygons intersect

Parameters
subPolygonlist of vertices of the first polygon
clipPolygonlist of vertices of the second polygon
Returns
the convex polygon resulting from the intersection

◆ compute3DIntersection()

T_Point geom::compute3DIntersection ( T_Point  subPolygon,
T_Point  clipPolygon 
)

Computes whether two convex polygons intersect

Parameters
subPolygonlist of vertices of the first polygon
clipPolygonlist of vertices of the second polygon
Returns
the convex polygon resulting from the intersection

◆ computePlanEquation()

void geom::computePlanEquation ( BVHModelOBConst_Ptr_t  plane,
Eigen::Ref< Point Pn,
Eigen::Ref< Point P0 
)

computePlanEquation compute a plan normal and a point in the plan from the first triangle of a BVHModel

Parameters
plane
Pnthe normal of the plan
P0a point in the plan

◆ computeTrianglePlaneDistance()

void geom::computeTrianglePlaneDistance ( hpp::fcl::Vec3f *  tri_point,
const hpp::fcl::Vec3f &  n,
double  t,
hpp::fcl::Vec3f *  distance,
unsigned int *  num_penetrating_points 
)

computeTrianglePlaneDistance compute distance between each vertice of the triangle and a plane

Parameters
tri_point
nplan's normal
tplan's offset
distancedistance for vertice i
num_penetrating_pointsnumber of point with negative distance

◆ contains()

bool geom::contains ( T_Point  points,
CPointRef  aPoint,
const double  epsilon = 10e-6 
)

Test whether a 2d point belongs to a 2d convex hull determined by a list of unordered points

Parameters
pointslist of points
aPointThe point for which to test belonging the the convex hull
Returns
whether aPoint belongs to the convex polygon determined as the convex hull of the rectangle indicated

◆ containsHull()

bool geom::containsHull ( T_Point  hull,
CPointRef  aPoint,
const double  epsilon = 10e-6 
)

Test whether a 2d point belongs to a 2d convex hull source http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm#wn_PinPolygon()

Parameters
pointsBegin,listof points of the convex hull. ATTENTION: first point is included twice in representation (it is also the last point).
aPointThe point for which to test belonging the the convex hull
Returns
whether aPoint belongs to the convex polygon determined as the convex hull of the rectangle indicated

◆ convertBVH()

T_Point geom::convertBVH ( BVHModelOBConst_Ptr_t  obj)

◆ convexHull()

T_Point geom::convexHull ( CIT_Point  pointsBegin,
CIT_Point  pointsEnd 
)

Implementation of the gift wrapping algorithm to determine the 2D projection of the convex hull of a set of points Dimension can be greater than two, in which case the points will be projected on the z = 0 plane and whether a point belongs to it or not.

Parameters
pointsBegin,pointsEnditerators to first and last points of a set
Returns
clockwise traversal of the 2D convex hull of the points ATTENTION: first point is included twice in representation (it is also the last point)

◆ distanceToPlane() [1/2]

double geom::distanceToPlane ( const hpp::fcl::Vec3f &  n,
double  t,
const hpp::fcl::Vec3f &  v 
)

distanceToPlane Distance point plan

Parameters
nplan's normal
tplan's offset
vpoint
Returns
distance

◆ distanceToPlane() [2/2]

double geom::distanceToPlane ( CPointRef  point,
CPointRef  Pn,
CPointRef  P0 
)

distanceToPlane compute the min distance from a point to an (infinite) plan

Parameters
pointthe point
Pnnormal of the plan
P0a point in the plan
Returns

◆ GetModel()

BVHModelOBConst_Ptr_t geom::GetModel ( const hpp::pinocchio::CollisionObjectConstPtr_t  object,
hpp::pinocchio::DeviceData &  deviceData 
)

◆ insideTriangle()

bool geom::insideTriangle ( const hpp::fcl::Vec3f &  a,
const hpp::fcl::Vec3f &  b,
const hpp::fcl::Vec3f &  c,
const hpp::fcl::Vec3f &  p 
)

insideTriangle check if a point is inside a triangle

Parameters
a
b
c
pthe point
Returns
bool

◆ intersect3DGeoms()

void geom::intersect3DGeoms ( BVHModelOBConst_Ptr_t  model1,
BVHModelOBConst_Ptr_t  model2,
hpp::fcl::CollisionResult  result 
)

intersectGeoms compute intersection between 2 OBBRSS geometries

Parameters
model1
model2
resultthe collision report between the 2 geometries

◆ intersectPolygonePlane()

T_Point geom::intersectPolygonePlane ( BVHModelOBConst_Ptr_t  polygone,
BVHModelOBConst_Ptr_t  plane,
Eigen::Ref< Point Pn 
)

intersectPolygonePlane compute the intersection between a polygone and an (infinite) plane

Parameters
polygone
planea model which first triangle will define the infinite plan used
pn: output, normal of the plan
Returns
an ordoned list of point (clockwise), which belong to both the polygone and the plane

◆ intersectSegmentPlane()

T_Point geom::intersectSegmentPlane ( Point  s0,
Point  s1,
Eigen::Vector3d  pn,
Point  p0 
)

intersectSegmentPlane compute the intersection between a segment and a plane (infinite)

Parameters
s0first point of the segment
s1last point of the segment
nnormal of the plan
p0a point in the plan
Returns
A vector of point, empty if no intersection, with one point on the plane if there is an intersection, with s0 and s1 if the segment lie in the plan

◆ intersectTriangles()

T_Point geom::intersectTriangles ( hpp::fcl::Vec3f *  tri,
hpp::fcl::Vec3f *  tri2,
std::ostringstream *  ss = 0 
)

intersectPolygonePlane Compute the intersection of a polygon and a plane The returned point belongs to the surfaces of

Parameters
model2corresponding to the plan equation.
polygonethe polygon
model2the meshs containing the plane
nnormal of the plan
toffset of the plan
useTif false, all the plan with a normal n are used, if true we also check that t is equal
resultintersectTriangles compute intersection between 2 triangles (not commutative)
trifirst triangle MUST BE DIMENSION 3
tri2second triangle (used to compute the plane, the returned point belong to this triangle
ss

◆ isLeft()

double geom::isLeft ( CPointRef  lA,
CPointRef  lB,
CPointRef  p2 
)

isLeft(): tests if a point is Left|On|Right of an infinite line.

Parameters
lA1st point of the line
lB2nd point of the line
p2point to test
Returns
: >0 for p2 left of the line through p0 and p1 =0 for p2 on the line <0 for p2 right of the line See: the January 2001 Algorithm "Area of 2D and 3D Triangles and Polygons"

◆ leftMost()

CIT_Point geom::leftMost ( CIT_Point  pointsBegin,
CIT_Point  pointsEnd 
)

leftMost(): returns the point most "on the left" for a given set

Parameters
pointsBegin,pointsEnditerators to first and last points of a set

◆ projectPointInsidePlan()

double geom::projectPointInsidePlan ( T_Point  plan,
CPointRef  point,
CPointRef  Pn,
CPointRef  P0,
Eigen::Ref< Point res 
)

projectPointInsidePlan project a point inside on the given plan, inside the given convex hull

Parameters
planconvex hull of points
pointoriginal point
Pnnormal of the plan
P0a point in the plan
resthe resulting projection
Returns
the distance between the original point and the projection

◆ projectPointOnPlane()

Point geom::projectPointOnPlane ( CPointRef  point,
CPointRef  Pn,
CPointRef  P0 
)

projectPointOnPlane othrogonal projection of a given point on the plan

Parameters
pointpoint to project
Pnnormal of the plan
P0a point in the plan
Returns
the orthogonal projection of the point in the plane

◆ projectZ()

void geom::projectZ ( IT_Point  pointsBegin,
IT_Point  pointsEnd 
)

Variable Documentation

◆ EPSILON

const double geom::EPSILON = 1e-5

◆ ZJUMP

const double geom::ZJUMP = 0.001