/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers
//
#include <mrpt/core/optional_ref.h>
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/math/CSparseMatrixTemplate.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/TLine2D.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject2D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace std;
using namespace mrpt::math;

static double geometryEpsilon = 1e-5;

double mrpt::math::getEpsilon() { return geometryEpsilon; }
void mrpt::math::setEpsilon(double eps) { geometryEpsilon = eps; }
/*---------------------------------------------------------------
  Returns the closest point to a segment
  ---------------------------------------------------------------*/
void math::closestFromPointToSegment(
    double Px, double Py, double x1, double y1, double x2, double y2, double& out_x, double& out_y)
{
  if (x1 == x2 && y1 == y2)
  {
    out_x = x1;
    out_y = y1;
  }
  else
  {
    double Dx = x2 - x1;
    double Dy = y2 - y1;
    double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
    if (Ratio < 0)
    {
      out_x = x1;
      out_y = y1;
    }
    else
    {
      if (Ratio > 1)
      {
        out_x = x2;
        out_y = y2;
      }
      else
      {
        out_x = x1 + (Ratio * Dx);
        out_y = y1 + (Ratio * Dy);
      }
    }
  }
}

/*---------------------------------------------------------------
  Returns the closest point to a line
  ---------------------------------------------------------------*/
void math::closestFromPointToLine(
    double Px, double Py, double x1, double y1, double x2, double y2, double& out_x, double& out_y)
{
  if (x1 == x2 && y1 == y2)
  {
    out_x = x1;
    out_y = y1;
  }
  else
  {
    double Dx = x2 - x1;
    double Dy = y2 - y1;
    double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);

    out_x = x1 + (Ratio * Dx);
    out_y = y1 + (Ratio * Dy);
  }
}

/*---------------------------------------------------------------
  Returns the sq. distance to closest point to a line
  ---------------------------------------------------------------*/
double math::squaredDistancePointToLine(
    double Px, double Py, double x1, double y1, double x2, double y2)
{
  if (x1 == x2 && y1 == y2)
  {
    return square(Px - x1) + square(Py - y1);
  }
  else
  {
    double Dx = x2 - x1;
    double Dy = y2 - y1;
    double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);

    return square(x1 + (Ratio * Dx) - Px) + square(y1 + (Ratio * Dy) - Py);
  }
}

/*---------------------------------------------------------------
        Rectangles Intersect
  ---------------------------------------------------------------*/
bool 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)
{
  // Compute the rotated R2:
  // ----------------------------------------
  CVectorDouble xs(4), ys(4);
  double ccos = cos(R2_pose_phi);
  double ssin = sin(R2_pose_phi);

  xs[0] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_min;
  ys[0] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_min;

  xs[1] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_min;
  ys[1] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_min;

  xs[2] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_max;
  ys[2] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_max;

  xs[3] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_max;
  ys[3] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_max;

  // Test for one vertice being inside the other rectangle:
  // -------------------------------------------------------
  if (R1_x_min <= xs[0] && xs[0] <= R1_x_max && R1_y_min <= ys[0] && ys[0] <= R1_y_max) return true;
  if (R1_x_min <= xs[1] && xs[1] <= R1_x_max && R1_y_min <= ys[1] && ys[1] <= R1_y_max) return true;
  if (R1_x_min <= xs[2] && xs[2] <= R1_x_max && R1_y_min <= ys[2] && ys[2] <= R1_y_max) return true;
  if (R1_x_min <= xs[3] && xs[3] <= R1_x_max && R1_y_min <= ys[3] && ys[3] <= R1_y_max) return true;

  CPolygon poly;
  poly.AddVertex(xs[0], ys[0]);
  poly.AddVertex(xs[1], ys[1]);
  poly.AddVertex(xs[2], ys[2]);
  poly.AddVertex(xs[3], ys[3]);

  if (poly.PointIntoPolygon(R1_x_min, R1_y_min)) return true;
  if (poly.PointIntoPolygon(R1_x_max, R1_y_min)) return true;
  if (poly.PointIntoPolygon(R1_x_max, R1_y_max)) return true;
  if (poly.PointIntoPolygon(R1_x_min, R1_y_max)) return true;

  // Test for intersections:
  // ----------------------------------------
  TObject2D intersectObj;

  for (int idx = 0; idx < 4; idx++)
  {
    const auto seg2 =
        TSegment2D::FromPoints({xs[idx], ys[idx]}, {xs[(idx + 1) % 4], ys[(idx + 1) % 4]});

    if (mrpt::math::intersect(
            TSegment2D::FromPoints({R1_x_min, R1_y_min}, {R1_x_max, R1_y_min}), seg2, intersectObj))
      return true;
    if (mrpt::math::intersect(
            TSegment2D::FromPoints({R1_x_max, R1_y_min}, {R1_x_max, R1_y_max}), seg2, intersectObj))
      return true;
    if (mrpt::math::intersect(
            TSegment2D::FromPoints({R1_x_max, R1_y_max}, {R1_x_min, R1_y_max}), seg2, intersectObj))
      return true;
    if (mrpt::math::intersect(
            TSegment2D::FromPoints({R1_x_min, R1_y_max}, {R1_x_min, R1_y_min}), seg2, intersectObj))
      return true;
  }

  // No intersections:
  return false;
}

// Auxiliary functions needed to avoid code repetition and unnecesary
// recalculations
template <class T2D, class U2D, class T3D, class U3D>
bool intersectInCommonPlane(
    const T3D& o1, const U3D& o2, const mrpt::math::TPlane& p, mrpt::math::TObject3D& obj)
{
  T3D proj1;
  U3D proj2;
  // Project into 3D plane, ignoring Z coordinate.
  TPose3D pose;
  TPlane(p).getAsPose3D(pose);
  TPose3D poseNeg = -pose;
  project3D(o1, poseNeg, proj1);
  project3D(o2, poseNeg, proj2);
  T2D proj1_2D;
  U2D proj2_2D;
  proj1.generate2DObject(proj1_2D);
  proj2.generate2DObject(proj2_2D);
  // Compute easier intersection in 2D space
  TObject2D obj2D;
  if (intersect(proj1_2D, proj2_2D, obj2D))
  {
    TObject3D tmp = obj2D.generate3DObject();
    // Undo projection
    project3D(tmp, pose, obj);
    return true;
  }
  else
    return false;
}
bool intersectInCommonLine(
    const mrpt::math::TSegment3D& s1,
    const mrpt::math::TSegment3D& s2,
    const mrpt::math::TLine3D& lin,
    mrpt::math::TObject3D& obj)
{
  // Move in a free coordinate, searching for minima and maxima.
  size_t i1 = 0;
  while (std::abs(lin.director[i1]) < geometryEpsilon) i1++;
  TSegment3D s11 = (s1[0][i1] > s1[1][i1]) ? TSegment3D(s1[1], s1[0]) : s1;
  TSegment3D s21 = (s2[0][i1] > s2[1][i1]) ? TSegment3D(s2[1], s2[0]) : s2;
  TPoint3D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
  TPoint3D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
  if (std::abs(pMax[i1] - pMin[i1]) < geometryEpsilon)
  {  // Intersection is a point
    obj.data = pMax;
    return true;
  }
  else if (pMax[i1] < pMin[i1])
    return false;  // No intersection
  else
  {
    obj.data = TSegment3D(pMin, pMax);  // Intersection is a segment
    return true;
  }
}
bool intersectInCommonLine(
    const TSegment2D& s1, const TSegment2D& s2, const TLine2D& lin, TObject2D& obj)
{
  // Move in a free coordinate, searching for minima and maxima
  size_t i1 = (std::abs(lin.coefs[0]) >= geometryEpsilon) ? 1 : 0;
  TSegment2D s11 = (s1[0][i1] > s1[1][i1]) ? TSegment2D(s1[1], s1[0]) : s1;
  TSegment2D s21 = (s2[0][i1] > s2[1][i1]) ? TSegment2D(s2[1], s2[0]) : s2;
  TPoint2D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
  TPoint2D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
  if (std::abs(pMax[i1] - pMin[i1]) < geometryEpsilon)
  {  // Intersection is a point
    obj.data = pMax;
    return true;
  }
  else if (pMax[i1] < pMin[i1])
    return false;  // No intersection
  else
  {
    obj.data = TSegment2D(pMin, pMax);  // Intersection is a segment
    return true;
  }
}
inline void unsafeProjectPoint(const TPoint3D& point, const TPose3D& pose, TPoint2D& newPoint)
{
  TPoint3D dummy;
  pose.composePoint(point, dummy);
  newPoint.x = dummy.x;
  newPoint.y = dummy.y;
}
void mrpt::math::internal::unsafeProjectPolygon(
    const TPolygon3D& poly, const TPose3D& pose, TPolygon2D& newPoly)
{
  size_t N = poly.size();
  newPoly.resize(N);
  for (size_t i = 0; i < N; i++) unsafeProjectPoint(poly[i], pose, newPoly[i]);
}
bool intersect(const TPolygonWithPlane& p1, const TLine3D& l2, double& d, double bestKnown)
{
  // LINE MUST BE UNITARY
  TObject3D obj;
  if (intersect(p1.plane, l2, obj))
    if (obj.isPoint())
    {
      const TPoint3D p = obj.getAs<TPoint3D>();
      for (size_t i = 0; i < 3; i++)
        if (std::abs(l2.director[i]) > geometryEpsilon)
        {
          d = (p[i] - l2.pBase[i]) / l2.director[i];
          break;
        }
      if (d < 0 || d > bestKnown) return false;
      TPolygon2D newPoly;
      TPoint2D newP;
      unsafeProjectPoint(p, p1.inversePose, newP);
      mrpt::math::internal::unsafeProjectPolygon(p1.poly, p1.inversePose, newPoly);
      return newPoly.contains(newP);
    }
  return false;
}
bool intersect(const TPolygonWithPlane& p1, const TPolygonWithPlane& p2, TObject3D& obj)
{
  if (!intersect(p1.plane, p2.plane, obj)) return false;

  TObject3D aux;
  if (obj.isLine())
  {
    const TLine3D lin3D = obj.getAs<TLine3D>();
    TLine3D lin3D1, lin3D2;
    TLine2D lin2D1, lin2D2;
    TObject2D obj2D1, obj2D2;
    project3D(lin3D, p1.inversePose, lin3D1);
    project3D(lin3D, p2.inversePose, lin3D2);
    lin3D1.generate2DObject(lin2D1);
    lin3D2.generate2DObject(lin2D2);
    if (intersect(p1.poly2D, lin2D1, obj2D1) && intersect(p2.poly2D, lin2D2, obj2D2))
    {
      TObject3D obj3Dp1, obj3Dp2;
      const TObject3D obj3D1 = obj2D1.generate3DObject();
      const TObject3D obj3D2 = obj2D2.generate3DObject();
      project3D(obj3D1, p1.pose, obj3Dp1);
      project3D(obj3D2, p2.pose, obj3Dp2);
      TSegment3D s1, s2;
      if (obj3D1.isPoint())
      {
        const auto po1 = obj3D1.getAs<TPoint3D>();
        s1 = TSegment3D(po1, po1);
      }
      else
      {
        s1 = obj3D1.getAs<TSegment3D>();
      }
      TPoint3D po2;
      if (obj3D2.getPoint(po2))
        s2 = TSegment3D(po2, po2);
      else
        obj3D2.getSegment(s2);
      return intersectInCommonLine(s1, s2, lin3D, obj);
    }
    else
      return false;
  }
  else
  {
    TObject2D obj2D;
    if (intersect(p1.poly2D, p2.poly2D, obj2D))
    {
      aux = obj2D.generate3DObject();
      project3D(aux, p1.pose, obj);
      return true;
    }
    else
      return false;
  }
}
// End of auxiliary methods

