hpp-rbprm  4.10.0
Implementation of RB-PRM planner using hpp.
algorithms.h
Go to the documentation of this file.
1 
2 #ifndef GEOM__ALGORITHMS
3 #define GEOM__ALGORITHMS
4 
5 
6 #include <iostream>
7 #include <Eigen/Dense>
8 #include <Eigen/src/Core/util/Macros.h>
9 #include <vector>
10 #include <pinocchio/fwd.hpp>
11 #include <hpp/fcl/collision_object.h>
12 #include <hpp/fcl/collision_data.h>
13 #include <hpp/fcl/BVH/BVH_model.h>
14 #include <hpp/pinocchio/collision-object.hh>
15 
16 
17 namespace geom
18 {
19 
20  typedef Eigen::Vector3d Point;
21  typedef std::vector<Point> T_Point;
22  typedef T_Point::const_iterator CIT_Point;
23  typedef T_Point::iterator IT_Point;
24  typedef const Eigen::Ref<const Point>& CPointRef;
25 
26  /*
27  typedef fcl::Vec3f Point;
28  typedef std::vector<Point> T_Point;
29  typedef T_Point::const_iterator CIT_Point;
30  typedef const Point& CPointRef;
31 
32 */
33  typedef std::vector<Eigen::Vector2d> T_Point2D;
34  typedef T_Point2D::const_iterator CIT_Point2D;
35 
39  {
40  hpp::fcl::Vec3f p1, p2, p3;
41  };
42 
43 
44 
45  const double EPSILON = 1e-5;
46  const double ZJUMP = 0.001; // value t for the floor in jump_easy_map
47 
48  typedef hpp::fcl::BVHModel<hpp::fcl::OBBRSS> BVHModelOB;
49  typedef boost::shared_ptr<const BVHModelOB> BVHModelOBConst_Ptr_t;
50 
51  BVHModelOBConst_Ptr_t GetModel(const hpp::pinocchio::CollisionObjectConstPtr_t object, hpp::pinocchio::DeviceData &deviceData);
52 
53  void projectZ(IT_Point pointsBegin, IT_Point pointsEnd);
54 
62 
63  T_Point convexHull(CIT_Point pointsBegin, CIT_Point pointsEnd);
64 
72  bool containsHull(T_Point hull, CPointRef aPoint, const double epsilon = 10e-6);
73 
79  bool contains(T_Point points, CPointRef aPoint, const double epsilon = 10e-6);
80 
86  T_Point compute2DIntersection(CIT_Point aPointsBegin, CIT_Point aPointsEnd, CIT_Point bPointsBegin, CIT_Point bPointsEnd);
87 
93  T_Point compute2DIntersection(T_Point subPolygon, T_Point clipPolygon);
94 
100  T_Point compute3DIntersection(T_Point subPolygon, T_Point clipPolygon);
101 
110 
111  double isLeft(CPointRef lA, CPointRef lB, CPointRef p2);
112 
115  CIT_Point leftMost(CIT_Point pointsBegin, CIT_Point pointsEnd);
116 
117 
124  double area(CIT_Point pointsBegin, CIT_Point pointsEnd);
125 
126 
135  Point centerPlanar (T_Point points, const hpp::fcl::Vec3f &n, double t);
136 
142  Point center(CIT_Point pointsBegin, CIT_Point pointsEnd);
143 
144  Point centroid(CIT_Point pointsBegin, CIT_Point pointsEnd,double &area);
145 
146 
154  double distanceToPlane(const hpp::fcl::Vec3f& n, double t, const hpp::fcl::Vec3f& v);
155 
163  double distanceToPlane(CPointRef point,CPointRef Pn,CPointRef P0);
164 
165 
174 
184  double projectPointInsidePlan(T_Point plan, CPointRef point, CPointRef Pn, CPointRef P0,Eigen::Ref<Point> res);
185 
194  void computeTrianglePlaneDistance(hpp::fcl::Vec3f* tri_point, const hpp::fcl::Vec3f& n, double t, hpp::fcl::Vec3f *distance, unsigned int* num_penetrating_points);
195 
204  bool insideTriangle(const hpp::fcl::Vec3f& a, const hpp::fcl::Vec3f& b, const hpp::fcl::Vec3f& c, const hpp::fcl::Vec3f&p);
205 
212  void intersect3DGeoms(BVHModelOBConst_Ptr_t model1,BVHModelOBConst_Ptr_t model2,hpp::fcl::CollisionResult result);
213 
224  //T_Point intersectPolygonePlane(BVHModelOBConst_Ptr_t polygone, BVHModelOBConst_Ptr_t model2, fcl::Vec3f n , double t, fcl::CollisionResult result, bool useT = true, double epsilon = EPSILON);
225 
226 
227 
234  T_Point intersectTriangles(hpp::fcl::Vec3f* tri, hpp::fcl::Vec3f* tri2,std::ostringstream* ss=0);
235 
244  T_Point intersectSegmentPlane(Point s0, Point s1, Eigen::Vector3d pn, Point p0 );
245 
253  T_Point intersectPolygonePlane(BVHModelOBConst_Ptr_t polygone, BVHModelOBConst_Ptr_t plane, Eigen::Ref<Point> Pn);
254 
256 
263  void computePlanEquation( BVHModelOBConst_Ptr_t plane,Eigen::Ref<Point> Pn,Eigen::Ref<Point> P0);
264 
265 } //namespace geom
266 
267 #endif //_FILE_ALGORITHMS
geom::TrianglePoints
Definition: algorithms.h:38
geom::compute2DIntersection
T_Point compute2DIntersection(CIT_Point aPointsBegin, CIT_Point aPointsEnd, CIT_Point bPointsBegin, CIT_Point bPointsEnd)
geom::centroid
Point centroid(CIT_Point pointsBegin, CIT_Point pointsEnd, double &area)
geom::center
Point center(CIT_Point pointsBegin, CIT_Point pointsEnd)
center compute the center using average method
geom::projectPointOnPlane
Point projectPointOnPlane(CPointRef point, CPointRef Pn, CPointRef P0)
projectPointOnPlane othrogonal projection of a given point on the plan
geom::leftMost
CIT_Point leftMost(CIT_Point pointsBegin, CIT_Point pointsEnd)
geom::projectPointInsidePlan
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
geom::computeTrianglePlaneDistance
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
geom::centerPlanar
Point centerPlanar(T_Point points, const hpp::fcl::Vec3f &n, double t)
center compute the center of the polygon (planar)
geom::containsHull
bool containsHull(T_Point hull, CPointRef aPoint, const double epsilon=10e-6)
hpp::tools::distance
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
geom::CIT_Point2D
T_Point2D::const_iterator CIT_Point2D
Definition: algorithms.h:34
geom::computePlanEquation
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...
geom::compute3DIntersection
T_Point compute3DIntersection(T_Point subPolygon, T_Point clipPolygon)
geom::IT_Point
T_Point::iterator IT_Point
Definition: algorithms.h:23
geom::T_Point2D
std::vector< Eigen::Vector2d > T_Point2D
Definition: algorithms.h:33
geom::TrianglePoints::p2
hpp::fcl::Vec3f p2
Definition: algorithms.h:40
geom::Point
Eigen::Vector3d Point
Definition: algorithms.h:20
geom::BVHModelOBConst_Ptr_t
boost::shared_ptr< const BVHModelOB > BVHModelOBConst_Ptr_t
Definition: algorithms.h:49
geom::BVHModelOB
hpp::fcl::BVHModel< hpp::fcl::OBBRSS > BVHModelOB
Definition: algorithms.h:48
geom::projectZ
void projectZ(IT_Point pointsBegin, IT_Point pointsEnd)
geom
Definition: algorithms.h:17
geom::intersectTriangles
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...
geom::area
double area(CIT_Point pointsBegin, CIT_Point pointsEnd)
area compute the area of a 2D polygon
geom::distanceToPlane
double distanceToPlane(const hpp::fcl::Vec3f &n, double t, const hpp::fcl::Vec3f &v)
distanceToPlane Distance point plan
geom::convexHull
T_Point convexHull(CIT_Point pointsBegin, CIT_Point pointsEnd)
geom::EPSILON
const double EPSILON
Definition: algorithms.h:45
geom::TrianglePoints::p3
hpp::fcl::Vec3f p3
Definition: algorithms.h:40
geom::insideTriangle
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
geom::CIT_Point
T_Point::const_iterator CIT_Point
Definition: algorithms.h:22
geom::GetModel
BVHModelOBConst_Ptr_t GetModel(const hpp::pinocchio::CollisionObjectConstPtr_t object, hpp::pinocchio::DeviceData &deviceData)
geom::contains
bool contains(T_Point points, CPointRef aPoint, const double epsilon=10e-6)
geom::T_Point
std::vector< Point > T_Point
Definition: algorithms.h:21
geom::intersectPolygonePlane
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
geom::TrianglePoints::p1
hpp::fcl::Vec3f p1
Definition: algorithms.h:40
geom::isLeft
double isLeft(CPointRef lA, CPointRef lB, CPointRef p2)
geom::intersectSegmentPlane
T_Point intersectSegmentPlane(Point s0, Point s1, Eigen::Vector3d pn, Point p0)
intersectSegmentPlane compute the intersection between a segment and a plane (infinite)
geom::convertBVH
T_Point convertBVH(BVHModelOBConst_Ptr_t obj)
geom::intersect3DGeoms
void intersect3DGeoms(BVHModelOBConst_Ptr_t model1, BVHModelOBConst_Ptr_t model2, hpp::fcl::CollisionResult result)
intersectGeoms compute intersection between 2 OBBRSS geometries
geom::ZJUMP
const double ZJUMP
Definition: algorithms.h:46
geom::CPointRef
const typedef Eigen::Ref< const Point > & CPointRef
Definition: algorithms.h:24