Go to the documentation of this file.
41 bool intersect(
const TSegment3D& s1,
const TSegment3D& s2, TObject3D& obj);
51 bool intersect(
const TSegment3D& s1,
const TPlane& p2, TObject3D& obj);
61 bool intersect(
const TSegment3D& s1,
const TLine3D& r2, TObject3D& obj);
82 bool intersect(
const TPlane& p1,
const TPlane& p2, TObject3D& obj);
92 bool intersect(
const TPlane& p1,
const TLine3D& p2, TObject3D& obj);
129 bool intersect(
const TLine3D& r1,
const TLine3D& r2, TObject3D& obj);
140 bool intersect(
const TLine2D& r1,
const TLine2D& r2, TObject2D& obj);
150 bool intersect(
const TLine2D& r1,
const TSegment2D& s2, TObject2D& obj);
173 bool intersect(
const TSegment2D& s1,
const TSegment2D& s2, TObject2D& obj);
185 double getAngle(
const TPlane& p1,
const TPlane& p2);
190 double getAngle(
const TPlane& p1,
const TLine3D& r2);
203 double getAngle(
const TLine3D& r1,
const TLine3D& r2);
208 double getAngle(
const TLine2D& r1,
const TLine2D& r2);
258 const TPose2D& p,
const double (&vector)[2], TLine2D& r);
275 bool conformAPlane(
const std::vector<TPoint3D>& points, TPlane& p);
280 bool areAligned(
const std::vector<TPoint2D>& points);
286 bool areAligned(
const std::vector<TPoint2D>& points, TLine2D& r);
291 bool areAligned(
const std::vector<TPoint3D>& points);
296 bool areAligned(
const std::vector<TPoint3D>& points, TLine3D& r);
330 TPolygon3D& newPolygon);
334 TObject3D& newObject);
352 const T& obj,
const TPlane& newXYPlane,
const TPoint3D& newOrigin,
365 std::vector<T>& newObjs)
367 size_t N = objs.size();
369 for (
size_t i = 0; i < N; i++)
project3D(objs[i], newXYpose, newObjs[i]);
385 void project2D(
const TLine2D& line,
const TPose2D& newXpose, TLine2D& newLine);
388 const TPolygon2D& polygon,
const TPose2D& newXpose, TPolygon2D& newPolygon);
391 const TObject2D&
object,
const TPose2D& newXpose, TObject2D& newObject);
398 template <
class T,
class CPOSE2D>
403 project2D(obj, CPOSE2D(0, 0, 0) - pose, newObj);
411 template <
class T,
class CPOSE2D>
413 const T& obj,
const TLine2D& newXLine,
const TPoint2D& newOrigin, T& newObj)
417 project2D(obj, CPOSE2D(0, 0, 0) - pose, newObj);
423 const std::vector<T>& objs,
const TPose2D& newXpose,
424 std::vector<T>& newObjs)
426 size_t N = objs.size();
428 for (
size_t i = 0; i < N; i++)
project2D(objs[i], newXpose, newObjs[i]);
439 bool intersect(
const TPolygon2D& p1,
const TSegment2D& s2, TObject2D& obj);
441 bool intersect(
const TPolygon2D& p1,
const TLine2D& r2, TObject2D& obj);
443 bool intersect(
const TPolygon2D& p1,
const TPolygon2D& p2, TObject2D& obj);
458 bool intersect(
const TPolygon3D& p1,
const TSegment3D& s2, TObject3D& obj);
460 bool intersect(
const TPolygon3D& p1,
const TLine3D& r2, TObject3D& obj);
462 bool intersect(
const TPolygon3D& p1,
const TPlane& p2, TObject3D& obj);
464 bool intersect(
const TPolygon3D& p1,
const TPolygon3D& p2, TObject3D& obj);
488 const std::vector<TPolygon3D>& v1,
const std::vector<TPolygon3D>& v2,
489 CSparseMatrixTemplate<TObject3D>& objs);
495 const std::vector<TPolygon3D>& v1,
const std::vector<TPolygon3D>& v2,
496 std::vector<TObject3D>& objs);
506 template <
class T,
class U,
class O>
508 const std::vector<T>& v1,
const std::vector<U>& v2,
511 size_t M = v1.size(), N = v2.size();
515 for (
size_t i = 0; i < M; i++)
516 for (
size_t j = 0; j < M; j++)
517 if (
intersect(v1[i], v2[j], obj)) objs(i, j) = obj;
524 template <
class T,
class U,
class O>
526 const std::vector<T>& v1,
const std::vector<U>& v2, std::vector<O> objs)
530 for (
typename std::vector<T>::const_iterator it1 = v1.begin();
531 it1 != v1.end(); ++it1)
533 const T& elem1 = *it1;
534 for (
typename std::vector<U>::const_iterator it2 = v2.begin();
535 it2 != v2.end(); ++it2)
536 if (
intersect(elem1, *it2, obj)) objs.push_back(obj);
542 bool intersect(
const TObject2D& o1,
const TObject2D& o2, TObject2D& obj);
544 bool intersect(
const TObject3D& o1,
const TObject3D& o2, TObject3D& obj);
556 double distance(
const TLine2D& r1,
const TLine2D& r2);
558 double distance(
const TLine3D& r1,
const TLine3D& r2);
561 double distance(
const TPlane& p1,
const TPlane& p2);
563 double distance(
const TPolygon2D& p1,
const TPolygon2D& p2);
565 double distance(
const TPolygon2D& p1,
const TSegment2D& s2);
572 double distance(
const TPolygon2D& p1,
const TLine2D& l2);
578 double distance(
const TPolygon3D& p1,
const TPolygon3D& p2);
580 double distance(
const TPolygon3D& p1,
const TSegment3D& s2);
587 double distance(
const TPolygon3D& p1,
const TLine3D& l2);
594 double distance(
const TPolygon3D& po,
const TPlane& pl);
682 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys);
688 const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys,
689 std::vector<TSegment3D>& remainder);
695 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys);
701 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
702 std::vector<TObject3D>& remainder);
708 const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
709 std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2);
715 const TPolygon2D& poly, std::vector<TPolygon2D>& components);
721 const TPolygon3D& poly, std::vector<TPolygon3D>& components);
758 std::vector<TPolygonWithPlane> pwp;
775 template <
class T,
class U,
class V>
778 vOut[0] = v0[1] * v1[2] - v0[2] * v1[1];
779 vOut[1] = v0[2] * v1[0] - v0[0] * v1[2];
780 vOut[2] = v0[0] * v1[1] - v0[1] * v1[0];
786 const std::vector<T>& v0,
const std::vector<T>& v1, std::vector<T>& v_out)
791 crossProduct3D<std::vector<T>, std::vector<T>, std::vector<T>>(
795 template <
class VEC1,
class VEC2>
799 crossProduct3D<VEC1, VEC2, VEC1>(v0, v1, vOut);
812 template <
class VECTOR,
class MATRIX>
828 template <
class VECTOR>
846 template <
class VECTOR,
class MATRIX>
850 ASSERT_(M.rows() == 3 && M.cols() == 3);
862 template <
class VECTOR>
874 template <
class T,
class U>
877 return std::abs(v1[0] * v2[1] - v2[0] * v1[1]) <
getEpsilon();
884 template <
class T,
class U>
887 if (std::abs(v1[0] * v2[1] - v2[0] * v1[1]) >=
getEpsilon())
return false;
888 if (std::abs(v1[1] * v2[2] - v2[1] * v1[2]) >=
getEpsilon())
return false;
889 return std::abs(v1[2] * v2[0] - v2[2] * v1[0]) <
getEpsilon();
896 double Px,
double Py,
double x1,
double y1,
double x2,
double y2,
897 double& out_x,
double& out_y);
903 double Px,
double Py,
double x1,
double y1,
double x2,
double y2,
904 double& out_x,
double& out_y);
909 double Px,
double Py,
double x1,
double y1,
double x2,
double y2);
912 template <
typename T>
919 template <
typename T>
921 const T x1,
const T y1,
const T z1,
const T x2,
const T y2,
const T z2)
927 template <
typename T>
934 template <
typename T>
936 const T x1,
const T y1,
const T z1,
const T x2,
const T y2,
const T z2)
944 template <
typename T>
946 const double Px,
const double Py,
const double x1,
const double y1,
947 const double x2,
const double y2, T& out_x, T& out_y)
951 out_x =
static_cast<T
>(ox);
952 out_y =
static_cast<T
>(oy);
959 const double x1,
const double y1,
const double x2,
const double y2,
960 const double x3,
const double y3,
const double x4,
const double y4,
961 double& ix,
double& iy);
966 const double x1,
const double y1,
const double x2,
const double y2,
967 const double x3,
const double y3,
const double x4,
const double y4,
968 float& ix,
float& iy);
974 double px,
double py,
unsigned int polyEdges,
const double* poly_xs,
975 const double* poly_ys);
980 template <
typename T>
982 T x, T y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y)
986 const T
a1 = atan2(v1y - y, v1x - x);
987 const T
a2 = atan2(v2y - y, v2x - x);
988 const T
a3 = atan2(v3y - y, v3x - x);
989 const T a4 = atan2(v4y - y, v4x - x);
995 if (
sign(da1) !=
sign(da2))
return false;
998 if (
sign(da2) !=
sign(da3))
return false;
1008 double px,
double py,
unsigned int polyEdges,
const double* poly_xs,
1009 const double* poly_ys);
1021 double p1_x,
double p1_y,
double p1_z,
double p2_x,
double p2_y,
1022 double p2_z,
double p3_x,
double p3_y,
double p3_z,
double p4_x,
1023 double p4_y,
double p4_z,
double& x,
double& y,
double& z,
double& dist);
1035 double R1_x_min,
double R1_x_max,
double R1_y_min,
double R1_y_max,
1036 double R2_x_min,
double R2_x_max,
double R2_y_min,
double R2_y_max,
1037 double R2_pose_x,
double R2_pose_y,
double R2_pose_phi);
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.
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X 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.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Base template for TPoint3D and TPoint3Df.
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.
void project2D(const TPoint2D &point, const TPose2D &newXpose, TPoint2D &newPoint)
Uses the given pose 2D to project a point into a new base.
void createFromPoseZ(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
void getAsPose2DForcingOrigin(const TPoint2D &origin, TPose2D &outPose) const
double distancePointToPolygon2D(double px, 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...
double getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
bool RectanglesIntersection(double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max, double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max, double R2_pose_x, double R2_pose_y, double R2_pose_phi)
Returns whether two rotated rectangles intersect.
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.
Standard type for storing any lightweight 2D type.
Standard object for storing any 3D lightweight object.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
CMatrixDouble44 generateAxisBaseFromDirectionAndAxis(const mrpt::math::TVector3D &vec, uint8_t coord)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y,...
#define ASSERT_(f)
Defines an assertion mechanism.
void getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
CMatrixFixed< double, 4, 4 > CMatrixDouble44
CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
TPoint3D point1
origin point
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.
TPoint3D point2
final point
double closestSquareDistanceFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2)
Returns the square distance from a point to a line.
void createPlaneFromPoseXY(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the square distance between 2 points in 2D.
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
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.
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.
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
void unsafeProjectPolygon(const TPolygon3D &poly, const TPose3D &pose, TPolygon2D &newPoly)
bool minDistBetweenLines(double p1_x, double p1_y, double p1_z, double p2_x, double p2_y, double p2_z, double p3_x, double p3_y, double p3_z, double p4_x, double p4_y, double p4_z, double &x, double &y, double &z, double &dist)
Calculates the minimum distance between a pair of lines.
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)
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
void closestFromPointToSegment(double Px, double Py, double x1, double y1, double x2, double y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment.
A sparse matrix container (with cells of any type), with iterators.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
void resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
return_t square(const num_t x)
Inline function for the square of a number.
2D polygon, inheriting from std::vector<TPoint2D>.
3D polygon, inheriting from std::vector<TPoint3D>
2D segment, consisting of two points.
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
3D segment, consisting of two points.
void getAsPose3D(mrpt::math::TPose3D &outPose) const
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
3D Plane, represented by its equation
void clear()
Completely removes all elements, although maintaining the matrix's size.
void closestFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
This base provides a set of functions for maths stuff.
2D line without bounds, represented by its equation .
3D line, represented by a base point and a director vector.
void composePoint(const TPoint3D &l, TPoint3D &g) const
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.
static void getPlanes(const std::vector< TPolygon3D > &oldPolys, std::vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
TPoint2D point1
Origin point.
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.
void getAsPose2D(TPose2D &outPose) const
TPoint2D point2
Destiny point.
bool pointIntoPolygon2D(double px, 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.
bool vectorsAreParallel2D(const T &v1, const U &v2)
Returns true if two 2D vectors are parallel.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
double getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
void createFromPoseY(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Y axis in a given pose.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020 | |