bool math::intersect(const TSegment3D& s1, const TSegment3D& s2, TObject3D& obj)
{
  TObject3D irr;
  auto l = TLine3D(s1);
  if (!intersect(l, TLine3D(s2), irr)) return false;
  if (irr.isPoint())
  {
    // Both lines cross in a point.
    TPoint3D p;
    irr.getPoint(p);
    if (s1.contains(p) && s2.contains(p))
    {
      obj.data = p;
      return true;
    }
    else
      return false;
  }
  else
    return intersectInCommonLine(s1, s2, l, obj);
}

bool math::intersect(const TSegment3D& s1, const TPlane& p1, TObject3D& obj)
{
  if (!intersect(TLine3D(s1), p1, obj)) return false;
  if (obj.isLine())
  {
    // Segment is fully inside the plane, so it is the return value.
    obj.data = s1;
    return true;
  }
  else
  {
    // Segment's line intersects the plane in a point. This may be or not be
    // part of the segment.
    TPoint3D p;
    if (!obj.getPoint(p)) return false;
    return s1.contains(p);
  }
}

bool math::intersect(const TSegment3D& s1, const TLine3D& r1, TObject3D& obj)
{
  if (!intersect(TLine3D(s1), r1, obj)) return false;
  if (obj.isLine())
  {
    // Segment's line is the other line.
    obj.data = s1;
    return true;
  }
  else
  {
    // Segment's line and the other line cross in a point, which may be or
    // not be inside the segment.
    TPoint3D p;
    if (!obj.getPoint(p)) return false;
    return s1.contains(p);
  }
}

bool math::intersect(const TPlane& p1, const TPlane& p2, TObject3D& obj)
{
  TLine3D lin;
  crossProduct3D(p1.coefs, p2.coefs, lin.director);
  if ((std::abs(lin.director[0]) < geometryEpsilon) &&
      (std::abs(lin.director[1]) < geometryEpsilon) &&
      (std::abs(lin.director[2]) < geometryEpsilon))
  {
    // Planes are parallel
    for (size_t i = 0; i < 3; i++)
      if (std::abs(p1.coefs[i] * p2.coefs[3] - p1.coefs[3] * p2.coefs[i]) >= geometryEpsilon)
        return false;
    // Planes are the same
    obj.data = p1;
    return true;
  }
  else
  {
    // Planes cross in a line whose director vector is already calculated
    // (normal to both planes' normal).
    // The following process manages to create a random point in the line
    // without loss of generality and almost without conditional sentences.
    size_t i1 = 0;
    while (std::abs(lin.director[i1]) < geometryEpsilon) i1++;
    // At this point, i1 points to a coordinate (0->x, 1->y, 2->z) in which
    // we can move freely.
    // If we arbitrarily assign this coordinate to 0, we'll find a suitable
    // base point by solving both planes' equations.
    size_t c1 = (i1 + 1) % 3, c2 = (i1 + 2) % 3;
    lin.pBase[i1] = 0.0;
    lin.pBase[c1] = (p2.coefs[3] * p1.coefs[c2] - p1.coefs[3] * p2.coefs[c2]) / lin.director[i1];
    lin.pBase[c2] = (p2.coefs[c1] * p1.coefs[3] - p1.coefs[c1] * p2.coefs[3]) / lin.director[i1];
    lin.unitarize();
    obj.data = lin;
    return true;
  }
}

bool math::intersect(const TPlane& p1, const TLine3D& r2, TObject3D& obj)
{
  // double
  // n=p1.coefs[0]*r2.director[0]+p1.coefs[1]*r2.director[1]+p1.coefs[2]*r2.director[2];
  double n = dotProduct<3, double>(p1.coefs, r2.director);
  double e = p1.evaluatePoint(r2.pBase);
  if (std::abs(n) < geometryEpsilon)
  {
    // Plane's normal and line's director are orthogonal, so both are
    // parallel
    if (std::abs(e) < geometryEpsilon)
    {
      // Line is contained in plane.
      obj.data = r2;
      return true;
    }
    else
      return false;
  }
  else
  {
    // Plane and line cross in a point.
    double t = e / n;
    TPoint3D p;
    p.x = r2.pBase.x - t * r2.director[0];
    p.y = r2.pBase.y - t * r2.director[1];
    p.z = r2.pBase.z - t * r2.director[2];
    obj.data = p;
    return true;
  }
}

bool math::intersect(const TLine3D& r1, const TLine3D& r2, TObject3D& obj)
{
  double u, d[3];
  TPoint3D p;
  const static size_t c1[] = {1, 2, 0};
  const static size_t c2[] = {2, 0, 1};
  // With indirect memory accesses, almost all the code goes in a single loop.
  for (size_t i = 0; i < 3; i++)
  {
    double sysDet =
        -r1.director[c1[i]] * r2.director[c2[i]] + r2.director[c1[i]] * r1.director[c2[i]];
    if (std::abs(sysDet) < geometryEpsilon) continue;
    // We've found a coordinate in which we can solve the associated system
    d[c1[i]] = r2.pBase[c1[i]] - r1.pBase[c1[i]];
    d[c2[i]] = r2.pBase[c2[i]] - r1.pBase[c2[i]];
    u = (r1.director[c1[i]] * d[c2[i]] - r1.director[c2[i]] * d[c1[i]]) / sysDet;
    for (size_t k = 0; k < 3; k++) p[k] = r2.pBase[k] + u * r2.director[k];
    if (r1.contains(p))
    {
      obj.data = p;
      return true;
    }
    else
      return false;
  }
  // Lines are parallel
  if (r1.contains(r2.pBase))
  {
    // Lines are the same
    obj.data = r1;
    return true;
  }
  else
    return false;
}

bool math::intersect(const TLine2D& r1, const TLine2D& r2, TObject2D& obj)
{
  double sysDet = r1.coefs[0] * r2.coefs[1] - r1.coefs[1] * r2.coefs[0];
  if (std::abs(sysDet) >= geometryEpsilon)
  {
    // Resulting point comes simply from solving an equation.
    TPoint2D p;
    p.x = (r1.coefs[1] * r2.coefs[2] - r1.coefs[2] * r2.coefs[1]) / sysDet;
    p.y = (r1.coefs[2] * r2.coefs[0] - r1.coefs[0] * r2.coefs[2]) / sysDet;
    obj.data = p;
    return true;
  }
  else
  {
    // Lines are parallel
    if (std::abs(r1.coefs[0] * r2.coefs[2] - r1.coefs[2] * r2.coefs[0]) >= geometryEpsilon ||
        std::abs(r1.coefs[1] * r2.coefs[2] - r1.coefs[2] * r2.coefs[1]) >= geometryEpsilon)
      return false;

    // Lines are the same
    obj.data = r1;
    return true;
  }
}

bool math::intersect(const TLine2D& r1, const TSegment2D& s2, TObject2D& obj)
{
  if (!intersect(r1, TLine2D(s2), obj)) return false;
  TPoint2D p;
  if (obj.isLine())
  {
    // Segment is inside the line
    obj.data = s2;
    return true;
  }
  else if (obj.getPoint(p))
    return s2.contains(p);  // Both lines cross in a point.
  return false;
}

