Gazebo Math

API Reference

7.5.1
gz/math/AdditivelySeparableScalarField3.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_SEPARABLE_SCALAR_FIELD3_HH_
18 #define GZ_MATH_SEPARABLE_SCALAR_FIELD3_HH_
19 
20 #include <limits>
21 #include <utility>
22 
23 #include <gz/math/Region3.hh>
24 #include <gz/math/Vector3.hh>
25 #include <gz/math/config.hh>
26 
27 namespace gz::math
28 {
29  // Inline bracket to help doxygen filtering.
30  inline namespace GZ_MATH_VERSION_NAMESPACE {
31  //
36  template<typename ScalarFunctionT, typename ScalarT>
60  {
67  ScalarT _k, ScalarFunctionT _p,
68  ScalarFunctionT _q, ScalarFunctionT _r)
69  : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r))
70  {
71  }
72 
76  public: ScalarT Evaluate(const Vector3<ScalarT> &_point) const
77  {
78  return this->k * (
79  this->p(_point.X()) +
80  this->q(_point.Y()) +
81  this->r(_point.Z()));
82  }
83 
88  public: ScalarT operator()(const Vector3<ScalarT> &_point) const
89  {
90  return this->Evaluate(_point);
91  }
92 
99  public: ScalarT Minimum(const Region3<ScalarT> &_region,
100  Vector3<ScalarT> &_pMin) const
101  {
102  if (_region.Empty())
103  {
104  _pMin = Vector3<ScalarT>::NaN;
106  }
107  return this->k * (
108  this->p.Minimum(_region.Ix(), _pMin.X()) +
109  this->q.Minimum(_region.Iy(), _pMin.Y()) +
110  this->r.Minimum(_region.Iz(), _pMin.Z()));
111  }
112 
117  public: ScalarT Minimum(const Region3<ScalarT> &_region) const
118  {
119  Vector3<ScalarT> pMin;
120  return this->Minimum(_region, pMin);
121  }
122 
127  public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
128  {
129  return this->Minimum(Region3<ScalarT>::Unbounded, _pMin);
130  }
131 
134  public: ScalarT Minimum() const
135  {
136  Vector3<ScalarT> pMin;
137  return this->Minimum(Region3<ScalarT>::Unbounded, pMin);
138  }
139 
144  public: friend std::ostream &operator<<(
145  std::ostream &_out,
147  ScalarFunctionT, ScalarT> &_field)
148  {
149  using std::abs; // enable ADL
150  constexpr ScalarT epsilon =
152  if ((abs(_field.k) - ScalarT(1)) < epsilon)
153  {
154  if (_field.k < ScalarT(0))
155  {
156  _out << "-";
157  }
158  }
159  else
160  {
161  _out << _field.k << " ";
162  }
163  _out << "[(";
164  _field.p.Print(_out, "x");
165  _out << ") + (";
166  _field.q.Print(_out, "y");
167  _out << ") + (";
168  _field.r.Print(_out, "z");
169  return _out << ")]";
170  }
171 
173  private: ScalarT k;
174 
176  private: ScalarFunctionT p;
177 
179  private: ScalarFunctionT q;
180 
182  private: ScalarFunctionT r;
183  };
184 
185  template<typename ScalarFunctionT>
188 
189  template<typename ScalarFunctionT>
192  } // namespace GZ_MATH_VERSION_NAMESPACE
193 } // namespace gz::math
194 #endif // GZ_MATH_SEPARABLE_SCALAR_FIELD3_HH_