Triangle.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef IGNITION_MATH_TRIANGLE_HH_
18 #define IGNITION_MATH_TRIANGLE_HH_
19 
20 #include <set>
21 #include <ignition/math/Helpers.hh>
22 #include <ignition/math/Line2.hh>
23 #include <ignition/math/Vector2.hh>
24 
25 namespace ignition
26 {
27  namespace math
28  {
31  template<typename T>
32  class Triangle
33  {
35  public: Triangle() = default;
36 
41  public: Triangle(const math::Vector2<T> &_pt1,
42  const math::Vector2<T> &_pt2,
43  const math::Vector2<T> &_pt3)
44  {
45  this->Set(_pt1, _pt2, _pt3);
46  }
47 
53  public: void Set(const unsigned int _index, const math::Vector2<T> &_pt)
54  {
55  this->pts[clamp(_index, 0u, 2u)] = _pt;
56  }
57 
62  public: void Set(const math::Vector2<T> &_pt1,
63  const math::Vector2<T> &_pt2,
64  const math::Vector2<T> &_pt3)
65  {
66  this->pts[0] = _pt1;
67  this->pts[1] = _pt2;
68  this->pts[2] = _pt3;
69  }
70 
75  public: bool Valid() const
76  {
77  T a = this->Side(0).Length();
78  T b = this->Side(1).Length();
79  T c = this->Side(2).Length();
80  return (a+b) > c && (b+c) > a && (c+a) > b;
81  }
82 
90  public: Line2<T> Side(const unsigned int _index) const
91  {
92  if (_index == 0)
93  return Line2<T>(this->pts[0], this->pts[1]);
94  else if (_index == 1)
95  return Line2<T>(this->pts[1], this->pts[2]);
96  else
97  return Line2<T>(this->pts[2], this->pts[0]);
98  }
99 
105  public: bool Contains(const Line2<T> &_line) const
106  {
107  return this->Contains(_line[0]) && this->Contains(_line[1]);
108  }
109 
113  public: bool Contains(const math::Vector2<T> &_pt) const
114  {
115  // Compute vectors
116  math::Vector2<T> v0 = this->pts[2] -this->pts[0];
117  math::Vector2<T> v1 = this->pts[1] -this->pts[0];
118  math::Vector2<T> v2 = _pt - this->pts[0];
119 
120  // Compute dot products
121  double dot00 = v0.Dot(v0);
122  double dot01 = v0.Dot(v1);
123  double dot02 = v0.Dot(v2);
124  double dot11 = v1.Dot(v1);
125  double dot12 = v1.Dot(v2);
126 
127  // Compute barycentric coordinates
128  double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
129  double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
130  double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
131 
132  // Check if point is in triangle
133  return (u >= 0) && (v >= 0) && (u + v <= 1);
134  }
135 
143  public: bool Intersects(const Line2<T> &_line,
144  math::Vector2<T> &_ipt1,
145  math::Vector2<T> &_ipt2) const
146  {
147  if (this->Contains(_line))
148  {
149  _ipt1 = _line[0];
150  _ipt2 = _line[1];
151  return true;
152  }
153 
154  Line2<T> line1(this->pts[0], this->pts[1]);
155  Line2<T> line2(this->pts[1], this->pts[2]);
156  Line2<T> line3(this->pts[2], this->pts[0]);
157 
158  math::Vector2<T> pt;
159  std::set<math::Vector2<T> > points;
160 
161  if (line1.Intersect(_line, pt))
162  points.insert(pt);
163 
164  if (line2.Intersect(_line, pt))
165  points.insert(pt);
166 
167  if (line3.Intersect(_line, pt))
168  points.insert(pt);
169 
170  if (points.empty())
171  {
172  return false;
173  }
174  else if (points.size() == 1)
175  {
176  auto iter = points.begin();
177 
178  _ipt1 = *iter;
179  if (this->Contains(_line[0]))
180  _ipt2 = _line[0];
181  else
182  {
183  _ipt2 = _line[1];
184  }
185  }
186  else
187  {
188  auto iter = points.begin();
189  _ipt1 = *(iter++);
190  _ipt2 = *iter;
191  }
192 
193  return true;
194  }
195 
198  public: T Perimeter() const
199  {
200  return this->Side(0).Length() + this->Side(1).Length() +
201  this->Side(2).Length();
202  }
203 
206  public: double Area() const
207  {
208  double s = this->Perimeter() / 2.0;
209  T a = this->Side(0).Length();
210  T b = this->Side(1).Length();
211  T c = this->Side(2).Length();
212 
213  // Heron's formula
214  // http://en.wikipedia.org/wiki/Heron%27s_formula
215  return sqrt(s * (s-a) * (s-b) * (s-c));
216  }
217 
223  public: math::Vector2<T> operator[](const size_t _index) const
224  {
225  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
226  }
227 
229  private: math::Vector2<T> pts[3];
230  };
231 
234 
237 
240  }
241 }
242 #endif
Line2< T > Side(const unsigned int _index) const
Get a line segment for one side of the triangle.
Definition: Triangle.hh:90
math::Vector2< T > operator[](const size_t _index) const
Get one of points that define the triangle.
Definition: Triangle.hh:223
T Perimeter() const
Get the length of the triangle&#39;s perimeter.
Definition: Triangle.hh:198
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:213
double Area() const
Get the area of this triangle.
Definition: Triangle.hh:206
Two dimensional (x, y) vector.
Definition: Vector2.hh:29
bool Contains(const Line2< T > &_line) const
Check if this triangle completely contains the given line segment.
Definition: Triangle.hh:105
A two dimensional line segment.
Definition: Line2.hh:31
void Set(const unsigned int _index, const math::Vector2< T > &_pt)
Set one vertex of the triangle.
Definition: Triangle.hh:53
bool Intersects(const Line2< T > &_line, math::Vector2< T > &_ipt1, math::Vector2< T > &_ipt2) const
Get whether the given line intersects this triangle.
Definition: Triangle.hh:143
T Dot(const Vector2< T > &_v) const
Get the dot product of this vector and _v.
Definition: Vector2.hh:112
Triangle< int > Trianglei
Integer specialization of the Triangle class.
Definition: Triangle.hh:233
Triangle< double > Triangled
Double specialization of the Triangle class.
Definition: Triangle.hh:236
void Set(const math::Vector2< T > &_pt1, const math::Vector2< T > &_pt2, const math::Vector2< T > &_pt3)
Set all vertices of the triangle.
Definition: Triangle.hh:62
Triangle()=default
Default constructor.
bool Intersect(const Line2< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line2.hh:176
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition: Helpers.hh:219
bool Valid() const
Get whether this triangle is valid, based on triangle inequality: the sum of the lengths of any two s...
Definition: Triangle.hh:75
Triangle class and related functions.
Definition: Triangle.hh:32
Triangle< float > Trianglef
Float specialization of the Triangle class.
Definition: Triangle.hh:239
bool Contains(const math::Vector2< T > &_pt) const
Get whether this triangle contains the given point.
Definition: Triangle.hh:113
Definition: Angle.hh:38
Triangle(const math::Vector2< T > &_pt1, const math::Vector2< T > &_pt2, const math::Vector2< T > &_pt3)
Constructor.
Definition: Triangle.hh:41
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:392