bool math::intersect(const TSegment2D& s1, const TSegment2D& s2, TObject2D& obj)
{
  auto lin = TLine2D(s1);
  if (!intersect(lin, TLine2D(s2), obj)) return false;
  TPoint2D p;
  if (obj.isLine())
    return intersectInCommonLine(s1, s2, lin,
                                 obj);  // Segments' lines are parallel
  else if (obj.getPoint(p))
    return s1.contains(p) && s2.contains(p);  // Segments' lines cross in a point
  return false;
}

double math::getAngle(const TPlane& s1, const TPlane& s2)
{
  double c = 0, n1 = 0, n2 = 0;
  for (size_t i = 0; i < 3; i++)
  {
    c += s1.coefs[i] * s2.coefs[i];
    n1 += s1.coefs[i] * s1.coefs[i];
    n2 += s2.coefs[i] * s2.coefs[i];
  }
  double s = sqrt(n1 * n2);
  if (s < geometryEpsilon) THROW_EXCEPTION("Invalid plane(s)");
  if (std::abs(s) < std::abs(c))
    return (c > 0) ? 0 : M_PI;
  else
    return acos(c / s);
}

double math::getAngle(const TPlane& s1, const TLine3D& r2)
{
  double c = 0, n1 = 0, n2 = 0;
  for (size_t i = 0; i < 3; i++)
  {
    c += s1.coefs[i] * r2.director[i];
    n1 += s1.coefs[i] * s1.coefs[i];
    n2 += r2.director[i] * r2.director[i];
  }
  double s = sqrt(n1 * n2);
  if (s < geometryEpsilon) THROW_EXCEPTION("Invalid plane or line");
  if (std::abs(s) < std::abs(c))
    return (c > 0) ? 0 : M_PI;
  else
    return asin(c / s);
}

double math::getAngle(const TLine3D& r1, const TLine3D& r2)
{
  double c = 0, n1 = 0, n2 = 0;
  for (size_t i = 0; i < 3; i++)
  {
    c += r1.director[i] * r2.director[i];
    n1 += r1.director[i] * r1.director[i];
    n2 += r2.director[i] * r2.director[i];
  }
  double s = sqrt(n1 * n2);
  if (s < geometryEpsilon) THROW_EXCEPTION("Invalid line(s)");
  if (std::abs(s) < std::abs(c))
    return (c / s < 0) ? M_PI : 0;
  else
    return acos(c / s);
}

double math::getAngle(const TLine2D& r1, const TLine2D& r2)
{
  const double ang1 = std::atan2(-r1.coefs[0], r1.coefs[1]);
  const double ang2 = std::atan2(-r2.coefs[0], r2.coefs[1]);
  return mrpt::math::wrapToPi(ang2 - ang1);
}

// Auxiliary method
void createFromPoseAndAxis(const TPose3D& p, TLine3D& r, size_t axis)
{
  CMatrixDouble44 m;
  p.getHomogeneousMatrix(m);
  for (size_t i = 0; i < 3; i++)
  {
    r.pBase[i] = m(i, 3);
    r.director[i] = m(i, axis);
  }
}
// End of auxiliary method

void math::createFromPoseX(const TPose3D& p, TLine3D& r) { createFromPoseAndAxis(p, r, 0); }

void math::createFromPoseY(const TPose3D& p, TLine3D& r) { createFromPoseAndAxis(p, r, 1); }

void math::createFromPoseZ(const TPose3D& p, TLine3D& r) { createFromPoseAndAxis(p, r, 2); }

void math::createFromPoseAndVector(const TPose3D& p, const double (&vector)[3], TLine3D& r)
{
  CMatrixDouble44 m;
  p.getHomogeneousMatrix(m);
  for (size_t i = 0; i < 3; i++)
  {
    r.pBase[i] = m(i, 3);
    r.director[i] = 0;
    for (size_t j = 0; j < 3; j++) r.director[i] += m(i, j) * vector[j];
  }
}

void math::createFromPoseX(const TPose2D& p, TLine2D& r)
{
  r.coefs[0] = cos(p.phi);
  r.coefs[1] = -sin(p.phi);
  r.coefs[2] = -r.coefs[0] * p.x - r.coefs[1] * p.y;
}

void math::createFromPoseY(const TPose2D& p, TLine2D& r)
{
  r.coefs[0] = sin(p.phi);
  r.coefs[1] = cos(p.phi);
  r.coefs[2] = -r.coefs[0] * p.x - r.coefs[1] * p.y;
}

void math::createFromPoseAndVector(const TPose2D& p, const double (&vector)[2], TLine2D& r)
{
  double c = cos(p.phi);
  double s = sin(p.phi);
  r.coefs[0] = vector[0] * c + vector[1] * s;
  r.coefs[1] = -vector[0] * s + vector[1] * c;
  r.coefs[2] = -r.coefs[0] * p.x - r.coefs[1] * p.y;
}

bool math::conformAPlane(const std::vector<TPoint3D>& points)
{
  size_t N = points.size();
  if (N < 3) return false;
  CMatrixDouble mat(N - 1, 3);
  const TPoint3D& orig = points[N - 1];
  for (size_t i = 0; i < N - 1; i++)
  {
    const TPoint3D& p = points[i];
    mat(i, 0) = p.x - orig.x;
    mat(i, 1) = p.y - orig.y;
    mat(i, 2) = p.z - orig.z;
  }
  return mat.rank(geometryEpsilon) == 2;
}

bool math::conformAPlane(const std::vector<TPoint3D>& points, TPlane& p)
{
  return std::abs(getRegressionPlane(points, p)) < geometryEpsilon;
}

bool math::areAligned(const std::vector<TPoint2D>& points)
{
  size_t N = points.size();
  if (N < 2) return false;
  CMatrixDouble mat(N - 1, 2);
  const TPoint2D& orig = points[N - 1];
  for (size_t i = 0; i < N - 1; i++)
  {
    const TPoint2D& p = points[i];
    mat(i, 0) = p.x - orig.x;
    mat(i, 1) = p.y - orig.y;
  }
  return mat.rank(geometryEpsilon) == 1;
}

bool math::areAligned(const std::vector<TPoint2D>& points, TLine2D& r)
{
  if (!areAligned(points)) return false;
  const TPoint2D& p0 = points[0];
  for (size_t i = 1;; i++)
  {
    try
    {
      r = TLine2D(p0, points[i]);
      return true;
    }
    catch (logic_error&)
    {
    }
  }
}

bool math::areAligned(const std::vector<TPoint3D>& points)
{
  size_t N = points.size();
  if (N < 2) return false;
  CMatrixDouble mat(N - 1, 3);
  const TPoint3D& orig = points[N - 1];
  for (size_t i = 0; i < N - 1; i++)
  {
    const TPoint3D& p = points[i];
    mat(i, 0) = p.x - orig.x;
    mat(i, 1) = p.y - orig.y;
    mat(i, 2) = p.z - orig.z;
  }
  return mat.rank(geometryEpsilon) == 1;
}

bool math::areAligned(const std::vector<TPoint3D>& points, TLine3D& r)
{
  if (!areAligned(points)) return false;
  const TPoint3D& p0 = points[0];
  for (size_t i = 1;; i++) try
    {
      r = TLine3D(p0, points[i]);
      return true;
    }
    catch (logic_error&)
    {
    }
}

void math::project3D(const TLine3D& line, const TPose3D& newXYpose, TLine3D& newLine)
{
  newXYpose.composePoint(line.pBase, newLine.pBase);
  CMatrixDouble44 mat;
  newXYpose.getHomogeneousMatrix(mat);
  for (size_t i = 0; i < 3; i++)
  {
    newLine.director[i] = 0;
    for (size_t j = 0; j < 3; j++) newLine.director[i] += mat(i, j) * line.director[j];
  }
  newLine.unitarize();
}

void math::project3D(const TPlane& plane, const TPose3D& newXYpose, TPlane& newPlane)
{
  CMatrixDouble44 mat;
  newXYpose.getHomogeneousMatrix(mat);
  for (size_t i = 0; i < 3; i++)
  {
    newPlane.coefs[i] = 0;
    for (size_t j = 0; j < 3; j++) newPlane.coefs[i] += mat(i, j) * plane.coefs[j];
  }
  // VORSICHT! NO INTENTEN HACER ESTO EN SUS CASAS (nota: comentar sí o sí,
  // más tarde)
  // La idea es mantener la distancia al nuevo origen igual a la distancia del
  // punto original antes de proyectar.
  // newPlane.coefs[3]=plane.evaluatePoint(TPoint3D(TPose3D(0,0,0,0,0,0)-newXYpose))*sqrt((newPlane.coefs[0]*newPlane.coefs[0]+newPlane.coefs[1]*newPlane.coefs[1]+newPlane.coefs[2]*newPlane.coefs[2])/(plane.coefs[0]*plane.coefs[0]+plane.coefs[1]*plane.coefs[1]+plane.coefs[2]*plane.coefs[2]));
  CMatrixDouble44 HMinv;
  newXYpose.getInverseHomogeneousMatrix(HMinv);
  newPlane.coefs[3] =
      plane.evaluatePoint(TPoint3D(HMinv(0, 3), HMinv(1, 3), HMinv(2, 3))) *
      sqrt(squareNorm<3, double>(newPlane.coefs) / squareNorm<3, double>(plane.coefs));
  newPlane.unitarize();
}

