Lightweight SE(2)/SE(3) types, geometry functions.

Lightweight SE(2)/SE(3) data types, geometry functions, etc.

The “lightweight” adjective is used here in contrast to classes derived from mrpt::poses::CPoseOrPoint. The “lightweight” alternative types here, defined in mrpt::math, are simple C++ structures without special memory alignment requirements and without a deep hiearchy of class inheritance, as the “heavier” classes in mrpt::poses have. In turn, the latter ones offer:

See list of classes below.

// typedefs

typedef TBoundingBox_<double> mrpt::math::TBoundingBox;
typedef TBoundingBox_<float> mrpt::math::TBoundingBoxf;
typedef TPlane mrpt::math::TPlane3D;
typedef TPoint2D_<double> mrpt::math::TPoint2D;
typedef TPoint2D_<float> mrpt::math::TPoint2Df;
typedef TPoint2D mrpt::math::TVector2D;
typedef TPoint2Df mrpt::math::TVector2Df;
typedef TPoint3D_<double> mrpt::math::TPoint3D;
typedef TPoint3D_<float> mrpt::math::TPoint3Df;
typedef TPoint3D mrpt::math::TVector3D;
typedef TPoint3Df mrpt::math::TVector3Df;

// structs

template <typename T>
struct mrpt::math::TBoundingBox_;

struct mrpt::math::TLine2D;
struct mrpt::math::TLine3D;
struct mrpt::math::TObject2D;
struct mrpt::math::TObject3D;
struct mrpt::math::TPlane;

template <typename T>
struct mrpt::math::TPoint2D_;

template <typename T>
struct mrpt::math::TPoint2D_data;

template <typename T>
struct mrpt::math::TPoint3D_;

template <typename T>
struct mrpt::math::TPoint3D_data;

struct mrpt::math::TPointXYZIu8;
struct mrpt::math::TPointXYZRGBAf;
struct mrpt::math::TPointXYZRGBu8;
struct mrpt::math::TPointXYZfIu8;
struct mrpt::math::TPointXYZfRGBAu8;
struct mrpt::math::TPointXYZfRGBu8;
struct mrpt::math::TPose2D;
struct mrpt::math::TPose3D;
struct mrpt::math::TPose3DQuat;
struct mrpt::math::TPoseOrPoint;
struct mrpt::math::TSegment2D;
struct mrpt::math::TSegment3D;
struct mrpt::math::TTwist2D;
struct mrpt::math::TTwist3D;

// classes

class mrpt::math::CPolygon;
class mrpt::math::TPolygon2D;
class mrpt::math::TPolygon3D;
class mrpt::math::TPolygonWithPlane;

// global variables

static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_POINT = 0;
static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_SEGMENT = 1;
static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_LINE = 2;
static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_POLYGON = 3;
static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_PLANE = 4;
static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_UNDEFINED = 255;

// global functions

bool mrpt::math::intersect(const TSegment3D& s1, const TSegment3D& s2, TObject3D& obj);
bool mrpt::math::intersect(const TSegment3D& s1, const TPlane& p2, TObject3D& obj);
bool mrpt::math::intersect(const TSegment3D& s1, const TLine3D& r2, TObject3D& obj);
bool mrpt::math::intersect(const TPlane& p1, const TSegment3D& s2, TObject3D& obj);
bool mrpt::math::intersect(const TPlane& p1, const TPlane& p2, TObject3D& obj);
bool mrpt::math::intersect(const TPlane& p1, const TLine3D& p2, TObject3D& obj);
bool mrpt::math::intersect(const TLine3D& r1, const TSegment3D& s2, TObject3D& obj);
bool mrpt::math::intersect(const TLine3D& r1, const TPlane& p2, TObject3D& obj);
bool mrpt::math::intersect(const TLine3D& r1, const TLine3D& r2, TObject3D& obj);
bool mrpt::math::intersect(const TLine2D& r1, const TLine2D& r2, TObject2D& obj);
bool mrpt::math::intersect(const TLine2D& r1, const TSegment2D& s2, TObject2D& obj);
bool mrpt::math::intersect(const TSegment2D& s1, const TLine2D& r2, TObject2D& obj);
bool mrpt::math::intersect(const TSegment2D& s1, const TSegment2D& s2, TObject2D& obj);
double mrpt::math::getAngle(const TPlane& p1, const TPlane& p2);
double mrpt::math::getAngle(const TPlane& p1, const TLine3D& r2);
double mrpt::math::getAngle(const TLine3D& r1, const TPlane& p2);
double mrpt::math::getAngle(const TLine3D& r1, const TLine3D& r2);
double mrpt::math::getAngle(const TLine2D& r1, const TLine2D& r2);
void mrpt::math::createFromPoseX(const mrpt::math::TPose3D& p, TLine3D& r);
void mrpt::math::createFromPoseY(const mrpt::math::TPose3D& p, TLine3D& r);
void mrpt::math::createFromPoseZ(const mrpt::math::TPose3D& p, TLine3D& r);
void mrpt::math::createFromPoseAndVector(const mrpt::math::TPose3D& p, const double(&) vector [3], TLine3D& r);
void mrpt::math::createFromPoseX(const TPose2D& p, TLine2D& r);
void mrpt::math::createFromPoseY(const TPose2D& p, TLine2D& r);
void mrpt::math::createFromPoseAndVector(const TPose2D& p, const double(&) vector [2], TLine2D& r);
bool mrpt::math::conformAPlane(const std::vector<TPoint3D>& points);
bool mrpt::math::conformAPlane(const std::vector<TPoint3D>& points, TPlane& p);
bool mrpt::math::areAligned(const std::vector<TPoint2D>& points);
bool mrpt::math::areAligned(const std::vector<TPoint2D>& points, TLine2D& r);
bool mrpt::math::areAligned(const std::vector<TPoint3D>& points);
bool mrpt::math::areAligned(const std::vector<TPoint3D>& points, TLine3D& r);
void mrpt::math::project3D(const TPoint3D& point, const mrpt::math::TPose3D& newXYpose, TPoint3D& newPoint);
void mrpt::math::project3D(const TSegment3D& segment, const mrpt::math::TPose3D& newXYpose, TSegment3D& newSegment);
void mrpt::math::project3D(const TLine3D& line, const mrpt::math::TPose3D& newXYpose, TLine3D& newLine);
void mrpt::math::project3D(const TPlane& plane, const mrpt::math::TPose3D& newXYpose, TPlane& newPlane);
void mrpt::math::project3D(const TPolygon3D& polygon, const mrpt::math::TPose3D& newXYpose, TPolygon3D& newPolygon);
void mrpt::math::project3D(const TObject3D& object, const mrpt::math::TPose3D& newXYPose, TObject3D& newObject);

template <typename Object>
Object mrpt::math::project3D(const Object& o, const mrpt::math::TPose3D& newPose);

template <class T>
void mrpt::math::project3D(const T& obj, const TPlane& newXYPlane, T& newObj);

