GeoAlgo.h

Go to the documentation of this file.

#ifndef BASICTOOL_GEOALGO_H
#define BASICTOOL_GEOALGO_H

#include <algorithm> // used for std::find
#include "GeoLine.h"
#include "GeoHalfLine.h"
#include "GeoLineSegment.h"
#include "GeoTrajectory.h"
#include "GeoCone.h"
#include "GeoAABox.h"
#include "GeoSphere.h"

namespace geoalgo {

  class GeoAlgo{
    
  public:
    
    GeoAlgo(){}
    
    virtual ~GeoAlgo(){}

    //
    // Intersections
    //

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

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

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

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


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

    //************************************************
    //CLOSEST APPROACH BETWEEN POINT AND INFINITE LINE
    //************************************************
    // min distance between point and infinite line
    double SqDist(const Line_t& line, const Point_t& pt) const
    { pt.compat(line.Pt1()); return _SqDist_(line,pt); }
    // min distance between point and infinite line
    double SqDist(const Point_t& pt, const Line_t& line) const
    { return SqDist(line,pt); }
    // closest point (on infinite line) from point
    Point_t ClosestPt(const Line_t& line, const Point_t& pt) const
    { pt.compat(line.Pt1()); return _ClosestPt_(pt,line); }
    // closest point (on infinite line) from point
    Point_t ClosestPt(const Point_t& pt, const Line_t& 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(const Line_t& l1, const Line_t& 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(const Line_t& l1, const Line_t& 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(const HalfLine_t& l1, const HalfLine_t& 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(const HalfLine_t& l1, const HalfLine_t& l2) const
    { Point_t L1; Point_t L2; return SqDist(l1, l2, L1, L2); }


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


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


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


    //*****************************************************
    //CLOSEST APPROACH BETWEEN SEGMENT AND VECTOR OF TRACKS
    //*****************************************************
    double SqDist(const LineSegment_t& 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, const LineSegment_t& 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, const LineSegment_t& seg) const
    { Point_t c1; Point_t c2; int trackIdx; return SqDist(seg, trj, c1, c2, trackIdx); }
    double SqDist(const LineSegment_t& 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(const HalfLine_t& hline, const Trajectory_t& trj, Point_t& c1, Point_t& c2) const;
    double SqDist(const Trajectory_t& trj, const HalfLine_t& hline, Point_t& c1, Point_t& c2) const
    { return SqDist(hline, trj, c2, c1); }
    double SqDist(const Trajectory_t& trj, const HalfLine_t& hline) const
    { Point_t c1; Point_t c2; return SqDist(hline, trj, c1, c2); }
    double SqDist(const HalfLine_t& hline, const Trajectory_t& trj) const
    { Point_t c1; Point_t c2; return SqDist(hline, trj, c1, c2); }


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


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


    //***********************************************
    //CLOSEST APPROACH BETWEEN POINT AND LINE SEGMENT
    //***********************************************
    double SqDist(const Point_t& pt, const LineSegment_t& line) const
    { pt.compat(line.Start()); return _SqDist_(pt,line); }
    double SqDist(const LineSegment_t& line, const Point_t& pt) const
    { return SqDist(pt,line); }
    Point_t ClosestPt(const Point_t& pt, const LineSegment_t& line) const
    { pt.compat(line.Start()); return _ClosestPt_(pt,line); }
    Point_t ClosestPt(const LineSegment_t& line, const Point_t& pt) const
    { return ClosestPt(pt,line); }
    
    //********************************************
    //CLOSEST APPROACH BETWEEN POINT AND HALF LINE
    //********************************************
    double SqDist(const Point_t& pt, const HalfLine_t& line) const
    { pt.compat(line.Start()); return _SqDist_(pt,line); }
    double SqDist(const HalfLine_t& line, const Point_t& pt) const
    { return SqDist(pt,line); }
    Point_t ClosestPt(const Point_t& pt, const HalfLine_t& line) const
    { pt.compat(line.Start()); return _ClosestPt_(pt,line); }
    Point_t ClosestPt(const HalfLine_t& line, const Point_t& 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(const HalfLine_t& hline, const LineSegment_t& 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(const LineSegment_t& seg, const HalfLine_t& 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(const HalfLine_t& hline, const LineSegment_t& 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(const LineSegment_t& seg, const HalfLine_t& hline) const
    { return SqDist(hline,seg); }  

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


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

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

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

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

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

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

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

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

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

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

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

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

    double _commonOrigin_(const Line_t& lin1, const Line_t& lin2, Point_t& origin) const;
    double _commonOrigin_(const HalfLine_t& lin1, const HalfLine_t& lin2, Point_t& origin, bool backwards) const;
    double _commonOrigin_(const HalfLine_t& lin, const LineSegment_t& seg, Point_t& origin, bool backwards) const;
    double _commonOrigin_(const LineSegment_t& seg1, const LineSegment_t& seg2, Point_t& origin, bool backwards) const;
    double _commonOrigin_(const Trajectory_t& trj1, const Trajectory_t& trj2, Point_t& origin, bool backwards) const;
    double _commonOrigin_(const Trajectory_t& trj, const LineSegment_t& seg, Point_t& origin, bool backwards) const;
    double _commonOrigin_(const Trajectory_t& trj, const HalfLine_t& 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, const Sphere_t& thisSphere)const;
    Sphere_t _WelzlSphere_(const std::vector<Point_t>& pts, int numPts, std::vector<Point_t> sosPts) const;

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

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

  };
}

#endif
 // end of doxygen group