Triangle3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 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 
18 #ifndef _IGNITION_TRIANGLE3_HH_
19 #define _IGNITION_TRIANGLE3_HH_
20 
21 #include <ignition/math/Line3.hh>
22 #include <ignition/math/Plane.hh>
23 #include <ignition/math/Vector3.hh>
25 
26 namespace ignition
27 {
28  namespace math
29  {
32  template<typename T>
33  class Triangle3
34  {
36  public: Triangle3() = default;
37 
46  public: Triangle3(const Vector3<T> &_pt1,
47  const Vector3<T> &_pt2,
48  const Vector3<T> &_pt3)
49  {
50  this->Set(_pt1, _pt2, _pt3);
51  }
52 
62  public: void Set(const unsigned int _index, const Vector3<T> &_pt)
63  {
64  if (_index > 2)
65  throw IndexException();
66  else
67  this->pts[_index] = _pt;
68  }
69 
79  public: void Set(const Vector3<T> &_pt1,
80  const Vector3<T> &_pt2,
81  const Vector3<T> &_pt3)
82  {
83  this->pts[0] = _pt1;
84  this->pts[1] = _pt2;
85  this->pts[2] = _pt3;
86  }
87 
92  public: bool Valid() const
93  {
94  T a = this->Side(0).Length();
95  T b = this->Side(1).Length();
96  T c = this->Side(2).Length();
97  return (a+b) > c && (b+c) > a && (c+a) > b;
98  }
99 
107  public: Line3<T> Side(const unsigned int _index) const
108  {
109  if (_index > 2)
110  throw IndexException();
111  else if (_index == 0)
112  return Line3<T>(this->pts[0], this->pts[1]);
113  else if (_index == 1)
114  return Line3<T>(this->pts[1], this->pts[2]);
115  else
116  return Line3<T>(this->pts[2], this->pts[0]);
117  }
118 
124  public: bool Contains(const Line3<T> &_line) const
125  {
126  return this->Contains(_line[0]) && this->Contains(_line[1]);
127  }
128 
132  public: bool Contains(const Vector3<T> &_pt) const
133  {
134  // Make sure the point is on the same plane as the triangle
135  if (Planed(this->Normal()).Side(_pt) == Planed::NO_SIDE)
136  {
137  Vector3d v0 = this->pts[2] - this->pts[0];
138  Vector3d v1 = this->pts[1] - this->pts[0];
139  Vector3d v2 = _pt - this->pts[0];
140 
141  double dot00 = v0.Dot(v0);
142  double dot01 = v0.Dot(v1);
143  double dot02 = v0.Dot(v2);
144  double dot11 = v1.Dot(v1);
145  double dot12 = v1.Dot(v2);
146 
147  // Compute barycentric coordinates
148  double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
149  double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
150  double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
151 
152  // Check if point is in triangle
153  return (u >= 0) && (v >= 0) && (u + v <= 1);
154  }
155  else
156  {
157  return false;
158  }
159  }
160 
163  public: Vector3d Normal() const
164  {
165  return Vector3d::Normal(this->pts[0], this->pts[1], this->pts[2]);
166  }
167 
184  public: bool Intersects(const Line3<T> &_line, Vector3<T> &_ipt1) const
185  {
186  // Triangle normal
187  Vector3d norm = this->Normal();
188 
189  // Ray direction to intersect with triangle
190  Vector3d dir = (_line[1] - _line[0]).Normalize();
191 
192  double denom = norm.Dot(dir);
193 
194  // Handle the case when the line is not co-planar with the triangle
195  if (!math::equal(denom, 0.0))
196  {
197  // Distance from line start to triangle intersection
198  double intersection =
199  -norm.Dot(_line[0] - this->pts[0]) / denom;
200 
201  // Make sure the ray intersects the triangle
202  if (intersection < 1.0 || intersection > _line.Length())
203  return false;
204 
205  // Return point of intersection
206  _ipt1 = _line[0] + (dir * intersection);
207 
208  return true;
209  }
210  // Line co-planar with triangle
211  else
212  {
213  // If the line is completely inside the triangle
214  if (this->Contains(_line))
215  {
216  _ipt1 = _line[0];
217  return true;
218  }
219  // If the line intersects the first side
220  else if (_line.Intersect(this->Side(0), _ipt1))
221  {
222  return true;
223  }
224  // If the line intersects the second side
225  else if (_line.Intersect(this->Side(1), _ipt1))
226  {
227  return true;
228  }
229  // If the line intersects the third side
230  else if (_line.Intersect(this->Side(2), _ipt1))
231  {
232  return true;
233  }
234  }
235 
236  return false;
237  }
238 
241  public: T Perimeter() const
242  {
243  return this->Side(0).Length() + this->Side(1).Length() +
244  this->Side(2).Length();
245  }
246 
249  public: double Area() const
250  {
251  double s = this->Perimeter() / 2.0;
252  T a = this->Side(0).Length();
253  T b = this->Side(1).Length();
254  T c = this->Side(2).Length();
255 
256  // Heron's formula
257  // http://en.wikipedia.org/wiki/Heron%27s_formula
258  return sqrt(s * (s-a) * (s-b) * (s-c));
259  }
260 
264  public: Vector3<T> operator[](size_t _index) const
265  {
266  if (_index > 2)
267  throw IndexException();
268  return this->pts[_index];
269  }
270 
272  private: Vector3<T> pts[3];
273  };
274 
277 
280 
283  }
284 }
285 #endif
T Length() const
Get the length of the line.
Definition: Line3.hh:142
Triangle3< int > Triangle3i
Integer specialization of the Triangle class.
Definition: Triangle3.hh:276
bool Intersect(const Line3< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition: Line3.hh:233
double Area() const
Get the area of this triangle.
Definition: Triangle3.hh:249
Vector3d Normal() const
Get the triangle's normal vector.
Definition: Triangle3.hh:163
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:92
Plane< double > Planed
Definition: Plane.hh:228
On the plane.
Definition: Plane.hh:48
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:187
bool Intersects(const Line3< T > &_line, Vector3< T > &_ipt1) const
Get whether the given line intersects an edge of this triangle.
Definition: Triangle3.hh:184
Line3< T > Side(const unsigned int _index) const
Get a line segment for one side of the triangle.
Definition: Triangle3.hh:107
Exception that is thrown when an out-of-bounds index is encountered.
Definition: IndexException.hh:37
T Perimeter() const
Get the length of the triangle's perimeter.
Definition: Triangle3.hh:241
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
Triangle3< double > Triangle3d
Double specialization of the Triangle class.
Definition: Triangle3.hh:279
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:240
A 3-dimensional triangle and related functions.
Definition: Triangle3.hh:33
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:79
Triangle3< float > Triangle3f
Float specialization of the Triangle class.
Definition: Triangle3.hh:282
A three dimensional line segment.
Definition: Line3.hh:32
Vector3< T > operator[](size_t _index) const
Get one of points that define the triangle.
Definition: Triangle3.hh:264
bool Contains(const Line3< T > &_line) const
Check if this triangle completely contains the given line segment.
Definition: Triangle3.hh:124
Definition: AffineException.hh:30
Triangle3(const Vector3< T > &_pt1, const Vector3< T > &_pt2, const Vector3< T > &_pt3)
Constructor.
Definition: Triangle3.hh:46
void Set(const unsigned int _index, const Vector3< T > &_pt)
Set one vertex of the triangle.
Definition: Triangle3.hh:62
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:257
bool Contains(const Vector3< T > &_pt) const
Get whether this triangle contains the given point.
Definition: Triangle3.hh:132