template <class T>
void mrpt::math::project3D(
    const T& obj,
    const TPlane& newXYPlane,
    const TPoint3D& newOrigin,
    T& newObj
    );

template <class T>
void mrpt::math::project3D(
    const std::vector<T>& objs,
    const mrpt::math::TPose3D& newXYpose,
    std::vector<T>& newObjs
    );

void mrpt::math::project2D(const TPoint2D& point, const TPose2D& newXpose, TPoint2D& newPoint);
void mrpt::math::project2D(const TSegment2D& segment, const TPose2D& newXpose, TSegment2D& newSegment);
void mrpt::math::project2D(const TLine2D& line, const TPose2D& newXpose, TLine2D& newLine);
void mrpt::math::project2D(const TPolygon2D& polygon, const TPose2D& newXpose, TPolygon2D& newPolygon);
void mrpt::math::project2D(const TObject2D& object, const TPose2D& newXpose, TObject2D& newObject);

template <typename Object>
Object mrpt::math::project2D(const Object& o, const mrpt::math::TPose2D& newPose);

template <class T, class CPOSE2D>
void mrpt::math::project2D(
    const T& obj,
    const TLine2D& newXLine,
    T& newObj
    );

template <class T, class CPOSE2D>
void mrpt::math::project2D(
    const T& obj,
    const TLine2D& newXLine,
    const TPoint2D& newOrigin,
    T& newObj
    );

template <class T>
void mrpt::math::project2D(
    const std::vector<T>& objs,
    const TPose2D& newXpose,
    std::vector<T>& newObjs
    );

bool mrpt::math::intersect(const TPolygon2D& p1, const TSegment2D& s2, TObject2D& obj);
bool mrpt::math::intersect(const TPolygon2D& p1, const TLine2D& r2, TObject2D& obj);
TPolygon2D mrpt::math::intersect(const TPolygon2D& subject, const TPolygon2D& clipping);

bool mrpt::math::intersect(
    const TPolygon2D& subject,
    const TPolygon2D& clipping,
    TObject2D& result
    );

bool mrpt::math::intersect(const TSegment2D& s1, const TPolygon2D& p2, TObject2D& obj);
bool mrpt::math::intersect(const TLine2D& r1, const TPolygon2D& p2, TObject2D& obj);
bool mrpt::math::intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj);
bool mrpt::math::intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj);
bool mrpt::math::intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj);
bool mrpt::math::intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj);
bool mrpt::math::intersect(const TSegment3D& s1, const TPolygon3D& p2, TObject3D& obj);
bool mrpt::math::intersect(const TLine3D& r1, const TPolygon3D& p2, TObject3D& obj);
bool mrpt::math::intersect(const TPlane& p1, const TPolygon3D& p2, TObject3D& obj);
size_t mrpt::math::intersect(const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2, CSparseMatrixTemplate<TObject3D>& objs);
size_t mrpt::math::intersect(const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2, std::vector<TObject3D>& objs);

template <class T, class U, class O>
size_t mrpt::math::intersect(
    const std::vector<T>& v1,
    const std::vector<U>& v2,
    CSparseMatrixTemplate<O>& objs
    );

template <class T, class U, class O>
size_t mrpt::math::intersect(
    const std::vector<T>& v1,
    const std::vector<U>& v2,
    std::vector<O> objs
    );

bool mrpt::math::intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj);
bool mrpt::math::intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj);
double mrpt::math::distance(const TPoint2D& p1, const TPoint2D& p2);
double mrpt::math::distance(const TPoint3D& p1, const TPoint3D& p2);
double mrpt::math::distance(const TLine2D& r1, const TLine2D& r2);
double mrpt::math::distance(const TLine3D& r1, const TLine3D& r2);
double mrpt::math::distance(const TPlane& p1, const TPlane& p2);
double mrpt::math::distance(const TPolygon2D& p1, const TPolygon2D& p2);
double mrpt::math::distance(const TPolygon2D& p1, const TSegment2D& s2);
double mrpt::math::distance(const TSegment2D& s1, const TPolygon2D& p2);
double mrpt::math::distance(const TPolygon2D& p1, const TLine2D& l2);

double mrpt::math::distance(
    const TLine2D& l1,
    const TPolygon2D& p2
    );

double mrpt::math::distance(const TPolygon3D& p1, const TPolygon3D& p2);
double mrpt::math::distance(const TPolygon3D& p1, const TSegment3D& s2);
double mrpt::math::distance(const TSegment3D& s1, const TPolygon3D& p2);
double mrpt::math::distance(const TPolygon3D& p1, const TLine3D& l2);
double mrpt::math::distance(const TLine3D& l1, const TPolygon3D& p2);
double mrpt::math::distance(const TPolygon3D& po, const TPlane& pl);
double mrpt::math::distance(const TPlane& pl, const TPolygon3D& po);
void mrpt::math::getRectangleBounds(const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax);
void mrpt::math::getPrismBounds(const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax);
void mrpt::math::createPlaneFromPoseXY(const mrpt::math::TPose3D& pose, TPlane& plane);
void mrpt::math::createPlaneFromPoseXZ(const mrpt::math::TPose3D& pose, TPlane& plane);
void mrpt::math::createPlaneFromPoseYZ(const mrpt::math::TPose3D& pose, TPlane& plane);
void mrpt::math::createPlaneFromPoseAndNormal(const mrpt::math::TPose3D& pose, const double(&) normal [3], TPlane& plane);
CMatrixDouble44 mrpt::math::generateAxisBaseFromDirectionAndAxis(const mrpt::math::TVector3D& vec, uint8_t coord);
double mrpt::math::getRegressionLine(const std::vector<TPoint2D>& points, TLine2D& line);
double mrpt::math::getRegressionLine(const std::vector<TPoint3D>& points, TLine3D& line);
double mrpt::math::getRegressionPlane(const std::vector<TPoint3D>& points, TPlane& plane);
void mrpt::math::assemblePolygons(const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys);

void mrpt::math::assemblePolygons(
    const std::vector<TSegment3D>& segms,
    std::vector<TPolygon3D>& polys,
    std::vector<TSegment3D>& remainder
    );

void mrpt::math::assemblePolygons(const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys);
void mrpt::math::assemblePolygons(const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys, std::vector<TObject3D>& remainder);

void mrpt::math::assemblePolygons(
    const std::vector<TObject3D>& objs,
    std::vector<TPolygon3D>& polys,
    std::vector<TSegment3D>& remainder1,
    std::vector<TObject3D>& remainder2
    );

bool mrpt::math::splitInConvexComponents(const TPolygon2D& poly, std::vector<TPolygon2D>& components);
bool mrpt::math::splitInConvexComponents(const TPolygon3D& poly, std::vector<TPolygon3D>& components);
void mrpt::math::getSegmentBisector(const TSegment2D& sgm, TLine2D& bis);
void mrpt::math::getSegmentBisector(const TSegment3D& sgm, TPlane& bis);
void mrpt::math::getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis);
void mrpt::math::getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis);
bool mrpt::math::traceRay(const std::vector<TPolygonWithPlane>& vec, const mrpt::math::TPose3D& pose, double& dist);
bool mrpt::math::traceRay(const std::vector<TPolygon3D>& vec, const mrpt::math::TPose3D& pose, double& dist);

