2 #ifndef GEOM__ALGORITHMS 3 #define GEOM__ALGORITHMS 8 #include <Eigen/src/Core/util/Macros.h> 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> 51 BVHModelOBConst_Ptr_t
GetModel(
const hpp::pinocchio::CollisionObjectConstPtr_t
object, hpp::pinocchio::DeviceData &deviceData);
53 void projectZ(IT_Point pointsBegin, IT_Point pointsEnd);
63 T_Point
convexHull(CIT_Point pointsBegin, CIT_Point pointsEnd);
72 bool containsHull(T_Point hull, CPointRef aPoint,
const double epsilon = 10e-6);
79 bool contains(T_Point points, CPointRef aPoint,
const double epsilon = 10e-6);
86 T_Point
compute2DIntersection(CIT_Point aPointsBegin, CIT_Point aPointsEnd, CIT_Point bPointsBegin, CIT_Point bPointsEnd);
111 double isLeft(CPointRef lA, CPointRef lB, CPointRef
p2);
115 CIT_Point
leftMost(CIT_Point pointsBegin, CIT_Point pointsEnd);
124 double area(CIT_Point pointsBegin, CIT_Point pointsEnd);
135 Point
centerPlanar (T_Point points,
const hpp::fcl::Vec3f &n,
double t);
142 Point
center(CIT_Point pointsBegin, CIT_Point pointsEnd);
144 Point
centroid(CIT_Point pointsBegin, CIT_Point pointsEnd,
double &
area);
154 double distanceToPlane(
const hpp::fcl::Vec3f& n,
double t,
const hpp::fcl::Vec3f& v);
184 double projectPointInsidePlan(T_Point plan, CPointRef point, CPointRef Pn, CPointRef P0,Eigen::Ref<Point> res);
204 bool insideTriangle(
const hpp::fcl::Vec3f& a,
const hpp::fcl::Vec3f& b,
const hpp::fcl::Vec3f& c,
const hpp::fcl::Vec3f&p);
212 void intersect3DGeoms(BVHModelOBConst_Ptr_t model1,BVHModelOBConst_Ptr_t model2,hpp::fcl::CollisionResult result);
234 T_Point
intersectTriangles(hpp::fcl::Vec3f* tri, hpp::fcl::Vec3f* tri2,std::ostringstream* ss=0);
253 T_Point
intersectPolygonePlane(BVHModelOBConst_Ptr_t polygone, BVHModelOBConst_Ptr_t plane, Eigen::Ref<Point> Pn);
255 T_Point
convertBVH(BVHModelOBConst_Ptr_t obj);
263 void computePlanEquation( BVHModelOBConst_Ptr_t plane,Eigen::Ref<Point> Pn,Eigen::Ref<Point> P0);
267 #endif //_FILE_ALGORITHMS const Eigen::Ref< const Point > & CPointRef
Definition: algorithms.h:24
bool containsHull(T_Point hull, CPointRef aPoint, const double epsilon=10e-6)
Point centerPlanar(T_Point points, const hpp::fcl::Vec3f &n, double t)
center compute the center of the polygon (planar)
std::vector< Eigen::Vector2d > T_Point2D
Definition: algorithms.h:33
double distanceToPlane(const hpp::fcl::Vec3f &n, double t, const hpp::fcl::Vec3f &v)
distanceToPlane Distance point plan
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...
const double ZJUMP
Definition: algorithms.h:46
T_Point compute3DIntersection(T_Point subPolygon, T_Point clipPolygon)
void projectZ(IT_Point pointsBegin, IT_Point pointsEnd)
BVHModelOBConst_Ptr_t GetModel(const hpp::pinocchio::CollisionObjectConstPtr_t object, hpp::pinocchio::DeviceData &deviceData)
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 ...
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
Eigen::Vector3d Point
Definition: algorithms.h:20
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...
boost::shared_ptr< const BVHModelOB > BVHModelOBConst_Ptr_t
Definition: algorithms.h:49
Point projectPointOnPlane(CPointRef point, CPointRef Pn, CPointRef P0)
projectPointOnPlane othrogonal projection of a given point on the plan
T_Point compute2DIntersection(CIT_Point aPointsBegin, CIT_Point aPointsEnd, CIT_Point bPointsBegin, CIT_Point bPointsEnd)
hpp::fcl::Vec3f p3
Definition: algorithms.h:40
std::vector< Point > T_Point
Definition: algorithms.h:21
Definition: algorithms.h:38
T_Point convexHull(CIT_Point pointsBegin, CIT_Point pointsEnd)
const double EPSILON
Definition: algorithms.h:45
Point center(CIT_Point pointsBegin, CIT_Point pointsEnd)
center compute the center using average method
void intersect3DGeoms(BVHModelOBConst_Ptr_t model1, BVHModelOBConst_Ptr_t model2, hpp::fcl::CollisionResult result)
intersectGeoms compute intersection between 2 OBBRSS geometries
hpp::fcl::Vec3f p2
Definition: algorithms.h:40
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
T_Point::iterator IT_Point
Definition: algorithms.h:23
Definition: algorithms.h:17
hpp::fcl::Vec3f p1
Definition: algorithms.h:40
T_Point2D::const_iterator CIT_Point2D
Definition: algorithms.h:34
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 ...
T_Point intersectSegmentPlane(Point s0, Point s1, Eigen::Vector3d pn, Point p0)
intersectSegmentPlane compute the intersection between a segment and a plane (infinite) ...
T_Point::const_iterator CIT_Point
Definition: algorithms.h:22
Point centroid(CIT_Point pointsBegin, CIT_Point pointsEnd, double &area)
double isLeft(CPointRef lA, CPointRef lB, CPointRef p2)
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 convertBVH(BVHModelOBConst_Ptr_t obj)
bool contains(T_Point points, CPointRef aPoint, const double epsilon=10e-6)
hpp::fcl::BVHModel< hpp::fcl::OBBRSS > BVHModelOB
Definition: algorithms.h:48