hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
algorithms.h
Go to the documentation of this file.
1
2#ifndef GEOM__ALGORITHMS
3#define GEOM__ALGORITHMS
4
5#include <iostream>
6#include <pinocchio/fwd.hpp>
7#include <vector>
8// include pinocchio first
9
10#include <Eigen/src/Core/util/Macros.h>
11#include <hpp/fcl/BVH/BVH_model.h>
12#include <hpp/fcl/collision_data.h>
13#include <hpp/fcl/collision_object.h>
14
15#include <Eigen/Dense>
16#include <hpp/pinocchio/collision-object.hh>
17
18namespace geom {
19
20typedef Eigen::Vector3d Point;
21typedef std::vector<Point> T_Point;
22typedef T_Point::const_iterator CIT_Point;
23typedef T_Point::iterator IT_Point;
24typedef const Eigen::Ref<const Point>& CPointRef;
25
26/*
27typedef fcl::Vec3f Point;
28typedef std::vector<Point> T_Point;
29typedef T_Point::const_iterator CIT_Point;
30typedef const Point& CPointRef;
31
32*/
33typedef std::vector<Eigen::Vector2d> T_Point2D;
34typedef T_Point2D::const_iterator CIT_Point2D;
35
39 hpp::fcl::Vec3f p1, p2, p3;
40};
41
42const double EPSILON = 1e-5;
43const double ZJUMP = 0.001; // value t for the floor in jump_easy_map
44
45typedef hpp::fcl::BVHModel<hpp::fcl::OBBRSS> BVHModelOB;
46typedef hpp::fcl::shared_ptr<const BVHModelOB> BVHModelOBConst_Ptr_t;
47
49 const hpp::pinocchio::CollisionObjectConstPtr_t object,
50 hpp::pinocchio::DeviceData& deviceData);
51
52void projectZ(IT_Point pointsBegin, IT_Point pointsEnd);
53
63
64T_Point convexHull(CIT_Point pointsBegin, CIT_Point pointsEnd);
65
75bool containsHull(T_Point hull, CPointRef aPoint, const double epsilon = 10e-6);
76
84bool contains(T_Point points, CPointRef aPoint, const double epsilon = 10e-6);
85
93 CIT_Point bPointsBegin, CIT_Point bPointsEnd);
94
101
108
117
119
122CIT_Point leftMost(CIT_Point pointsBegin, CIT_Point pointsEnd);
123
130double area(CIT_Point pointsBegin, CIT_Point pointsEnd);
131
140Point centerPlanar(T_Point points, const hpp::fcl::Vec3f& n, double t);
141
147Point center(CIT_Point pointsBegin, CIT_Point pointsEnd);
148
149Point centroid(CIT_Point pointsBegin, CIT_Point pointsEnd, double& area);
150
158double distanceToPlane(const hpp::fcl::Vec3f& n, double t,
159 const hpp::fcl::Vec3f& v);
160
170
179
191 CPointRef P0, Eigen::Ref<Point> res);
192
202void computeTrianglePlaneDistance(hpp::fcl::Vec3f* tri_point,
203 const hpp::fcl::Vec3f& n, double t,
204 hpp::fcl::Vec3f* distance,
205 unsigned int* num_penetrating_points);
206
215bool insideTriangle(const hpp::fcl::Vec3f& a, const hpp::fcl::Vec3f& b,
216 const hpp::fcl::Vec3f& c, const hpp::fcl::Vec3f& p);
217
226 hpp::fcl::CollisionResult result);
227
240// T_Point intersectPolygonePlane(BVHModelOBConst_Ptr_t polygone,
241// BVHModelOBConst_Ptr_t model2, fcl::Vec3f n , double t, fcl::CollisionResult
242// result, bool useT = true, double epsilon = EPSILON);
243
252T_Point intersectTriangles(hpp::fcl::Vec3f* tri, hpp::fcl::Vec3f* tri2,
253 std::ostringstream* ss = 0);
254
266T_Point intersectSegmentPlane(Point s0, Point s1, Eigen::Vector3d pn, Point p0);
267
279 Eigen::Ref<Point> Pn);
280
282
290void computePlanEquation(BVHModelOBConst_Ptr_t plane, Eigen::Ref<Point> Pn,
291 Eigen::Ref<Point> P0);
292
293} // namespace geom
294
295#endif //_FILE_ALGORITHMS
Definition: algorithms.h:18
const double ZJUMP
Definition: algorithms.h:43
T_Point2D::const_iterator CIT_Point2D
Definition: algorithms.h:34
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
T_Point compute3DIntersection(T_Point subPolygon, T_Point clipPolygon)
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
double distanceToPlane(const hpp::fcl::Vec3f &n, double t, const hpp::fcl::Vec3f &v)
distanceToPlane Distance point plan
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
hpp::fcl::BVHModel< hpp::fcl::OBBRSS > BVHModelOB
Definition: algorithms.h:45
T_Point convertBVH(BVHModelOBConst_Ptr_t obj)
double isLeft(CPointRef lA, CPointRef lB, CPointRef p2)
CIT_Point leftMost(CIT_Point pointsBegin, CIT_Point pointsEnd)
void projectZ(IT_Point pointsBegin, IT_Point pointsEnd)
hpp::fcl::shared_ptr< const BVHModelOB > BVHModelOBConst_Ptr_t
Definition: algorithms.h:46
const Eigen::Ref< const Point > & CPointRef
Definition: algorithms.h:24
void intersect3DGeoms(BVHModelOBConst_Ptr_t model1, BVHModelOBConst_Ptr_t model2, hpp::fcl::CollisionResult result)
intersectGeoms compute intersection between 2 OBBRSS geometries
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 t...
bool contains(T_Point points, CPointRef aPoint, const double epsilon=10e-6)
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 BVHMod...
Eigen::Vector3d Point
Definition: algorithms.h:20
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
T_Point intersectSegmentPlane(Point s0, Point s1, Eigen::Vector3d pn, Point p0)
intersectSegmentPlane compute the intersection between a segment and a plane (infinite)
bool containsHull(T_Point hull, CPointRef aPoint, const double epsilon=10e-6)
T_Point::const_iterator CIT_Point
Definition: algorithms.h:22
Point center(CIT_Point pointsBegin, CIT_Point pointsEnd)
center compute the center using average method
std::vector< Point > T_Point
Definition: algorithms.h:21
T_Point::iterator IT_Point
Definition: algorithms.h:23
double area(CIT_Point pointsBegin, CIT_Point pointsEnd)
area compute the area of a 2D polygon
std::vector< Eigen::Vector2d > T_Point2D
Definition: algorithms.h:33
BVHModelOBConst_Ptr_t GetModel(const hpp::pinocchio::CollisionObjectConstPtr_t object, hpp::pinocchio::DeviceData &deviceData)
Point projectPointOnPlane(CPointRef point, CPointRef Pn, CPointRef P0)
projectPointOnPlane othrogonal projection of a given point on the plan
Point centerPlanar(T_Point points, const hpp::fcl::Vec3f &n, double t)
center compute the center of the polygon (planar)
T_Point convexHull(CIT_Point pointsBegin, CIT_Point pointsEnd)
T_Point compute2DIntersection(CIT_Point aPointsBegin, CIT_Point aPointsEnd, CIT_Point bPointsBegin, CIT_Point bPointsEnd)
Point centroid(CIT_Point pointsBegin, CIT_Point pointsEnd, double &area)
const double EPSILON
Definition: algorithms.h:42
Definition: algorithms.h:38
hpp::fcl::Vec3f p2
Definition: algorithms.h:39
hpp::fcl::Vec3f p1
Definition: algorithms.h:39
hpp::fcl::Vec3f p3
Definition: algorithms.h:39