template <class T, class U, class V>
void mrpt::math::crossProduct3D(const T& v0, const U& v1, V& vOut);

template <class T>
void mrpt::math::crossProduct3D(
    const std::vector<T>& v0,
    const std::vector<T>& v1,
    std::vector<T>& v_out
    );

template <class VEC1, class VEC2>
VEC1 mrpt::math::crossProduct3D(const VEC1& v0, const VEC2& v1);

template <class VECTOR, class MATRIX>
void mrpt::math::skew_symmetric3(const VECTOR& v, MATRIX& M);

template <class VECTOR>
mrpt::math::CMatrixDouble33 mrpt::math::skew_symmetric3(const VECTOR& v);

template <class VECTOR, class MATRIX>
void mrpt::math::skew_symmetric3_neg(const VECTOR& v, MATRIX& M);

template <class VECTOR>
mrpt::math::CMatrixDouble33 mrpt::math::skew_symmetric3_neg(const VECTOR& v);

template <class T, class U>
bool mrpt::math::vectorsAreParallel2D(const T& v1, const U& v2);

template <class T, class U>
bool mrpt::math::vectorsAreParallel3D(const T& v1, const U& v2);

void mrpt::math::closestFromPointToSegment(
    double Px,
    double Py,
    double x1,
    double y1,
    double x2,
    double y2,
    double& out_x,
    double& out_y
    );

mrpt::math::TPoint2D mrpt::math::closestFromPointToSegment(
    const mrpt::math::TPoint2D& query,
    const mrpt::math::TPoint2D& segPt1,
    const mrpt::math::TPoint2D& segPt2
    );

void mrpt::math::closestFromPointToLine(
    double Px,
    double Py,
    double x1,
    double y1,
    double x2,
    double y2,
    double& out_x,
    double& out_y
    );

mrpt::math::TPoint2D mrpt::math::closestFromPointToLine(
    const mrpt::math::TPoint2D& query,
    const mrpt::math::TPoint2D& linePt1,
    const mrpt::math::TPoint2D& linePt2
    );

double mrpt::math::squaredDistancePointToLine(double Px, double Py, double x1, double y1, double x2, double y2);

double mrpt::math::squaredDistancePointToLine(
    const mrpt::math::TPoint2D& query,
    const mrpt::math::TPoint2D& linePt1,
    const mrpt::math::TPoint2D& linePt2
    );

template <typename T>
T mrpt::math::distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2);

template <typename T>
T mrpt::math::distanceBetweenPoints(
    const T x1,
    const T y1,
    const T z1,
    const T x2,
    const T y2,
    const T z2
    );

template <typename T>
T mrpt::math::distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2);

template <typename T>
T mrpt::math::distanceSqrBetweenPoints(
    const T x1,
    const T y1,
    const T z1,
    const T x2,
    const T y2,
    const T z2
    );

template <typename T>
double mrpt::math::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
    );

template <typename T>
bool mrpt::math::pointIntoQuadrangle(
    T x,
    T y,
    T v1x,
    T v1y,
    T v2x,
    T v2y,
    T v3x,
    T v3y,
    T v4x,
    T v4y
    );

bool mrpt::math::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
    );

CMatrixDouble33 mrpt::math::generateAxisBaseFromDirection(double dx, double dy, double dz);
double mrpt::math::signedArea(const mrpt::math::TPolygon2D& p);

template <typename T>
void mrpt::math::slerp(
    const CQuaternion<T>& q0,
    const CQuaternion<T>& q1,
    const double t,
    CQuaternion<T>& q
    );

void mrpt::math::slerp(const TPose3D& q0, const TPose3D& q1, const double t, TPose3D& p);

void mrpt::math::slerp_ypr(
    const mrpt::math::TPose3D& q0,
    const mrpt::math::TPose3D& q1,
    const double t,
    mrpt::math::TPose3D& p
    );

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TBoundingBoxf& bb
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TBoundingBoxf& bb
    );

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TBoundingBox& bb
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TBoundingBox& bb
    );

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TLine2D& l
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TLine2D& l
    );

std::ostream& mrpt::math::operator << (std::ostream& o, const TLine2D& p);

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TLine3D& l
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TLine3D& l
    );

std::ostream& mrpt::math::operator << (std::ostream& o, const TLine3D& p);

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TObject2D& o
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TObject2D& o
    );

std::ostream& mrpt::math::operator << (std::ostream& o, const mrpt::math::TObject2D& obj);

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TObject3D& o
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TObject3D& o
    );

std::ostream& mrpt::math::operator << (std::ostream& o, const mrpt::math::TObject3D& obj);

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TPlane& p
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TPlane& p
    );

std::ostream& mrpt::math::operator << (std::ostream& o, const TPlane& p);

template <typename T>
constexpr TPoint2D_<T> mrpt::math::operator - (const TPoint2D_<T>& p1);

template <
    typename T,
    typename Scalar,
    std::enable_if_t<std::is_convertible_v<Scalar, T>>* = nullptr
    >
constexpr TPoint2D_<T> mrpt::math::operator * (
    const Scalar scalar,
    const TPoint2D_<T>& p
    );

template <typename T>
constexpr bool mrpt::math::operator == (const TPoint2D_<T>& p1, const TPoint2D_<T>& p2);

template <typename T>
constexpr bool mrpt::math::operator != (const TPoint2D_<T>& p1, const TPoint2D_<T>& p2);

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TPointXYZfRGBu8& p
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TPointXYZfRGBu8& p
    );

mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    mrpt::math::TPointXYZfRGBAu8& p
    );

mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const mrpt::math::TPointXYZfRGBAu8& p
    );

template <typename T>
constexpr TPoint3D_<T> mrpt::math::operator - (const TPoint3D_<T>& p1);

template <
    typename T,
    typename Scalar,
    std::enable_if_t<std::is_convertible_v<Scalar, T>>* = nullptr
    >
constexpr TPoint3D_<T> mrpt::math::operator * (
    const Scalar scalar,
    const TPoint3D_<T>& p
    );

template <typename T>
constexpr bool mrpt::math::operator == (const TPoint3D_<T>& p1, const TPoint3D_<T>& p2);

template <typename T>
constexpr bool mrpt::math::operator != (const TPoint3D_<T>& p1, const TPoint3D_<T>& p2);

template <
    class PoseOrPoint,
    typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>>
    >
std::ostream& mrpt::math::operator << (
    std::ostream& o,
    const PoseOrPoint& p
    );

template <
    class PoseOrPoint,
    typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>>
    >
mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    PoseOrPoint& o
    );

