Loading...
Searching...
No Matches
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
24#include <ignition/math/config.hh>
25
26namespace ignition
27{
28 namespace math
29 {
30 inline namespace IGNITION_MATH_VERSION_NAMESPACE
31 {
34 template<typename T>
36 {
38 public: Triangle3() = default;
39
48 public: Triangle3(const Vector3<T> &_pt1,
49 const Vector3<T> &_pt2,
50 const Vector3<T> &_pt3)
51 {
52 this->Set(_pt1, _pt2, _pt3);
53 }
54
64 public: void Set(const unsigned int _index, const Vector3<T> &_pt)
65 {
66 this->pts[clamp(_index, 0u, 2u)] = _pt;
67 }
68
78 public: void Set(const Vector3<T> &_pt1,
79 const Vector3<T> &_pt2,
80 const Vector3<T> &_pt3)
81 {
82 this->pts[0] = _pt1;
83 this->pts[1] = _pt2;
84 this->pts[2] = _pt3;
85 }
86
91 public: bool Valid() const
92 {
93 T a = this->Side(0).Length();
94 T b = this->Side(1).Length();
95 T c = this->Side(2).Length();
96 return (a+b) > c && (b+c) > a && (c+a) > b;
97 }
98
106 public: Line3<T> Side(const unsigned int _index) const
107 {
108 if (_index == 0)
109 return Line3<T>(this->pts[0], this->pts[1]);
110 else if (_index == 1)
111 return Line3<T>(this->pts[1], this->pts[2]);
112 else
113 return Line3<T>(this->pts[2], this->pts[0]);
114 }
115
121 public: bool Contains(const Line3<T> &_line) const
122 {
123 return this->Contains(_line[0]) && this->Contains(_line[1]);
124 }
125
129 public: bool Contains(const Vector3<T> &_pt) const
130 {
131 // Make sure the point is on the same plane as the triangle
132 if (Planed(this->Normal()).Side(_pt) == Planed::NO_SIDE)
133 {
134 Vector3d v0 = this->pts[2] - this->pts[0];
135 Vector3d v1 = this->pts[1] - this->pts[0];
136 Vector3d v2 = _pt - this->pts[0];
137
138 double dot00 = v0.Dot(v0);
139 double dot01 = v0.Dot(v1);
140 double dot02 = v0.Dot(v2);
141 double dot11 = v1.Dot(v1);
142 double dot12 = v1.Dot(v2);
143
144 // Compute barycentric coordinates
145 double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
146 double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
147 double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
148
149 // Check if point is in triangle
150 return (u >= 0) && (v >= 0) && (u + v <= 1);
151 }
152 else
153 {
154 return false;
155 }
156 }
157
160 public: Vector3d Normal() const
161 {
162 return Vector3d::Normal(this->pts[0], this->pts[1], this->pts[2]);
163 }
164
181 public: bool Intersects(const Line3<T> &_line, Vector3<T> &_ipt1) const
182 {
183 // Triangle normal
184 Vector3d norm = this->Normal();
185
186 // Ray direction to intersect with triangle
187 Vector3d dir = (_line[1] - _line[0]).Normalize();
188
189 double denom = norm.Dot(dir);
190
191 // Handle the case when the line is not co-planar with the triangle
192 if (!math::equal(denom, 0.0))
193 {
194 // Distance from line start to triangle intersection
195 double intersection =
196 -norm.Dot(_line[0] - this->pts[0]) / denom;
197
198 // Make sure the ray intersects the triangle
199 if (intersection < 1.0 || intersection > _line.Length())
200 return false;
201
202 // Return point of intersection
203 _ipt1 = _line[0] + (dir * intersection);
204
205 return true;
206 }
207 // Line co-planar with triangle
208 else
209 {
210 // If the line is completely inside the triangle
211 if (this->Contains(_line))
212 {
213 _ipt1 = _line[0];
214 return true;
215 }
216 // If the line intersects the first side
217 else if (_line.Intersect(this->Side(0), _ipt1))
218 {
219 return true;
220 }
221 // If the line intersects the second side
222 else if (_line.Intersect(this->Side(1), _ipt1))
223 {
224 return true;
225 }
226 // If the line intersects the third side
227 else if (_line.Intersect(this->Side(2), _ipt1))
228 {
229 return true;
230 }
231 }
232
233 return false;
234 }
235
238 public: T Perimeter() const
239 {
240 return this->Side(0).Length() + this->Side(1).Length() +
241 this->Side(2).Length();
242 }
243
246 public: double Area() const
247 {
248 double s = this->Perimeter() / 2.0;
249 T a = this->Side(0).Length();
250 T b = this->Side(1).Length();
251 T c = this->Side(2).Length();
252
253 // Heron's formula
254 // http://en.wikipedia.org/wiki/Heron%27s_formula
255 return sqrt(s * (s-a) * (s-b) * (s-c));
256 }
257
262 public: Vector3<T> operator[](const size_t _index) const
263 {
264 return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
265 }
266
268 private: Vector3<T> pts[3];
269 };
270
273
276
279 }
280 }
281}
282#endif
A three dimensional line segment.
Definition Line3.hh:35
bool Intersect(const Line3< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition Line3.hh:235
T Length() const
Get the length of the line.
Definition Line3.hh:144
@ NO_SIDE
On the plane.
Definition Plane.hh:50
A 3-dimensional triangle and related functions.
Definition Triangle3.hh:36
Vector3d Normal() const
Get the triangle's normal vector.
Definition Triangle3.hh:160
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:91
bool Contains(const Vector3< T > &_pt) const
Get whether this triangle contains the given point.
Definition Triangle3.hh:129
T Perimeter() const
Get the length of the triangle's perimeter.
Definition Triangle3.hh:238
void Set(const Vector3< T > &_pt1, const Vector3< T > &_pt2, const Vector3< T > &_pt3)
Set all vertices of the triangle.
Definition Triangle3.hh:78
Vector3< T > operator[](const size_t _index) const
Get one of points that define the triangle.
Definition Triangle3.hh:262
double Area() const
Get the area of this triangle.
Definition Triangle3.hh:246
bool Contains(const Line3< T > &_line) const
Check if this triangle completely contains the given line segment.
Definition Triangle3.hh:121
void Set(const unsigned int _index, const Vector3< T > &_pt)
Set one vertex of the triangle.
Definition Triangle3.hh:64
Triangle3(const Vector3< T > &_pt1, const Vector3< T > &_pt2, const Vector3< T > &_pt3)
Constructor.
Definition Triangle3.hh:48
bool Intersects(const Line3< T > &_line, Vector3< T > &_ipt1) const
Get whether the given line intersects an edge of this triangle.
Definition Triangle3.hh:181
Line3< T > Side(const unsigned int _index) const
Get a line segment for one side of the triangle.
Definition Triangle3.hh:106
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
static Vector3 Normal(const Vector3< T > &_v1, const Vector3< T > &_v2, const Vector3< T > &_v3)
Get a normal vector to a triangle.
Definition Vector3.hh:251
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition Vector3.hh:198
Triangle3< double > Triangle3d
Double specialization of the Triangle class.
Definition Triangle3.hh:275
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition Helpers.hh:395
Plane< double > Planed
Definition Plane.hh:229
static const size_t IGN_TWO_SIZE_T
size_t type with a value of 2
Definition Helpers.hh:222
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:545
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition Helpers.hh:216
Triangle3< float > Triangle3f
Float specialization of the Triangle class.
Definition Triangle3.hh:278
Triangle3< int > Triangle3i
Integer specialization of the Triangle class.
Definition Triangle3.hh:272
Definition Angle.hh:40