Gazebo Math

API Reference

7.4.0
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
30namespace gz
31{
32 namespace math
33 {
34 // Inline bracket to help doxygen filtering.
35 inline namespace GZ_MATH_VERSION_NAMESPACE {
36 //
40
59 template<typename ScalarField3T, typename ScalarT>
61 {
64 public: struct Piece {
66 ScalarField3T field;
67 };
68
70 public: PiecewiseScalarField3() = default;
71
75 public: explicit PiecewiseScalarField3(const std::vector<Piece> &_pieces)
76 : pieces(_pieces)
77 {
78 for (size_t i = 0; i < pieces.size(); ++i)
79 {
80 if (pieces[i].region.Empty())
81 {
82 std::cerr << "Region #" << i << " (" << pieces[i].region
83 << ") in piecewise scalar field definition is empty."
84 << std::endl;
85 }
86 for (size_t j = i + 1; j < pieces.size(); ++j)
87 {
88 if (pieces[i].region.Intersects(pieces[j].region))
89 {
90 std::cerr << "Detected overlap between regions in "
91 << "piecewise scalar field definition: "
92 << "region #" << i << " (" << pieces[i].region
93 << ") overlaps with region #" << j << " ("
94 << pieces[j].region << "). Region #" << i
95 << " will take precedence when overlapping."
96 << std::endl;
97 }
98 }
99 }
100 }
101
105 public: static PiecewiseScalarField3 Throughout(ScalarField3T _field)
106 {
109 }
110
115 public: ScalarT Evaluate(const Vector3<ScalarT> &_p) const
116 {
117 auto it = std::find_if(
118 this->pieces.begin(), this->pieces.end(),
119 [&](const Piece &piece)
120 {
121 return piece.region.Contains(_p);
122 });
123 if (it == this->pieces.end())
124 {
126 }
127 return it->field(_p);
128 }
129
135 public: ScalarT operator()(const Vector3<ScalarT> &_p) const
136 {
137 return this->Evaluate(_p);
138 }
139
150 public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
151 {
152 if (this->pieces.empty())
153 {
154 _pMin = Vector3<ScalarT>::NaN;
156 }
158 for (const Piece &piece : this->pieces)
159 {
160 if (!piece.region.Empty())
161 {
163 const ScalarT y = piece.field.Minimum(piece.region, p);
164 if (y < yMin)
165 {
166 _pMin = p;
167 yMin = y;
168 }
169 }
170 }
171 return yMin;
172 }
173
178 public: ScalarT Minimum() const
179 {
180 Vector3<ScalarT> pMin;
181 return this->Minimum(pMin);
182 }
183
188 public: friend std::ostream &operator<<(
189 std::ostream &_out,
191 ScalarField3T, ScalarT> &_field)
192 {
193 if (_field.pieces.empty())
194 {
195 return _out << "undefined";
196 }
197 for (size_t i = 0; i < _field.pieces.size() - 1; ++i)
198 {
199 _out << _field.pieces[i].field << " if (x, y, z) in "
200 << _field.pieces[i].region << "; ";
201 }
202 return _out << _field.pieces.back().field
203 << " if (x, y, z) in "
204 << _field.pieces.back().region;
205 }
206
208 private: std::vector<Piece> pieces;
209 };
210
211 template<typename ScalarField3T>
213 template<typename ScalarField3T>
215 }
216 }
217}
218
219#endif