GeoAlgo.h

Go to the documentation of this file.

#ifndef BASICTOOL_GEOALGO_H
#define BASICTOOL_GEOALGO_H

#include "larcorealg/GeoAlgo/GeoAABox.h"
#include "larcorealg/GeoAlgo/GeoHalfLine.h"
#include "larcorealg/GeoAlgo/GeoLine.h"
#include "larcorealg/GeoAlgo/GeoLineSegment.h"
#include "larcorealg/GeoAlgo/GeoSphere.h"
#include "larcorealg/GeoAlgo/GeoTrajectory.h"
#include "larcorealg/GeoAlgo/GeoVector.h"

#include <vector>

namespace geoalgo {

  class GeoAlgo {

  public:
    //
    // Intersections
    //

    std::vector<Point_t> Intersection(AABox_t const& box,
                                      HalfLine_t const& line,
                                      bool back = false) const;
    std::vector<Point_t> Intersection(HalfLine_t const& line,
                                      AABox_t const& box,
                                      bool back = false) const
    {
      return Intersection(box, line, back);
    }

    std::vector<Point_t> Intersection(AABox_t const& box, LineSegment_t const& l) const;
    std::vector<Point_t> Intersection(LineSegment_t const& l, AABox_t const& box) const
    {
      return Intersection(box, l);
    }

    std::vector<Point_t> Intersection(AABox_t const& box, Trajectory_t const& trj) const;
    std::vector<Point_t> Intersection(Trajectory_t const& trj, AABox_t const& box) const
    {
      return Intersection(box, trj);
    }

    LineSegment_t BoxOverlap(AABox_t const& box, HalfLine_t const& line) const;
    LineSegment_t BoxOverlap(HalfLine_t const& line, AABox_t const& box) const
    {
      return BoxOverlap(box, line);
    }

    Trajectory_t BoxOverlap(AABox_t const& box, Trajectory_t const& trj) const;
    Trajectory_t BoxOverlap(Trajectory_t const& trj, AABox_t const& box) const
    {
      return BoxOverlap(box, trj);
    }

    //************************************************
    //CLOSEST APPROACH BETWEEN POINT AND INFINITE LINE
    //************************************************
    // min distance between point and infinite line
    double SqDist(Line_t const& line, Point_t const& pt) const
    {
      pt.compat(line.Pt1());
      return _SqDist_(line, pt);
    }
    // min distance between point and infinite line
    double SqDist(Point_t const& pt, Line_t const& line) const { return SqDist(line, pt); }
    // closest point (on infinite line) from point
    Point_t ClosestPt(Line_t const& line, Point_t const& pt) const
    {
      pt.compat(line.Pt1());
      return _ClosestPt_(pt, line);
    }
    // closest point (on infinite line) from point
    Point_t ClosestPt(Point_t const& pt, Line_t const& line) const { return _ClosestPt_(pt, line); }

    //*******************************************
    //CLOSEST APPROACH BETWEEN TWO INFINITE LINES
    //*******************************************
    // Closest approach between two infinite line segments - keep track of closest approach points
    double SqDist(Line_t const& l1, Line_t const& l2, Point_t& L1, Point_t& L2) const
    {
      l1.Pt1().compat(l2.Pt1());
      return _SqDist_(l1, l2, L1, L2);
    }
    // Closest approach between two infinite line segments - don't keep track of closest approach points
    double SqDist(Line_t const& l1, Line_t const& l2) const
    {
      Point_t L1;
      Point_t L2;
      return SqDist(l1, l2, L1, L2);
    }

    //************************************************
    //CLOSEST APPROACH BETWEEN TWO HALF-INFINITE LINES
    //************************************************
    // Closest approach between two infinite line segments - keep track of closest approach points
    double SqDist(HalfLine_t const& l1, HalfLine_t const& l2, Point_t& L1, Point_t& L2) const
    {
      l1.Start().compat(l2.Start());
      return _SqDist_(l1, l2, L1, L2);
    }
    // Closest approach between two infinite line segments - don't keep track of closest approach points
    double SqDist(HalfLine_t const& l1, HalfLine_t const& l2) const
    {
      Point_t L1;
      Point_t L2;
      return SqDist(l1, l2, L1, L2);
    }