namespace mrpt::math
{
// dummy, required for std::visit()
void project3D(const std::monostate&, const TPose3D&, std::monostate&) {}
void project2D(const std::monostate&, const TPose3D&, std::monostate&) {}
}  // namespace mrpt::math

void math::project3D(const TPolygon3D& polygon, const TPose3D& newXYpose, TPolygon3D& newPolygon)
{
  size_t N = polygon.size();
  newPolygon.resize(N);
  for (size_t i = 0; i < N; i++) project3D(polygon[i], newXYpose, newPolygon[i]);
}

void math::project3D(const TObject3D& object, const TPose3D& newXYpose, TObject3D& newObject)
{
  std::visit(
      [&](auto& o)
      {
        using T = std::decay_t<decltype(o)>;
        newObject.data.emplace<T>();
        project3D(o, newXYpose, std::get<T>(newObject.data));
      },
      object.data);
}

void math::project2D(const TPoint2D& point, const TPose2D& newXpose, TPoint2D& newPoint)
{
  newPoint = newXpose.composePoint(point);
}

void math::project2D(const TLine2D& line, const TPose2D& newXpose, TLine2D& newLine)
{
  double c = cos(newXpose.phi);
  double s = sin(newXpose.phi);
  newLine.coefs[0] = line.coefs[0] * c - line.coefs[1] * s;
  newLine.coefs[1] = line.coefs[1] * c + line.coefs[0] * s;
  newLine.coefs[2] =
      line.coefs[2] - (newLine.coefs[0] * newXpose.x + newLine.coefs[1] * newXpose.y);
  return;
}

void math::project2D(const TPolygon2D& line, const TPose2D& newXpose, TPolygon2D& newLine)
{
  size_t N = line.size();
  newLine.resize(N);
  for (size_t i = 0; i < N; i++) newLine[i] = newXpose + line[i];
  return;
}

void math::project2D(const TObject2D& obj, const TPose2D& newXpose, TObject2D& newObject)
{
  std::visit(
      [&](auto& o)
      {
        using T = std::decay_t<decltype(o)>;
        newObject.data.emplace<T>();
        project2D(o, newXpose, std::get<T>(newObject.data));
      },
      obj.data);
}

bool math::intersect(const TPolygon2D& p1, const TSegment2D& s2, TObject2D& obj)
{
  auto l2 = TLine2D(s2);
  if (!intersect(p1, l2, obj)) return false;
  TPoint2D p;
  TSegment2D s;
  if (obj.getPoint(p))
    return s2.contains(p);
  else if (obj.getSegment(s))
    return intersectInCommonLine(s, s2, l2, obj);
  return false;
}

bool math::intersect(const TPolygon2D& p1, const TLine2D& r2, TObject2D& obj)
{
  // Proceeding: project polygon so that the line happens to be y=0 (phi=0).
  // Then, search this new polygons for intersections with the X axis (very
  // simple).
  if (p1.size() < 3) return false;
  TPose2D pose, poseNeg;
  r2.getAsPose2D(pose);
  poseNeg = TPose2D(0, 0, 0) - pose;
  TPolygon2D projPoly;
  project2D(p1, poseNeg, projPoly);
  size_t N = projPoly.size();
  projPoly.push_back(projPoly[0]);
  double pre = projPoly[0].y;
  vector<TPoint2D> pnts;
  pnts.reserve(2);
  for (size_t i = 1; i <= N; i++)
  {
    double cur = projPoly[i].y;
    if (std::abs(cur) < geometryEpsilon)
    {
      if (std::abs(pre) < geometryEpsilon)
      {
        pnts.resize(2);
        pnts[0] = projPoly[i - 1];
        pnts[1] = projPoly[i];
        break;
      }
      else
        pnts.push_back(projPoly[i]);
    }
    else if ((std::abs(pre) >= geometryEpsilon) && (sign(cur) != sign(pre)))
    {
      double a = projPoly[i - 1].x;
      double c = projPoly[i].x;
      double x = a - pre * (c - a) / (cur - pre);
      pnts.emplace_back(x, 0);
    }
    pre = cur;
  }
  // All results must undo the initial projection
  switch (pnts.size())
  {
    case 0:
      return false;
    case 1:
    {
      TPoint2D p;
      project2D(pnts[0], pose, p);
      obj.data = p;
      return true;
    }
    case 2:
    {
      TSegment2D s;
      project2D(TSegment2D(pnts[0], pnts[1]), pose, s);
      obj.data = s;
      return true;
    }
    default:
      THROW_EXCEPTION("Polygon is not convex");
  }
}

// Auxiliary structs and code for 2D polygon intersection
struct T2ListsOfSegments
{
  vector<TSegment2D> l1;
  vector<TSegment2D> l2;
};

struct TCommonRegion
{
  std::shared_ptr<TPoint2D> point;
  std::shared_ptr<TSegment2D> segment;

  TCommonRegion(const TPoint2D& p) { point = std::make_shared<TPoint2D>(p); }
  TCommonRegion(const TSegment2D& s) { segment = std::make_shared<TSegment2D>(s); }
};

struct TTempIntersection
{
  std::shared_ptr<T2ListsOfSegments> segms;
  std::shared_ptr<TCommonRegion> common;

  TTempIntersection(const T2ListsOfSegments& s) { segms = std::make_shared<T2ListsOfSegments>(s); }
  TTempIntersection(const TCommonRegion& c) { common = std::make_shared<TCommonRegion>(c); }
};
struct TSegmentWithLine
{
  TSegment2D segment;
  TLine2D line;
  explicit TSegmentWithLine(const TSegment2D& s) : segment(s) { line = TLine2D(s[0], s[1]); }
  TSegmentWithLine(const TPoint2D& p1, const TPoint2D& p2) : segment(p1, p2)
  {
    line = TLine2D(p1, p2);
  }
  TSegmentWithLine() = default;
};
bool intersect(const TSegmentWithLine& s1, const TSegmentWithLine& s2, TObject2D& obj)
{
  if (!intersect(s1.line, s2.line, obj)) return false;
  if (obj.isLine()) return intersectInCommonLine(s1.segment, s2.segment, s1.line, obj);
  TPoint2D p;
  obj.getPoint(p);
  return s1.segment.contains(p) && s2.segment.contains(p);
}
void getSegmentsWithLine(const TPolygon2D& poly, vector<TSegmentWithLine>& segs)
{
  size_t N = poly.size();
  segs.resize(N);
  for (size_t i = 0; i < N - 1; i++) segs[i] = TSegmentWithLine(poly[i], poly[i + 1]);
  segs[N - 1] = TSegmentWithLine(poly[N - 1], poly[0]);
}

bool math::intersect(const TPolygon2D& subject, const TPolygon2D& clipping, TObject2D& result)
{
  const auto p = intersect(subject, clipping);
  if (p.empty()) return false;

  result = TObject2D::From(p);
  return true;
}

double mrpt::math::signedArea(const mrpt::math::TPolygon2D& p)
{
  if (p.size() <= 2) return 0;
  double sa = 0;

  for (int i = 0; i < static_cast<int>(p.size()); i++)
  {
    const auto cp = p.at(i);
    const auto np = p.at((i + 1) % p.size());

    sa += cp.x * np.y - np.x * cp.y;
  }

  return sa * 0.5;
}

TPolygon2D mrpt::math::intersect(const TPolygon2D& subject, const TPolygon2D& clipping)
{
  /* Sutherland-Hodgman algorithm:
   * https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm
   *
   *List outputList = subjectPolygon;
   *
   *for (Edge clipEdge in clipPolygon) do
   *    List inputList = outputList;
   *    outputList.clear();
   *
   *    for (int i = 0; i < inputList.count; i += 1) do
   *        Point current_point = inputList[i];
   *        Point prev_point = inputList[(i − 1) % inputList.count];
   *
   *        Point Intersecting_point = ComputeIntersection(prev_point,
   *current_point, clipEdge)
   *
   *        if (current_point inside clipEdge) then
   *            if (prev_point not inside clipEdge) then
   *                outputList.add(Intersecting_point);
   *            end if
   *            outputList.add(current_point);
   *
   *        else if (prev_point inside clipEdge) then
   *            outputList.add(Intersecting_point);
   *        end if
   *
   *    done
   *done
   */
  // We need polygons (may handle degenerated triangles=segments too)
  ASSERT_GE_(subject.size(), 2);
  ASSERT_GE_(clipping.size(), 2);

  TPolygon2D outputList = subject;

  std::vector<TSegmentWithLine> segsClip;
  getSegmentsWithLine(clipping, segsClip);

  const bool clipIsCW = signedArea(clipping) < 0;

  for (const auto& clipEdge : segsClip)
  {
    const auto inputList = outputList;
    outputList.clear();

    for (size_t i = 0; i < inputList.size(); i++)
    {
      const auto current_point = inputList.at(i);
      const auto prev_point = inputList.at(i > 0 ? (i - 1) : (inputList.size() - 1));

      const bool curInside = (clipEdge.line.signedDistance(current_point) < 0) ^ clipIsCW;
      const bool prevInside = (clipEdge.line.signedDistance(prev_point) < 0) ^ clipIsCW;

      mrpt::math::TObject2D intersectPt;
      const bool doIntersect = mrpt::math::intersect(
          clipEdge.line, mrpt::math::TLine2D::FromTwoPoints(current_point, prev_point),
          intersectPt);
      std::optional<mrpt::math::TPoint2D> iPt;
      if (doIntersect)
      {
        intersectPt.getPoint(iPt.emplace());
      }

      if (curInside)
      {
        if (!prevInside)
        {
          if (iPt) outputList.push_back(*iPt);
        }
        outputList.push_back(current_point);
      }
      else if (prevInside)
      {
        if (iPt) outputList.push_back(*iPt);
      }
    }
  }

  return outputList;
}