template <
    class PoseOrPoint,
    typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>>
    >
mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const PoseOrPoint& o
    );

Typedefs

typedef TBoundingBox_<double> mrpt::math::TBoundingBox

A bounding box defined by the 3D points at each minimum-maximum corner.

See also:

mrpt::math::TPoint3D, mrpt::math::TPoint3Df

typedef TPoint2D_<double> mrpt::math::TPoint2D

Lightweight 2D point.

Allows coordinate access using [] operator.

See also:

mrpt::poses::CPoint2D

typedef TPoint2D mrpt::math::TVector2D

Useful type alias for double 2-vectors.

typedef TPoint2Df mrpt::math::TVector2Df

Useful type alias for float 2-vectors.

typedef TPoint3D_<double> mrpt::math::TPoint3D

Lightweight 3D point.

Allows coordinate access using [] operator. (1-byte memory packed, no padding).

See also:

mrpt::poses::CPoint3D, mrpt::math::TPoint3Df

typedef TPoint3D mrpt::math::TVector3D

Useful type alias for 3-vectors.

(1-byte memory packed, no padding)

Global Variables

static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_POINT = 0

Object type identifier for TPoint2D or TPoint3D.

See also:

TObject2D, TObject3D

static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_SEGMENT = 1

Object type identifier for TSegment2D or TSegment3D.

See also:

TObject2D, TObject3D

static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_LINE = 2

Object type identifier for TLine2D or TLine3D.

See also:

TObject2D, TObject3D

static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_POLYGON = 3

Object type identifier for TPolygon2D or TPolygon3D.

See also:

TObject2D, TObject3D

static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_PLANE = 4

Object type identifier for TPlane.

See also:

TObject3D

static constexpr unsigned char mrpt::math::GEOMETRIC_TYPE_UNDEFINED = 255

Object type identifier for empty TObject2D or TObject3D.

See also:

TObject2D, TObject3D

Global Functions

bool mrpt::math::intersect(const TSegment3D& s1, const TSegment3D& s2, TObject3D& obj)

Gets the intersection between two 3D segments.

Possible outcomes:

  • Segments intersect: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

  • Segments don’t intersect & are parallel: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT, obj is the segment “in between” both segments.

  • Segments don’t intersect & aren’t parallel: Return=false.

See also:

TObject3D

bool mrpt::math::intersect(const TSegment3D& s1, const TPlane& p2, TObject3D& obj)

Gets the intersection between a 3D segment and a plane.

Possible outcomes:

  • Don’t intersect: Return=false

  • s1 is within the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT

  • s1 intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject3D

bool mrpt::math::intersect(const TSegment3D& s1, const TLine3D& r2, TObject3D& obj)

Gets the intersection between a 3D segment and a 3D line.

Possible outcomes:

  • They don’t intersect : Return=false

  • s1 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT

  • s1 intersects the line at a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject3D

bool mrpt::math::intersect(const TPlane& p1, const TSegment3D& s2, TObject3D& obj)

Gets the intersection between a plane and a 3D segment.

Possible outcomes:

  • Don’t intersect: Return=false

  • s2 is within the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT

  • s2 intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject3D

bool mrpt::math::intersect(const TPlane& p1, const TPlane& p2, TObject3D& obj)

Gets the intersection between two planes.

Possible outcomes:

  • Planes are parallel: Return=false

  • Planes intersect into a line: Return=true, obj.getType()=GEOMETRIC_TYPE_LINE

See also:

TObject3D

bool mrpt::math::intersect(const TPlane& p1, const TLine3D& p2, TObject3D& obj)

Gets the intersection between a plane and a 3D line.

Possible outcomes:

  • Line is parallel to plane but not within it: Return=false

  • Line is contained in the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_LINE

  • Line intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject3D

bool mrpt::math::intersect(const TLine3D& r1, const TSegment3D& s2, TObject3D& obj)

Gets the intersection between a 3D line and a 3D segment.

Possible outcomes:

  • They don’t intersect : Return=false

  • s2 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT

  • s2 intersects the line at a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject3D

bool mrpt::math::intersect(const TLine3D& r1, const TPlane& p2, TObject3D& obj)

Gets the intersection between a 3D line and a plane.

Possible outcomes:

  • Line is parallel to plane but not within it: Return=false

  • Line is contained in the plane: Return=true, obj.getType()=GEOMETRIC_TYPE_LINE

  • Line intersects the plane at one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject3D

bool mrpt::math::intersect(const TLine3D& r1, const TLine3D& r2, TObject3D& obj)

Gets the intersection between two 3D lines.

Possible outcomes:

  • Lines do not intersect: Return=false

  • Lines are parallel and do not coincide: Return=false

  • Lines coincide (are the same): Return=true, obj.getType()=GEOMETRIC_TYPE_LINE

  • Lines intesect in a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject3D

bool mrpt::math::intersect(const TLine2D& r1, const TLine2D& r2, TObject2D& obj)

Gets the intersection between two 2D lines.

Possible outcomes:

  • Lines do not intersect: Return=false

  • Lines are parallel and do not coincide: Return=false

  • Lines coincide (are the same): Return=true, obj.getType()=GEOMETRIC_TYPE_LINE

  • Lines intesect in a point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject2D

bool mrpt::math::intersect(const TLine2D& r1, const TSegment2D& s2, TObject2D& obj)

Gets the intersection between a 2D line and a 2D segment.

Possible outcomes:

  • They don’t intersect: Return=false

  • s2 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT

  • Both intersects in one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject2D

bool mrpt::math::intersect(const TSegment2D& s1, const TLine2D& r2, TObject2D& obj)

Gets the intersection between a 2D line and a 2D segment.

Possible outcomes:

  • They don’t intersect: Return=false

  • s1 lies within the line: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT

  • Both intersects in one point: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

See also:

TObject2D

bool mrpt::math::intersect(const TSegment2D& s1, const TSegment2D& s2, TObject2D& obj)

Gets the intersection between two 2D segments.

Possible outcomes:

  • Segments intersect: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT

  • Segments don’t intersect & are parallel: Return=true, obj.getType()=GEOMETRIC_TYPE_SEGMENT, obj is the segment “in between” both segments.

  • Segments don’t intersect & aren’t parallel: Return=false.

See also:

TObject2D

double mrpt::math::getAngle(const TPlane& p1, const TPlane& p2)

Computes the angle between two planes.

double mrpt::math::getAngle(const TPlane& p1, const TLine3D& r2)

Computes the angle between a plane and a 3D line or segment (implicit constructor will be used if passing a segment instead of a line).

double mrpt::math::getAngle(const TLine3D& r1, const TPlane& p2)

Computes the angle between a 3D line or segment and a plane (implicit constructor will be used if passing a segment instead of a line).

double mrpt::math::getAngle(const TLine3D& r1, const TLine3D& r2)

Computes the accute relative angle (range: [-PI/2,PI/2]) between two lines.

Implicit constructor allows passing a segment as argument too.

