Gazebo Math

API Reference

8.1.0
Triangle.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2014 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_TRIANGLE_HH_
18#define GZ_MATH_TRIANGLE_HH_
19
20#include <set>
21#include <gz/math/Helpers.hh>
22#include <gz/math/Line2.hh>
23#include <gz/math/Vector2.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: Triangle() = default;
38
46 {
47 this->Set(_pt1, _pt2, _pt3);
48 }
49
55 public: void Set(const unsigned int _index, const math::Vector2<T> &_pt)
56 {
57 this->pts[clamp(_index, 0u, 2u)] = _pt;
58 }
59
64 public: void Set(const math::Vector2<T> &_pt1,
67 {
68 this->pts[0] = _pt1;
69 this->pts[1] = _pt2;
70 this->pts[2] = _pt3;
71 }
72
77 public: bool Valid() const
78 {
79 T a = this->Side(0).Length();
80 T b = this->Side(1).Length();
81 T c = this->Side(2).Length();
82 return (a+b) > c && (b+c) > a && (c+a) > b;
83 }
84
92 public: Line2<T> Side(const unsigned int _index) const
93 {
94 if (_index == 0)
95 return Line2<T>(this->pts[0], this->pts[1]);
96 else if (_index == 1)
97 return Line2<T>(this->pts[1], this->pts[2]);
98 else
99 return Line2<T>(this->pts[2], this->pts[0]);
100 }
101
107 public: bool Contains(const Line2<T> &_line) const
108 {
109 return this->Contains(_line[0]) && this->Contains(_line[1]);
110 }
111
115 public: bool Contains(const math::Vector2<T> &_pt) const
116 {
117 // Compute vectors
118 math::Vector2<T> v0 = this->pts[2] -this->pts[0];
119 math::Vector2<T> v1 = this->pts[1] -this->pts[0];
120 math::Vector2<T> v2 = _pt - this->pts[0];
121
122 // Compute dot products
123 double dot00 = v0.Dot(v0);
124 double dot01 = v0.Dot(v1);
125 double dot02 = v0.Dot(v2);
126 double dot11 = v1.Dot(v1);
127 double dot12 = v1.Dot(v2);
128
129 // Compute barycentric coordinates
130 double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
131 double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
132 double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
133
134 // Check if point is in triangle
135 return (u >= 0) && (v >= 0) && (u + v <= 1);
136 }
137
145 public: bool Intersects(const Line2<T> &_line,
147 math::Vector2<T> &_ipt2) const
148 {
149 if (this->Contains(_line))
150 {
151 _ipt1 = _line[0];
152 _ipt2 = _line[1];
153 return true;
154 }
155
156 Line2<T> line1(this->pts[0], this->pts[1]);
157 Line2<T> line2(this->pts[1], this->pts[2]);
158 Line2<T> line3(this->pts[2], this->pts[0]);
159
162
163 if (line1.Intersect(_line, pt))
164 points.insert(pt);
165
166 if (line2.Intersect(_line, pt))
167 points.insert(pt);
168
169 if (line3.Intersect(_line, pt))
170 points.insert(pt);
171
172 if (points.empty())
173 {
174 return false;
175 }
176 else if (points.size() == 1)
177 {
178 auto iter = points.begin();
179
180 _ipt1 = *iter;
181 if (this->Contains(_line[0]))
182 _ipt2 = _line[0];
183 else
184 {
185 _ipt2 = _line[1];
186 }
187 }
188 else
189 {
190 auto iter = points.begin();
191 _ipt1 = *(iter++);
192 _ipt2 = *iter;
193 }
194
195 return true;
196 }
197
200 public: T Perimeter() const
201 {
202 return this->Side(0).Length() + this->Side(1).Length() +
203 this->Side(2).Length();
204 }
205
208 public: double Area() const
209 {
210 double s = this->Perimeter() / 2.0;
211 T a = this->Side(0).Length();
212 T b = this->Side(1).Length();
213 T c = this->Side(2).Length();
214
215 // Heron's formula
216 // http://en.wikipedia.org/wiki/Heron%27s_formula
217 return sqrt(s * (s-a) * (s-b) * (s-c));
218 }
219
225 public: math::Vector2<T> operator[](const size_t _index) const
226 {
227 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
228 }
229
231 private: math::Vector2<T> pts[3];
232 };
233
236
239
242 } // namespace GZ_MATH_VERSION_NAMESPACE
243} // namespace gz::math
244#endif // GZ_MATH_TRIANGLE_HH_