Gazebo Math

API Reference

6.15.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 ignition
29 {
30  namespace math
31  {
32  // Inline bracket to help doxygen filtering.
33  inline namespace IGNITION_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: 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)
87  : normal(_plane.normal), size(_plane.size), d(_plane.d)
88  {}
89 
91  public: virtual ~Plane() {}
92 
96  public: void Set(const Vector3<T> &_normal, T _offset)
97  {
98  this->normal = _normal;
99  this->d = _offset;
100  }
101 
106  public: void Set(const Vector3<T> &_normal, const Vector2<T> &_size,
107  T _offset)
108  {
109  this->normal = _normal;
110  this->size = _size;
111  this->d = _offset;
112  }
113 
120  public: T Distance(const Vector3<T> &_point) const
121  {
122  return this->normal.Dot(_point) - this->d;
123  }
124 
133  public: std::optional<Vector3<T>> Intersection(
134  const Vector3<T> &_point,
135  const Vector3<T> &_gradient,
136  const double &_tolerance = 1e-6) const
137  {
138  if (std::abs(this->Normal().Dot(_gradient)) < _tolerance)
139  {
140  return std::nullopt;
141  }
142  auto constant = this->Offset() - this->Normal().Dot(_point);
143  auto param = constant / this->Normal().Dot(_gradient);
144  auto intersection = _point + _gradient*param;
145 
146  if (this->Size() == Vector2<T>(0, 0))
147  return intersection;
148 
149  // Check if the point is within the size bounds
150  // To do this we create a Quaternion using Angle, Axis constructor and
151  // rotate the Y and X axis the same amount as the normal.
152  auto dotProduct = Vector3<T>::UnitZ.Dot(this->Normal());
153  auto angle = acos(dotProduct / this->Normal().Length());
154  auto axis = Vector3<T>::UnitZ.Cross(this->Normal().Normalized());
155  Quaternion<T> rotation(axis, angle);
156 
157  Vector3<T> rotatedXAxis = rotation * Vector3<T>::UnitX;
158  Vector3<T> rotatedYAxis = rotation * Vector3<T>::UnitY;
159 
160  auto xBasis = rotatedXAxis.Dot(intersection);
161  auto yBasis = rotatedYAxis.Dot(intersection);
162 
163  if (std::abs(xBasis) < this->Size().X() / 2 &&
164  std::abs(yBasis) < this->Size().Y() / 2)
165  {
166  return intersection;
167  }
168  return std::nullopt;
169  }
170 
177  public: PlaneSide Side(const Vector3<T> &_point) const
178  {
179  T dist = this->Distance(_point);
180 
181  if (dist < 0.0)
182  return NEGATIVE_SIDE;
183 
184  if (dist > 0.0)
185  return POSITIVE_SIDE;
186 
187  return NO_SIDE;
188  }
189 
196  public: PlaneSide Side(const math::AxisAlignedBox &_box) const
197  {
198  double dist = this->Distance(_box.Center());
199  double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0);
200 
201  if (dist < -maxAbsDist)
202  return NEGATIVE_SIDE;
203 
204  if (dist > maxAbsDist)
205  return POSITIVE_SIDE;
206 
207  return BOTH_SIDE;
208  }
209 
214  public: T Distance(const Vector3<T> &_origin,
215  const Vector3<T> &_dir) const
216  {
217  T denom = this->normal.Dot(_dir);
218 
219  if (std::abs(denom) < 1e-3)
220  {
221  // parallel
222  return 0;
223  }
224  else
225  {
226  T nom = _origin.Dot(this->normal) - this->d;
227  T t = -(nom/denom);
228  return t;
229  }
230  }
231 
233  public: inline const Vector2<T> &Size() const
234  {
235  return this->size;
236  }
237 
239  public: inline Vector2<T> &Size()
240  {
241  return this->size;
242  }
243 
245  public: inline const Vector3<T> &Normal() const
246  {
247  return this->normal;
248  }
249 
251  public: inline Vector3<T> &Normal()
252  {
253  return this->normal;
254  }
255 
257  public: inline T Offset() const
258  {
259  return this->d;
260  }
261 
265  public: Plane<T> &operator=(const Plane<T> &_p)
266  {
267  this->normal = _p.normal;
268  this->size = _p.size;
269  this->d = _p.d;
270 
271  return *this;
272  }
273 
275  private: Vector3<T> normal;
276 
278  private: Vector2<T> size;
279 
281  private: T d;
282  };
283 
287  }
288  }
289 }
290 
291 #endif
PlaneSide
Enum used to indicate a side of the plane, no side, or both sides for entities on the plane.
Definition: gz/math/Plane.hh:43
Two dimensional (x, y) vector.
Definition: gz/math/Vector2.hh:37
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
PlaneSide Side(const math::AxisAlignedBox &_box) const
The side of the plane a box is on.
Definition: gz/math/Plane.hh:196
Plane< double > Planed
Definition: gz/math/Plane.hh:285
math::Vector3d Size() const
Get the size of the box.
T Distance(const Vector3< T > &_point) const
The distance to the plane from the given point. The distance can be negative, which indicates the poi...
Definition: gz/math/Plane.hh:120
A plane and related functions.
Definition: gz/math/Plane.hh:38
@ BOTH_SIDE
On both sides of the plane.
Definition: gz/math/Plane.hh:57
T Distance(const Vector3< T > &_origin, const Vector3< T > &_dir) const
Get distance to the plane give an origin and direction.
Definition: gz/math/Plane.hh:214
PlaneSide Side(const Vector3< T > &_point) const
The side of the plane a point is on.
Definition: gz/math/Plane.hh:177
math::Vector3d Center() const
Get the box center.
@ NEGATIVE_SIDE
Negative side of the plane. This is the side that is opposite the normal.
Definition: gz/math/Plane.hh:47
virtual ~Plane()
Destructor.
Definition: gz/math/Plane.hh:91
Plane< float > Planef
Definition: gz/math/Plane.hh:286
T Offset() const
Get the plane offset.
Definition: gz/math/Plane.hh:257
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
@ NO_SIDE
On the plane.
Definition: gz/math/Plane.hh:54
const Vector3< T > & Normal() const
Get the plane offset.
Definition: gz/math/Plane.hh:245
void Set(const Vector3< T > &_normal, T _offset)
Set the plane.
Definition: gz/math/Plane.hh:96
std::optional< Vector3< T > > Intersection(const Vector3< T > &_point, const Vector3< T > &_gradient, const double &_tolerance=1e-6) const
Get the intersection of an infinite line with the plane, given the line's gradient and a point in par...
Definition: gz/math/Plane.hh:133
Vector3< T > & Normal()
Get the plane offset.
Definition: gz/math/Plane.hh:251
@ POSITIVE_SIDE
Positive side of the plane. This is the side that has the normal vector.
Definition: gz/math/Plane.hh:51
void Set(const Vector3< T > &_normal, const Vector2< T > &_size, T _offset)
Set the plane.
Definition: gz/math/Plane.hh:106
Plane()
Constructor.
Definition: gz/math/Plane.hh:61
Plane(const Plane &_plane)
Copy constructor.
Definition: gz/math/Plane.hh:86
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: gz/math/Vector3.hh:205
Mathematical representation of a box that is aligned along an X,Y,Z axis.
Definition: gz/math/AxisAlignedBox.hh:42
Vector2< T > & Size()
Get the plane size.
Definition: gz/math/Plane.hh:239
Plane(const Vector3< T > &_normal, T _offset=0.0)
Constructor from a normal and a distance.
Definition: gz/math/Plane.hh:69
Plane< int > Planei
Definition: gz/math/Plane.hh:284
Plane< T > & operator=(const Plane< T > &_p)
Equal operator.
Definition: gz/math/Plane.hh:265
Plane(const Vector3< T > &_normal, const Vector2< T > &_size, T _offset)
Constructor.
Definition: gz/math/Plane.hh:78
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: gz/math/Vector3.hh:195
const Vector2< T > & Size() const
Get the plane size.
Definition: gz/math/Plane.hh:233