bool math::intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj)
{
  TPlane p;
  if (!p1.getPlane(p)) return false;
  if (!intersect(p, s2, obj)) return false;
  TPoint3D pnt;
  TSegment3D sgm;
  if (obj.getPoint(pnt))
  {
    const TPose3D poseNeg = -p.getAsPose3DForcingOrigin(p1[0]);
    TPolygon3D projPoly;
    TPoint3D projPnt;
    project3D(p1, poseNeg, projPoly);
    project3D(pnt, poseNeg, projPnt);
    return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
  }
  else if (obj.getSegment(sgm))
    return intersectInCommonPlane<TPolygon2D, TSegment2D>(p1, s2, p, obj);
  return false;
}

bool math::intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj)
{
  TPlane p;
  if (!p1.getPlane(p)) return false;
  if (!intersect(p, r2, obj)) return false;
  TPoint3D pnt;
  if (obj.getPoint(pnt))
  {
    const TPose3D poseNeg = -p.getAsPose3DForcingOrigin(p1[0]);
    TPolygon3D projPoly;
    TPoint3D projPnt;
    project3D(p1, poseNeg, projPoly);
    project3D(pnt, poseNeg, projPnt);
    return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
  }
  else if (obj.isLine())
    return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, r2, p, obj);
  return false;
}

bool math::intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj)
{
  TPlane p;
  if (!p1.getPlane(p)) return false;
  if (!intersect(p, p2, obj)) return false;
  TLine3D ln;
  if (obj.isPlane())
  {
    // Polygon is inside the plane
    obj.data = p1;
    return true;
  }
  else if (obj.getLine(ln))
    return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, p, obj);
  return false;
}

// Auxiliary function2
bool intersectAux(
    const TPolygon3D& p1,
    const TPlane& pl1,
    const TPolygon3D& p2,
    const TPlane& pl2,
    TObject3D& obj)
{
  if (!intersect(pl1, pl2, obj)) return false;
  TLine3D ln;
  if (obj.isPlane())
    return intersectInCommonPlane<TPolygon2D, TPolygon2D>(
        p1, p2, pl1,
        obj);  // Polygons are on the same plane
  else if (obj.getLine(ln))
  {
    TObject3D obj1, obj2;
    if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, pl1, obj1)) return false;
    if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p2, ln, pl2, obj2)) return false;
    TPoint3D po1, po2;
    TSegment3D s1, s2;
    if (obj1.getPoint(po1))
      s1 = TSegment3D(po1, po1);
    else
      obj1.getSegment(s1);
    if (obj2.getPoint(po2))
      s2 = TSegment3D(po2, po2);
    else
      obj2.getSegment(s2);
    return intersectInCommonLine(s1, s2, ln, obj);
  }
  return false;
}

bool compatibleBounds(
    const TPoint3D& min1, const TPoint3D& max1, const TPoint3D& min2, const TPoint3D& max2)
{
  for (size_t i = 0; i < 3; i++)
    if ((min1[i] > max2[i]) || (min2[i] > max1[i])) return false;
  return true;
}
// End of auxiliary functions

bool math::intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj)
{
  TPoint3D min1, max1, min2, max2;
  getPrismBounds(p1, min1, max1);
  getPrismBounds(p2, min2, max2);
  if (!compatibleBounds(min1, max1, min2, max2)) return false;
  TPlane pl1, pl2;
  if (!p1.getPlane(pl1)) return false;
  if (!p2.getPlane(pl2)) return false;
  return intersectAux(p1, pl1, p2, pl2, obj);
}

// Auxiliary function
inline void getPlanes(const std::vector<TPolygon3D>& polys, std::vector<TPlane>& planes)
{
  size_t N = polys.size();
  planes.resize(N);
  for (size_t i = 0; i < N; i++) getRegressionPlane(polys[i], planes[i]);
}

// Auxiliary functions
void getMinAndMaxBounds(
    const std::vector<TPolygon3D>& v1, std::vector<TPoint3D>& minP, std::vector<TPoint3D>& maxP)
{
  minP.resize(0);
  maxP.resize(0);
  size_t N = v1.size();
  minP.reserve(N);
  maxP.reserve(N);
  TPoint3D p1, p2;
  for (const auto& it : v1)
  {
    getPrismBounds(it, p1, p2);
    minP.push_back(p1);
    maxP.push_back(p2);
  }
}

size_t math::intersect(
    const std::vector<TPolygon3D>& v1,
    const std::vector<TPolygon3D>& v2,
    CSparseMatrixTemplate<TObject3D>& objs)
{
  std::vector<TPlane> w1, w2;
  getPlanes(v1, w1);
  getPlanes(v2, w2);
  std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
  getMinAndMaxBounds(v1, minBounds1, maxBounds1);
  getMinAndMaxBounds(v2, minBounds2, maxBounds2);
  size_t M = v1.size(), N = v2.size();
  objs.clear();
  objs.resize(M, N);
  TObject3D obj;
  for (size_t i = 0; i < M; i++)
    for (size_t j = 0; j < N; j++)
      if (!compatibleBounds(minBounds1[i], maxBounds1[i], minBounds2[j], maxBounds2[j]))
        continue;
      else if (intersectAux(v1[i], w1[i], v2[j], w2[j], obj))
        objs(i, j) = obj;
  return objs.getNonNullElements();
}

size_t math::intersect(
    const std::vector<TPolygon3D>& v1,
    const std::vector<TPolygon3D>& v2,
    std::vector<TObject3D>& objs)
{
  objs.resize(0);
  std::vector<TPlane> w1, w2;
  getPlanes(v1, w1);
  getPlanes(v2, w2);
  std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
  getMinAndMaxBounds(v1, minBounds1, maxBounds1);
  getMinAndMaxBounds(v2, minBounds2, maxBounds2);
  TObject3D obj;
  auto itP1 = w1.begin();
  auto itMin1 = minBounds1.begin();
  auto itMax1 = maxBounds1.begin();
  for (auto it1 = v1.begin(); it1 != v1.end(); ++it1, ++itP1, ++itMin1, ++itMax1)
  {
    const TPolygon3D& poly1 = *it1;
    const TPlane& plane1 = *itP1;
    auto itP2 = w2.begin();
    const TPoint3D &min1 = *itMin1, max1 = *itMax1;
    auto itMin2 = minBounds2.begin();
    auto itMax2 = maxBounds2.begin();
    for (auto it2 = v2.begin(); it2 != v2.end(); ++it2, ++itP2, ++itMin2, ++itMax2)
      if (!compatibleBounds(min1, max1, *itMin2, *itMax2))
        continue;
      else if (intersectAux(poly1, plane1, *it2, *itP2, obj))
        objs.push_back(obj);
  }
  return objs.size();
}

bool math::intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj)
{
  TPoint2D p1, p2;
  TSegment2D s1, s2;
  TLine2D l1, l2;
  TPolygon2D po1, po2;
  if (o1.getPoint(p1))
  {
    obj.data = p1;
    if (o2.getPoint(p2))
      return distance(p1, p2) < geometryEpsilon;
    else if (o2.getSegment(s2))
      return s2.contains(p1);
    else if (o2.getLine(l2))
      return l2.contains(p1);
    else if (o2.getPolygon(po2))
      return po2.contains(p1);  // else return false;
  }
  else if (o1.getSegment(s1))
  {
    if (o2.getPoint(p2))
    {
      if (s1.contains(p2))
      {
        obj.data = p2;
        return true;
      }  // else return false;
    }
    else if (o2.getSegment(s2))
      return intersect(s1, s2, obj);
    else if (o2.getLine(l2))
      return intersect(s1, l2, obj);
    else if (o2.getPolygon(po2))
      return intersect(s1, po2, obj);  // else return false;
  }
  else if (o1.getLine(l1))
  {
    if (o2.getPoint(p2))
    {
      if (l1.contains(p2))
      {
        obj.data = p2;
        return true;
      }  // else return false;
    }
    else if (o2.getSegment(s2))
      return intersect(l1, s2, obj);
    else if (o2.getLine(l2))
      return intersect(l1, l2, obj);
    else if (o2.getPolygon(po2))
      return intersect(l1, po2, obj);  // else return false;
  }
  else if (o1.getPolygon(po1))
  {
    if (o2.getPoint(p2))
    {
      if (po1.contains(p2))
      {
        obj.data = p2;
        return true;
      }  // else return false;
    }
    else if (o2.getSegment(s2))
      return intersect(po1, s2, obj);
    else if (o2.getLine(l2))
      return intersect(po1, l2, obj);
    else if (o2.getPolygon(po2))
      return intersect(po1, po2, obj);  // else return false;
  }                                     // else return false;
  return false;
}