    //******************************************
    //CLOSEST APPROACH BETWEEN TWO LINE SEGMENTS
    //******************************************
    double SqDist(LineSegment_t const& seg1,
                  LineSegment_t const& seg2,
                  Point_t& c1,
                  Point_t& c2) const
    {
      seg1.Start().compat(seg2.Start());
      return _SqDist_(seg1, seg2, c1, c2);
    }
    double SqDist(LineSegment_t const& seg1, LineSegment_t const& seg2) const
    {
      Point_t c1;
      Point_t c2;
      return SqDist(seg1, seg2, c1, c2);
    }

    //******************************************
    //CLOSEST APPROACH BETWEEN SEGMENT AND TRACK
    //******************************************
    double SqDist(LineSegment_t const& seg,
                  Trajectory_t const& trj,
                  Point_t& c1,
                  Point_t& c2) const;
    double SqDist(Trajectory_t const& trj, LineSegment_t const& seg, Point_t& c1, Point_t& c2) const
    {
      return SqDist(seg, trj, c1, c2);
    }
    double SqDist(Trajectory_t const& trj, LineSegment_t const& seg) const
    {
      Point_t c1;
      Point_t c2;
      return SqDist(seg, trj, c1, c2);
    }
    double SqDist(LineSegment_t const& seg, Trajectory_t const& trj) const
    {
      Point_t c1;
      Point_t c2;
      return SqDist(seg, trj, c1, c2);
    }

    //****************************************
    //CLOSEST APPROACH BETWEEN TRACK AND TRACK
    //****************************************
    double SqDist(Trajectory_t const& trj1,
                  Trajectory_t const& trj2,
                  Point_t& c1,
                  Point_t& c2) const;
    double SqDist(Trajectory_t const& trj1, Trajectory_t const& trj2) const
    {
      Point_t c1;
      Point_t c2;
      return SqDist(trj1, trj2, c1, c2);
    }

    //*****************************************************
    //CLOSEST APPROACH BETWEEN SEGMENT AND VECTOR OF TRACKS
    //*****************************************************
    double SqDist(LineSegment_t const& seg,
                  const std::vector<geoalgo::Trajectory_t>& trj,
                  Point_t& c1,
                  Point_t& c2,
                  int& trackIdx) const;
    double SqDist(const std::vector<geoalgo::Trajectory_t>& trj,
                  LineSegment_t const& seg,
                  Point_t& c1,
                  Point_t& c2,
                  int& trackIdx) const
    {
      return SqDist(seg, trj, c2, c1, trackIdx);
    }
    double SqDist(const std::vector<geoalgo::Trajectory_t>& trj, LineSegment_t const& seg) const
    {
      Point_t c1;
      Point_t c2;
      int trackIdx;
      return SqDist(seg, trj, c1, c2, trackIdx);
    }
    double SqDist(LineSegment_t const& seg, const std::vector<geoalgo::Trajectory_t>& trj) const
    {
      Point_t c1;
      Point_t c2;
      int trackIdx;
      return SqDist(seg, trj, c1, c2, trackIdx);
    }

    //*******************************************
    //CLOSEST APPROACH BETWEEN HALFLINE AND TRACK
    //*******************************************
    double SqDist(HalfLine_t const& hline, Trajectory_t const& trj, Point_t& c1, Point_t& c2) const;
    double SqDist(Trajectory_t const& trj, HalfLine_t const& hline, Point_t& c1, Point_t& c2) const
    {
      return SqDist(hline, trj, c2, c1);
    }
    double SqDist(Trajectory_t const& trj, HalfLine_t const& hline) const
    {
      Point_t c1;
      Point_t c2;
      return SqDist(hline, trj, c1, c2);
    }
    double SqDist(HalfLine_t const& hline, Trajectory_t const& trj) const
    {
      Point_t c1;
      Point_t c2;
      return SqDist(hline, trj, c1, c2);
    }

