51 const std::vector<TPolygon3D>& oldPolys,
52 std::vector<TPolygonWithPlane>& newPolys);
67 bool intersect(
const TSegment3D& s1,
const TSegment3D& s2, TObject3D&
obj);
77 bool intersect(
const TSegment3D& s1,
const TPlane& p2, TObject3D&
obj);
87 bool intersect(
const TSegment3D& s1,
const TLine3D& r2, TObject3D&
obj);
108 bool intersect(
const TPlane& p1,
const TPlane& p2, TObject3D&
obj);
118 bool intersect(
const TPlane& p1,
const TLine3D& p2, TObject3D&
obj);
155 bool intersect(
const TLine3D& r1,
const TLine3D& r2, TObject3D&
obj);
166 bool intersect(
const TLine2D& r1,
const TLine2D& r2, TObject2D&
obj);
176 bool intersect(
const TLine2D& r1,
const TSegment2D& s2, TObject2D&
obj);
199 bool intersect(
const TSegment2D& s1,
const TSegment2D& s2, TObject2D&
obj);
211 double getAngle(
const TPlane& p1,
const TPlane& p2);
216 double getAngle(
const TPlane& p1,
const TLine3D& r2);
229 double getAngle(
const TLine3D& r1,
const TLine3D& r2);
234 double getAngle(
const TLine2D& r1,
const TLine2D& r2);
284 const TPose2D&
p,
const double (&vector)[2], TLine2D&
r);
339 TSegment3D& newSegment)
341 project3D(segment.point1, newXYpose, newSegment.point1);
342 project3D(segment.point2, newXYpose, newSegment.point2);
356 TPolygon3D& newPolygon);
360 TObject3D& newObject);
391 std::vector<T>& newObjs)
393 size_t N = objs.size();
395 for (
size_t i = 0; i < N; i++)
project3D(objs[i], newXYpose, newObjs[i]);
411 void project2D(
const TLine2D& line,
const TPose2D& newXpose, TLine2D& newLine);
414 const TPolygon2D& polygon,
const TPose2D& newXpose, TPolygon2D& newPolygon);
417 const TObject2D&
object,
const TPose2D& newXpose, TObject2D& newObject);
424 template <
class T,
class CPOSE2D>
437 template <
class T,
class CPOSE2D>
449 const std::vector<T>& objs,
const TPose2D& newXpose,
450 std::vector<T>& newObjs)
452 size_t N = objs.size();
454 for (
size_t i = 0; i < N; i++)
project2D(objs[i], newXpose, newObjs[i]);
465 bool intersect(
const TPolygon2D& p1,
const TSegment2D& s2, TObject2D&
obj);
478 inline bool intersect(
const TLine2D& r1,
const TPolygon2D& p2, TObject2D&
obj)
484 bool intersect(
const TPolygon3D& p1,
const TSegment3D& s2, TObject3D&
obj);
486 bool intersect(
const TPolygon3D& p1,
const TLine3D& r2, TObject3D&
obj);
504 inline bool intersect(
const TPlane& p1,
const TPolygon3D& p2, TObject3D&
obj)
514 const std::vector<TPolygon3D>&
v1,
const std::vector<TPolygon3D>&
v2,
515 CSparseMatrixTemplate<TObject3D>& objs);
521 const std::vector<TPolygon3D>&
v1,
const std::vector<TPolygon3D>&
v2,
522 std::vector<TObject3D>& objs);
532 template <
class T,
class U,
class O>
534 const std::vector<T>&
v1,
const std::vector<U>&
v2,
537 size_t M =
v1.size(), N =
v2.size();
541 for (
size_t i = 0; i < M; i++)
542 for (
size_t j = 0; j < M; j++)
550 template <
class T,
class U,
class O>
552 const std::vector<T>&
v1,
const std::vector<U>&
v2, std::vector<O> objs)
557 it1 !=
v1.end(); ++it1)
559 const T& elem1 = *it1;
561 it2 !=
v2.end(); ++it2)
568 bool intersect(
const TObject2D& o1,
const TObject2D& o2, TObject2D&
obj);
570 bool intersect(
const TObject3D& o1,
const TObject3D& o2, TObject3D&
obj);
578 double distance(
const TPoint2D& p1,
const TPoint2D& p2);
580 double distance(
const TPoint3D& p1,
const TPoint3D& p2);
582 double distance(
const TLine2D& r1,
const TLine2D& r2);
584 double distance(
const TLine3D& r1,
const TLine3D& r2);
598 double distance(
const TPolygon2D& p1,
const TLine2D& l2);
599 inline double distance(
const TLine2D& l1,
const TPolygon2D& p2)
604 double distance(
const TPolygon3D& p1,
const TPolygon3D& p2);
606 double distance(
const TPolygon3D& p1,
const TSegment3D& s2);
613 double distance(
const TPolygon3D& p1,
const TLine3D& l2);
620 double distance(
const TPolygon3D& po,
const TPlane& pl);
622 inline double distance(
const TPlane& pl,
const TPolygon3D& po)
634 const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax);
637 const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax);
708 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys);
714 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys,
715 std::vector<TSegment3D>& remainder);
721 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys);
727 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
728 std::vector<TObject3D>& remainder);
734 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
735 std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2);
751 const TPolygon2D& poly, std::vector<TPolygon2D>&
components);
757 const TPolygon3D& poly, std::vector<TPolygon3D>&
components);
794 std::vector<TPolygonWithPlane> pwp;
811 template <
class T,
class U,
class V>
814 vOut[0] =
v0[1] *
v1[2] -
v0[2] *
v1[1];
815 vOut[1] =
v0[2] *
v1[0] -
v0[0] *
v1[2];
816 vOut[2] =
v0[0] *
v1[1] -
v0[1] *
v1[0];
822 const std::vector<T>&
v0,
const std::vector<T>&
v1, std::vector<T>& v_out)
827 v_out[0] =
v0[1] *
v1[2] -
v0[2] *
v1[1];
828 v_out[1] = -
v0[0] *
v1[2] +
v0[2] *
v1[0];
829 v_out[2] =
v0[0] *
v1[1] -
v0[1] *
v1[0];
833 template <
class VEC1,
class VEC2>
835 const VEC1&
v0,
const VEC2&
v1)
837 Eigen::Matrix<double, 3, 1> vOut;
838 vOut[0] =
v0[1] *
v1[2] -
v0[2] *
v1[1];
839 vOut[1] =
v0[2] *
v1[0] -
v0[0] *
v1[2];
840 vOut[2] =
v0[0] *
v1[1] -
v0[1] *
v1[0];
853 template <
class VECTOR,
class MATRIX>
858 M.set_unsafe(0, 0, 0);
859 M.set_unsafe(0, 1, -
v[2]);
860 M.set_unsafe(0, 2,
v[1]);
861 M.set_unsafe(1, 0,
v[2]);
862 M.set_unsafe(1, 1, 0);
863 M.set_unsafe(1, 2, -
v[0]);
864 M.set_unsafe(2, 0, -
v[1]);
865 M.set_unsafe(2, 1,
v[0]);
866 M.set_unsafe(2, 2, 0);
869 template <
class VECTOR>
887 template <
class VECTOR,
class MATRIX>
892 M.set_unsafe(0, 0, 0);
893 M.set_unsafe(0, 1,
v[2]);
894 M.set_unsafe(0, 2, -
v[1]);
895 M.set_unsafe(1, 0, -
v[2]);
896 M.set_unsafe(1, 1, 0);
897 M.set_unsafe(1, 2,
v[0]);
898 M.set_unsafe(2, 0,
v[1]);
899 M.set_unsafe(2, 1, -
v[0]);
900 M.set_unsafe(2, 2, 0);
903 template <
class VECTOR>
915 template <
class T,
class U>
925 template <
class T,
class U>
937 const double& Px,
const double& Py,
const double& x1,
const double& y1,
938 const double& x2,
const double& y2,
double& out_x,
double& out_y);
944 const double& Px,
const double& Py,
const double& x1,
const double& y1,
945 const double& x2,
const double& y2,
double& out_x,
double& out_y);
950 const double& Px,
const double& Py,
const double& x1,
const double& y1,
951 const double& x2,
const double& y2);
954 template <
typename T>
961 template <
typename T>
963 const T x1,
const T y1,
const T z1,
const T x2,
const T y2,
const T z2)
969 template <
typename T>
976 template <
typename T>
978 const T x1,
const T y1,
const T z1,
const T x2,
const T y2,
const T z2)
986 template <
typename T>
988 const double Px,
const double Py,
const double x1,
const double y1,
989 const double x2,
const double y2, T& out_x, T& out_y)
993 out_x =
static_cast<T
>(ox);
994 out_y =
static_cast<T
>(oy);
1001 const double x1,
const double y1,
const double x2,
const double y2,
1002 const double x3,
const double y3,
const double x4,
const double y4,
1003 double& ix,
double& iy);
1008 const double x1,
const double y1,
const double x2,
const double y2,
1009 const double x3,
const double y3,
const double x4,
const double y4,
1010 float& ix,
float& iy);
1016 const double& px,
const double& py,
unsigned int polyEdges,
1017 const double* poly_xs,
const double* poly_ys);
1022 template <
typename T>
1024 T
x, T
y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y)
1028 const T
a1 = atan2(v1y -
y, v1x -
x);
1029 const T
a2 = atan2(v2y -
y, v2x -
x);
1030 const T
a3 = atan2(v3y -
y, v3x -
x);
1031 const T a4 = atan2(v4y -
y, v4x -
x);
1037 if (
sign(da1) !=
sign(da2))
return false;
1040 if (
sign(da2) !=
sign(da3))
return false;
1050 const double& px,
const double& py,
unsigned int polyEdges,
1051 const double* poly_xs,
const double* poly_ys);
1063 const double& p1_x,
const double& p1_y,
const double& p1_z,
1064 const double& p2_x,
const double& p2_y,
const double& p2_z,
1065 const double& p3_x,
const double& p3_y,
const double& p3_z,
1066 const double& p4_x,
const double& p4_y,
const double& p4_z,
double&
x,
1067 double&
y,
double&
z,
double& dist);
1079 const double& R1_x_min,
const double& R1_x_max,
const double& R1_y_min,
1080 const double& R1_y_max,
const double& R2_x_min,
const double& R2_x_max,
1081 const double& R2_y_min,
const double& R2_y_max,
const double& R2_pose_x,
1082 const double& R2_pose_y,
const double& R2_pose_phi);
1126 if (dx == 0 && dy == 0 && dz == 0)
1133 T
n = sqrt(n_xy +
square(dz));
1140 if (fabs(dx) > 1e-4 || fabs(dy) > 1e-4)
1142 P(0, 1) = -dy / n_xy;
1143 P(1, 1) = dx / n_xy;
void project3D(const TPoint3D &point, const mrpt::math::TPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
bool RectanglesIntersection(const double &R1_x_min, const double &R1_x_max, const double &R1_y_min, const double &R1_y_max, const double &R2_x_min, const double &R2_x_max, const double &R2_y_min, const double &R2_y_max, const double &R2_pose_x, const double &R2_pose_y, const double &R2_pose_phi)
Returns whether two rotated rectangles intersect.
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
#define THROW_EXCEPTION(msg)
void closestFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
void closestFromPointToSegment(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment.
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
bool minDistBetweenLines(const double &p1_x, const double &p1_y, const double &p1_z, const double &p2_x, const double &p2_y, const double &p2_z, const double &p3_x, const double &p3_y, const double &p3_z, const double &p4_x, const double &p4_y, const double &p4_z, double &x, double &y, double &z, double &dist)
Calculates the minimum distance between a pair of lines.
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
double closestSquareDistanceFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2)
Returns the square distance from a point to a line.
bool pointIntoPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns true if the 2D point (px,py) falls INTO the given polygon.
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
void getAsPose3D(mrpt::math::TPose3D &outPose)
void assemblePolygons(const std::vector< TSegment3D > &segms, std::vector< TPolygon3D > &polys)
Tries to assemble a set of segments into a set of closed polygons.
void getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
mrpt::math::TPose3D inversePose
Plane's inverse pose.
CMatrixFixedNumeric< double, 4, 4 > CMatrixDouble44
void createFromPoseAndVector(const mrpt::math::TPose3D &p, const double(&vector)[3], TLine3D &r)
Gets a 3D line corresponding to any arbitrary vector, in the base given by the pose.
Slightly heavyweight type to speed-up calculations with polygons in 3D.
bool vectorsAreParallel2D(const T &v1, const U &v2)
Returns true if two 2D vectors are parallel.
void createPlaneFromPoseXY(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
Standard type for storing any lightweight 2D type.
Standard object for storing any 3D lightweight object.
void getAsPose2D(TPose2D &outPose) const
GLenum GLenum GLuint components
void createPlaneFromPoseAndNormal(const mrpt::math::TPose3D &pose, const double(&normal)[3], TPlane &plane)
Given a pose and any vector, creates a plane orthogonal to that vector in the pose's coordinates...
void composePoint(const TPoint3D l, TPoint3D &g) const
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
GLsizei GLsizei GLuint * obj
void getAngleBisector(const TLine2D &l1, const TLine2D &l2, TLine2D &bis)
Gets the bisector of two lines or segments (implicit constructor will be used if necessary) ...
A sparse matrix container (with cells of any type), with iterators.
void createFromPoseY(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Y axis in a given pose.
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
GLsizei const GLfloat * points
T square(const T x)
Inline function for the square of a number.
void generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], char coord, CMatrixDouble44 &matrix)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
TPolygonWithPlane()
Basic constructor.
#define ASSERT_(f)
Defines an assertion mechanism.
void skew_symmetric3_neg(const VECTOR &v, MATRIX &M)
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array: ...
bool pointIntoQuadrangle(T x, T y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y)
Specialized method to check whether a point (x,y) falls into a quadrangle.
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
3D segment, consisting of two points.
double distancePointToPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygo...
void getRectangleBounds(const std::vector< TPoint2D > &poly, TPoint2D &pMin, TPoint2D &pMax)
Gets the rectangular bounds of a 2D polygon or set of 2D points.
void createPlaneFromPoseXZ(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
TPolygon3D poly
Actual polygon.
void setEpsilon(double nE)
Changes the value of the geometric epsilon (default = 1e-5)
3D Plane, represented by its equation
double getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
TPoint2D point2
Destiny point.
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
static void getPlanes(const std::vector< TPolygon3D > &oldPolys, std::vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
TPoint2D point1
Origin point.
bool SegmentsIntersection(const double x1, const double y1, const double x2, const double y2, const double x3, const double y3, const double x4, const double y4, double &ix, double &iy)
Returns the intersection point, and if it exists, between two segments.
void clear()
Completely removes all elements, although maintaining the matrix's size.
CMatrixTemplateNumeric< T > generateAxisBaseFromDirection(T dx, T dy, T dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
void getAsPose2DForcingOrigin(const TPoint2D &origin, TPose2D &outPose) const
GLdouble GLdouble GLdouble r
double getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
mrpt::math::TPose3D pose
Plane's pose.
void getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
void resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
void project2D(const TPoint2D &point, const TPose2D &newXpose, TPoint2D &newPoint)
Uses the given pose 2D to project a point into a new base.
A matrix of dynamic size.
void createFromPoseZ(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
void createPlaneFromPoseYZ(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its X vector.
int sign(T x)
Returns the sign of X as "1" or "-1".
bool areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
GLfloat GLfloat GLfloat v2
T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the square distance between 2 points in 2D.
TPlane plane
Plane containing the polygon.
double minimumDistanceFromPointToSegment(const double Px, const double Py, const double x1, const double y1, const double x2, const double y2, T &out_x, T &out_y)
Computes the closest point from a given point to a segment, and returns that minimum distance...
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
const Scalar * const_iterator
2D polygon, inheriting from std::vector<TPoint2D>.
3D polygon, inheriting from std::vector<TPoint3D>
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
3D line, represented by a base point and a director vector.
2D line without bounds, represented by its equation .