bool math::intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj)
{
  TPoint3D p1, p2;
  TSegment3D s1, s2;
  TLine3D l1, l2;
  TPolygon3D po1, po2;
  TPlane pl1, pl2;
  if (o1.getPoint(p1))
  {
    obj.data = p1;
    if (o2.getPoint(p2))
      return distance(p1, p2) < geometryEpsilon;
    else if (o2.getSegment(s2))
      return s2.contains(p1);
    else if (o2.getLine(l2))
      return l2.contains(p1);
    else if (o2.getPolygon(po2))
      return po2.contains(p1);
    else if (o2.getPlane(pl2))
      return pl2.contains(p1);  // else return false;
  }
  else if (o1.getSegment(s1))
  {
    if (o2.getPoint(p2))
    {
      if (s1.contains(p2))
      {
        obj.data = p2;
        return true;
      }  // else return false;
    }
    else if (o2.getSegment(s2))
      return intersect(s1, s2, obj);
    else if (o2.getLine(l2))
      return intersect(s1, l2, obj);
    else if (o2.getPolygon(po2))
      return intersect(s1, po2, obj);
    else if (o2.getPlane(pl2))
      return intersect(s1, pl2, obj);  // else return false;
  }
  else if (o1.getLine(l1))
  {
    if (o2.getPoint(p2))
    {
      if (l1.contains(p2))
      {
        obj.data = p2;
        return true;
      }  // else return false;
    }
    else if (o2.getSegment(s2))
      return intersect(l1, s2, obj);
    else if (o2.getLine(l2))
      return intersect(l1, l2, obj);
    else if (o2.getPolygon(po2))
      return intersect(l1, po2, obj);
    else if (o2.getPlane(pl2))
      return intersect(l2, pl2, obj);  // else return false;
  }
  else if (o1.getPolygon(po1))
  {
    if (o2.getPoint(p2))
    {
      if (po1.contains(p2))
      {
        obj.data = p2;
        return true;
      }  // else return false;
    }
    else if (o2.getSegment(s2))
      return intersect(po1, s2, obj);
    else if (o2.getLine(l2))
      return intersect(po1, l2, obj);
    else if (o2.getPolygon(po2))
      return intersect(po1, po2, obj);
    else if (o2.getPlane(pl2))
      return intersect(po1, pl2, obj);  // else return false;
  }
  else if (o1.getPlane(pl1))
  {
    if (o2.getPoint(p2))
    {
      if (pl1.contains(p2))
      {
        obj.data = p2;
        return true;
      }  // else return false;
    }
    else if (o2.getSegment(s2))
      return intersect(pl1, s2, obj);
    else if (o2.getLine(l2))
      return intersect(pl1, l2, obj);
    else if (o2.getPlane(pl2))
      return intersect(pl1, pl2, obj);  // else return false;
  }                                     // else return false;
  return false;
}

double math::distance(const TPoint2D& p1, const TPoint2D& p2)
{
  double dx = p2.x - p1.x;
  double dy = p2.y - p1.y;
  return sqrt(dx * dx + dy * dy);
}

double math::distance(const TPoint3D& p1, const TPoint3D& p2)
{
  double dx = p2.x - p1.x;
  double dy = p2.y - p1.y;
  double dz = p2.z - p1.z;
  return sqrt(dx * dx + dy * dy + dz * dz);
}

void math::getRectangleBounds(const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax)
{
  size_t N = poly.size();
  if (N < 1) throw logic_error("Empty polygon");
  pMin = poly[0];
  pMax = poly[0];
  for (size_t i = 1; i < N; i++)
  {
    pMin.x = min(pMin.x, poly[i].x);
    pMin.y = min(pMin.y, poly[i].y);
    pMax.x = max(pMax.x, poly[i].x);
    pMax.y = max(pMax.y, poly[i].y);
  }
}

double math::distance(const TLine2D& r1, const TLine2D& r2)
{
  if (std::abs(r1.coefs[0] * r2.coefs[1] - r2.coefs[0] * r1.coefs[1]) < geometryEpsilon)
  {
    // Lines are parallel
    size_t i1 = (std::abs(r1.coefs[0]) < geometryEpsilon) ? 0 : 1;
    TPoint2D p;
    p[i1] = 0.0;
    p[1 - i1] = -r1.coefs[2] / r1.coefs[1 - i1];
    return r2.distance(p);
  }
  else
    return 0;  // Lines cross in some point
}

double math::distance(const TLine3D& r1, const TLine3D& r2)
{
  if (std::abs(getAngle(r1, r2)) < geometryEpsilon)
    return r1.distance(r2.pBase);  // Lines are parallel
  else
  {
    // We build a plane parallel to r2 which contains r1
    TPlane p;
    crossProduct3D(r1.director, r2.director, p.coefs);
    p.coefs[3] = -(p.coefs[0] * r1.pBase[0] + p.coefs[1] * r1.pBase[1] + p.coefs[2] * r1.pBase[2]);
    return p.distance(r2.pBase);
  }
}

double math::distance(const TPlane& p1, const TPlane& p2)
{
  if (std::abs(getAngle(p1, p2)) < geometryEpsilon)
  {
    // Planes are parallel
    TPoint3D p(0, 0, 0);
    for (size_t i = 0; i < 3; i++)
      if (std::abs(p1.coefs[i]) >= geometryEpsilon)
      {
        p[i] = -p1.coefs[3] / p1.coefs[i];
        break;
      }
    return p2.distance(p);
  }
  else
    return 0;  // Planes cross in a line
}

double math::distance([[maybe_unused]] const TPolygon2D& p1, [[maybe_unused]] const TPolygon2D& p2)
{
  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TPolygon2D)");
}

double math::distance([[maybe_unused]] const TPolygon2D& p1, [[maybe_unused]] const TSegment2D& s2)
{
  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TSegment)");
}

double math::distance([[maybe_unused]] const TPolygon2D& p1, [[maybe_unused]] const TLine2D& l2)
{
  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TLine2D)");
}

double math::distance([[maybe_unused]] const TPolygon3D& p1, [[maybe_unused]] const TPolygon3D& p2)
{
  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPolygon3D");
}

double math::distance([[maybe_unused]] const TPolygon3D& p1, [[maybe_unused]] const TSegment3D& s2)
{
  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TSegment3D");
}

double math::distance([[maybe_unused]] const TPolygon3D& p1, [[maybe_unused]] const TLine3D& l2)
{
  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TLine3D");
}

double math::distance([[maybe_unused]] const TPolygon3D& po, [[maybe_unused]] const TPlane& pl)
{
  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPlane");
}

void math::getPrismBounds(const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax)
{
  size_t N = poly.size();
  if (N < 1) throw logic_error("Empty polygon");
  pMin = poly[0];
  pMax = poly[0];
  for (size_t i = 1; i < N; i++)
  {
    pMin.x = min(pMin.x, poly[i].x);
    pMin.y = min(pMin.y, poly[i].y);
    pMin.z = min(pMin.z, poly[i].z);
    pMax.x = max(pMax.x, poly[i].x);
    pMax.y = max(pMax.y, poly[i].y);
    pMax.z = max(pMax.z, poly[i].z);
  }
}

void createPlaneFromPoseAndAxis(const TPose3D& pose, TPlane& plane, size_t axis)
{
  plane.coefs[3] = 0;
  CMatrixDouble44 m;
  pose.getHomogeneousMatrix(m);
  for (size_t i = 0; i < 3; i++)
  {
    plane.coefs[i] = m(i, axis);
    plane.coefs[3] -= plane.coefs[i] * m(i, 3);
  }
}

void math::createPlaneFromPoseXY(const TPose3D& pose, TPlane& plane)
{
  createPlaneFromPoseAndAxis(pose, plane, 2);
}

void math::createPlaneFromPoseXZ(const TPose3D& pose, TPlane& plane)
{
  createPlaneFromPoseAndAxis(pose, plane, 1);
}

void math::createPlaneFromPoseYZ(const TPose3D& pose, TPlane& plane)
{
  createPlaneFromPoseAndAxis(pose, plane, 0);
}

void math::createPlaneFromPoseAndNormal(
    const TPose3D& pose, const double (&normal)[3], TPlane& plane)
{
  plane.coefs[3] = 0;
  CMatrixDouble44 m;
  pose.getHomogeneousMatrix(m);
  for (size_t i = 0; i < 3; i++)
  {
    plane.coefs[i] = 0;
    for (size_t j = 0; j < 3; j++) plane.coefs[i] += normal[j] * m(i, j);
    plane.coefs[3] -= plane.coefs[i] * m(i, 3);
  }
}

CMatrixDouble44 math::generateAxisBaseFromDirectionAndAxis(const TVector3D& vec, uint8_t coord)
{
  CMatrixDouble44 m;
  // Assumes vector is unitary.
  // coord: 0=x, 1=y, 2=z.
  const uint8_t coord1 = (coord + 1) % 3;
  const uint8_t coord2 = (coord + 2) % 3;
  m.setZero();
  m(3, 3) = 1.0;
  for (size_t i = 0; i < 3; i++) m(i, coord) = vec[i];
  m(0, coord1) = 0;
  double h = hypot(vec[1], vec[2]);
  if (h < geometryEpsilon)
  {
    m(1, coord1) = 1;
    m(2, coord1) = 0;
  }
  else
  {
    m(1, coord1) = -vec[2] / h;
    m(2, coord1) = vec[1] / h;
  }
  m(0, coord2) = m(1, coord) * m(2, coord1) - m(2, coord) * m(1, coord1);
  m(1, coord2) = m(2, coord) * m(0, coord1) - m(0, coord) * m(2, coord1);
  m(2, coord2) = m(0, coord) * m(1, coord1) - m(1, coord) * m(0, coord1);
  return m;
}