    //****************************************
    //CLOSEST APPROACH BETWEEN POINT AND TRACK
    //****************************************
    double SqDist(Point_t const& pt, Trajectory_t const& trj) const;
    double SqDist(Trajectory_t const& trj, Point_t const& pt) const { return SqDist(pt, trj); }
    Point_t ClosestPt(Point_t const& pt, Trajectory_t const& trj) const
    {
      int idx = 0;
      return ClosestPt(pt, trj, idx);
    }
    Point_t ClosestPt(Trajectory_t const& trj, Point_t const& pt) const
    {
      int idx = 0;
      return ClosestPt(pt, trj, idx);
    }
    Point_t ClosestPt(Point_t const& pt, Trajectory_t const& trj, int& idx) const;
    Point_t ClosestPt(Trajectory_t const& trj, Point_t const& pt, int& idx) const
    {
      return ClosestPt(pt, trj, idx);
    }

    //***************************************************
    //CLOSEST APPROACH BETWEEN POINT AND VECTOR OF TRACKS
    //***************************************************
    double SqDist(Point_t const& pt,
                  const std::vector<geoalgo::Trajectory_t>& trj,
                  int& trackIdx) const;
    double SqDist(const std::vector<geoalgo::Trajectory_t>& trj,
                  Point_t const& pt,
                  int& trackIdx) const
    {
      return SqDist(pt, trj, trackIdx);
    }
    double SqDist(Point_t const& pt, const std::vector<geoalgo::Trajectory_t>& trj) const
    {
      int trackIdx;
      return SqDist(pt, trj, trackIdx);
    }
    double SqDist(const std::vector<geoalgo::Trajectory_t>& trj, Point_t const& pt) const
    {
      int trackIdx;
      return SqDist(pt, trj, trackIdx);
    }
    Point_t ClosestPt(Point_t const& pt,
                      const std::vector<geoalgo::Trajectory_t>& trj,
                      int& trackIdx) const;
    Point_t ClosestPt(const std::vector<geoalgo::Trajectory_t>& trj,
                      Point_t const& pt,
                      int& trackIdx) const
    {
      return ClosestPt(pt, trj, trackIdx);
    }
    Point_t ClosestPt(Point_t const& pt, const std::vector<geoalgo::Trajectory_t>& trj) const
    {
      int trackIdx;
      return ClosestPt(pt, trj, trackIdx);
    }
    Point_t ClosestPt(const std::vector<geoalgo::Trajectory_t>& trj, Point_t const& pt) const
    {
      int trackIdx;
      return ClosestPt(pt, trj, trackIdx);
    }

    //***********************************************
    //CLOSEST APPROACH BETWEEN POINT AND LINE SEGMENT
    //***********************************************
    double SqDist(Point_t const& pt, LineSegment_t const& line) const
    {
      pt.compat(line.Start());
      return _SqDist_(pt, line);
    }
    double SqDist(LineSegment_t const& line, Point_t const& pt) const { return SqDist(pt, line); }
    Point_t ClosestPt(Point_t const& pt, LineSegment_t const& line) const
    {
      pt.compat(line.Start());
      return _ClosestPt_(pt, line);
    }
    Point_t ClosestPt(LineSegment_t const& line, Point_t const& pt) const
    {
      return ClosestPt(pt, line);
    }

    //********************************************
    //CLOSEST APPROACH BETWEEN POINT AND HALF LINE
    //********************************************
    double SqDist(Point_t const& pt, HalfLine_t const& line) const
    {
      pt.compat(line.Start());
      return _SqDist_(pt, line);
    }
    double SqDist(HalfLine_t const& line, Point_t const& pt) const { return SqDist(pt, line); }
    Point_t ClosestPt(Point_t const& pt, HalfLine_t const& line) const
    {
      pt.compat(line.Start());
      return _ClosestPt_(pt, line);
    }
    Point_t ClosestPt(HalfLine_t const& line, Point_t const& pt) const
    {
      return ClosestPt(pt, line);
    }

