Gazebo Math

API Reference

8.1.0
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 GZ_MATH_TRIANGLE3_HH_
18#define GZ_MATH_TRIANGLE3_HH_
19
20#include <gz/math/Helpers.hh>
21#include <gz/math/Line3.hh>
22#include <gz/math/Plane.hh>
23#include <gz/math/Vector3.hh>
24#include <gz/math/config.hh>
25
26namespace gz::math
27{
28 // Inline bracket to help doxygen filtering.
29 inline namespace GZ_MATH_VERSION_NAMESPACE {
30 //
33 template<typename T>
35 {
37 public: Triangle3() = default;
38
47 public: Triangle3(const Vector3<T> &_pt1,
48 const Vector3<T> &_pt2,
49 const Vector3<T> &_pt3)
50 {
51 this->Set(_pt1, _pt2, _pt3);
52 }
53
63 public: void Set(const unsigned int _index, const Vector3<T> &_pt)
64 {
65 this->pts[clamp(_index, 0u, 2u)] = _pt;
66 }
67
77 public: void Set(const Vector3<T> &_pt1,
78 const Vector3<T> &_pt2,
79 const Vector3<T> &_pt3)
80 {
81 this->pts[0] = _pt1;
82 this->pts[1] = _pt2;
83 this->pts[2] = _pt3;
84 }
85
90 public: bool Valid() const
91 {
92 T a = this->Side(0).Length();
93 T b = this->Side(1).Length();
94 T c = this->Side(2).Length();
95 return (a+b) > c && (b+c) > a && (c+a) > b;
96 }
97
105 public: Line3<T> Side(const unsigned int _index) const
106 {
107 if (_index == 0)
108 return Line3<T>(this->pts[0], this->pts[1]);
109 else if (_index == 1)
110 return Line3<T>(this->pts[1], this->pts[2]);
111 else
112 return Line3<T>(this->pts[2], this->pts[0]);
113 }
114
120 public: bool Contains(const Line3<T> &_line) const
121 {
122 return this->Contains(_line[0]) && this->Contains(_line[1]);
123 }
124
128 public: bool Contains(const Vector3<T> &_pt) const
129 {
130 // Make sure the point is on the same plane as the triangle
131 if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2]))
132 == Planed::NO_SIDE)
133 {
134 Vector3<T> v0 = this->pts[2] - this->pts[0];
135 Vector3<T> v1 = this->pts[1] - this->pts[0];
136 Vector3<T> 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 return false;
153 }
154
157 public: Vector3d Normal() const
158 {
159 return math::Vector3d::Normal(
160 Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]),
161 Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]),
162 Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2]));
163 }
164
181 public: bool Intersects(
182 const Line3<T> &_line, Vector3<T> &_ipt1) const
183 {
184 // Triangle normal
185 Vector3d norm = this->Normal();
186
187 // Ray direction to intersect with triangle
188 Vector3<T> dir = (_line[1] - _line[0]).Normalize();
189
190 double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2]));
191
192 // Handle the case when the line is not co-planar with the triangle
193 if (!math::equal(denom, 0.0))
194 {
195 // Distance from line start to triangle intersection
196 Vector3<T> diff = _line[0] - this->pts[0];
197 double intersection =
198 -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom;
199
200 // Make sure the ray intersects the triangle
202 return false;
203
204 // Return point of intersection
205 _ipt1 = _line[0] + (dir * intersection);
206
207 return true;
208 }
209 // Line co-planar with triangle
210 else
211 {
212 // If the line is completely inside the triangle
213 if (this->Contains(_line))
214 {
215 _ipt1 = _line[0];
216 return true;
217 }
218 // If the line intersects the first side
219 else if (_line.Intersect(this->Side(0), _ipt1))
220 {
221 return true;
222 }
223 // If the line intersects the second side
224 else if (_line.Intersect(this->Side(1), _ipt1))
225 {
226 return true;
227 }
228 // If the line intersects the third side
229 else if (_line.Intersect(this->Side(2), _ipt1))
230 {
231 return true;
232 }
233 }
234
235 return false;
236 }
237
240 public: T Perimeter() const
241 {
242 return this->Side(0).Length() + this->Side(1).Length() +
243 this->Side(2).Length();
244 }
245
248 public: double Area() const
249 {
250 double s = this->Perimeter() / 2.0;
251 T a = this->Side(0).Length();
252 T b = this->Side(1).Length();
253 T c = this->Side(2).Length();
254
255 // Heron's formula
256 // http://en.wikipedia.org/wiki/Heron%27s_formula
257 return sqrt(s * (s-a) * (s-b) * (s-c));
258 }
259
264 public: Vector3<T> operator[](const size_t _index) const
265 {
266 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
267 }
268
270 private: Vector3<T> pts[3];
271 };
272
275
278
281 } // namespace GZ_MATH_VERSION_NAMESPACE
282} // namespace gz::math
283#endif // GZ_MATH_TRIANGLE3_HH_