Triangle3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 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_TRIANGLE3_HH_
18 #define IGNITION_MATH_TRIANGLE3_HH_
19 
20 #include <ignition/math/Helpers.hh>
21 #include <ignition/math/Line3.hh>
22 #include <ignition/math/Plane.hh>
23 #include <ignition/math/Vector3.hh>
24 
25 namespace ignition
26 {
27  namespace math
28  {
31  template<typename T>
32  class Triangle3
33  {
35  public: Triangle3() = default;
36 
45  public: Triangle3(const Vector3<T> &_pt1,
46  const Vector3<T> &_pt2,
47  const Vector3<T> &_pt3)
48  {
49  this->Set(_pt1, _pt2, _pt3);
50  }
51 
61  public: void Set(const unsigned int _index, const Vector3<T> &_pt)
62  {
63  this->pts[clamp(_index, 0u, 2u)] = _pt;
64  }
65 
75  public: void Set(const Vector3<T> &_pt1,
76  const Vector3<T> &_pt2,
77  const Vector3<T> &_pt3)
78  {
79  this->pts[0] = _pt1;
80  this->pts[1] = _pt2;
81  this->pts[2] = _pt3;
82  }
83 
88  public: bool Valid() const
89  {
90  T a = this->Side(0).Length();
91  T b = this->Side(1).Length();
92  T c = this->Side(2).Length();
93  return (a+b) > c && (b+c) > a && (c+a) > b;
94  }
95 
103  public: Line3<T> Side(const unsigned int _index) const
104  {
105  if (_index == 0)
106  return Line3<T>(this->pts[0], this->pts[1]);
107  else if (_index == 1)
108  return Line3<T>(this->pts[1], this->pts[2]);
109  else
110  return Line3<T>(this->pts[2], this->pts[0]);
111  }
112 
118  public: bool Contains(const Line3<T> &_line) const
119  {
120  return this->Contains(_line[0]) && this->Contains(_line[1]);
121  }
122 
126  public: bool Contains(const Vector3<T> &_pt) const
127  {
128  // Make sure the point is on the same plane as the triangle
129  if (Planed(this->Normal()).Side(_pt) == Planed::NO_SIDE)
130  {
131  Vector3d v0 = this->pts[2] - this->pts[0];
132  Vector3d v1 = this->pts[1] - this->pts[0];
133  Vector3d v2 = _pt - this->pts[0];
134 
135  double dot00 = v0.Dot(v0);
136  double dot01 = v0.Dot(v1);
137  double dot02 = v0.Dot(v2);
138  double dot11 = v1.Dot(v1);
139  double dot12 = v1.Dot(v2);
140 
141  // Compute barycentric coordinates
142  double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
143  double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
144  double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
145 
146  // Check if point is in triangle
147  return (u >= 0) && (v >= 0) && (u + v <= 1);
148  }
149  else
150  {
151  return false;
152  }
153  }
154 
157  public: Vector3d Normal() const
158  {
159  return Vector3d::Normal(this->pts[0], this->pts[1], this->pts[2]);
160  }
161 
178  public: bool Intersects(const Line3<T> &_line, Vector3<T> &_ipt1) const
179  {
180  // Triangle normal
181  Vector3d norm = this->Normal();
182 
183  // Ray direction to intersect with triangle
184  Vector3d dir = (_line[1] - _line[0]).Normalize();
185 
186  double denom = norm.Dot(dir);
187 
188  // Handle the case when the line is not co-planar with the triangle
189  if (!math::equal(denom, 0.0))
190  {
191  // Distance from line start to triangle intersection
192  double intersection =
193  -norm.Dot(_line[0] - this->pts[0]) / denom;
194 
195  // Make sure the ray intersects the triangle
196  if (intersection < 1.0 || intersection > _line.Length())
197  return false;
198 
199  // Return point of intersection
200  _ipt1 = _line[0] + (dir * intersection);
201 
202  return true;
203  }
204  // Line co-planar with triangle
205  else
206  {
207  // If the line is completely inside the triangle
208  if (this->Contains(_line))
209  {
210  _ipt1 = _line[0];
211  return true;
212  }
213  // If the line intersects the first side
214  else if (_line.Intersect(this->Side(0), _ipt1))
215  {
216  return true;
217  }
218  // If the line intersects the second side
219  else if (_line.Intersect(this->Side(1), _ipt1))
220  {
221  return true;
222  }
223  // If the line intersects the third side
224  else if (_line.Intersect(this->Side(2), _ipt1))
225  {
226  return true;
227  }
228  }
229 
230  return false;
231  }
232 
235  public: T Perimeter() const
236  {
237  return this->Side(0).Length() + this->Side(1).Length() +
238  this->Side(2).Length();
239  }
240 
243  public: double Area() const
244  {
245  double s = this->Perimeter() / 2.0;
246  T a = this->Side(0).Length();
247  T b = this->Side(1).Length();
248  T c = this->Side(2).Length();
249 
250  // Heron's formula
251  // http://en.wikipedia.org/wiki/Heron%27s_formula
252  return sqrt(s * (s-a) * (s-b) * (s-c));
253  }
254 
259  public: Vector3<T> operator[](const size_t _index) const
260  {
261  return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
262  }
263 
265  private: Vector3<T> pts[3];
266  };
267 
270 
273 
276  }
277 }
278 #endif
double Area() const
Get the area of this triangle.
Definition: Triangle3.hh:243
Line3< T > Side(const unsigned int _index) const
Get a line segment for one side of the triangle.
Definition: Triangle3.hh:103
Triangle3< int > Triangle3i
Integer specialization of the Triangle class.
Definition: Triangle3.hh:269
T Length() const
Get the length of the line.
Definition: Line3.hh:141
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition: Helpers.hh:213
bool Contains(const Line3< T > &_line) const
Check if this triangle completely contains the given line segment.
Definition: Triangle3.hh:118
bool Intersect(const Line3< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line3.hh:232
Vector3< T > operator[](const size_t _index) const
Get one of points that define the triangle.
Definition: Triangle3.hh:259
Plane< double > Planed
Definition: Plane.hh:226
On the plane.
Definition: Plane.hh:47
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition: Helpers.hh:542
Vector3d Normal() const
Get the triangle&#39;s normal vector.
Definition: Triangle3.hh:157
bool Valid() const
Get whether this triangle is valid, based on triangle inequality: the sum of the lengths of any two s...
Definition: Triangle3.hh:88
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:195
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition: Helpers.hh:219
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
Triangle3< double > Triangle3d
Double specialization of the Triangle class.
Definition: Triangle3.hh:272
static Vector3 Normal(const Vector3< double > &_v1, const Vector3< double > &_v2, const Vector3< double > &_v3)
Get a normal vector to a triangle.
Definition: Vector3.hh:248
A 3-dimensional triangle and related functions.
Definition: Triangle3.hh:32
Triangle3()=default
Default constructor.
void Set(const Vector3< T > &_pt1, const Vector3< T > &_pt2, const Vector3< T > &_pt3)
Set all vertices of the triangle.
Definition: Triangle3.hh:75
Triangle3< float > Triangle3f
Float specialization of the Triangle class.
Definition: Triangle3.hh:275
A three dimensional line segment.
Definition: Line3.hh:31
bool Contains(const Vector3< T > &_pt) const
Get whether this triangle contains the given point.
Definition: Triangle3.hh:126
Definition: Angle.hh:38
Triangle3(const Vector3< T > &_pt1, const Vector3< T > &_pt2, const Vector3< T > &_pt3)
Constructor.
Definition: Triangle3.hh:45
void Set(const unsigned int _index, const Vector3< T > &_pt)
Set one vertex of the triangle.
Definition: Triangle3.hh:61
T Perimeter() const
Get the length of the triangle&#39;s perimeter.
Definition: Triangle3.hh:235
bool Intersects(const Line3< T > &_line, Vector3< T > &_ipt1) const
Get whether the given line intersects an edge of this triangle.
Definition: Triangle3.hh:178
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:392