    //***************************************************
    //CLOSEST APPROACH BETWEEN HALF LINE AND LINE SEGMENT
    //***************************************************
    // half-line and line-segment. keep track of closest approach points
    double SqDist(HalfLine_t const& hline, LineSegment_t const& seg, Point_t& L1, Point_t& L2) const
    {
      hline.Start().compat(seg.Start());
      return _SqDist_(hline, seg, L1, L2);
    }
    // half-line and line-segment. keep track of closest approach points
    double SqDist(LineSegment_t const& seg, HalfLine_t const& hline, Point_t& L1, Point_t& L2) const
    {
      return SqDist(hline, seg, L2, L1);
    }
    // half-line and line-segment. Do not keep track of closest approach points
    double SqDist(HalfLine_t const& hline, LineSegment_t const& seg) const
    {
      Point_t L1;
      Point_t L2;
      return SqDist(hline, seg, L1, L2);
    }
    // half-line and line-segment. Do not keep track of closest approach points
    double SqDist(LineSegment_t const& seg, HalfLine_t const& hline) const
    {
      return SqDist(hline, seg);
    }

    //***************************************************
    //CLOSEST APPROACH BETWEEN POINT AND AXIS ALIGNED BOX
    //***************************************************
    double SqDist(Point_t const& pt, AABox_t const& box) const
    {
      pt.compat(box.Min());
      return _SqDist_(pt, box);
    }
    double SqDist(AABox_t const& box, Point_t const& pt) { return SqDist(pt, box); }
    Point_t ClosestPt(Point_t const& pt, AABox_t const& box) const
    {
      pt.compat(box.Min());
      return _ClosestPt_(pt, box);
    }
    Point_t ClosestPt(AABox_t const& box, Point_t const& pt) const { return ClosestPt(pt, box); }

