gz/math/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 GZ_MATH_TRIANGLE_HH_
#define GZ_MATH_TRIANGLE_HH_
 
#include <set>
#include <gz/math/Helpers.hh>
#include <gz/math/Line2.hh>
#include <gz/math/Vector2.hh>
#include <gz/math/config.hh>
 
namespace gz::math
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_MATH_VERSION_NAMESPACE {
  //
  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(const unsigned int _index, const math::Vector2<T> &_pt)
    {
      this->pts[clamp(_index, 0u, 2u)] = _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(const unsigned int _index) const
    {
      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)
      {
        auto iter = points.begin();
 
        _ipt1 = *iter;
        if (this->Contains(_line[0]))
          _ipt2 = _line[0];
        else
        {
          _ipt2 = _line[1];
        }
      }
      else
      {
        auto 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[](const size_t _index) const
    {
      return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
    }
 
    private: math::Vector2<T> pts[3];
  };
 
  typedef Triangle<int> Trianglei;
 
  typedef Triangle<double> Triangled;
 
  typedef Triangle<float> Trianglef;
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_TRIANGLE_HH_