Gazebo Math

API Reference

7.5.1
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 
26 namespace gz::math
27 {
28  // Inline bracket to help doxygen filtering.
29  inline namespace GZ_MATH_VERSION_NAMESPACE {
30  //
33  template<typename T>
34  class Triangle3
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
201  if (intersection < 1.0 || intersection > _line.Length())
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_