    //***************************************************************************
    //COMMON ORIGIN ALGORITHMS: DETERMINE IF TWO GEO-OBJECTS HAVE A COMMON ORIGIN
    //***************************************************************************
    double commonOrigin(Line_t const& lin1, Line_t const& lin2) const
    {
      Point_t origin(lin1.Pt1().size());
      return commonOrigin(lin1, lin2, origin);
    }
    double commonOrigin(Line_t const& lin1, Line_t const& lin2, Point_t& origin) const
    {
      lin1.Pt1().compat(lin2.Pt1());
      return _commonOrigin_(lin1, lin2, origin);
    }
    double commonOrigin(LineSegment_t const& seg1,
                        LineSegment_t const& seg2,
                        bool backwards = false) const
    {
      Point_t origin(seg1.Start().size());
      return commonOrigin(seg1, seg2, origin, backwards);
    }
    double commonOrigin(LineSegment_t const& seg1,
                        LineSegment_t const& seg2,
                        Point_t& origin,
                        bool backwards = false) const
    {
      seg1.Start().compat(seg2.Start());
      return _commonOrigin_(seg1, seg2, origin, backwards);
    }
    double commonOrigin(HalfLine_t const& lin,
                        LineSegment_t const& seg,
                        bool backwards = false) const
    {
      Point_t origin(lin.Start().size());
      return commonOrigin(lin, seg, origin, backwards);
    }
    double commonOrigin(HalfLine_t const& lin,
                        LineSegment_t const& seg,
                        Point_t& origin,
                        bool backwards = false) const
    {
      lin.Start().compat(seg.Start());
      return _commonOrigin_(lin, seg, origin, backwards);
    }
    double commonOrigin(LineSegment_t const& seg,
                        HalfLine_t const& lin,
                        bool backwards = false) const
    {
      Point_t origin(lin.Start().size());
      return commonOrigin(lin, seg, origin, backwards);
    }
    double commonOrigin(LineSegment_t const& seg,
                        HalfLine_t const& lin,
                        Point_t& origin,
                        bool backwards = false) const
    {
      lin.Start().compat(seg.Start());
      return _commonOrigin_(lin, seg, origin, backwards);
    }
    double commonOrigin(HalfLine_t const& lin1,
                        HalfLine_t const& lin2,
                        bool backwards = false) const
    {
      Point_t origin(lin1.Start().size());
      return commonOrigin(lin1, lin2, origin, backwards);
    }
    double commonOrigin(HalfLine_t const& lin1,
                        HalfLine_t const& lin2,
                        Point_t& origin,
                        bool backwards = false) const
    {
      lin1.Start().compat(lin2.Start());
      return _commonOrigin_(lin1, lin2, origin, backwards);
    }
    double commonOrigin(Trajectory_t const& trj1,
                        Trajectory_t const& trj2,
                        bool backwards = false) const
    {
      Point_t origin(trj1.front().size());
      return commonOrigin(trj1, trj2, origin, backwards);
    }
    double commonOrigin(Trajectory_t const& trj1,
                        Trajectory_t const& trj2,
                        Point_t& origin,
                        bool backwards = false) const
    {
      trj1.front().compat(trj2.front());
      return _commonOrigin_(trj1, trj2, origin, backwards);
    }
    double commonOrigin(Trajectory_t const& trj,
                        HalfLine_t const& lin,
                        bool backwards = false) const
    {
      Point_t origin(trj.front().size());
      return commonOrigin(trj, lin, origin, backwards);
    }
    double commonOrigin(Trajectory_t const& trj,
                        HalfLine_t const& lin,
                        Point_t& origin,
                        bool backwards = false) const
    {
      trj.front().compat(lin.Start());
      return _commonOrigin_(trj, lin, origin, backwards);
    }
    double commonOrigin(HalfLine_t const& lin,
                        Trajectory_t const& trj,
                        bool backwards = false) const
    {
      Point_t origin(trj.front().size());
      return commonOrigin(trj, lin, origin, backwards);
    }
    double commonOrigin(HalfLine_t const& lin,
                        Trajectory_t const& trj,
                        Point_t& origin,
                        bool backwards = false) const
    {
      trj.front().compat(lin.Start());
      return _commonOrigin_(trj, lin, origin, backwards);
    }
    double commonOrigin(Trajectory_t const& trj,
                        LineSegment_t const& seg,
                        bool backwards = false) const
    {
      Point_t origin(trj.front().size());
      return commonOrigin(trj, seg, origin, backwards);
    }
    double commonOrigin(Trajectory_t const& trj,
                        LineSegment_t const& seg,
                        Point_t& origin,
                        bool backwards = false) const
    {
      trj.front().compat(seg.Start());
      return _commonOrigin_(trj, seg, origin, backwards);
    }
    double commonOrigin(LineSegment_t const& seg,
                        Trajectory_t const& trj,
                        bool backwards = false) const
    {
      Point_t origin(trj.front().size());
      return commonOrigin(trj, seg, origin, backwards);
    }
    double commonOrigin(LineSegment_t const& seg,
                        Trajectory_t const& trj,
                        Point_t& origin,
                        bool backwards = false) const
    {
      trj.front().compat(seg.Start());
      return _commonOrigin_(trj, seg, origin, backwards);
    }

    //************************************************************************
    //BOUNDING SPHERE ALGORITHM: RETURN SMALLEST SPHERE THAT BOUNDS ALL POINTS
    //************************************************************************
    // Bounding Sphere problem given a vector of 3D points
    Sphere_t boundingSphere(const std::vector<Point_t>& pts) const
    {
      for (auto& p : pts) {
        pts.front().compat(p);
      }
      return _boundingSphere_(pts);
    }

  protected:
    double _SqDist_(Line_t const& l1, Line_t const& l2, Point_t& L1, Point_t& L2) const;

    double _SqDist_(HalfLine_t const& l1, HalfLine_t const& l2, Point_t& L1, Point_t& L2) const;

    double _SqDist_(Point_t const& pt, LineSegment_t const& line) const
    {
      return _SqDist_(pt, line.Start(), line.End());
    }

    double _SqDist_(Point_t const& pt, Point_t const& line_s, Point_t const& line_e) const;

