Simbody 3.7
Loading...
Searching...
No Matches
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"
32#include <cassert>
33
34namespace SimTK {
35
50template <class T>
51class Function_ {
52public:
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
125template <class T>
126class Function_<T>::Constant : public Function_<T> {
127public:
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 {
150 return std::numeric_limits<int>::max();
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
160private:
161 int argumentSize;
162 T value;
163};
164
169template <class T>
170class Function_<T>::Linear : public Function_<T> {
171public:
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 {
204 return std::numeric_limits<int>::max();
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); }
213private:
214 Vector_<T> coefficients;
215};
216
217
222template <class T>
223class Function_<T>::Polynomial : public Function_<T> {
224public:
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 {
261 return std::numeric_limits<int>::max();
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); }
270private:
271 Vector_<T> coefficients;
272};
273
274
282template <>
283class Function_<Real>::Sinusoid : public Function_<Real> {
284public:
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 {
333 return std::numeric_limits<int>::max();
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); }
343private:
344 Real a, w, p;
345};
346
354template <class T>
355class Function_<T>::Step : public Function_<T> {
356public:
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); }
431private:
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
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition ExceptionMacros.h:285
#define SimTK_THROW2(exc, a1, a2)
Definition Exception.h:318
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.
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition Array.h:324
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition Array.h:1520
size_type size() const
Return the current number of elements stored in this array.
Definition Array.h:2075
This is a Function_ subclass which simply returns a fixed value, independent of its arguments.
Definition Function.h:126
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
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition Function.h:138
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition Function.h:149
Constant * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition Function.h:153
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition Function.h:146
Constant(T value, int argumentSize=1)
Create a Function_::Constant object.
Definition Function.h:135
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
This is a Function_ subclass whose output value is a linear function of its arguments: f(x,...
Definition Function.h:170
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
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition Function.h:203
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
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition Function.h:184
Linear * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition Function.h:207
Linear(const Vector_< T > &coefficients)
Create a Function_::Linear object.
Definition Function.h:182
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
Polynomial * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition Function.h:264
T calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition Function.h:233
Polynomial(const Vector_< T > &coefficients)
Create a Function_::Polynomial object.
Definition Function.h:231
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition Function.h:260
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
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
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition Function.h:257
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
virtual Real calcValue(const Vector &x) const override
Calculate the value of this function at a particular point.
Definition Function.h:305
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
Real getFrequency() const
Definition Function.h:300
void setAmplitude(Real amplitude)
Definition Function.h:295
void setPhase(Real phase)
Definition Function.h:297
Sinusoid * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition Function.h:336
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition Function.h:332
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition Function.h:331
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
Sinusoid(Real amplitude, Real frequency, Real phase=0)
Create a Function::Sinusoid object, returning a*sin(w*x+p).
Definition Function.h:292
void setFrequency(Real frequency)
Definition Function.h:296
Real getAmplitude() const
Definition Function.h:299
Real getPhase() const
Definition Function.h:301
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
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 calcValue(const Vector &xin) const override
Calculate the value of this function at a particular point.
Definition Function.h:386
Step * clone() const override
Create a new heap-allocated copy of this concrete Function.
Definition Function.h:425
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
int getMaxDerivativeOrder() const override
Get the maximum derivative order this Function_ object can calculate.
Definition Function.h:423
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
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
int getArgumentSize() const override
Get the number of components expected in the input vector.
Definition Function.h:422
This abstract class represents a mathematical function that calculates a value of arbitrary type base...
Definition Function.h:51
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
virtual Function_ * clone() const
Create a new heap-allocated copy of this concrete Function.
Definition Function.h:109
virtual int getArgumentSize() const =0
Get the number of components expected in the input vector.
virtual T calcValue(const Vector &x) const =0
Calculate the value of this function at a particular point.
virtual T calcDerivative(const Array_< int > &derivComponents, const Vector &x) const =0
Calculate a partial derivative of this function at a particular point.
virtual ~Function_()
Definition Function.h:58
virtual int getMaxDerivativeOrder() const =0
Get the maximum derivative order this Function_ object can calculate.
int size() const
Definition VectorBase.h:396
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
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
double d3stepAny(double yRange, double x0, double oneOverXRange, double x)
Third derivative of stepAny(): d^3/dx^3 stepAny(x).
Definition Scalar.h:949
double dstepAny(double yRange, double x0, double oneOverXRange, double x)
First derivative of stepAny(): d/dx stepAny(x).
Definition Scalar.h:899
double d2stepAny(double yRange, double x0, double oneOverXRange, double x)
Second derivative of stepAny(): d^2/dx^2 stepAny(x).
Definition Scalar.h:924
unsigned int sign(unsigned char u)
Definition Scalar.h:311
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_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