Gazebo Math

API Reference

7.5.1
gz/math/PiecewiseScalarField3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 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_PIECEWISE_SCALAR_FIELD3_HH_
18 #define GZ_MATH_PIECEWISE_SCALAR_FIELD3_HH_
19 
20 #include <algorithm>
21 #include <iostream>
22 #include <limits>
23 #include <utility>
24 #include <vector>
25 
26 #include <gz/math/Region3.hh>
27 #include <gz/math/Vector3.hh>
28 #include <gz/math/config.hh>
29 
30 namespace gz::math
31 {
32  // Inline bracket to help doxygen filtering.
33  inline namespace GZ_MATH_VERSION_NAMESPACE {
34  //
38  template<typename ScalarField3T, typename ScalarT>
59  {
62  public: struct Piece {
64  ScalarField3T field;
65  };
66 
68  public: PiecewiseScalarField3() = default;
69 
73  public: explicit PiecewiseScalarField3(const std::vector<Piece> &_pieces)
74  : pieces(_pieces)
75  {
76  for (size_t i = 0; i < pieces.size(); ++i)
77  {
78  if (pieces[i].region.Empty())
79  {
80  std::cerr << "Region #" << i << " (" << pieces[i].region
81  << ") in piecewise scalar field definition is empty."
82  << std::endl;
83  }
84  for (size_t j = i + 1; j < pieces.size(); ++j)
85  {
86  if (pieces[i].region.Intersects(pieces[j].region))
87  {
88  std::cerr << "Detected overlap between regions in "
89  << "piecewise scalar field definition: "
90  << "region #" << i << " (" << pieces[i].region
91  << ") overlaps with region #" << j << " ("
92  << pieces[j].region << "). Region #" << i
93  << " will take precedence when overlapping."
94  << std::endl;
95  }
96  }
97  }
98  }
99 
103  public: static PiecewiseScalarField3 Throughout(ScalarField3T _field)
104  {
107  }
108 
113  public: ScalarT Evaluate(const Vector3<ScalarT> &_p) const
114  {
115  auto it = std::find_if(
116  this->pieces.begin(), this->pieces.end(),
117  [&](const Piece &piece)
118  {
119  return piece.region.Contains(_p);
120  });
121  if (it == this->pieces.end())
122  {
124  }
125  return it->field(_p);
126  }
127 
133  public: ScalarT operator()(const Vector3<ScalarT> &_p) const
134  {
135  return this->Evaluate(_p);
136  }
137 
148  public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
149  {
150  if (this->pieces.empty())
151  {
152  _pMin = Vector3<ScalarT>::NaN;
154  }
156  for (const Piece &piece : this->pieces)
157  {
158  if (!piece.region.Empty())
159  {
161  const ScalarT y = piece.field.Minimum(piece.region, p);
162  if (y < yMin)
163  {
164  _pMin = p;
165  yMin = y;
166  }
167  }
168  }
169  return yMin;
170  }
171 
176  public: ScalarT Minimum() const
177  {
178  Vector3<ScalarT> pMin;
179  return this->Minimum(pMin);
180  }
181 
186  public: friend std::ostream &operator<<(
187  std::ostream &_out,
189  ScalarField3T, ScalarT> &_field)
190  {
191  if (_field.pieces.empty())
192  {
193  return _out << "undefined";
194  }
195  for (size_t i = 0; i < _field.pieces.size() - 1; ++i)
196  {
197  _out << _field.pieces[i].field << " if (x, y, z) in "
198  << _field.pieces[i].region << "; ";
199  }
200  return _out << _field.pieces.back().field
201  << " if (x, y, z) in "
202  << _field.pieces.back().region;
203  }
204 
206  private: std::vector<Piece> pieces;
207  };
208 
209  template<typename ScalarField3T>
211  template<typename ScalarField3T>
213  } // namespace GZ_MATH_VERSION_NAMESPACE
214 } // namespace gz::math
215 #endif // GZ_MATH_PIECEWISE_SCALAR_FIELD3_HH_