Gazebo Math

API Reference

8.0.0
Plane.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012 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_PLANE_HH_
18#define GZ_MATH_PLANE_HH_
19
21#include <gz/math/Vector2.hh>
22#include <gz/math/Vector3.hh>
23#include <gz/math/config.hh>
24#include <gz/math/Line2.hh>
25#include <gz/math/Quaternion.hh>
26#include <optional>
27
28namespace gz::math
29{
30 // Inline bracket to help doxygen filtering.
31 inline namespace GZ_MATH_VERSION_NAMESPACE {
32 //
35 template<typename T>
36 class Plane
37 {
41 public: enum PlaneSide
42 {
45 NEGATIVE_SIDE = 0,
46
49 POSITIVE_SIDE = 1,
50
52 NO_SIDE = 2,
53
55 BOTH_SIDE = 3
56 };
57
59 public: Plane()
60 : d(0.0)
61 {
62 }
63
67 public: explicit Plane(const Vector3<T> &_normal, T _offset = 0.0)
68 : normal(_normal), d(_offset)
69 {
70 }
71
76 public: Plane(const Vector3<T> &_normal, const Vector2<T> &_size,
77 T _offset)
78 {
79 this->Set(_normal, _size, _offset);
80 }
81
85 public: void Set(const Vector3<T> &_normal, T _offset)
86 {
87 this->normal = _normal;
88 this->d = _offset;
89 }
90
95 public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
96 T _offset)
97 {
98 this->normal = _normal;
99 this->size = _size;
100 this->d = _offset;
101 }
102
109 public: T Distance(const Vector3<T> &_point) const
110 {
111 return this->normal.Dot(_point) - this->d;
112 }
113
122 public: std::optional<Vector3<T>> Intersection(
123 const Vector3<T> &_point,
124 const Vector3<T> &_gradient,
125 const double &_tolerance = 1e-6) const
126 {
127 if (std::abs(this->Normal().Dot(_gradient)) < _tolerance)
128 {
129 return std::nullopt;
130 }
131 auto constant = this->Offset() - this->Normal().Dot(_point);
132 auto param = constant / this->Normal().Dot(_gradient);
133 auto intersection = _point + _gradient*param;
134
135 if (this->Size() == Vector2<T>(0, 0))
136 return intersection;
137
138 // Check if the point is within the size bounds
139 // To do this we create a Quaternion using Angle, Axis constructor and
140 // rotate the Y and X axis the same amount as the normal.
141 auto dotProduct = Vector3<T>::UnitZ.Dot(this->Normal());
142 auto angle = acos(dotProduct / this->Normal().Length());
143 auto axis = Vector3<T>::UnitZ.Cross(this->Normal().Normalized());
145
148
149 auto xBasis = rotatedXAxis.Dot(intersection);
150 auto yBasis = rotatedYAxis.Dot(intersection);
151
152 if (std::abs(xBasis) < this->Size().X() / 2 &&
153 std::abs(yBasis) < this->Size().Y() / 2)
154 {
155 return intersection;
156 }
157 return std::nullopt;
158 }
159
166 public: PlaneSide Side(const Vector3<T> &_point) const
167 {
168 T dist = this->Distance(_point);
169
170 if (dist < 0.0)
171 return NEGATIVE_SIDE;
172
173 if (dist > 0.0)
174 return POSITIVE_SIDE;
175
176 return NO_SIDE;
177 }
178
186 {
187 double dist = this->Distance(_box.Center());
188 double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
189
190 if (dist < -maxAbsDist)
191 return NEGATIVE_SIDE;
192
193 if (dist > maxAbsDist)
194 return POSITIVE_SIDE;
195
196 return BOTH_SIDE;
197 }
198
203 public: T Distance(const Vector3<T> &_origin,
204 const Vector3<T> &_dir) const
205 {
206 T denom = this->normal.Dot(_dir);
207
208 if (std::abs(denom) < 1e-3)
209 {
210 // parallel
211 return 0;
212 }
213 else
214 {
215 T nom = _origin.Dot(this->normal) - this->d;
216 T t = -(nom/denom);
217 return t;
218 }
219 }
220
222 public: inline const Vector2<T> &Size() const
223 {
224 return this->size;
225 }
226
228 public: inline Vector2<T> &Size()
229 {
230 return this->size;
231 }
232
234 public: inline const Vector3<T> &Normal() const
235 {
236 return this->normal;
237 }
238
240 public: inline Vector3<T> &Normal()
241 {
242 return this->normal;
243 }
244
246 public: inline T Offset() const
247 {
248 return this->d;
249 }
250
252 private: Vector3<T> normal;
253
255 private: Vector2<T> size;
256
258 private: T d;
259 };
260
264 } // namespace GZ_MATH_VERSION_NAMESPACE
265} // namespace gz::math
266#endif // GZ_MATH_PLANE_HH_