Gazebo Math
API Reference
8.1.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
src
gz-math
include
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
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
Triangle
35
{
37
public
:
Triangle
() =
default
;
38
43
public
:
Triangle
(
const
math::Vector2<T>
&
_pt1
,
44
const
math::Vector2<T>
&
_pt2
,
45
const
math::Vector2<T>
&
_pt3
)
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
, 0
u
, 2u)] =
_pt
;
58
}
59
64
public
:
void
Set
(
const
math::Vector2<T>
&
_pt1
,
65
const
math::Vector2<T>
&
_pt2
,
66
const
math::Vector2<T>
&
_pt3
)
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
,
146
math::Vector2<T>
&
_ipt1
,
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
160
math::Vector2<T>
pt
;
161
std::set<math::Vector2<T>
>
points
;
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
235
typedef
Triangle<int>
Trianglei
;
236
238
typedef
Triangle<double>
Triangled
;
239
241
typedef
Triangle<float>
Trianglef
;
242
}
// namespace GZ_MATH_VERSION_NAMESPACE
243
}
// namespace gz::math
244
#endif
// GZ_MATH_TRIANGLE_HH_