double mrpt::math::getAngle(const TLine2D& r1, const TLine2D& r2)

Computes the relative angle (range: [-PI,PI]) of line 2 wrt line 1.

Implicit constructor allows passing a segment as argument too.

void mrpt::math::createFromPoseX(const mrpt::math::TPose3D& p, TLine3D& r)

Gets a 3D line corresponding to the X axis in a given pose.

An implicit constructor is used if a TPose3D is given.

See also:

createFromPoseY, createFromPoseZ, createFromPoseAndVector

void mrpt::math::createFromPoseY(const mrpt::math::TPose3D& p, TLine3D& r)

Gets a 3D line corresponding to the Y axis in a given pose.

An implicit constructor is used if a TPose3D is given.

See also:

createFromPoseX, createFromPoseZ, createFromPoseAndVector

void mrpt::math::createFromPoseZ(const mrpt::math::TPose3D& p, TLine3D& r)

Gets a 3D line corresponding to the Z axis in a given pose.

An implicit constructor is used if a TPose3D is given.

See also:

createFromPoseX, createFromPoseY, createFromPoseAndVector

void mrpt::math::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.

An implicit constructor is used if a TPose3D is given.

See also:

createFromPoseX, createFromPoseY, createFromPoseZ

void mrpt::math::createFromPoseX(const TPose2D& p, TLine2D& r)

Gets a 2D line corresponding to the X axis in a given pose.

An implicit constructor is used if a CPose2D is given.

See also:

createFromPoseY, createFromPoseAndVector

void mrpt::math::createFromPoseY(const TPose2D& p, TLine2D& r)

Gets a 2D line corresponding to the Y axis in a given pose.

An implicit constructor is used if a CPose2D is given.

See also:

createFromPoseX, createFromPoseAndVector

void mrpt::math::createFromPoseAndVector(
    const TPose2D& p,
    const double(&) vector [2],
    TLine2D& r
    )

Gets a 2D line corresponding to any arbitrary vector, in the base given the given pose.

An implicit constructor is used if a CPose2D is given.

See also:

createFromPoseY, createFromPoseAndVector

bool mrpt::math::conformAPlane(const std::vector<TPoint3D>& points)

Checks whether this polygon or set of points acceptably fits a plane.

See also:

TPolygon3D, getEpsilon

bool mrpt::math::conformAPlane(const std::vector<TPoint3D>& points, TPlane& p)

Checks whether this polygon or set of points acceptably fits a plane, and if it’s the case returns it in the second argument.

See also:

TPolygon3D, getEpsilon

bool mrpt::math::areAligned(const std::vector<TPoint2D>& points)

Checks whether this set of points acceptably fits a 2D line.

See also:

getEpsilon

bool mrpt::math::areAligned(const std::vector<TPoint2D>& points, TLine2D& r)

Checks whether this set of points acceptably fits a 2D line, and if it’s the case returns it in the second argument.

See also:

getEpsilon

bool mrpt::math::areAligned(const std::vector<TPoint3D>& points)

Checks whether this set of points acceptably fits a 3D line.

See also:

getEpsilon

bool mrpt::math::areAligned(const std::vector<TPoint3D>& points, TLine3D& r)

Checks whether this set of points acceptably fits a 3D line, and if it’s the case returns it in the second argument.

void mrpt::math::project3D(const TPoint3D& point, const mrpt::math::TPose3D& newXYpose, TPoint3D& newPoint)

Uses the given pose 3D to project a point into a new base.

void mrpt::math::project3D(const TSegment3D& segment, const mrpt::math::TPose3D& newXYpose, TSegment3D& newSegment)

Uses the given pose 3D to project a segment into a new base

void mrpt::math::project3D(const TLine3D& line, const mrpt::math::TPose3D& newXYpose, TLine3D& newLine)

Uses the given pose 3D to project a line into a new base.

void mrpt::math::project3D(const TPlane& plane, const mrpt::math::TPose3D& newXYpose, TPlane& newPlane)

Uses the given pose 3D to project a plane into a new base.

void mrpt::math::project3D(const TPolygon3D& polygon, const mrpt::math::TPose3D& newXYpose, TPolygon3D& newPolygon)

Uses the given pose 3D to project a polygon into a new base.

void mrpt::math::project3D(const TObject3D& object, const mrpt::math::TPose3D& newXYPose, TObject3D& newObject)

Uses the given pose 3D to project any 3D object into a new base.

template <typename Object>
Object mrpt::math::project3D(
    const Object& o,
    const mrpt::math::TPose3D& newPose
    )

Returns the transformed point/line/segment/plane after applying the given SE(3) transformation.

[New in MRPT 2.3.1]

template <class T>
void mrpt::math::project3D(
    const T& obj,
    const TPlane& newXYPlane,
    T& newObj
    )

Projects any 3D object into the plane’s base, using its inverse pose.

If the object is exactly inside the plane, this projection will zero its Z coordinates

template <class T>
void mrpt::math::project3D(
    const T& obj,
    const TPlane& newXYPlane,
    const TPoint3D& newOrigin,
    T& newObj
    )

Projects any 3D object into the plane’s base, using its inverse pose and forcing the position of the new coordinates origin.

If the object is exactly inside the plane, this projection will zero its Z coordinates

template <class T>
void mrpt::math::project3D(
    const std::vector<T>& objs,
    const mrpt::math::TPose3D& newXYpose,
    std::vector<T>& newObjs
    )

Projects a set of 3D objects into the plane’s base.

void mrpt::math::project2D(const TPoint2D& point, const TPose2D& newXpose, TPoint2D& newPoint)

Uses the given pose 2D to project a point into a new base.

void mrpt::math::project2D(const TSegment2D& segment, const TPose2D& newXpose, TSegment2D& newSegment)

Uses the given pose 2D to project a segment into a new base

void mrpt::math::project2D(const TLine2D& line, const TPose2D& newXpose, TLine2D& newLine)

Uses the given pose 2D to project a line into a new base.

void mrpt::math::project2D(const TPolygon2D& polygon, const TPose2D& newXpose, TPolygon2D& newPolygon)

Uses the given pose 2D to project a polygon into a new base.

void mrpt::math::project2D(const TObject2D& object, const TPose2D& newXpose, TObject2D& newObject)

Uses the given pose 2D to project any 2D object into a new base.

template <typename Object>
Object mrpt::math::project2D(
    const Object& o,
    const mrpt::math::TPose2D& newPose
    )

Returns the transformed point/line/segment after applying the given SE(2) transformation.

[New in MRPT 2.3.1]

template <class T, class CPOSE2D>
void mrpt::math::project2D(
    const T& obj,
    const TLine2D& newXLine,
    T& newObj
    )

Projects any 2D object into the line’s base, using its inverse pose.

If the object is exactly inside the line, this projection will zero its Y coordinate.

Parameters:

CPOSE2D

set to TPose2D

