Simbody  3.6
Function.h
Go to the documentation of this file.
1 #ifndef SimTK_SimTKCOMMON_FUNCTION_H_
2 #define SimTK_SimTKCOMMON_FUNCTION_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2008-15 Stanford University and the Authors. *
13  * Authors: Peter Eastman *
14  * Contributors: Michael Sherman *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 // Note: this file was moved from Simmath to SimTKcommon 20100601; see the
28 // Simmath repository for earlier history.
29 
30 #include "SimTKcommon/basics.h"
31 #include "SimTKcommon/Simmatrix.h"
32 #include <cassert>
33 
34 namespace SimTK {
35 
50 template <class T>
51 class Function_ {
52 public:
53  class Constant;
54  class Linear;
55  class Polynomial;
56  class Sinusoid;
57  class Step;
58  virtual ~Function_() {
59  }
66  virtual T calcValue(const Vector& x) const = 0;
86  virtual T calcDerivative(const Array_<int>& derivComponents,
87  const Vector& x) const = 0;
88 
91  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
92  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
93 
97  virtual int getArgumentSize() const = 0;
101  virtual int getMaxDerivativeOrder() const = 0;
102 
109  virtual Function_* clone() const {
111  "clone");
112  }
113 };
114 
118 
119 
120 
125 template <class T>
126 class Function_<T>::Constant : public Function_<T> {
127 public:
135  explicit Constant(T value, int argumentSize=1)
136  : argumentSize(argumentSize), value(value) {
137  }
138  T calcValue(const Vector& x) const override {
139  assert(x.size() == argumentSize);
140  return value;
141  }
142  T calcDerivative(const Array_<int>& derivComponents,
143  const Vector& x) const override {
144  return static_cast<T>(0);
145  }
146  int getArgumentSize() const override {
147  return argumentSize;
148  }
149  int getMaxDerivativeOrder() const override {
151  }
152 
153  Constant* clone() const override {return new Constant(*this);}
154 
157  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
158  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
159 
160 private:
161  int argumentSize;
162  T value;
163 };
164 
169 template <class T>
170 class Function_<T>::Linear : public Function_<T> {
171 public:
182  explicit Linear(const Vector_<T>& coefficients) : coefficients(coefficients) {
183  }
184  T calcValue(const Vector& x) const override {
185  assert(x.size() == coefficients.size()-1);
186  T value = static_cast<T>(0);
187  for (int i = 0; i < x.size(); ++i)
188  value += x[i]*coefficients[i];
189  value += coefficients[x.size()];
190  return value;
191  }
192  T calcDerivative(const Array_<int>& derivComponents,
193  const Vector& x) const override {
194  assert(x.size() == coefficients.size()-1);
195  assert(derivComponents.size() > 0);
196  if (derivComponents.size() == 1)
197  return coefficients(derivComponents[0]);
198  return static_cast<T>(0);
199  }
200  int getArgumentSize() const override {
201  return coefficients.size()-1;
202  }
203  int getMaxDerivativeOrder() const override {
205  }
206 
207  Linear* clone() const override {return new Linear(*this);}
208 
211  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
212  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
213 private:
214  Vector_<T> coefficients;
215 };
216 
217 
222 template <class T>
223 class Function_<T>::Polynomial : public Function_<T> {
224 public:
231  Polynomial(const Vector_<T>& coefficients) : coefficients(coefficients) {
232  }
233  T calcValue(const Vector& x) const override {
234  assert(x.size() == 1);
235  Real arg = x[0];
236  T value = static_cast<T>(0);
237  for (int i = 0; i < coefficients.size(); ++i)
238  value = value*arg + coefficients[i];
239  return value;
240  }
241  T calcDerivative(const Array_<int>& derivComponents,
242  const Vector& x) const override {
243  assert(x.size() == 1);
244  assert(derivComponents.size() > 0);
245  Real arg = x[0];
246  T value = static_cast<T>(0);
247  const int derivOrder = (int)derivComponents.size();
248  const int polyOrder = coefficients.size()-1;
249  for (int i = 0; i <= polyOrder-derivOrder; ++i) {
250  T coeff = coefficients[i];
251  for (int j = 0; j < derivOrder; ++j)
252  coeff *= polyOrder-i-j;
253  value = value*arg + coeff;
254  }
255  return value;
256  }
257  int getArgumentSize() const override {
258  return 1;
259  }
260  int getMaxDerivativeOrder() const override {
262  }
263 
264  Polynomial* clone() const override {return new Polynomial(*this);}
265 
268  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
269  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
270 private:
271  Vector_<T> coefficients;
272 };
273 
274 
282 template <>
283 class Function_<Real>::Sinusoid : public Function_<Real> {
284 public:
292  Sinusoid(Real amplitude, Real frequency, Real phase=0)
293  : a(amplitude), w(frequency), p(phase) {}
294 
295  void setAmplitude(Real amplitude) {a=amplitude;}
296  void setFrequency(Real frequency) {w=frequency;}
297  void setPhase (Real phase) {p=phase;}
298 
299  Real getAmplitude() const {return a;}
300  Real getFrequency() const {return w;}
301  Real getPhase () const {return p;}
302 
303  // Implementation of Function_<T> virtuals.
304 
305  virtual Real calcValue(const Vector& x) const override {
306  const Real t = x[0]; // we expect just one argument
307  return a*std::sin(w*t + p);
308  }
309 
310  virtual Real calcDerivative(const Array_<int>& derivComponents,
311  const Vector& x) const override {
312  const Real t = x[0]; // time is the only argument
313  const int order = derivComponents.size();
314  // The n'th derivative is
315  // sign * a * w^n * sc
316  // where sign is -1 if floor(order/2) is odd, else 1
317  // and sc is cos(w*t+p) if order is odd, else sin(w*t+p)
318  switch(order) {
319  case 0: return a* std::sin(w*t + p);
320  case 1: return a*w* std::cos(w*t + p);
321  case 2: return -a*w*w* std::sin(w*t + p);
322  case 3: return -a*w*w*w*std::cos(w*t + p);
323  default:
324  const Real sign = Real(((order/2) & 0x1) ? -1 : 1);
325  const Real sc = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p);
326  const Real wn = std::pow(w, order);
327  return sign*a*wn*sc;
328  }
329  }
330 
331  int getArgumentSize() const override {return 1;}
332  int getMaxDerivativeOrder() const override {
334  }
335 
336  Sinusoid* clone() const override {return new Sinusoid(*this);}
337 
340  Real calcDerivative(const std::vector<int>& derivComponents,
341  const Vector& x) const
342  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
343 private:
344  Real a, w, p;
345 };
346 
354 template <class T>
355 class Function_<T>::Step : public Function_<T> {
356 public:
374  Step(const T& y0, const T& y1, Real x0, Real x1)
375  { setParameters(y0,y1,x0,x1); }
376 
379  void setParameters(const T& y0, const T& y1, Real x0, Real x1) {
380  SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_<T>::Step::setParameters()",
381  "A zero-length switching interval is illegal; both ends were %g.", x0);
382  m_y0 = y0; m_y1 = y1; m_yr = y1-y0; m_zero = Real(0)*y0;
383  m_x0 = x0; m_x1 = x1; m_ooxr = 1/(x1-x0); m_sign = sign(m_ooxr);
384  }
385 
386  T calcValue(const Vector& xin) const override {
387  SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
388  "Function_<T>::Step::calcValue()",
389  "Expected just one input argument but got %d.", xin.size());
390 
391  const Real x = xin[0];
392  if ((x-m_x0)*m_sign <= 0) return m_y0;
393  if ((x-m_x1)*m_sign >= 0) return m_y1;
394  // f goes from 0 to 1 as x goes from x0 to x1.
395  const Real f = stepAny(0,1,m_x0,m_ooxr, x);
396  return m_y0 + f*m_yr;
397  }
398 
399  T calcDerivative(const Array_<int>& derivComponents,
400  const Vector& xin) const override {
401  SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
402  "Function_<T>::Step::calcDerivative()",
403  "Expected just one input argument but got %d.", xin.size());
404 
405  const int derivOrder = (int)derivComponents.size();
406  SimTK_ERRCHK1_ALWAYS(1 <= derivOrder && derivOrder <= 3,
407  "Function_<T>::Step::calcDerivative()",
408  "Only 1st, 2nd, and 3rd derivatives of the step are available,"
409  " but derivative %d was requested.", derivOrder);
410  const Real x = xin[0];
411  if ((x-m_x0)*m_sign <= 0) return m_zero;
412  if ((x-m_x1)*m_sign >= 0) return m_zero;
413  switch(derivOrder) {
414  case 1: return dstepAny (1,m_x0,m_ooxr, x) * m_yr;
415  case 2: return d2stepAny(1,m_x0,m_ooxr, x) * m_yr;
416  case 3: return d3stepAny(1,m_x0,m_ooxr, x) * m_yr;
417  default: assert(!"impossible derivOrder");
418  }
419  return NaN*m_yr; /*NOTREACHED*/
420  }
421 
422  int getArgumentSize() const override {return 1;}
423  int getMaxDerivativeOrder() const override {return 3;}
424 
425  Step* clone() const override {return new Step(*this);}
426 
429  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
430  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
431 private:
432  T m_y0, m_y1, m_yr; // precalculate yr=(y1-y0)
433  T m_zero; // precalculate T(0)
434  Real m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0)
435  Real m_sign; // sign(x1-x0) is 1 or -1
436 };
437 
438 } // namespace SimTK
439 
440 #endif // SimTK_SimTKCOMMON_FUNCTION_H_
441 
442 
SimTK::Function_::Step
This is a Function_ subclass whose output value y=f(x) is smoothly stepped from y=y0 to y1 as its inp...
Definition: Function.h:355
SimTK::Function_::Constant::getArgumentSize
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:146
SimTK::Function_::Sinusoid::setPhase
void setPhase(Real phase)
Definition: Function.h:297
SimTK::Function_::Polynomial::getArgumentSize
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:257
SimTK::Function_
This abstract class represents a mathematical function that calculates a value of arbitrary type base...
Definition: Function.h:51
SimTK::Function_::Linear::calcValue
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:184
SimTK::Function_::Sinusoid::calcDerivative
virtual Real calcDerivative(const Array_< int > &derivComponents, const Vector &x) const override
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:310
SimTK::Function_::Sinusoid::Sinusoid
Sinusoid(Real amplitude, Real frequency, Real phase=0)
Create a Function::Sinusoid object, returning a*sin(w*x+p).
Definition: Function.h:292
SimTK::Function_::calcDerivative
virtual T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const =0
Calculate a partial derivative of this function at a particular point.
SimTK::Function_::Linear
This is a Function_ subclass whose output value is a linear function of its arguments: f(x,...
Definition: Function.h:170
SimTK
This is a System that represents the dynamics of a particle moving along a smooth surface.
Definition: Assembler.h:37
Simmatrix.h
SimTK::Function_::Sinusoid::getPhase
Real getPhase() const
Definition: Function.h:301
SimTK::NaN
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
SimTK::Function_::Step::getArgumentSize
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:422
SimTK::Function_::Sinusoid::setAmplitude
void setAmplitude(Real amplitude)
Definition: Function.h:295
SimTK::Function_::Step::Step
Step(const T &y0, const T &y1, Real x0, Real x1)
Create a Function_::Step object that smoothly interpolates its output through a given range as its in...
Definition: Function.h:374
SimTK::Function_::Polynomial
This is a Function_ subclass whose output value is a polynomial of its argument: f(x) = ax^n+bx^(n-1)...
Definition: Function.h:223
SimTK::Function_::Step::clone
Step * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:425
SimTK::Function_::Step::calcValue
T calcValue(const Vector &xin) const override
Calculate the value of this function at a particular point.
Definition: Function.h:386
SimTK::Function_::Linear::getMaxDerivativeOrder
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:203
SimTK::Function_::Step::calcDerivative
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:429
SimTK::Function_::Constant::Constant
Constant(T value, int argumentSize=1)
Create a Function_::Constant object.
Definition: Function.h:135
SimTK_THROW2
#define SimTK_THROW2(exc, a1, a2)
Definition: Exception.h:318
SimTK::dstepAny
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition: Scalar.h:899
SimTK::Function_::Step::calcDerivative
T calcDerivative(const Array_< int > &derivComponents, const Vector &xin) const override
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:399
SimTK::Exception::UnimplementedVirtualMethod
Definition: Exception.h:248
SimTK::d2stepAny
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition: Scalar.h:924
SimTK::Function_::Polynomial::calcDerivative
T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const override
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:241
SimTK::max
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
SimTK::Function_::clone
virtual Function_ * clone() const
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:109
SimTK::Function_::Linear::getArgumentSize
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:200
SimTK::Function_::Sinusoid::setFrequency
void setFrequency(Real frequency)
Definition: Function.h:296
SimTK::Function_::Polynomial::getMaxDerivativeOrder
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:260
SimTK::ArrayViewConst_
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:51
SimTK::Function_::Constant::clone
Constant * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:153
SimTK::d3stepAny
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition: Scalar.h:949
SimTK::Function_::Linear::Linear
Linear(const Vector_< T > &coefficients)
Create a Function_::Linear object.
Definition: Function.h:182
SimTK::Function_::Constant::calcDerivative
T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const override
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:142
SimTK::Function
Function_< Real > Function
This typedef is used for the very common case that the return type of the Function object is Real.
Definition: Function.h:117
SimTK::Function_::Constant
This is a Function_ subclass which simply returns a fixed value, independent of its arguments.
Definition: Function.h:126
SimTK::Function_::Linear::calcDerivative
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:211
SimTK::Function_::Constant::getMaxDerivativeOrder
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:149
SimTK_ERRCHK1_ALWAYS
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
SimTK::Function_::Constant::calcDerivative
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:157
SimTK::Function_::Linear::calcDerivative
T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const override
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:192
SimTK::Function_::Sinusoid::calcDerivative
Real calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:340
SimTK::Array_::size
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2075
SimTK::Function_::getArgumentSize
virtual int getArgumentSize() const =0
Get the number of components expected in the input vector.
SimTK::Array_< int >
SimTK::Function_::Sinusoid
This is a Function_ subclass whose output value is a sinusoid of its argument: f(x) = a*sin(w*x + p) ...
Definition: Function.h:283
SimTK::Real
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
SimTK::Function_::calcValue
virtual T calcValue(const Vector &x) const =0
Calculate the value of this function at a particular point.
SimTK::Function_::Polynomial::calcDerivative
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:268
SimTK::Function_::Polynomial::Polynomial
Polynomial(const Vector_< T > &coefficients)
Create a Function_::Polynomial object.
Definition: Function.h:231
SimTK::Function_::Sinusoid::getFrequency
Real getFrequency() const
Definition: Function.h:300
SimTK::Function_::Sinusoid::clone
Sinusoid * clone() const override
Definition: Function.h:336
SimTK::Vector_< Real >
SimTK::Function_::Sinusoid::getArgumentSize
int getArgumentSize() const override
Definition: Function.h:331
basics.h
SimTK::Function_::Constant::calcValue
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:138
SimTK::Function_::Linear::clone
Linear * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:207
SimTK::Function_::Polynomial::clone
Polynomial * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:264
SimTK::Function_::getMaxDerivativeOrder
virtual int getMaxDerivativeOrder() const =0
Get the maximum derivative order this Function_ object can calculate.
SimTK::Function_::Step::getMaxDerivativeOrder
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:423
SimTK::Function_::~Function_
virtual ~Function_()
Definition: Function.h:58
SimTK::stepAny
double stepAny(double y0, double yRange, double x0, double oneOverXRange, double x)
Interpolate smoothly from y0 to y1 as the input argument goes from x0 to x1, with first and second de...
Definition: Scalar.h:873
SimTK::Function_::Sinusoid::getAmplitude
Real getAmplitude() const
Definition: Function.h:299
SimTK::Function_::calcDerivative
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:91
SimTK::Function_::Sinusoid::calcValue
virtual Real calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:305
SimTK::sign
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
SimTK::Function_::Polynomial::calcValue
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:233
SimTK::Function_::Step::setParameters
void setParameters(const T &y0, const T &y1, Real x0, Real x1)
Change the four parameters that define the step function; see constructor for documentation.
Definition: Function.h:379
SimTK::Function_::Sinusoid::getMaxDerivativeOrder
int getMaxDerivativeOrder() const override
Definition: Function.h:332