Line2.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef _IGNITION_LINE2_HH_
#define _IGNITION_LINE2_HH_

#include <algorithm>
#include <ignition/math/Vector2.hh>
#include <ignition/math/IndexException.hh>

namespace ignition
{
  namespace math
  {
    template<typename T>
    class Line2
    {
      public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
      {
        this->Set(_ptA, _ptB);
      }

      public: Line2(double _x1, double _y1, double _x2, double _y2)
      {
        this->Set(_x1, _y1, _x2, _y2);
      }

      public: void Set(const math::Vector2<T> &_ptA,
                       const math::Vector2<T> &_ptB)
      {
        this->pts[0] = _ptA;
        this->pts[1] = _ptB;
      }

      public: void Set(double _x1, double _y1, double _x2, double _y2)
      {
        this->pts[0].Set(_x1, _y1);
        this->pts[1].Set(_x2, _y2);
      }

      public: double CrossProduct(const Line2<T> &_line) const
      {
        return (this->pts[0].X() - this->pts[1].X()) *
               (_line[0].Y() -_line[1].Y()) -
               (this->pts[0].Y() - this->pts[1].Y()) *
               (_line[0].X() - _line[1].X());
      }

      //  (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
      public: double CrossProduct(const Vector2<T> &_pt) const
      {
        return (_pt.Y() - this->pts[0].Y()) *
               (this->pts[1].X() - this->pts[0].X()) -
               (_pt.X() - this->pts[0].X()) *
               (this->pts[1].Y() - this->pts[0].Y());
      }

      public: bool Collinear(const math::Vector2<T> &_pt,
                             double _epsilon = 1e-6) const
      {
        return math::equal(this->CrossProduct(_pt),
            static_cast<T>(0), _epsilon);
      }

      public: bool Parallel(const math::Line2<T> &_line,
                            double _epsilon = 1e-6) const
      {
        return math::equal(this->CrossProduct(_line),
            static_cast<T>(0), _epsilon);
      }

      public: bool Collinear(const math::Line2<T> &_line,
                             double _epsilon = 1e-6) const
      {
        return this->Parallel(_line, _epsilon) &&
               this->Intersect(_line, _epsilon);
      }

      public: bool OnSegment(const math::Vector2<T> &_pt,
                             double _epsilon = 1e-6) const
      {
        return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
      }

      public: bool Within(const math::Vector2<T> &_pt,
                          double _epsilon = 1e-6) const
      {
        return _pt.X() <= std::max(this->pts[0].X(),
                                   this->pts[1].X()) + _epsilon &&
               _pt.X() >= std::min(this->pts[0].X(),
                                   this->pts[1].X()) - _epsilon &&
               _pt.Y() <= std::max(this->pts[0].Y(),
                                   this->pts[1].Y()) + _epsilon &&
               _pt.Y() >= std::min(this->pts[0].Y(),
                                   this->pts[1].Y()) - _epsilon;
      }

      public: bool Intersect(const Line2<T> &_line,
                             double _epsilon = 1e-6) const
      {
        static math::Vector2<T> ignore;
        return this->Intersect(_line, ignore, _epsilon);
      }

      public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
                             double _epsilon = 1e-6) const
      {
        double d = this->CrossProduct(_line);

        // d is zero if the two line are collinear. Must check special
        // cases.
        if (math::equal(d, 0.0, _epsilon))
        {
          // Check if _line's starting point is on the line.
          if (this->Within(_line[0], _epsilon))
          {
            _pt = _line[0];
            return true;
          }
          // Check if _line's ending point is on the line.
          else if (this->Within(_line[1], _epsilon))
          {
            _pt = _line[1];
            return true;
          }
          // Other wise return false.
          else
            return false;
        }

        _pt.X((_line[0].X() - _line[1].X()) *
              (this->pts[0].X() * this->pts[1].Y() -
               this->pts[0].Y() * this->pts[1].X()) -
              (this->pts[0].X() - this->pts[1].X()) *
              (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));

        _pt.Y((_line[0].Y() - _line[1].Y()) *
              (this->pts[0].X() * this->pts[1].Y() -
               this->pts[0].Y() * this->pts[1].X()) -
              (this->pts[0].Y() - this->pts[1].Y()) *
              (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));

        _pt /= d;

        if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
            _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
            _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
            _pt.X() > std::max(_line[0].X(), _line[1].X()))
        {
          return false;
        }

        if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
            _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
            _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
            _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
        {
          return false;
        }

        return true;
      }

      public: T Length() const
      {
        return sqrt((this->pts[0].X() - this->pts[1].X()) *
                    (this->pts[0].X() - this->pts[1].X()) +
                    (this->pts[0].Y() - this->pts[1].Y()) *
                    (this->pts[0].Y() - this->pts[1].Y()));
      }

      public: double Slope() const
      {
        if (math::equal(this->pts[1].X(), this->pts[0].X()))
          return NAN_D;

        return (this->pts[1].Y() - this->pts[0].Y()) /
               static_cast<double>(this->pts[1].X() - this->pts[0].X());
      }

      public: bool operator==(const Line2<T> &_line) const
      {
        return this->pts[0] == _line[0] && this->pts[1] == _line[1];
      }

      public: bool operator!=(const Line2<T> &_line) const
      {
        return !(*this == _line);
      }

      public: math::Vector2<T> operator[](size_t _index) const
      {
        if (_index > 1)
          throw IndexException();
        return this->pts[_index];
      }

      public: friend std::ostream &operator<<(
                  std::ostream &_out, const Line2<T> &_line)
      {
        _out << _line[0] << " " << _line[1];
        return _out;
      }

      private: math::Vector2<T> pts[2];
    };


    typedef Line2<int> Line2i;
    typedef Line2<double> Line2d;
    typedef Line2<float> Line2f;
  }
}
#endif