template <class T, class CPOSE2D>
void mrpt::math::project2D(
    const T& obj,
    const TLine2D& newXLine,
    const TPoint2D& newOrigin,
    T& newObj
    )

Projects any 2D object into the line’s base, using its inverse pose and forcing the position of the new coordinate origin.

If the object is exactly inside the line, this projection will zero its Y coordinate.

Parameters:

CPOSE2D

set to TPose2D

template <class T>
void mrpt::math::project2D(
    const std::vector<T>& objs,
    const TPose2D& newXpose,
    std::vector<T>& newObjs
    )

Projects a set of 2D objects into the line’s base.

bool mrpt::math::intersect(const TPolygon2D& p1, const TSegment2D& s2, TObject2D& obj)

Gets the intersection between a 2D polygon and a 2D segment.

See also:

TObject2D

bool mrpt::math::intersect(const TPolygon2D& p1, const TLine2D& r2, TObject2D& obj)

Gets the intersection between a 2D polygon and a 2D line.

See also:

TObject2D

TPolygon2D mrpt::math::intersect(const TPolygon2D& subject, const TPolygon2D& clipping)

The Sutherland-Hodgman algorithm for finding the intersection between two 2D polygons.

Note that at least one of the polygons (clipping) must be convex. The other polygon (subject) may be convex or concave.

See code example: Example: math_polygon_intersection

(New in MRPT 2.4.1)

Returns:

The intersection, or an empty (no points) polygon if there is no intersection at all.

bool mrpt::math::intersect(const TSegment2D& s1, const TPolygon2D& p2, TObject2D& obj)

Gets the intersection between a 2D segment and a 2D polygon.

See also:

TObject2D

bool mrpt::math::intersect(const TLine2D& r1, const TPolygon2D& p2, TObject2D& obj)

Gets the intersection between a 2D line and a 2D polygon.

See also:

TObject2D

bool mrpt::math::intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj)

Gets the intersection between a 3D polygon and a 3D segment.

See also:

TObject3D

bool mrpt::math::intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj)

Gets the intersection between a 3D polygon and a 3D line.

See also:

TObject3D

bool mrpt::math::intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj)

Gets the intersection between a 3D polygon and a plane.

See also:

TObject3D

bool mrpt::math::intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj)

Gets the intersection between two 3D polygons.

See also:

TObject3D

bool mrpt::math::intersect(const TSegment3D& s1, const TPolygon3D& p2, TObject3D& obj)

Gets the intersection between a 3D segment and a 3D polygon.

See also:

TObject3D

bool mrpt::math::intersect(const TLine3D& r1, const TPolygon3D& p2, TObject3D& obj)

Gets the intersection between a 3D line and a 3D polygon.

See also:

TObject3D

bool mrpt::math::intersect(const TPlane& p1, const TPolygon3D& p2, TObject3D& obj)

Gets the intersection between a plane and a 3D polygon.

See also:

TObject3D

size_t mrpt::math::intersect(
    const std::vector<TPolygon3D>& v1,
    const std::vector<TPolygon3D>& v2,
    CSparseMatrixTemplate<TObject3D>& objs
    )

Gets the intersection between two sets of 3D polygons.

The intersection is returned as an sparse matrix with each pair of polygons’ intersections, and the return value is the amount of intersections found.

See also:

TObject3D, CSparseMatrixTemplate

size_t mrpt::math::intersect(
    const std::vector<TPolygon3D>& v1,
    const std::vector<TPolygon3D>& v2,
    std::vector<TObject3D>& objs
    )

Gets the intersection between two sets of 3D polygons.

The intersection is returned as a vector with every intersection found, and the return value is the amount of intersections found.

See also:

TObject3D

template <class T, class U, class O>
size_t mrpt::math::intersect(
    const std::vector<T>& v1,
    const std::vector<U>& v2,
    CSparseMatrixTemplate<O>& objs
    )

Gets the intersection between vectors of geometric objects and returns it in a sparse matrix of either TObject2D or TObject3D.

See also:

TObject2D, TObject3D, CSparseMatrix

template <class T, class U, class O>
size_t mrpt::math::intersect(
    const std::vector<T>& v1,
    const std::vector<U>& v2,
    std::vector<O> objs
    )

Gets the intersection between vectors of geometric objects and returns it in a vector of either TObject2D or TObject3D.

See also:

TObject2D, TObject3D

bool mrpt::math::intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj)

Gets the intersection between any pair of 2D objects.

bool mrpt::math::intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj)

Gets the intersection between any pair of 3D objects.

double mrpt::math::distance(const TPoint2D& p1, const TPoint2D& p2)

Gets the distance between two points in a 2D space.

double mrpt::math::distance(const TPoint3D& p1, const TPoint3D& p2)

Gets the distance between two points in a 3D space.

double mrpt::math::distance(const TLine2D& r1, const TLine2D& r2)

Gets the distance between two lines in a 2D space.

double mrpt::math::distance(const TLine3D& r1, const TLine3D& r2)

Gets the distance between two lines in a 3D space.

double mrpt::math::distance(const TPlane& p1, const TPlane& p2)

Gets the distance between two planes.

It will be zero if the planes are not parallel.

double mrpt::math::distance(const TPolygon2D& p1, const TPolygon2D& p2)

Gets the distance between two polygons in a 2D space.

double mrpt::math::distance(const TPolygon2D& p1, const TSegment2D& s2)

Gets the distance between a polygon and a segment in a 2D space.

double mrpt::math::distance(const TSegment2D& s1, const TPolygon2D& p2)

Gets the distance between a segment and a polygon in a 2D space.

double mrpt::math::distance(const TPolygon2D& p1, const TLine2D& l2)

Gets the distance between a polygon and a line in a 2D space.

double mrpt::math::distance(const TPolygon3D& p1, const TPolygon3D& p2)

Gets the distance between two polygons in a 3D space.

double mrpt::math::distance(const TPolygon3D& p1, const TSegment3D& s2)

Gets the distance between a polygon and a segment in a 3D space.

double mrpt::math::distance(const TSegment3D& s1, const TPolygon3D& p2)

Gets the distance between a segment and a polygon in a 3D space.

double mrpt::math::distance(const TPolygon3D& p1, const TLine3D& l2)

Gets the distance between a polygon and a line in a 3D space.

double mrpt::math::distance(const TLine3D& l1, const TPolygon3D& p2)

Gets the distance between a line and a polygon in a 3D space.

double mrpt::math::distance(const TPolygon3D& po, const TPlane& pl)

Gets the distance between a polygon and a plane.

double mrpt::math::distance(const TPlane& pl, const TPolygon3D& po)

Gets the distance between a plane and a polygon.

void mrpt::math::getRectangleBounds(const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax)

Gets the rectangular bounds of a 2D polygon or set of 2D points.

void mrpt::math::getPrismBounds(const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax)

Gets the prism bounds of a 3D polygon or set of 3D points.

void mrpt::math::createPlaneFromPoseXY(const mrpt::math::TPose3D& pose, TPlane& plane)

