gz/math/Triangle3.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2016 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_TRIANGLE3_HH_
#define GZ_MATH_TRIANGLE3_HH_
 
#include <gz/math/Helpers.hh>
#include <gz/math/Line3.hh>
#include <gz/math/Plane.hh>
#include <gz/math/Vector3.hh>
#include <gz/math/config.hh>
 
namespace ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
    //
    template<typename T>
    class Triangle3
    {
      public: Triangle3() = default;
 
      public: Triangle3(const Vector3<T> &_pt1,
                        const Vector3<T> &_pt2,
                        const Vector3<T> &_pt3)
      {
        this->Set(_pt1, _pt2, _pt3);
      }
 
      public: void Set(const unsigned int _index, const Vector3<T> &_pt)
      {
        this->pts[clamp(_index, 0u, 2u)] = _pt;
      }
 
      public: void Set(const Vector3<T> &_pt1,
                       const Vector3<T> &_pt2,
                       const Vector3<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: Line3<T> Side(const unsigned int _index) const
      {
        if (_index == 0)
          return Line3<T>(this->pts[0], this->pts[1]);
        else if (_index == 1)
          return Line3<T>(this->pts[1], this->pts[2]);
        else
          return Line3<T>(this->pts[2], this->pts[0]);
      }
 
      public: bool Contains(const Line3<T> &_line) const
      {
        return this->Contains(_line[0]) && this->Contains(_line[1]);
      }
 
      public: bool Contains(const Vector3<T> &_pt) const
      {
        // Make sure the point is on the same plane as the triangle
        if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2]))
          == Planed::NO_SIDE)
        {
          Vector3<T> v0 = this->pts[2] - this->pts[0];
          Vector3<T> v1 = this->pts[1] - this->pts[0];
          Vector3<T> v2 = _pt - this->pts[0];
 
          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);
        }
        return false;
      }
 
      public: Vector3d Normal() const
      {
        return math::Vector3d::Normal(
          Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]),
          Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]),
          Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2]));
      }
 
      public: bool Intersects(
        const Line3<T> &_line, Vector3<T> &_ipt1) const
      {
        // Triangle normal
        Vector3d norm = this->Normal();
 
        // Ray direction to intersect with triangle
        Vector3<T> dir = (_line[1] - _line[0]).Normalize();
 
        double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2]));
 
        // Handle the case when the line is not co-planar with the triangle
        if (!math::equal(denom, 0.0))
        {
          // Distance from line start to triangle intersection
          Vector3<T> diff = _line[0] - this->pts[0];
          double intersection =
            -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom;
 
          // Make sure the ray intersects the triangle
          if (intersection < 1.0 || intersection > _line.Length())
            return false;
 
          // Return point of intersection
          _ipt1 = _line[0] + (dir * intersection);
 
          return true;
        }
        // Line co-planar with triangle
        else
        {
          // If the line is completely inside the triangle
          if (this->Contains(_line))
          {
            _ipt1 = _line[0];
            return true;
          }
          // If the line intersects the first side
          else if (_line.Intersect(this->Side(0), _ipt1))
          {
            return true;
          }
          // If the line intersects the second side
          else if (_line.Intersect(this->Side(1), _ipt1))
          {
            return true;
          }
          // If the line intersects the third side
          else if (_line.Intersect(this->Side(2), _ipt1))
          {
            return true;
          }
        }
 
        return false;
      }
 
      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: Vector3<T> operator[](const size_t _index) const
      {
        return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
      }
 
      private: Vector3<T> pts[3];
    };
 
    typedef Triangle3<int> Triangle3i;
 
    typedef Triangle3<double> Triangle3d;
 
    typedef Triangle3<float> Triangle3f;
    }
  }
}
#endif