Gazebo Math

API Reference

7.4.0
gz/math/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
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: Triangle() = default;
40
45 public: Triangle(const math::Vector2<T> &_pt1,
46 const math::Vector2<T> &_pt2,
47 const math::Vector2<T> &_pt3)
48 {
49 this->Set(_pt1, _pt2, _pt3);
50 }
51
57 public: void Set(const unsigned int _index, const math::Vector2<T> &_pt)
58 {
59 this->pts[clamp(_index, 0u, 2u)] = _pt;
60 }
61
66 public: void Set(const math::Vector2<T> &_pt1,
67 const math::Vector2<T> &_pt2,
68 const math::Vector2<T> &_pt3)
69 {
70 this->pts[0] = _pt1;
71 this->pts[1] = _pt2;
72 this->pts[2] = _pt3;
73 }
74
79 public: bool Valid() const
80 {
81 T a = this->Side(0).Length();
82 T b = this->Side(1).Length();
83 T c = this->Side(2).Length();
84 return (a+b) > c && (b+c) > a && (c+a) > b;
85 }
86
94 public: Line2<T> Side(const unsigned int _index) const
95 {
96 if (_index == 0)
97 return Line2<T>(this->pts[0], this->pts[1]);
98 else if (_index == 1)
99 return Line2<T>(this->pts[1], this->pts[2]);
100 else
101 return Line2<T>(this->pts[2], this->pts[0]);
102 }
103
109 public: bool Contains(const Line2<T> &_line) const
110 {
111 return this->Contains(_line[0]) && this->Contains(_line[1]);
112 }
113
117 public: bool Contains(const math::Vector2<T> &_pt) const
118 {
119 // Compute vectors
120 math::Vector2<T> v0 = this->pts[2] -this->pts[0];
121 math::Vector2<T> v1 = this->pts[1] -this->pts[0];
122 math::Vector2<T> v2 = _pt - this->pts[0];
123
124 // Compute dot products
125 double dot00 = v0.Dot(v0);
126 double dot01 = v0.Dot(v1);
127 double dot02 = v0.Dot(v2);
128 double dot11 = v1.Dot(v1);
129 double dot12 = v1.Dot(v2);
130
131 // Compute barycentric coordinates
132 double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
133 double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
134 double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
135
136 // Check if point is in triangle
137 return (u >= 0) && (v >= 0) && (u + v <= 1);
138 }
139
147 public: bool Intersects(const Line2<T> &_line,
148 math::Vector2<T> &_ipt1,
149 math::Vector2<T> &_ipt2) const
150 {
151 if (this->Contains(_line))
152 {
153 _ipt1 = _line[0];
154 _ipt2 = _line[1];
155 return true;
156 }
157
158 Line2<T> line1(this->pts[0], this->pts[1]);
159 Line2<T> line2(this->pts[1], this->pts[2]);
160 Line2<T> line3(this->pts[2], this->pts[0]);
161
164
165 if (line1.Intersect(_line, pt))
166 points.insert(pt);
167
168 if (line2.Intersect(_line, pt))
169 points.insert(pt);
170
171 if (line3.Intersect(_line, pt))
172 points.insert(pt);
173
174 if (points.empty())
175 {
176 return false;
177 }
178 else if (points.size() == 1)
179 {
180 auto iter = points.begin();
181
182 _ipt1 = *iter;
183 if (this->Contains(_line[0]))
184 _ipt2 = _line[0];
185 else
186 {
187 _ipt2 = _line[1];
188 }
189 }
190 else
191 {
192 auto iter = points.begin();
193 _ipt1 = *(iter++);
194 _ipt2 = *iter;
195 }
196
197 return true;
198 }
199
202 public: T Perimeter() const
203 {
204 return this->Side(0).Length() + this->Side(1).Length() +
205 this->Side(2).Length();
206 }
207
210 public: double Area() const
211 {
212 double s = this->Perimeter() / 2.0;
213 T a = this->Side(0).Length();
214 T b = this->Side(1).Length();
215 T c = this->Side(2).Length();
216
217 // Heron's formula
218 // http://en.wikipedia.org/wiki/Heron%27s_formula
219 return sqrt(s * (s-a) * (s-b) * (s-c));
220 }
221
227 public: math::Vector2<T> operator[](const size_t _index) const
228 {
229 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_TWO_SIZE_T)];
230 }
231
233 private: math::Vector2<T> pts[3];
234 };
235
238
241
244 }
245 }
246}
247#endif