Gazebo Math

API Reference

7.4.0
TimeVaryingVolumetricGridLookupField.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
18#ifndef GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
19#define GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
21#include <gz/math/detail/InterpolationPoint.hh>
22
23#include <map>
24#include <optional>
25#include <utility>
26#include <vector>
27
28namespace gz
29{
30 namespace math
31 {
37 template<typename T, typename V, typename S>
39 {
42
45 const T& time, const VolumetricGridLookupField<V> &_field);
46
48 public: S CreateSession() const;
49
52 public: S CreateSession(const T &_time) const;
53
59 public: std::optional<S> StepTo(const S &_session, const T &_time);
60
67 LookUp(const S &_session,
68 const Vector3<V> &_point,
69 const Vector3<V> &_tol) const;
70
82 public: template<typename X>
83 std::optional<X> EstimateQuadrilinear(
84 const S &_session,
85 const std::vector<InterpolationPoint4D<T, V>> &_points,
86 const std::vector<X> &_values1,
87 const std::vector<X> &_values2,
88 const X _default = X(0)
89 ) const;
90
93 public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session);
94 };
95
98 template<typename T, typename V>
100 {
104 private:
105 typename std::map<T, VolumetricGridLookupField<V>>::const_iterator iter;
106
108 public: T time;
109
110 friend class
112 };
113
116 template<typename T, typename V>
118 {
122
125 const T &_time, const VolumetricGridLookupField<V> &_field) {
126 this->gridFields.emplace(_time, _field);
127 }
128
132 sess.iter = this->gridFields.begin();
133 sess.time = T(0);
134 return sess;
135 }
136
138 public: InMemorySession<T, V> CreateSession(const T &_time) const {
140 sess.iter = this->gridFields.lower_bound(_time);
141 sess.time = _time;
142 return sess;
143 }
144
146 public: bool IsValid(const InMemorySession<T, V> &_session) const {
147 return this->gridFields.end() != _session.iter;
148 }
149
151 public: std::optional<InMemorySession<T, V>> StepTo(
152 const InMemorySession<T, V> &_session, const T &_time) const {
153 if (_session.iter == gridFields.end())
154 {
155 return std::nullopt;
156 }
157
158 InMemorySession<T, V> newSess(_session);
159
160 auto nextTime = std::next(_session.iter);
161 if (nextTime == this->gridFields.end() || _time < _session.iter->first)
162 {
163 return std::nullopt;
164 }
165
166 while (nextTime != this->gridFields.end()
167 && nextTime->first <= _time)
168 {
169 newSess.iter = nextTime;
170 nextTime = std::next(nextTime);
171 }
172 newSess.time = _time;
173 return newSess;
174 }
175
179 const Vector3<V> &_point,
180 const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6}) const {
181
183
184 if (_session.iter == this->gridFields.end())
185 {
186 // Out of bounds
187 return res;
188 }
189
190 InterpolationPoint4D<T, V> slice1;
191 slice1.timeSlice = _session.iter->second.GetInterpolators(
192 _point, _tol.X(), _tol.Y(), _tol.Z());
193 slice1.time = _session.iter->first;
194 res.push_back(slice1);
195
196 auto nextTime = std::next(_session.iter);
197 if (nextTime != this->gridFields.end())
198 {
199 // Only add next item if field exists
200 InterpolationPoint4D<T, V> slice2;
201 slice2.timeSlice = nextTime->second.GetInterpolators(
202 _point, _tol.X(), _tol.Y(), _tol.Z());
203 slice2.time = nextTime->first;
204 res.push_back(slice2);
205 }
206 return res;
207 }
208
221 public: template<typename X>
222 std::optional<X> EstimateQuadrilinear(
223 const InMemorySession<T, V> &_session,
224 const std::vector<InterpolationPoint4D<T, V>> &_interpolators,
225 const std::vector<X> &_values1,
226 const std::vector<X> &_values2,
227 const Vector3<X> &_position,
228 const X _default = X(0)
229 ) const
230 {
231 if (_session.iter == this->gridFields.end())
232 {
233 // Out of bounds
234 return std::nullopt;
235 }
236
237 auto time = _session.time;
238 if (_interpolators.size() == 0) return std::nullopt;
239 if (_interpolators.size() == 1)
240 {
241 // This happens we reach the end of time
242 return _session.iter->second.EstimateValueUsingTrilinear(
243 _position,
244 _values2,
245 _default);
246 }
247
249 if (_interpolators[0].timeSlice.size() == 0
250 && _interpolators[1].timeSlice.size() == 0)
251 return std::nullopt;
252
254 auto next = std::next(_session.iter);
255 if (_interpolators[1].timeSlice.size() == 0)
256 {
257 return _session.iter->second.EstimateValueUsingTrilinear(
258 _position,
259 _values1,
260 _default
261 );
262 }
263 if (_interpolators[0].timeSlice.size() == 0)
264 {
265 return next->second.EstimateValueUsingTrilinear(
266 _position,
267 _values2,
268 _default
269 );
270 }
271
273 auto res1 = _session.iter->second.EstimateValueUsingTrilinear(
274 _position,
275 _values1,
276 _default
277 );
278
279 auto res2 = next->second.EstimateValueUsingTrilinear(
280 _position,
281 _values2,
282 _default
283 );
284
285 if (res1.has_value() || res2.has_value())
286 {
287 InterpolationPoint1D<T>
288 pt1{_session.iter->first, 0}, pt2{next->first, 1};
289 // If either has value interpolate using default value.
290 std::vector<X> times{
291 res1.value_or(_default), res2.value_or(_default)};
292 return LinearInterpolate(pt1, pt2, times, time);
293 }
294 // Return nullopt if we are out of range
295 return std::nullopt;
296 }
297
301 const InMemorySession<T, V> &_session) const
302 {
303 if (_session.iter == this->gridFields.end())
304 {
305 // Out of bounds
307 Vector3<T>{0, 0, 0}, Vector3<T>{0, 0, 0}
308 );
309 }
310 return _session.iter->second.Bounds();
311 }
312
313 private: std::map<T, VolumetricGridLookupField<V>> gridFields;
314 };
315 }
316}
317#endif