Gazebo Math

API Reference

7.4.0
gz/math/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
27{
28 namespace math
29 {
30 // Inline bracket to help doxygen filtering.
31 inline namespace GZ_MATH_VERSION_NAMESPACE {
32 //
35 template<typename T>
37 {
39 public: Triangle3() = default;
40
49 public: Triangle3(const Vector3<T> &_pt1,
50 const Vector3<T> &_pt2,
51 const Vector3<T> &_pt3)
52 {
53 this->Set(_pt1, _pt2, _pt3);
54 }
55
65 public: void Set(const unsigned int _index, const Vector3<T> &_pt)
66 {
67 this->pts[clamp(_index, 0u, 2u)] = _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 == 0)
110 return Line3<T>(this->pts[0], this->pts[1]);
111 else if (_index == 1)
112 return Line3<T>(this->pts[1], this->pts[2]);
113 else
114 return Line3<T>(this->pts[2], this->pts[0]);
115 }
116
122 public: bool Contains(const Line3<T> &_line) const
123 {
124 return this->Contains(_line[0]) && this->Contains(_line[1]);
125 }
126
130 public: bool Contains(const Vector3<T> &_pt) const
131 {
132 // Make sure the point is on the same plane as the triangle
133 if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2]))
134 == Planed::NO_SIDE)
135 {
136 Vector3<T> v0 = this->pts[2] - this->pts[0];
137 Vector3<T> v1 = this->pts[1] - this->pts[0];
138 Vector3<T> v2 = _pt - this->pts[0];
139
140 double dot00 = v0.Dot(v0);
141 double dot01 = v0.Dot(v1);
142 double dot02 = v0.Dot(v2);
143 double dot11 = v1.Dot(v1);
144 double dot12 = v1.Dot(v2);
145
146 // Compute barycentric coordinates
147 double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
148 double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
149 double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
150
151 // Check if point is in triangle
152 return (u >= 0) && (v >= 0) && (u + v <= 1);
153 }
154 return false;
155 }
156
159 public: Vector3d Normal() const
160 {
161 return math::Vector3d::Normal(
162 Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]),
163 Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]),
164 Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2]));
165 }
166
183 public: bool Intersects(
184 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 Vector3<T> dir = (_line[1] - _line[0]).Normalize();
191
192 double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2]));
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 Vector3<T> diff = _line[0] - this->pts[0];
199 double intersection =
200 -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom;
201
202 // Make sure the ray intersects the triangle
203 if (intersection < 1.0 || intersection > _line.Length())
204 return false;
205
206 // Return point of intersection
207 _ipt1 = _line[0] + (dir * intersection);
208
209 return true;
210 }
211 // Line co-planar with triangle
212 else
213 {
214 // If the line is completely inside the triangle
215 if (this->Contains(_line))
216 {
217 _ipt1 = _line[0];
218 return true;
219 }
220 // If the line intersects the first side
221 else if (_line.Intersect(this->Side(0), _ipt1))
222 {
223 return true;
224 }
225 // If the line intersects the second side
226 else if (_line.Intersect(this->Side(1), _ipt1))
227 {
228 return true;
229 }
230 // If the line intersects the third side
231 else if (_line.Intersect(this->Side(2), _ipt1))
232 {
233 return true;
234 }
235 }
236
237 return false;
238 }
239
242 public: T Perimeter() const
243 {
244 return this->Side(0).Length() + this->Side(1).Length() +
245 this->Side(2).Length();
246 }
247
250 public: double Area() const
251 {
252 double s = this->Perimeter() / 2.0;
253 T a = this->Side(0).Length();
254 T b = this->Side(1).Length();
255 T c = this->Side(2).Length();
256
257 // Heron's formula
258 // http://en.wikipedia.org/wiki/Heron%27s_formula
259 return sqrt(s * (s-a) * (s-b) * (s-c));
260 }
261
266 public: Vector3<T> operator[](const size_t _index) const
267 {
268 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
269 }
270
272 private: Vector3<T> pts[3];
273 };
274
277
280
283 }
284 }
285}
286#endif