geometricComputations.hpp

#ifndef GEOMETRICTOOLS_HPP
#define GEOMETRICTOOLS_HPP

#include "P3d-pkg.h"
#include "API/Device/robot.hpp"
#include "Logging/Logger.h"


class ParallelepipedForm;


class GeometricComputations
{
    MOVE3D_STATIC_LOGGER;
public:
    GeometricComputations();

    p3d_point getRandomPointInTriangle(p3d_point A, p3d_point B, p3d_point C);


    p3d_point getRandomPointInRectangle(p3d_point A, p3d_point B, p3d_point C, p3d_point D);

    std::vector<p3d_point> getLowerPointsInObject(Robot* obj, double steepness);

    std::vector<p3d_point> getUpperPointsInObject(Robot* obj, double steepness);


    p3d_point getCenterOfObj(Robot* obje);

    p3d_point getBotomCenterOfObj(Robot* obje, double threshhold);

    double compute_triangle_area_x_y2(p3d_point a, p3d_point b, p3d_point c);

    bool point_in_rect2(p3d_point a, p3d_point b, p3d_point c, p3d_point d, p3d_point x);

    bool point_in_segment2d(p3d_point a, p3d_point b, p3d_point x);

    double distBetweenPoints(p3d_point a, p3d_point b);

    double distBetweenMatrix4(p3d_matrix4 a, p3d_matrix4 b);

    double distBetweenPoints2D(p3d_point a, p3d_point b);

    bool hexahedron_in_hexahedron(p3d_point x1,p3d_point x2, p3d_point x3, p3d_point x4,
                                  p3d_point x5, p3d_point x6, p3d_point x7, p3d_point x8,
                                  p3d_point y1,p3d_point y2, p3d_point y3, p3d_point y4,
                                  p3d_point y5, p3d_point y6, p3d_point y7, p3d_point y8);

    bool point_in_hexahedron(p3d_point a, p3d_point b, p3d_point c, p3d_point d,
                             p3d_point e, p3d_point f, p3d_point g, p3d_point h,
                             p3d_point x);

    double compute_pyramid_area(p3d_point a, p3d_point b, p3d_point c, p3d_point d, p3d_point h);

    std::vector<p3d_point> getRandomPointsInHexahedron(double nbPoints, p3d_point a, p3d_point b, p3d_point c, p3d_point d, p3d_point e, p3d_point f, p3d_point g, p3d_point h);

    p3d_point getClosestBBPointFromPoint(Robot* e, p3d_point s);

    ParallelepipedForm getBoxFromObjects(std::vector<Robot*> vr, Robot* gripper);

    double distConfs(confPtr_t q1, confPtr_t q2);

    std::vector<p3d_point> getSampled(std::vector<p3d_point> vp, double sampleRate);

    double computeCost(std::vector<p3d_point> vHP, std::vector<p3d_point> vp, double angleWeight, double distHWeight, double distWeight);


    std::vector<p3d_point> smoothTraj(std::vector<p3d_point> origTraj, std::vector<p3d_point> vHP, Robot *r, int nbIterMax, double gainLim, double trajDistWeight, double distToHWeight, double angleWeight);

};







#endif // GEOMETRICTOOLS_HPP