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 
void setAmplitude(Real amplitude)
Definition: Function.h:295
Polynomial(const Vector_< T > &coefficients)
Create a Function_::Polynomial object.
Definition: Function.h:231
virtual Function_ * clone() const
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:109
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:184
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2075
This is a Function_ subclass whose output value is a linear function of its arguments: f(x...
Definition: Function.h:170
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
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
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition: Scalar.h:924
Polynomial * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:264
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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
Step * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:425
virtual T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const =0
Calculate a partial derivative of this function at a particular point.
virtual Real calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:305
Constant(T value, int argumentSize=1)
Create a Function_::Constant object.
Definition: Function.h:135
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
virtual T calcValue(const Vector &x) const =0
Calculate the value of this function at a particular point.
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
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
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
void setPhase(Real phase)
Definition: Function.h:297
virtual int getArgumentSize() const =0
Get the number of components expected in the input vector.
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:200
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
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:233
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:203
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
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
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
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
virtual int getMaxDerivativeOrder() const =0
Get the maximum derivative order this Function_ object can calculate.
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition: Scalar.h:899
Sinusoid(Real amplitude, Real frequency, Real phase=0)
Create a Function::Sinusoid object, returning a*sin(w*x+p).
Definition: Function.h:292
#define SimTK_THROW2(exc, a1, a2)
Definition: Exception.h:318
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:149
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
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:260
Linear(const Vector_< T > &coefficients)
Create a Function_::Linear object.
Definition: Function.h:182
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
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:423
Constant * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:153
This abstract class represents a mathematical function that calculates a value of arbitrary type base...
Definition: Function.h:51
Real getAmplitude() const
Definition: Function.h:299
This is a Function_ subclass which simply returns a fixed value, independent of its arguments...
Definition: Function.h:126
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition: Scalar.h:949
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:51
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:332
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
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:146
Real getPhase() const
Definition: Function.h:301
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
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
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
int size() const
Definition: VectorBase.h:396
T calcValue(const Vector &xin) const override
Calculate the value of this function at a particular point.
Definition: Function.h:386
void setFrequency(Real frequency)
Definition: Function.h:296
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
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:422
virtual ~Function_()
Definition: Function.h:58
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
Real getFrequency() const
Definition: Function.h:300
This is the header which should be included in user programs that would like to make use of all the S...
Sinusoid * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:336
Linear * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition: Function.h:207
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:331
Includes internal headers providing declarations for the basic SimTK Core classes.
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
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition: Function.h:138
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
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition: Function.h:257