Given a pose, creates a plane orthogonal to its Z vector.

See also:

createPlaneFromPoseXZ, createPlaneFromPoseYZ, createPlaneFromPoseAndNormal

void mrpt::math::createPlaneFromPoseXZ(const mrpt::math::TPose3D& pose, TPlane& plane)

Given a pose, creates a plane orthogonal to its Y vector.

See also:

createPlaneFromPoseXY, createPlaneFromPoseYZ, createPlaneFromPoseAndNormal

void mrpt::math::createPlaneFromPoseYZ(const mrpt::math::TPose3D& pose, TPlane& plane)

Given a pose, creates a plane orthogonal to its X vector.

See also:

createPlaneFromPoseXY, createPlaneFromPoseXZ, createPlaneFromPoseAndNormal

void mrpt::math::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.

See also:

createPlaneFromPoseXY, createPlaneFromPoseXZ, createPlaneFromPoseYZ

CMatrixDouble44 mrpt::math::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, 2 for z) corresponds to the provided vector.

Parameters:

vec

must be a unitary vector

See also:

generateAxisBaseFromDirectionAndAxis()

double mrpt::math::getRegressionLine(const std::vector<TPoint2D>& points, TLine2D& line)

Using eigenvalues, gets the best fitting line for a set of 2D points.

Returns an estimation of the error.

See also:

spline, leastSquareLinearFit

double mrpt::math::getRegressionLine(const std::vector<TPoint3D>& points, TLine3D& line)

Using eigenvalues, gets the best fitting line for a set of 3D points.

Returns an estimation of the error.

See also:

spline, leastSquareLinearFit

double mrpt::math::getRegressionPlane(const std::vector<TPoint3D>& points, TPlane& plane)

Using eigenvalues, gets the best fitting plane for a set of 3D points.

Returns an estimation of the error.

See also:

spline, leastSquareLinearFit

void mrpt::math::assemblePolygons(
    const std::vector<TSegment3D>& segms,
    std::vector<TPolygon3D>& polys
    )

Tries to assemble a set of segments into a set of closed polygons.

void mrpt::math::assemblePolygons(
    const std::vector<TSegment3D>& segms,
    std::vector<TPolygon3D>& polys,
    std::vector<TSegment3D>& remainder
    )

Tries to assemble a set of segments into a set of closed polygons, returning the unused segments as another out parameter.

void mrpt::math::assemblePolygons(
    const std::vector<TObject3D>& objs,
    std::vector<TPolygon3D>& polys
    )

Extracts all the polygons, including those formed from segments, from the set of objects.

void mrpt::math::assemblePolygons(
    const std::vector<TObject3D>& objs,
    std::vector<TPolygon3D>& polys,
    std::vector<TObject3D>& remainder
    )

Extracts all the polygons, including those formed from segments, from the set of objects.

void mrpt::math::assemblePolygons(
    const std::vector<TObject3D>& objs,
    std::vector<TPolygon3D>& polys,
    std::vector<TSegment3D>& remainder1,
    std::vector<TObject3D>& remainder2
    )

Extracts all the polygons, including those formed from segments, from the set of objects.

bool mrpt::math::splitInConvexComponents(
    const TPolygon2D& poly,
    std::vector<TPolygon2D>& components
    )

Splits a 2D polygon into convex components.

See also:

Example: math_polygon_split

bool mrpt::math::splitInConvexComponents(
    const TPolygon3D& poly,
    std::vector<TPolygon3D>& components
    )

Splits a 3D polygon into convex components.

Parameters:

std::logic_error

if the polygon can’t be fit into a plane.

void mrpt::math::getSegmentBisector(const TSegment2D& sgm, TLine2D& bis)

Gets the bisector of a 2D segment.

void mrpt::math::getSegmentBisector(const TSegment3D& sgm, TPlane& bis)

Gets the bisector of a 3D segment.

void mrpt::math::getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis)

Gets the bisector of two lines or segments (implicit constructor will be used if necessary)

void mrpt::math::getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis)

Gets the bisector of two lines or segments (implicit constructor will be used if necessary)

Parameters:

std::logic_error

if the lines do not fit in a single plane.

bool mrpt::math::traceRay(const std::vector<TPolygonWithPlane>& vec, const mrpt::math::TPose3D& pose, double& dist)

Fast ray tracing method using polygons’ properties.

See also:

CRenderizable::rayTrace

bool mrpt::math::traceRay(const std::vector<TPolygon3D>& vec, const mrpt::math::TPose3D& pose, double& dist)

Fast ray tracing method using polygons’ properties.

See also:

CRenderizable::rayTrace

template <class T, class U, class V>
void mrpt::math::crossProduct3D(
    const T& v0,
    const U& v1,
    V& vOut
    )

Computes the cross product of two 3D vectors, returning a vector normal to both.

It uses the simple implementation:

\[\begin{split}v_out = \left( \begin{array}{c c c} \hat{i} ~ \hat{j} ~ \hat{k} \\ x0 ~ y0 ~ z0 \\ x1 ~ y1 ~ z1 \\ \end{array} \right)\end{split}\]
template <class T>
void mrpt::math::crossProduct3D(
    const std::vector<T>& v0,
    const std::vector<T>& v1,
    std::vector<T>& v_out
    )

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

template <class VEC1, class VEC2>
VEC1 mrpt::math::crossProduct3D(
    const VEC1& v0,
    const VEC2& v1
    )

overload (returning a vector of size 3 by value).

template <class VECTOR, class MATRIX>
void mrpt::math::skew_symmetric3(
    const VECTOR& v,
    MATRIX& M
    )

Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:

\[\begin{split}M([x ~ y ~ z]^\top) = \left( \begin{array}{c c c} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{array} \right)\end{split}\]
template <class VECTOR>
mrpt::math::CMatrixDouble33 mrpt::math::skew_symmetric3(const VECTOR& v)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

template <class VECTOR, class MATRIX>
void mrpt::math::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:

\[\begin{split}-M([x ~ y ~ z]^\top) = \left( \begin{array}{c c c} 0 & z & -y \\ -z & 0 & x \\ y & -x & 0 \end{array} \right)\end{split}\]
template <class VECTOR>
mrpt::math::CMatrixDouble33 mrpt::math::skew_symmetric3_neg(const VECTOR& v)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

template <class T, class U>
bool mrpt::math::vectorsAreParallel2D(
    const T& v1,
    const U& v2
    )

Returns true if two 2D vectors are parallel.

The arguments may be points, arrays, etc.

template <class T, class U>
bool mrpt::math::vectorsAreParallel3D(
    const T& v1,
    const U& v2
    )

Returns true if two 3D vectors are parallel.

The arguments may be points, arrays, etc.

void mrpt::math::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.

See also:

closestFromPointToLine

void mrpt::math::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.

See also:

closestFromPointToSegment

double mrpt::math::squaredDistancePointToLine(
    double Px,
    double Py,
    double x1,
    double y1,
    double x2,
    double y2
    )