double math::getRegressionLine(const vector<TPoint2D>& points, TLine2D& line)
{
  CVectorFixedDouble<2> means;
  CMatrixDouble22 covars;
  covariancesAndMean(points, covars, means);

  std::vector<double> eigenVals;
  CMatrixDouble22 eigenVecs;
  covars.eig_symmetric(eigenVecs, eigenVals);

  // size_t selected = (eigenVal[0] >= eigenVal[1]) ? 0 : 1;
  line.coefs[0] = -eigenVecs(1, 1);  // 1: largest eigenVal
  line.coefs[1] = eigenVecs(0, 1);
  line.coefs[2] = -line.coefs[0] * means[0] - line.coefs[1] * means[1];
  return std::sqrt(eigenVals[0] / eigenVals[1]);
}

template <class T>
inline size_t getIndexOfMin(const T& e1, const T& e2, const T& e3)
{
  return (e1 < e2) ? ((e1 < e3) ? 0 : 2) : ((e2 < e3) ? 1 : 2);
}

double math::getRegressionLine(const vector<TPoint3D>& points, TLine3D& line)
{
  CVectorFixedDouble<3> means;
  CMatrixDouble33 covars;
  covariancesAndMean(points, covars, means);

  std::vector<double> eigenVal;
  CMatrixDouble33 eigenVec;
  covars.eig_symmetric(eigenVec, eigenVal);

  const size_t selected = 2;  // sorted: max eigenvalue
  for (size_t i = 0; i < 3; i++)
  {
    line.pBase[i] = means[i];
    line.director[i] = eigenVec(i, selected);
  }
  size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
  return std::sqrt((eigenVal[i1] + eigenVal[i2]) / eigenVal[selected]);
}

double math::getRegressionPlane(const vector<TPoint3D>& points, TPlane& plane)
{
  vector<double> means;
  CMatrixDouble33 covars;
  covariancesAndMean(points, covars, means);

  std::vector<double> eigenVal;
  CMatrixDouble33 eigenVec;
  covars.eig_symmetric(eigenVec, eigenVal);

  for (size_t i = 0; i < 3; ++i)
  {
    if (eigenVal[i] < 0 && std::abs(eigenVal[i]) < geometryEpsilon) eigenVal[i] = 0;
  }

  if (std::abs(eigenVal.at(1)) < geometryEpsilon)
    THROW_EXCEPTION("Points are aligned, cannot fit a plane (infinite solutions)");

  const size_t selected = 0;  // sorted: minimum eigenVal
  plane.coefs[3] = 0;
  for (size_t i = 0; i < 3; i++)
  {
    plane.coefs[i] = eigenVec(i, selected);
    plane.coefs[3] -= plane.coefs[i] * means[i];
  }
  size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
  const auto condNumbInv = eigenVal[selected] / (eigenVal[i1] + eigenVal[i2]);
  return condNumbInv;
}

void math::assemblePolygons(const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys)
{
  std::vector<TSegment3D> tmp;
  assemblePolygons(segms, polys, tmp);
}

struct MatchingVertex
{
  size_t seg1;
  size_t seg2;
  bool seg1Point;  // true for point2, false for point1
  bool seg2Point;  // same
  MatchingVertex() = default;
  MatchingVertex(size_t s1, size_t s2, bool s1p, bool s2p) :
      seg1(s1), seg2(s2), seg1Point(s1p), seg2Point(s2p)
  {
  }
};
class FCreatePolygon
{
 public:
  const std::vector<TSegment3D>& segs;
  FCreatePolygon(const std::vector<TSegment3D>& s) : segs(s) {}
  TPolygon3D operator()(const std::vector<MatchingVertex>& vertices)
  {
    TPolygon3D res;
    size_t N = vertices.size();
    res.reserve(N);
    for (const auto& vertice : vertices)
      res.push_back(segs[vertice.seg2][vertice.seg2Point ? 1 : 0]);
    return res;
  }
};
inline bool firstOrNonPresent(size_t i, const std::vector<MatchingVertex>& v)
{
  if (v.size() > 0 && v[0].seg1 == i) return true;
  for (const auto& it : v)
    if (it.seg1 == i || it.seg2 == i) return false;
  return true;
}
bool depthFirstSearch(
    const CSparseMatrixTemplate<unsigned char>& mat,
    std::vector<std::vector<MatchingVertex>>& res,
    std::vector<bool>& used,
    size_t searching,
    unsigned char mask,
    std::vector<MatchingVertex>& current)
{
  for (size_t i = 0; i < mat.cols(); i++)
    if (!used[i] && mat.isNotNull(searching, i))
    {
      unsigned char match = mat(searching, i) & mask;
      if (!match)
        continue;
      else if (firstOrNonPresent(i, current))
      {
        bool s1p, s2p;
        if (true == (s1p = (!(match & 3)))) match >>= 2;
        s2p = !(match & 1);
        if (current.size() >= 2 && current[0].seg1 == i)
        {
          if (s2p != current[0].seg1Point)
          {
            current.emplace_back(searching, i, s1p, s2p);
            for (auto it = current.begin(); it != current.end(); ++it) used[it->seg2] = true;
            res.push_back(current);
            return true;
          }
          else
            continue;  // Strange shape... not a polygon, although
                       // it'll be without the first segment
        }
        else
        {
          current.emplace_back(searching, i, s1p, s2p);
          if (depthFirstSearch(mat, res, used, i, s2p ? 0x3 : 0xC, current)) return true;
          current.pop_back();
        }
      }
    }
  // No match has been found. Backtrack
  return false;
}
void depthFirstSearch(
    const CSparseMatrixTemplate<unsigned char>& mat,
    std::vector<std::vector<MatchingVertex>>& res,
    std::vector<bool>& used)
{
  vector<MatchingVertex> cur;
  for (size_t i = 0; i < used.size(); i++)
    if (!used[i])
      if (depthFirstSearch(mat, res, used, i, 0xf, cur)) cur.clear();
}
void math::assemblePolygons(
    const std::vector<TSegment3D>& segms,
    std::vector<TPolygon3D>& polys,
    std::vector<TSegment3D>& remainder)
{
  std::vector<TSegment3D> tmp;
  tmp.reserve(segms.size());
  for (const auto& segm : segms)
    if (segm.length() >= geometryEpsilon)
      tmp.push_back(segm);
    else
      remainder.push_back(segm);
  size_t N = tmp.size();
  CSparseMatrixTemplate<unsigned char> matches(N, N);
  for (size_t i = 0; i < N - 1; i++)
    for (size_t j = i + 1; j < N; j++)
    {
      if (distance(tmp[i].point1, tmp[j].point1) < geometryEpsilon)
      {
        matches(i, j) |= 1;
        matches(j, i) |= 1;
      }
      if (distance(tmp[i].point1, tmp[j].point2) < geometryEpsilon)
      {
        matches(i, j) |= 2;
        matches(j, i) |= 4;
      }
      if (distance(tmp[i].point2, tmp[j].point1) < geometryEpsilon)
      {
        matches(i, j) |= 4;
        matches(j, i) |= 2;
      }
      if (distance(tmp[i].point2, tmp[j].point2) < geometryEpsilon)
      {
        matches(i, j) |= 8;
        matches(j, i) |= 8;
      }
    }
  std::vector<std::vector<MatchingVertex>> results;
  std::vector<bool> usedSegments(N, false);
  depthFirstSearch(matches, results, usedSegments);
  polys.resize(results.size());
  transform(results.begin(), results.end(), polys.begin(), FCreatePolygon(segms));
  for (size_t i = 0; i < N; i++)
    if (!usedSegments[i]) remainder.push_back(tmp[i]);
}

namespace
{
std::vector<TPolygon3D> getPolygons(
    const std::vector<TObject3D>& objs,
    const mrpt::optional_ref<std::vector<mrpt::math::TObject3D>>& others = std::nullopt)
{
  std::vector<TPolygon3D> r;
  for (const auto& o : objs)
  {
    if (o.isPolygon())
      r.emplace_back(o.getAs<TPolygon3D>());
    else if (others)
      others->get().push_back(o);
  }
  return r;
}
std::vector<TSegment3D> getSegments(
    const std::vector<TObject3D>& objs,
    const mrpt::optional_ref<std::vector<mrpt::math::TObject3D>>& others = std::nullopt)

{
  std::vector<TSegment3D> r;
  for (const auto& o : objs)
  {
    if (o.isSegment())
      r.emplace_back(o.getAs<TSegment3D>());
    else if (others)
      others->get().push_back(o);
  }
  return r;
}
}  // namespace

