Triangle.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_TRIANGLE_HH_
#define _IGNITION_TRIANGLE_HH_

#include <set>
#include <ignition/math/Line2.hh>
#include <ignition/math/Vector2.hh>
#include <ignition/math/IndexException.hh>

namespace ignition
{
  namespace math
  {
    template<typename T>
    class Triangle
    {
      public: Triangle() = default;

      public: Triangle(const math::Vector2<T> &_pt1,
                       const math::Vector2<T> &_pt2,
                       const math::Vector2<T> &_pt3)
      {
        this->Set(_pt1, _pt2, _pt3);
      }

      public: void Set(unsigned int _index, const math::Vector2<T> &_pt)
      {
        if (_index >2)
          throw IndexException();
        else
          this->pts[_index] = _pt;
      }

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

      public: bool Valid() const
      {
        T a = this->Side(0).Length();
        T b = this->Side(1).Length();
        T c = this->Side(2).Length();
        return (a+b) > c && (b+c) > a && (c+a) > b;
      }

      public: Line2<T> Side(unsigned int _index) const
      {
        if (_index > 2)
          throw IndexException();
        else if (_index == 0)
          return Line2<T>(this->pts[0], this->pts[1]);
        else if (_index == 1)
          return Line2<T>(this->pts[1], this->pts[2]);
        else
          return Line2<T>(this->pts[2], this->pts[0]);
      }

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

      public: bool Contains(const math::Vector2<T> &_pt) const
      {
        // Compute vectors
        math::Vector2<T> v0 = this->pts[2] -this->pts[0];
        math::Vector2<T> v1 = this->pts[1] -this->pts[0];
        math::Vector2<T> v2 = _pt - this->pts[0];

        // Compute dot products
        double dot00 = v0.Dot(v0);
        double dot01 = v0.Dot(v1);
        double dot02 = v0.Dot(v2);
        double dot11 = v1.Dot(v1);
        double dot12 = v1.Dot(v2);

        // Compute barycentric coordinates
        double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
        double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
        double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

        // Check if point is in triangle
        return (u >= 0) && (v >= 0) && (u + v <= 1);
      }

      public: bool Intersects(const Line2<T> &_line,
                              math::Vector2<T> &_ipt1,
                              math::Vector2<T> &_ipt2) const
      {
        if (this->Contains(_line))
        {
          _ipt1 = _line[0];
          _ipt2 = _line[1];
          return true;
        }

        Line2<T> line1(this->pts[0], this->pts[1]);
        Line2<T> line2(this->pts[1], this->pts[2]);
        Line2<T> line3(this->pts[2], this->pts[0]);

        math::Vector2<T> pt;
        std::set<math::Vector2<T> > points;

        if (line1.Intersect(_line, pt))
          points.insert(pt);

        if (line2.Intersect(_line, pt))
          points.insert(pt);

        if (line3.Intersect(_line, pt))
          points.insert(pt);

        if (points.empty())
        {
          return false;
        }
        else if (points.size() == 1)
        {
          typename std::set<math::Vector2<T> >::iterator iter = points.begin();

          _ipt1 = *iter;
          if (this->Contains(_line[0]))
            _ipt2 = _line[0];
          else
          {
            _ipt2 = _line[1];
          }
        }
        else
        {
          typename std::set<math::Vector2<T> >::iterator iter = points.begin();
          _ipt1 = *(iter++);
          _ipt2 = *iter;
        }

        return true;
      }

      public: T Perimeter() const
      {
        return this->Side(0).Length() + this->Side(1).Length() +
               this->Side(2).Length();
      }

      public: double Area() const
      {
        double s = this->Perimeter() / 2.0;
        T a = this->Side(0).Length();
        T b = this->Side(1).Length();
        T c = this->Side(2).Length();

        // Heron's formula
        // http://en.wikipedia.org/wiki/Heron%27s_formula
        return sqrt(s * (s-a) * (s-b) * (s-c));
      }

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

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

    typedef Triangle<int> Trianglei;

    typedef Triangle<double> Triangled;

    typedef Triangle<float> Trianglef;
  }
}
#endif