Returns the square distance from a point to a line.

template <typename T>
T mrpt::math::distanceBetweenPoints(
    const T x1,
    const T y1,
    const T x2,
    const T y2
    )

Returns the distance between 2 points in 2D.

template <typename T>
T mrpt::math::distanceBetweenPoints(
    const T x1,
    const T y1,
    const T z1,
    const T x2,
    const T y2,
    const T z2
    )

Returns the distance between 2 points in 3D.

template <typename T>
T mrpt::math::distanceSqrBetweenPoints(
    const T x1,
    const T y1,
    const T x2,
    const T y2
    )

Returns the square distance between 2 points in 2D.

template <typename T>
T mrpt::math::distanceSqrBetweenPoints(
    const T x1,
    const T y1,
    const T z1,
    const T x2,
    const T y2,
    const T z2
    )

Returns the square distance between 2 points in 3D.

template <typename T>
double mrpt::math::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.

template <typename T>
bool mrpt::math::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.

See also:

pointIntoPolygon2D

bool mrpt::math::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.

The first rectangle is not rotated and given by (R1_x_min,R1_x_max)-(R1_y_min,R1_y_max). The second rectangle is given is a similar way, but it is internally rotated according to the given coordinates translation (R2_pose_x,R2_pose_y,R2_pose_phi(radians)), relative to the coordinates system of rectangle 1.

CMatrixDouble33 mrpt::math::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 them (“X”) NOTE: Make sure of passing all floats or doubles and that the template of the receiving matrix is of the same type!

If \(d = [ dx ~ dy ~ dz ]\) is the input vector, then this function returns a matrix \(M\) such as:

\[\begin{split}M = \left( \begin{array}{c c c} v^1_x ~ v^2_x ~ v^3_x \\ v^1_y ~ v^2_y ~ v^3_y \\ v^1_z ~ v^2_z ~ v^3_z \end{array} \right)\end{split}\]

And the three normal vectors are computed as:

\[v^1 = \frac{d}{|d|}\]

If (dx!=0 or dy!=0):

\[v^2 = \frac{[-dy ~ dx ~ 0 ]}{\sqrt{dx^2+dy^2}}\]

otherwise (the direction vector is vertical):

\[v^2 = [1 ~ 0 ~ 0]\]

And finally, the third vector is the cross product of the others:

\[v^3 = v^1 \times v^2\]

(JLB @ 18-SEP-2007)

Returns:

The 3x3 matrix (CMatrixDynamic<T>), containing one vector per column. except Throws an std::exception on invalid input (i.e. null direction vector)

See also:

generateAxisBaseFromDirectionAndAxis()

double mrpt::math::signedArea(const mrpt::math::TPolygon2D& p)

Returns the area of a polygon, positive if vertices listed in CCW ordering, negative if CW.

(New in MRPT 2.4.1)

template <typename T>
void mrpt::math::slerp(
    const CQuaternion<T>& q0,
    const CQuaternion<T>& q1,
    const double t,
    CQuaternion<T>& q
    )

SLERP interpolation between two quaternions.

Parameters:

q0

The quaternion for t=0

q1

The quaternion for t=1

t

A “time” parameter, in the range [0,1].

q

The output, interpolated quaternion.

T

The type of the quaternion (e.g. float, double).

std::exception

Only in Debug, if t is not in the valid range.

See also:

http://en.wikipedia.org/wiki/Slerp

void mrpt::math::slerp(const TPose3D& q0, const TPose3D& q1, const double t, TPose3D& p)

SLERP interpolation between two 6D poses - like mrpt::math::slerp for quaternions, but interpolates the [X,Y,Z] coordinates as well.

Parameters:

p0

The pose for t=0

p1

The pose for t=1

t

A “time” parameter, in the range [0,1].

p

The output, interpolated pose.

std::exception

Only in Debug, if t is not in the valid range.

std::ostream& mrpt::math::operator << (std::ostream& o, const TLine2D& p)

Text streaming function.

std::ostream& mrpt::math::operator << (std::ostream& o, const TLine3D& p)

Text streaming function.

std::ostream& mrpt::math::operator << (std::ostream& o, const mrpt::math::TObject2D& obj)

Textual print stream operator.

See also:

TObject2D::asString()

std::ostream& mrpt::math::operator << (std::ostream& o, const mrpt::math::TObject3D& obj)

Textual print stream operator.

See also:

TObject3D::asString()

std::ostream& mrpt::math::operator << (std::ostream& o, const TPlane& p)

Text streaming function.

template <typename T>
constexpr TPoint2D_<T> mrpt::math::operator - (const TPoint2D_<T>& p1)

Unary minus operator for 2D points/vectors.

template <
    typename T,
    typename Scalar,
    std::enable_if_t<std::is_convertible_v<Scalar, T>>* = nullptr
    >
constexpr TPoint2D_<T> mrpt::math::operator * (
    const Scalar scalar,
    const TPoint2D_<T>& p
    )

scalar times vector operator.

template <typename T>
constexpr bool mrpt::math::operator == (
    const TPoint2D_<T>& p1,
    const TPoint2D_<T>& p2
    )

Exact comparison between 2D points.

template <typename T>
constexpr bool mrpt::math::operator != (
    const TPoint2D_<T>& p1,
    const TPoint2D_<T>& p2
    )

Exact comparison between 2D points.

template <typename T>
constexpr TPoint3D_<T> mrpt::math::operator - (const TPoint3D_<T>& p1)

Unary minus operator for 3D points/vectors.

template <
    typename T,
    typename Scalar,
    std::enable_if_t<std::is_convertible_v<Scalar, T>>* = nullptr
    >
constexpr TPoint3D_<T> mrpt::math::operator * (
    const Scalar scalar,
    const TPoint3D_<T>& p
    )

scalar times vector operator.

template <typename T>
constexpr bool mrpt::math::operator == (
    const TPoint3D_<T>& p1,
    const TPoint3D_<T>& p2
    )

Exact comparison between 3D points.

template <typename T>
constexpr bool mrpt::math::operator != (
    const TPoint3D_<T>& p1,
    const TPoint3D_<T>& p2
    )

Exact comparison between 3D points.

template <
    class PoseOrPoint,
    typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>>
    >
std::ostream& mrpt::math::operator << (
    std::ostream& o,
    const PoseOrPoint& p
    )

Text streaming function.

template <
    class PoseOrPoint,
    typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>>
    >
mrpt::serialization::CArchive& mrpt::math::operator >> (
    mrpt::serialization::CArchive& in,
    PoseOrPoint& o
    )

Binary streaming function.

template <
    class PoseOrPoint,
    typename = std::enable_if_t<std::is_base_of_v<mrpt::math::TPoseOrPoint, PoseOrPoint>>
    >
mrpt::serialization::CArchive& mrpt::math::operator << (
    mrpt::serialization::CArchive& out,
    const PoseOrPoint& o
    )

Binary streaming function.