void math::assemblePolygons(const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
{
  polys = getPolygons(objs);
  const auto segms = getSegments(objs);
  assemblePolygons(segms, polys);
}

void math::assemblePolygons(
    const std::vector<TObject3D>& objs,
    std::vector<TPolygon3D>& polys,
    std::vector<TObject3D>& remainder)
{
  std::vector<TObject3D> tmp;
  polys = getPolygons(objs, tmp);

  std::vector<TSegment3D> remainderSgms;
  const auto sgms = getSegments(tmp, remainder);
  assemblePolygons(sgms, polys, remainderSgms);
  for (const auto& o : remainderSgms) remainder.emplace_back(TObject3D::From(o));
}

void math::assemblePolygons(
    const std::vector<TObject3D>& objs,
    std::vector<TPolygon3D>& polys,
    std::vector<TSegment3D>& remainder1,
    std::vector<TObject3D>& remainder2)
{
  std::vector<TObject3D> tmp;
  polys = getPolygons(objs, tmp);
  std::vector<TSegment3D> sgms = getSegments(tmp, remainder2);
  assemblePolygons(sgms, polys, remainder1);
}

bool intersect(const TLine2D& l1, const TSegmentWithLine& s2, TObject2D& obj)
{
  if (intersect(l1, s2.line, obj))
  {
    if (obj.isLine())
    {
      obj.data = s2.segment;
      return true;
    }
    else
    {
      TPoint2D p;
      obj.getPoint(p);
      return s2.segment.contains(p);
    }
  }
  else
    return false;
}

inline bool intersect(const TSegmentWithLine& s1, const TLine2D& l2, TObject2D& obj)
{
  return intersect(l2, s1, obj);
}

bool math::splitInConvexComponents(const TPolygon2D& poly, vector<TPolygon2D>& components)
{
  components.clear();
  size_t N = poly.size();
  if (N <= 3) return false;
  vector<TSegmentWithLine> segms(N);
  for (size_t i = 0; i < N - 1; i++) segms[i] = TSegmentWithLine(poly[i], poly[i + 1]);
  segms[N - 1] = TSegmentWithLine(poly[N - 1], poly[0]);
  TObject2D obj;
  TPoint2D pnt;
  for (size_t i = 0; i < N; i++)
  {
    size_t ii = (i + 2) % N, i_ = (i + N - 1) % N;
    for (size_t j = ii; j != i_; j = (j + 1) % N)
      if (intersect(segms[i].line, segms[j], obj) && obj.getPoint(pnt))
      {
        TSegmentWithLine sTmp = TSegmentWithLine(
            pnt,
            segms[i].segment
                [(distance(pnt, segms[i].segment.point1) < distance(pnt, segms[i].segment.point2))
                     ? 0
                     : 1]);
        bool cross = false;
        TPoint2D pTmp;
        for (size_t k = 0; (k < N) && !cross; k++)
          if (intersect(sTmp, segms[k], obj))
          {
            if (obj.getPoint(pTmp) && (distance(pTmp, sTmp.segment[0]) >= geometryEpsilon) &&
                (distance(pTmp, sTmp.segment[1]) >= geometryEpsilon))
              cross = true;
          }
        if (cross) continue;
        // At this point, we have a suitable point, although we must
        // check if the division is right.
        // We do this by evaluating, in the expanded segment's line, the
        // next and previous points. If both signs differ, proceed.
        if (sign(segms[i].line.evaluatePoint(poly[(i + N - 1) % N])) ==
            sign(segms[i].line.evaluatePoint(poly[(i + 2) % N])))
          continue;
        TPolygon2D p1, p2;
        if (i > j)
        {
          p1.insert(p1.end(), poly.begin() + i + 1, poly.end());
          p1.insert(p1.end(), poly.begin(), poly.begin() + j + 1);
          p2.insert(p2.end(), poly.begin() + j + 1, poly.begin() + i + 1);
        }
        else
        {
          p1.insert(p1.end(), poly.begin() + i + 1, poly.begin() + j + 1);
          p2.insert(p2.end(), poly.begin() + j + 1, poly.end());
          p2.insert(p2.end(), poly.begin(), poly.begin() + i + 1);
        }
        if (distance(*(p1.rbegin()), pnt) >= geometryEpsilon) p1.push_back(pnt);
        if (distance(*(p2.rbegin()), pnt) >= geometryEpsilon) p2.push_back(pnt);
        p1.removeRedundantVertices();
        p2.removeRedundantVertices();
        vector<TPolygon2D> tempComps;
        if (splitInConvexComponents(p1, tempComps))
          components.insert(components.end(), tempComps.begin(), tempComps.end());
        else
          components.push_back(p1);
        if (splitInConvexComponents(p2, tempComps))
          components.insert(components.end(), tempComps.begin(), tempComps.end());
        else
          components.push_back(p2);
        return true;
      }
  }
  return false;
}

class FUnprojectPolygon2D
{
 public:
  const TPose3D& pose;
  TPolygon3D tmp1, tmp2;
  FUnprojectPolygon2D(const TPose3D& p) : pose(p), tmp1(0), tmp2(0) {}
  TPolygon3D operator()(const TPolygon2D& poly2D)
  {
    tmp1 = TPolygon3D(poly2D);
    project3D(tmp1, pose, tmp2);
    return tmp2;
  }
};
bool math::splitInConvexComponents(const TPolygon3D& poly, vector<TPolygon3D>& components)
{
  TPlane p;
  if (!poly.getPlane(p)) THROW_EXCEPTION("Polygon is skew");
  TPose3D pose1;
  p.getAsPose3DForcingOrigin(poly[0], pose1);
  const TPose3D pose2 = -pose1;
  TPolygon3D polyTmp;
  project3D(poly, pose2, polyTmp);
  TPolygon2D poly2D = TPolygon2D(polyTmp);
  vector<TPolygon2D> components2D;
  if (splitInConvexComponents(poly2D, components2D))
  {
    components.resize(components2D.size());
    transform(
        components2D.begin(), components2D.end(), components.begin(), FUnprojectPolygon2D(pose1));
    return true;
  }
  else
    return false;
}

void math::getSegmentBisector(const TSegment2D& sgm, TLine2D& bis)
{
  TPoint2D p;
  sgm.getCenter(p);
  bis.coefs[0] = sgm.point2.x - sgm.point1.x;
  bis.coefs[1] = sgm.point2.y - sgm.point1.y;
  bis.coefs[2] = -bis.coefs[0] * p.x - bis.coefs[1] * p.y;
  bis.unitarize();
}

void math::getSegmentBisector(const TSegment3D& sgm, TPlane& bis)
{
  TPoint3D p;
  sgm.getCenter(p);
  bis.coefs[0] = sgm.point2.x - sgm.point1.x;
  bis.coefs[1] = sgm.point2.y - sgm.point1.y;
  bis.coefs[2] = sgm.point2.z - sgm.point1.z;
  bis.coefs[2] = -bis.coefs[0] * p.x - bis.coefs[1] * p.y - bis.coefs[2] * p.z;
  bis.unitarize();
}

void math::getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis)
{
  TPoint2D p;
  TObject2D obj;
  if (!intersect(l1, l2, obj))
  {
    // Both lines are parallel
    double mod1 = sqrt(square(l1.coefs[0]) + square(l1.coefs[1]));
    double mod2 = sqrt(square(l2.coefs[0]) + square(l2.coefs[2]));
    bis.coefs[0] = l1.coefs[0] / mod1;
    bis.coefs[1] = l1.coefs[1] / mod1;
    bool sameSign;
    if (std::abs(bis.coefs[0]) < geometryEpsilon)
      sameSign = (l1.coefs[1] * l2.coefs[1]) > 0;
    else
      sameSign = (l1.coefs[0] * l2.coefs[0]) > 0;
    if (sameSign)
      bis.coefs[2] = (l1.coefs[2] / mod1) + (l2.coefs[2] / mod2);
    else
      bis.coefs[2] = (l1.coefs[2] / mod1) - (l2.coefs[2] / mod2);
  }
  else if (obj.getPoint(p))
  {
    // Both lines cross
    double ang1 = atan2(-l1.coefs[0], l1.coefs[1]);
    double ang2 = atan2(-l2.coefs[0], l2.coefs[1]);
    double ang = (ang1 + ang2) / 2;
    bis.coefs[0] = -sin(ang);
    bis.coefs[1] = cos(ang);
    bis.coefs[2] = -bis.coefs[0] * p.x - bis.coefs[1] * p.y;
  }
  else
  {
    bis = l1;
    bis.unitarize();
  }
}

void math::getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis)
{
  TPlane p = TPlane(l1, l2);  // May throw an exception
  TLine3D l1P, l2P;
  TLine2D bis2D;
  TPose3D pose, pose2;
  p.getAsPose3D(pose);
  pose2 = -pose;
  project3D(l1, pose2, l1P);
  project3D(l2, pose2, l2P);
  getAngleBisector(TLine2D(l1P), TLine2D(l2P), bis2D);
  project3D(TLine3D(bis2D), pose, bis);
}

bool math::traceRay(const vector<TPolygonWithPlane>& vec, const TPose3D& pose, double& dist)
{
  dist = HUGE_VAL;
  double nDist = 0;
  TLine3D lin;
  createFromPoseX(pose, lin);
  lin.unitarize();
  bool res = false;
  for (const auto& it : vec)
    if (::intersect(it, lin, nDist, dist))
    {
      res = true;
      dist = nDist;
    }
  return res;
}

CMatrixDouble33 mrpt::math::generateAxisBaseFromDirection(double dx, double dy, double dz)
{
  MRPT_START

  if (std::abs(dx) < 1e-10 && std::abs(dy) < 1e-10 && std::abs(dz) < 1e-10)
    THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0);");

  CMatrixDouble33 P;

  // 1st vector:
  double n_xy = square(dx) + square(dy);
  double n = sqrt(n_xy + square(dz));
  n_xy = sqrt(n_xy);
  P(0, 0) = dx / n;
  P(1, 0) = dy / n;
  P(2, 0) = dz / n;

  // 2nd perpendicular vector:
  if (fabs(dx) > 1e-4 || fabs(dy) > 1e-4)
  {
    P(0, 1) = -dy / n_xy;
    P(1, 1) = dx / n_xy;
    P(2, 1) = 0;
  }
  else
  {
    // Any vector in the XY plane will work:
    P(0, 1) = 1;
    P(1, 1) = 0;
    P(2, 1) = 0;
  }

  // 3rd perpendicular vector: cross product of the two last vectors:
  Eigen::Vector3d c2;
  crossProduct3D(P.col(0), P.col(1), c2);
  P.col(2) = c2;

  return P;
  MRPT_END
}
