Simbody  3.5
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-12 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 };
103 
107 
108 
109 
114 template <class T>
115 class Function_<T>::Constant : public Function_<T> {
116 public:
124  explicit Constant(T value, int argumentSize=1)
125  : argumentSize(argumentSize), value(value) {
126  }
127  T calcValue(const Vector& x) const {
128  assert(x.size() == argumentSize);
129  return value;
130  }
131  T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
132  return static_cast<T>(0);
133  }
134  virtual int getArgumentSize() const {
135  return argumentSize;
136  }
137  int getMaxDerivativeOrder() const {
139  }
140 
143  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
144  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
145 
146 private:
147  const int argumentSize;
148  const T value;
149 };
150 
155 template <class T>
156 class Function_<T>::Linear : public Function_<T> {
157 public:
168  explicit Linear(const Vector_<T>& coefficients) : coefficients(coefficients) {
169  }
170  T calcValue(const Vector& x) const {
171  assert(x.size() == coefficients.size()-1);
172  T value = static_cast<T>(0);
173  for (int i = 0; i < x.size(); ++i)
174  value += x[i]*coefficients[i];
175  value += coefficients[x.size()];
176  return value;
177  }
178  T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
179  assert(x.size() == coefficients.size()-1);
180  assert(derivComponents.size() > 0);
181  if (derivComponents.size() == 1)
182  return coefficients(derivComponents[0]);
183  return static_cast<T>(0);
184  }
185  virtual int getArgumentSize() const {
186  return coefficients.size()-1;
187  }
188  int getMaxDerivativeOrder() const {
190  }
191 
194  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
195  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
196 private:
197  const Vector_<T> coefficients;
198 };
199 
200 
205 template <class T>
206 class Function_<T>::Polynomial : public Function_<T> {
207 public:
214  Polynomial(const Vector_<T>& coefficients) : coefficients(coefficients) {
215  }
216  T calcValue(const Vector& x) const {
217  assert(x.size() == 1);
218  Real arg = x[0];
219  T value = static_cast<T>(0);
220  for (int i = 0; i < coefficients.size(); ++i)
221  value = value*arg + coefficients[i];
222  return value;
223  }
224  T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
225  assert(x.size() == 1);
226  assert(derivComponents.size() > 0);
227  Real arg = x[0];
228  T value = static_cast<T>(0);
229  const int derivOrder = (int)derivComponents.size();
230  const int polyOrder = coefficients.size()-1;
231  for (int i = 0; i <= polyOrder-derivOrder; ++i) {
232  T coeff = coefficients[i];
233  for (int j = 0; j < derivOrder; ++j)
234  coeff *= polyOrder-i-j;
235  value = value*arg + coeff;
236  }
237  return value;
238  }
239  virtual int getArgumentSize() const {
240  return 1;
241  }
242  int getMaxDerivativeOrder() const {
244  }
245 
248  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
249  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
250 private:
251  const Vector_<T> coefficients;
252 };
253 
254 
262 template <>
263 class Function_<Real>::Sinusoid : public Function_<Real> {
264 public:
272  Sinusoid(Real amplitude, Real frequency, Real phase=0)
273  : a(amplitude), w(frequency), p(phase) {}
274 
275  void setAmplitude(Real amplitude) {a=amplitude;}
276  void setFrequency(Real frequency) {w=frequency;}
277  void setPhase (Real phase) {p=phase;}
278 
279  Real getAmplitude() const {return a;}
280  Real getFrequency() const {return w;}
281  Real getPhase () const {return p;}
282 
283  // Implementation of Function_<T> virtuals.
284 
285  virtual Real calcValue(const Vector& x) const {
286  const Real t = x[0]; // we expect just one argument
287  return a*std::sin(w*t + p);
288  }
289 
290  virtual Real calcDerivative(const Array_<int>& derivComponents,
291  const Vector& x) const {
292  const Real t = x[0]; // time is the only argument
293  const int order = derivComponents.size();
294  // The n'th derivative is
295  // sign * a * w^n * sc
296  // where sign is -1 if floor(order/2) is odd, else 1
297  // and sc is cos(w*t+p) if order is odd, else sin(w*t+p)
298  switch(order) {
299  case 0: return a* std::sin(w*t + p);
300  case 1: return a*w* std::cos(w*t + p);
301  case 2: return -a*w*w* std::sin(w*t + p);
302  case 3: return -a*w*w*w*std::cos(w*t + p);
303  default:
304  const Real sign = Real(((order/2) & 0x1) ? -1 : 1);
305  const Real sc = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p);
306  const Real wn = std::pow(w, order);
307  return sign*a*wn*sc;
308  }
309  }
310 
311  virtual int getArgumentSize() const {return 1;} // just time
312  virtual int getMaxDerivativeOrder() const {
314  }
315 
318  Real calcDerivative(const std::vector<int>& derivComponents,
319  const Vector& x) const
320  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
321 private:
322  Real a, w, p;
323 };
324 
332 template <class T>
333 class Function_<T>::Step : public Function_<T> {
334 public:
352  Step(const T& y0, const T& y1, Real x0, Real x1)
353  : m_y0(y0), m_y1(y1), m_yr(y1-y0), m_zero(Real(0)*y0),
354  m_x0(x0), m_x1(x1), m_ooxr(1/(x1-x0)), m_sign(sign(m_ooxr))
355  { SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_<T>::Step::ctor()",
356  "A zero-length switching interval is illegal; both ends were %g.", x0);
357  }
358 
359  T calcValue(const Vector& xin) const {
360  SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
361  "Function_<T>::Step::calcValue()",
362  "Expected just one input argument but got %d.", xin.size());
363 
364  const Real x = xin[0];
365  if ((x-m_x0)*m_sign <= 0) return m_y0;
366  if ((x-m_x1)*m_sign >= 0) return m_y1;
367  // f goes from 0 to 1 as x goes from x0 to x1.
368  const Real f = stepAny(0,1,m_x0,m_ooxr, x);
369  return m_y0 + f*m_yr;
370  }
371 
372  T calcDerivative(const Array_<int>& derivComponents, const Vector& xin) const {
373  SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
374  "Function_<T>::Step::calcDerivative()",
375  "Expected just one input argument but got %d.", xin.size());
376 
377  const int derivOrder = (int)derivComponents.size();
378  SimTK_ERRCHK1_ALWAYS(1 <= derivOrder && derivOrder <= 3,
379  "Function_<T>::Step::calcDerivative()",
380  "Only 1st, 2nd, and 3rd derivatives of the step are available,"
381  " but derivative %d was requested.", derivOrder);
382  const Real x = xin[0];
383  if ((x-m_x0)*m_sign <= 0) return m_zero;
384  if ((x-m_x1)*m_sign >= 0) return m_zero;
385  switch(derivOrder) {
386  case 1: return dstepAny (1,m_x0,m_ooxr, x) * m_yr;
387  case 2: return d2stepAny(1,m_x0,m_ooxr, x) * m_yr;
388  case 3: return d3stepAny(1,m_x0,m_ooxr, x) * m_yr;
389  default: assert(!"impossible derivOrder");
390  }
391  return NaN*m_yr; /*NOTREACHED*/
392  }
393 
394  virtual int getArgumentSize() const {return 1;}
395  int getMaxDerivativeOrder() const {return 3;}
396 
399  T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
400  { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
401 private:
402  const T m_y0, m_y1, m_yr; // precalculate yr=(y1-y0)
403  const T m_zero; // precalculate T(0)
404  const Real m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0)
405  const Real m_sign; // sign(x1-x0) is 1 or -1
406 };
407 
408 } // namespace SimTK
409 
410 #endif // SimTK_SimTKCOMMON_FUNCTION_H_
411 
412 
void setAmplitude(Real amplitude)
Definition: Function.h:275
int getMaxDerivativeOrder() const
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:242
int getMaxDerivativeOrder() const
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:395
Polynomial(const Vector_< T > &coefficients)
Create a Function_::Polynomial object.
Definition: Function.h:214
This is a Function_ subclass whose output value is a linear function of its arguments: f(x...
Definition: Function.h:156
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition: Scalar.h:971
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
virtual int getArgumentSize() const
Get the number of components expected in the input vector.
Definition: Function.h:134
virtual Real calcDerivative(const Array_< int > &derivComponents, const Vector &x) const
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:290
int size() const
Definition: VectorBase.h:396
virtual Real calcValue(const Vector &x) const
Calculate the value of this function at a particular point.
Definition: Function.h:285
T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:224
Real getAmplitude() const
Definition: Function.h:279
Constant(T value, int argumentSize=1)
Create a Function_::Constant object.
Definition: Function.h:124
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:399
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2037
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:248
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:593
virtual T calcValue(const Vector &x) const =0
Calculate the value of this function at a particular point.
Real getFrequency() const
Definition: Function.h:280
int getMaxDerivativeOrder() const
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:137
T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:178
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:352
T calcDerivative(const Array_< int > &derivComponents, const Vector &xin) const
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:372
virtual int getMaxDerivativeOrder() const
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:312
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:277
This is a Function_ subclass whose output value is a polynomial of its argument: f(x) = ax^n+bx^(n-1)...
Definition: Function.h:206
Real calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:318
virtual int getArgumentSize() const =0
Get the number of components expected in the input vector.
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition: Scalar.h:946
Sinusoid(Real amplitude, Real frequency, Real phase=0)
Create a Function::Sinusoid object, returning a*sin(w*x+p).
Definition: Function.h:272
T calcValue(const Vector &x) const
Calculate the value of this function at a particular point.
Definition: Function.h:127
virtual int getArgumentSize() const
Get the number of components expected in the input vector.
Definition: Function.h:185
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:143
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
Linear(const Vector_< T > &coefficients)
Create a Function_::Linear object.
Definition: Function.h:168
T calcValue(const Vector &x) const
Calculate the value of this function at a particular point.
Definition: Function.h:216
This is a Function_ subclass whose output value is a sinusoid of its argument: f(x) = a*sin(w*x + p) ...
Definition: Function.h:263
This abstract class represents a mathematical function that calculates a value of arbitrary type base...
Definition: Function.h:51
This is a Function_ subclass which simply returns a fixed value, independent of its arguments...
Definition: Function.h:115
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition: Scalar.h:996
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48
T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const
Calculate a partial derivative of this function at a particular point.
Definition: Function.h:131
virtual T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const =0
Calculate a partial derivative of this function at a particular point.
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
virtual int getArgumentSize() const
Get the number of components expected in the input vector.
Definition: Function.h:311
virtual int getArgumentSize() const
Get the number of components expected in the input vector.
Definition: Function.h:239
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:920
int getMaxDerivativeOrder() const
Get the maximum derivative order this Function_ object can calculate.
Definition: Function.h:188
void setFrequency(Real frequency)
Definition: Function.h:276
T calcValue(const Vector &x) const
Calculate the value of this function at a particular point.
Definition: Function.h:170
virtual ~Function_()
Definition: Function.h:58
T calcValue(const Vector &xin) const
Calculate the value of this function at a particular point.
Definition: Function.h:359
T calcDerivative(const std::vector< int > &derivComponents, const Vector &x) const
This provides compatibility with std::vector without requiring any copying.
Definition: Function.h:194
This is the header which should be included in user programs that would like to make use of all the S...
Includes internal headers providing declarations for the basic SimTK Core classes.
Real getPhase() const
Definition: Function.h:281
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:106
virtual int getArgumentSize() const
Get the number of components expected in the input vector.
Definition: Function.h:394
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
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:333
virtual int getMaxDerivativeOrder() const =0
Get the maximum derivative order this Function_ object can calculate.