Gazebo Math

API Reference

7.4.0
gz/math/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
29{
30 namespace math
31 {
32 // Inline bracket to help doxygen filtering.
33 inline namespace GZ_MATH_VERSION_NAMESPACE {
34 //
37 template<typename T>
38 class Plane
39 {
43 public: enum PlaneSide
44 {
47 NEGATIVE_SIDE = 0,
48
51 POSITIVE_SIDE = 1,
52
54 NO_SIDE = 2,
55
57 BOTH_SIDE = 3
58 };
59
61 public: Plane()
62 : d(0.0)
63 {
64 }
65
69 public: explicit Plane(const Vector3<T> &_normal, T _offset = 0.0)
70 : normal(_normal), d(_offset)
71 {
72 }
73
78 public: Plane(const Vector3<T> &_normal, const Vector2<T> &_size,
79 T _offset)
80 {
81 this->Set(_normal, _size, _offset);
82 }
83
86 public: Plane(const Plane &_plane) = default;
87
89 public: ~Plane() = default;
90
94 public: void Set(const Vector3<T> &_normal, T _offset)
95 {
96 this->normal = _normal;
97 this->d = _offset;
98 }
99
104 public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
105 T _offset)
106 {
107 this->normal = _normal;
108 this->size = _size;
109 this->d = _offset;
110 }
111
118 public: T Distance(const Vector3<T> &_point) const
119 {
120 return this->normal.Dot(_point) - this->d;
121 }
122
131 public: std::optional<Vector3<T>> Intersection(
132 const Vector3<T> &_point,
133 const Vector3<T> &_gradient,
134 const double &_tolerance = 1e-6) const
135 {
136 if (std::abs(this->Normal().Dot(_gradient)) < _tolerance)
137 {
138 return std::nullopt;
139 }
140 auto constant = this->Offset() - this->Normal().Dot(_point);
141 auto param = constant / this->Normal().Dot(_gradient);
142 auto intersection = _point + _gradient*param;
143
144 if (this->Size() == Vector2<T>(0, 0))
145 return intersection;
146
147 // Check if the point is within the size bounds
148 // To do this we create a Quaternion using Angle, Axis constructor and
149 // rotate the Y and X axis the same amount as the normal.
150 auto dotProduct = Vector3<T>::UnitZ.Dot(this->Normal());
151 auto angle = acos(dotProduct / this->Normal().Length());
152 auto axis = Vector3<T>::UnitZ.Cross(this->Normal().Normalized());
153 Quaternion<T> rotation(axis, angle);
154
155 Vector3<T> rotatedXAxis = rotation * Vector3<T>::UnitX;
156 Vector3<T> rotatedYAxis = rotation * Vector3<T>::UnitY;
157
158 auto xBasis = rotatedXAxis.Dot(intersection);
159 auto yBasis = rotatedYAxis.Dot(intersection);
160
161 if (std::abs(xBasis) < this->Size().X() / 2 &&
162 std::abs(yBasis) < this->Size().Y() / 2)
163 {
164 return intersection;
165 }
166 return std::nullopt;
167 }
168
175 public: PlaneSide Side(const Vector3<T> &_point) const
176 {
177 T dist = this->Distance(_point);
178
179 if (dist < 0.0)
180 return NEGATIVE_SIDE;
181
182 if (dist > 0.0)
183 return POSITIVE_SIDE;
184
185 return NO_SIDE;
186 }
187
194 public: PlaneSide Side(const math::AxisAlignedBox &_box) const
195 {
196 double dist = this->Distance(_box.Center());
197 double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
198
199 if (dist < -maxAbsDist)
200 return NEGATIVE_SIDE;
201
202 if (dist > maxAbsDist)
203 return POSITIVE_SIDE;
204
205 return BOTH_SIDE;
206 }
207
212 public: T Distance(const Vector3<T> &_origin,
213 const Vector3<T> &_dir) const
214 {
215 T denom = this->normal.Dot(_dir);
216
217 if (std::abs(denom) < 1e-3)
218 {
219 // parallel
220 return 0;
221 }
222 else
223 {
224 T nom = _origin.Dot(this->normal) - this->d;
225 T t = -(nom/denom);
226 return t;
227 }
228 }
229
231 public: inline const Vector2<T> &Size() const
232 {
233 return this->size;
234 }
235
237 public: inline Vector2<T> &Size()
238 {
239 return this->size;
240 }
241
243 public: inline const Vector3<T> &Normal() const
244 {
245 return this->normal;
246 }
247
249 public: inline Vector3<T> &Normal()
250 {
251 return this->normal;
252 }
253
255 public: inline T Offset() const
256 {
257 return this->d;
258 }
259
263 public: Plane<T> &operator=(const Plane<T> &_p) = default;
264
266 private: Vector3<T> normal;
267
269 private: Vector2<T> size;
270
272 private: T d;
273 };
274
278 }
279 }
280}
281
282#endif