Gazebo Math

API Reference

7.5.1
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 
28 namespace 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 
84  public: Plane(const Plane &_plane) = default;
85 
87  public: ~Plane() = default;
88 
92  public: void Set(const Vector3<T> &_normal, T _offset)
93  {
94  this->normal = _normal;
95  this->d = _offset;
96  }
97 
102  public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
103  T _offset)
104  {
105  this->normal = _normal;
106  this->size = _size;
107  this->d = _offset;
108  }
109 
116  public: T Distance(const Vector3<T> &_point) const
117  {
118  return this->normal.Dot(_point) - this->d;
119  }
120 
129  public: std::optional<Vector3<T>> Intersection(
130  const Vector3<T> &_point,
131  const Vector3<T> &_gradient,
132  const double &_tolerance = 1e-6) const
133  {
134  if (std::abs(this->Normal().Dot(_gradient)) < _tolerance)
135  {
136  return std::nullopt;
137  }
138  auto constant = this->Offset() - this->Normal().Dot(_point);
139  auto param = constant / this->Normal().Dot(_gradient);
140  auto intersection = _point + _gradient*param;
141 
142  if (this->Size() == Vector2<T>(0, 0))
143  return intersection;
144 
145  // Check if the point is within the size bounds
146  // To do this we create a Quaternion using Angle, Axis constructor and
147  // rotate the Y and X axis the same amount as the normal.
148  auto dotProduct = Vector3<T>::UnitZ.Dot(this->Normal());
149  auto angle = acos(dotProduct / this->Normal().Length());
150  auto axis = Vector3<T>::UnitZ.Cross(this->Normal().Normalized());
151  Quaternion<T> rotation(axis, angle);
152 
153  Vector3<T> rotatedXAxis = rotation * Vector3<T>::UnitX;
154  Vector3<T> rotatedYAxis = rotation * Vector3<T>::UnitY;
155 
156  auto xBasis = rotatedXAxis.Dot(intersection);
157  auto yBasis = rotatedYAxis.Dot(intersection);
158 
159  if (std::abs(xBasis) < this->Size().X() / 2 &&
160  std::abs(yBasis) < this->Size().Y() / 2)
161  {
162  return intersection;
163  }
164  return std::nullopt;
165  }
166 
173  public: PlaneSide Side(const Vector3<T> &_point) const
174  {
175  T dist = this->Distance(_point);
176 
177  if (dist < 0.0)
178  return NEGATIVE_SIDE;
179 
180  if (dist > 0.0)
181  return POSITIVE_SIDE;
182 
183  return NO_SIDE;
184  }
185 
192  public: PlaneSide Side(const math::AxisAlignedBox &_box) const
193  {
194  double dist = this->Distance(_box.Center());
195  double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
196 
197  if (dist < -maxAbsDist)
198  return NEGATIVE_SIDE;
199 
200  if (dist > maxAbsDist)
201  return POSITIVE_SIDE;
202 
203  return BOTH_SIDE;
204  }
205 
210  public: T Distance(const Vector3<T> &_origin,
211  const Vector3<T> &_dir) const
212  {
213  T denom = this->normal.Dot(_dir);
214 
215  if (std::abs(denom) < 1e-3)
216  {
217  // parallel
218  return 0;
219  }
220  else
221  {
222  T nom = _origin.Dot(this->normal) - this->d;
223  T t = -(nom/denom);
224  return t;
225  }
226  }
227 
229  public: inline const Vector2<T> &Size() const
230  {
231  return this->size;
232  }
233 
235  public: inline Vector2<T> &Size()
236  {
237  return this->size;
238  }
239 
241  public: inline const Vector3<T> &Normal() const
242  {
243  return this->normal;
244  }
245 
247  public: inline Vector3<T> &Normal()
248  {
249  return this->normal;
250  }
251 
253  public: inline T Offset() const
254  {
255  return this->d;
256  }
257 
261  public: Plane<T> &operator=(const Plane<T> &_p) = default;
262 
264  private: Vector3<T> normal;
265 
267  private: Vector2<T> size;
268 
270  private: T d;
271  };
272 
276  } // namespace GZ_MATH_VERSION_NAMESPACE
277 } // namespace gz::math
278 #endif // GZ_MATH_PLANE_HH_