Gazebo Math

API Reference

6.15.1
gz/math/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 GZ_MATH_TRIANGLE_HH_
18 #define GZ_MATH_TRIANGLE_HH_
19 
20 #include <set>
21 #include <gz/math/Helpers.hh>
22 #include <gz/math/Line2.hh>
23 #include <gz/math/Vector2.hh>
24 #include <gz/math/config.hh>
25 
26 namespace ignition
27 {
28  namespace math
29  {
30  // Inline bracket to help doxygen filtering.
31  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
32  //
35  template<typename T>
36  class Triangle
37  {
39  public: Triangle() = default;
40 
45  public: Triangle(const math::Vector2<T> &_pt1,
46  const math::Vector2<T> &_pt2,
47  const math::Vector2<T> &_pt3)
48  {
49  this->Set(_pt1, _pt2, _pt3);
50  }
51 
57  public: void Set(const unsigned int _index, const math::Vector2<T> &_pt)
58  {
59  this->pts[clamp(_index, 0u, 2u)] = _pt;
60  }
61 
66  public: void Set(const math::Vector2<T> &_pt1,
67  const math::Vector2<T> &_pt2,
68  const math::Vector2<T> &_pt3)
69  {
70  this->pts[0] = _pt1;
71  this->pts[1] = _pt2;
72  this->pts[2] = _pt3;
73  }
74 
79  public: bool Valid() const
80  {
81  T a = this->Side(0).Length();
82  T b = this->Side(1).Length();
83  T c = this->Side(2).Length();
84  return (a+b) > c && (b+c) > a && (c+a) > b;
85  }
86 
94  public: Line2<T> Side(const unsigned int _index) const
95  {
96  if (_index == 0)
97  return Line2<T>(this->pts[0], this->pts[1]);
98  else if (_index == 1)
99  return Line2<T>(this->pts[1], this->pts[2]);
100  else
101  return Line2<T>(this->pts[2], this->pts[0]);
102  }
103 
109  public: bool Contains(const Line2<T> &_line) const
110  {
111  return this->Contains(_line[0]) && this->Contains(_line[1]);
112  }
113 
117  public: bool Contains(const math::Vector2<T> &_pt) const
118  {
119  // Compute vectors
120  math::Vector2<T> v0 = this->pts[2] -this->pts[0];
121  math::Vector2<T> v1 = this->pts[1] -this->pts[0];
122  math::Vector2<T> v2 = _pt - this->pts[0];
123 
124  // Compute dot products
125  double dot00 = v0.Dot(v0);
126  double dot01 = v0.Dot(v1);
127  double dot02 = v0.Dot(v2);
128  double dot11 = v1.Dot(v1);
129  double dot12 = v1.Dot(v2);
130 
131  // Compute barycentric coordinates
132  double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
133  double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
134  double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
135 
136  // Check if point is in triangle
137  return (u >= 0) && (v >= 0) && (u + v <= 1);
138  }
139 
147  public: bool Intersects(const Line2<T> &_line,
148  math::Vector2<T> &_ipt1,
149  math::Vector2<T> &_ipt2) const
150  {
151  if (this->Contains(_line))
152  {
153  _ipt1 = _line[0];
154  _ipt2 = _line[1];
155  return true;
156  }
157 
158  Line2<T> line1(this->pts[0], this->pts[1]);
159  Line2<T> line2(this->pts[1], this->pts[2]);
160  Line2<T> line3(this->pts[2], this->pts[0]);
161 
162  math::Vector2<T> pt;
163  std::set<math::Vector2<T> > points;
164 
165  if (line1.Intersect(_line, pt))
166  points.insert(pt);
167 
168  if (line2.Intersect(_line, pt))
169  points.insert(pt);
170 
171  if (line3.Intersect(_line, pt))
172  points.insert(pt);
173 
174  if (points.empty())
175  {
176  return false;
177  }
178  else if (points.size() == 1)
179  {
180  auto iter = points.begin();
181 
182  _ipt1 = *iter;
183  if (this->Contains(_line[0]))
184  _ipt2 = _line[0];
185  else
186  {
187  _ipt2 = _line[1];
188  }
189  }
190  else
191  {
192  auto iter = points.begin();
193  _ipt1 = *(iter++);
194  _ipt2 = *iter;
195  }
196 
197  return true;
198  }
199 
202  public: T Perimeter() const
203  {
204  return this->Side(0).Length() + this->Side(1).Length() +
205  this->Side(2).Length();
206  }
207 
210  public: double Area() const
211  {
212  double s = this->Perimeter() / 2.0;
213  T a = this->Side(0).Length();
214  T b = this->Side(1).Length();
215  T c = this->Side(2).Length();
216 
217  // Heron's formula
218  // http://en.wikipedia.org/wiki/Heron%27s_formula
219  return sqrt(s * (s-a) * (s-b) * (s-c));
220  }
221 
227  public: math::Vector2<T> operator[](const size_t _index) const
228  {
229  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
230  }
231 
233  private: math::Vector2<T> pts[3];
234  };
235 
238 
241 
244  }
245  }
246 }
247 #endif
T Perimeter() const
Get the length of the triangle's perimeter.
Definition: gz/math/Triangle.hh:202
Triangle< double > Triangled
Double specialization of the Triangle class.
Definition: gz/math/Triangle.hh:240
bool Intersects(const Line2< T > &_line, math::Vector2< T > &_ipt1, math::Vector2< T > &_ipt2) const
Get whether the given line intersects this triangle.
Definition: gz/math/Triangle.hh:147
Two dimensional (x, y) vector.
Definition: gz/math/Vector2.hh:37
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
Triangle(const math::Vector2< T > &_pt1, const math::Vector2< T > &_pt2, const math::Vector2< T > &_pt3)
Constructor.
Definition: gz/math/Triangle.hh:45
T size(T... args)
Triangle class and related functions.
Definition: gz/math/Triangle.hh:36
bool Contains(const math::Vector2< T > &_pt) const
Get whether this triangle contains the given point.
Definition: gz/math/Triangle.hh:117
A two dimensional line segment. The line is defined by a start and end point.
Definition: gz/math/Line2.hh:35
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition: gz/math/Helpers.hh:233
Triangle< int > Trianglei
Integer specialization of the Triangle class.
Definition: gz/math/Triangle.hh:237
bool Valid() const
Get whether this triangle is valid, based on triangle inequality: the sum of the lengths of any two s...
Definition: gz/math/Triangle.hh:79
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: gz/math/Helpers.hh:406
Triangle()=default
Default constructor.
Triangle< float > Trianglef
Float specialization of the Triangle class.
Definition: gz/math/Triangle.hh:243
math::Vector2< T > operator[](const size_t _index) const
Get one of points that define the triangle.
Definition: gz/math/Triangle.hh:227
bool Contains(const Line2< T > &_line) const
Check if this triangle completely contains the given line segment.
Definition: gz/math/Triangle.hh:109
Line2< T > Side(const unsigned int _index) const
Get a line segment for one side of the triangle.
Definition: gz/math/Triangle.hh:94
T begin(T... args)
T insert(T... args)
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: gz/math/Helpers.hh:227
bool Intersect(const Line2< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: gz/math/Line2.hh:180
double Area() const
Get the area of this triangle.
Definition: gz/math/Triangle.hh:210
T empty(T... args)
T Dot(const Vector2< T > &_v) const
Get the dot product of this vector and _v.
Definition: gz/math/Vector2.hh:158
void Set(const unsigned int _index, const math::Vector2< T > &_pt)
Set one vertex of the triangle.
Definition: gz/math/Triangle.hh:57
void Set(const math::Vector2< T > &_pt1, const math::Vector2< T > &_pt2, const math::Vector2< T > &_pt3)
Set all vertices of the triangle.
Definition: gz/math/Triangle.hh:66
STL class.