Line3.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2015 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_MATH_LINE3_HH_
#define IGNITION_MATH_LINE3_HH_
 
#include <algorithm>
#include <ignition/math/Vector3.hh>
#include <ignition/math/config.hh>
 
namespace ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
    //
    template<typename T>
    class Line3
    {
      public: Line3() = default;
 
      public: Line3(const Line3<T> &_line)
      {
        this->pts[0] = _line[0];
        this->pts[1] = _line[1];
      }
 
      public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
      {
        this->Set(_ptA, _ptB);
      }
 
      public: Line3(const double _x1, const double _y1,
        const double _x2, const double _y2)
      {
        this->Set(_x1, _y1, _x2, _y2);
      }
 
      public: Line3(const double _x1, const double _y1,
        const double _z1, const double _x2,
        const double _y2, const double _z2)
      {
        this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
      }
 
      public: void Set(const math::Vector3<T> &_ptA,
                       const math::Vector3<T> &_ptB)
      {
        this->pts[0] = _ptA;
        this->pts[1] = _ptB;
      }
 
      public: void SetA(const math::Vector3<T> &_ptA)
      {
        this->pts[0] = _ptA;
      }
 
      public: void SetB(const math::Vector3<T> &_ptB)
      {
        this->pts[1] = _ptB;
      }
 
      public: void Set(const double _x1, const double _y1,
        const double _x2, const double _y2,
        const double _z = 0)
      {
        this->pts[0].Set(_x1, _y1, _z);
        this->pts[1].Set(_x2, _y2, _z);
      }
 
      public: void Set(const double _x1, const double _y1,
        const double _z1, const double _x2,
        const double _y2, const double _z2)
      {
        this->pts[0].Set(_x1, _y1, _z1);
        this->pts[1].Set(_x2, _y2, _z2);
      }
 
      public: math::Vector3<T> Direction() const
      {
        return (this->pts[1] - this->pts[0]).Normalize();
      }
 
      public: T Length() const
      {
        return this->pts[0].Distance(this->pts[1]);
      }
 
      public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
                            const double _epsilon = 1e-6) const
      {
        Vector3<T> p13 = this->pts[0] - _line[0];
        Vector3<T> p43 = _line[1] - _line[0];
 
        if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
            std::abs(p43.Z()) < _epsilon)
        {
          return false;
        }
 
        Vector3<T> p21 = this->pts[1] - this->pts[0];
 
        if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
            std::abs(p21.Z()) < _epsilon)
        {
          return false;
        }
 
        double d1343 = p13.Dot(p43);
        double d4321 = p43.Dot(p21);
        double d1321 = p13.Dot(p21);
        double d4343 = p43.Dot(p43);
        double d2121 = p21.Dot(p21);
 
        double denom = d2121 * d4343 - d4321 * d4321;
 
        // In this case, we choose the first point in this line,
        // and the closest point in the provided line.
        if (std::abs(denom) < _epsilon)
        {
          double d1 = this->pts[0].Distance(_line[0]);
          double d2 = this->pts[0].Distance(_line[1]);
 
          double d3 = this->pts[1].Distance(_line[0]);
          double d4 = this->pts[1].Distance(_line[1]);
 
          if (d1 <= d2 && d1 <= d3 && d1 <= d4)
          {
            _result.SetA(this->pts[0]);
            _result.SetB(_line[0]);
          }
          else if (d2 <= d3 && d2 <= d4)
          {
            _result.SetA(this->pts[0]);
            _result.SetB(_line[1]);
          }
          else if (d3 <= d4)
          {
            _result.SetA(this->pts[1]);
            _result.SetB(_line[0]);
          }
          else
          {
            _result.SetA(this->pts[1]);
            _result.SetB(_line[1]);
          }
 
          return true;
        }
 
        double numer = d1343 * d4321 - d1321 * d4343;
 
        double mua = clamp(numer / denom, 0.0, 1.0);
        double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
 
        _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
 
        return true;
      }
 
      public: T Distance(const Vector3<T> &_pt)
      {
        auto line = this->pts[1] - this->pts[0];
        auto ptTo0 = _pt - this->pts[0];
        auto ptTo1 = _pt - this->pts[1];
 
        // Point is projected beyond pt0 or the line has length 0
        if (ptTo0.Dot(line) <= 0.0)
        {
          return ptTo0.Length();
        }
 
        // Point is projected beyond pt1
        if (ptTo1.Dot(line) >= 0.0)
        {
          return ptTo1.Length();
        }
 
        // Distance to point projected onto line
        // line.Length() will have to be > 0 at this point otherwise it would
        // return at line 244.
        auto d = ptTo0.Cross(line);
        auto lineLength = line.Length();
        assert(lineLength > 0);
        return d.Length() / lineLength;
      }
 
      public: bool Intersect(const Line3<T> &_line,
                             double _epsilon = 1e-6) const
      {
        static math::Vector3<T> ignore;
        return this->Intersect(_line, ignore, _epsilon);
      }
 
      public: bool Coplanar(const Line3<T> &_line,
                            const double _epsilon = 1e-6) const
      {
        return std::abs((_line[0] - this->pts[0]).Dot(
              (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
          <= _epsilon;
      }
 
      public: bool Parallel(const Line3<T> &_line,
                            const double _epsilon = 1e-6) const
      {
        return (this->pts[1] - this->pts[0]).Cross(
            _line[1] - _line[0]).Length() <= _epsilon;
      }
 
      public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
                             double _epsilon = 1e-6) const
      {
        // Handle special case when lines are parallel
        if (this->Parallel(_line, _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;
          }
          // Otherwise return false.
          else
            return false;
        }
 
        // Get the line that is the shortest distance between this and _line
        math::Line3<T> distLine;
        this->Distance(_line, distLine, _epsilon);
 
        // If the length of the line is less than epsilon, then they
        // intersect.
        if (distLine.Length() < _epsilon)
        {
          _pt = distLine[0];
          return true;
        }
 
        return false;
      }
 
      public: bool Within(const math::Vector3<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 &&
               _pt.Z() <= std::max(this->pts[0].Z(),
                                   this->pts[1].Z()) + _epsilon &&
               _pt.Z() >= std::min(this->pts[0].Z(),
                                   this->pts[1].Z()) - _epsilon;
      }
 
      public: bool operator==(const Line3<T> &_line) const
      {
        return this->pts[0] == _line[0] && this->pts[1] == _line[1];
      }
 
      public: bool operator!=(const Line3<T> &_line) const
      {
        return !(*this == _line);
      }
 
      public: math::Vector3<T> operator[](const size_t _index) const
      {
        return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
      }
 
      public: friend std::ostream &operator<<(
                  std::ostream &_out, const Line3<T> &_line)
      {
        _out << _line[0] << " " << _line[1];
        return _out;
      }
 
      public: Line3 &operator=(const Line3<T> &_line)
      {
        this->pts[0] = _line[0];
        this->pts[1] = _line[1];
 
        return *this;
      }
 
      private: math::Vector3<T> pts[2];
    };
 
    typedef Line3<int> Line3i;
    typedef Line3<double> Line3d;
    typedef Line3<float> Line3f;
    }
  }
}
#endif