    double _SqDist_(LineSegment_t const& line, Point_t const& pt) const
    {
      return _SqDist_(pt, line);
    }

    double _SqDist_(HalfLine_t const& hline,
                    LineSegment_t const& seg,
                    Point_t& L1,
                    Point_t& L2) const;

    double _SqDist_(LineSegment_t const& seg1,
                    LineSegment_t const& seg2,
                    Point_t& c1,
                    Point_t& c2) const;

    // Point & LineSegment closest point w/o dimensionality check
    Point_t _ClosestPt_(Point_t const& pt, LineSegment_t const& line) const;
    // Point & LineSegment closest point w/o dimensionality check
    Point_t _ClosestPt_(LineSegment_t const& line, Point_t const& pt) const
    {
      return _ClosestPt_(pt, line);
    }

    double _SqDist_(Point_t const& pt, HalfLine_t const& line) const;
    double _SqDist_(HalfLine_t const& line, Point_t const& pt) const { return _SqDist_(pt, line); }
    // Point & HalfLine closest point w/o dimensionality check
    Point_t _ClosestPt_(Point_t const& pt, HalfLine_t const& line) const;
    // Point & HalfLine closest point w/o dimensionality check
    Point_t _ClosestPt_(HalfLine_t const& line, Point_t const& pt) const
    {
      return _ClosestPt_(pt, line);
    }

    // Point & InfLine closest point w/o dimensionality check
    Point_t _ClosestPt_(Line_t const& line, Point_t const& pt) const;
    // Point & InfLine closest point w/o dimensionality check
    Point_t _ClosestPt_(Point_t const& pt, Line_t const& line) const
    {
      return _ClosestPt_(line, pt);
    }
    // Point & InfLine  distance w/o dimensionality check
    double _SqDist_(Line_t const& line, Point_t const& pt) const;
    // Point & InfLine  distance w/o dimensionality check
    double _SqDist_(Point_t const& pt, Line_t const& line) const { return _SqDist_(line, pt); }

    double _SqDist_(Point_t const& pt, AABox_t const& box) const;
    double _SqDist_(AABox_t const& box, Point_t const& pt) const { return _SqDist_(pt, box); }

    Point_t _ClosestPt_(Point_t const& pt, AABox_t const& box) const;
    Point_t _ClosestPt_(AABox_t const& box, Point_t const& pt) const
    {
      return _ClosestPt_(pt, box);
    }

    double _commonOrigin_(Line_t const& lin1, Line_t const& lin2, Point_t& origin) const;
    double _commonOrigin_(HalfLine_t const& lin1,
                          HalfLine_t const& lin2,
                          Point_t& origin,
                          bool backwards) const;
    double _commonOrigin_(HalfLine_t const& lin,
                          LineSegment_t const& seg,
                          Point_t& origin,
                          bool backwards) const;
    double _commonOrigin_(LineSegment_t const& seg1,
                          LineSegment_t const& seg2,
                          Point_t& origin,
                          bool backwards) const;
    double _commonOrigin_(Trajectory_t const& trj1,
                          Trajectory_t const& trj2,
                          Point_t& origin,
                          bool backwards) const;
    double _commonOrigin_(Trajectory_t const& trj,
                          LineSegment_t const& seg,
                          Point_t& origin,
                          bool backwards) const;
    double _commonOrigin_(Trajectory_t const& trj,
                          HalfLine_t const& lin,
                          Point_t& origin,
                          bool backwards) const;

    // Bounding Sphere given a vector of points
    Sphere_t _boundingSphere_(const std::vector<Point_t>& pts) const;
    Sphere_t _RemainingPoints_(std::vector<Point_t>& remaining, Sphere_t const& thisSphere) const;
    Sphere_t _WelzlSphere_(const std::vector<Point_t>& pts,
                           int numPts,
                           std::vector<Point_t> sosPts) const;

    double _Clamp_(double const n, double const min, double const max) const;

    inline void _Swap_(double& tmin, double& tmax) const
    {
      if (tmin > tmax) std::swap(tmin, tmax);
    }
  };
}

#endif
 // end of doxygen group