1 /* -------------------------------------------------------------------------- *
2  *                     OpenSim:  XYFunctionInterface.cpp                      *
3  * -------------------------------------------------------------------------- *
4  * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
5  * See http://opensim.stanford.edu and the NOTICE file for more information.  *
6  * OpenSim is developed at Stanford University and supported by the US        *
7  * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
8  * through the Warrior Web program.                                           *
9  *                                                                            *
10  * Copyright (c) 2005-2017 Stanford University and the Authors                *
11  * Author(s): Peter Loan                                                      *
12  *                                                                            *
13  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
14  * not use this file except in compliance with the License. You may obtain a  *
15  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
16  *                                                                            *
17  * Unless required by applicable law or agreed to in writing, software        *
18  * distributed under the License is distributed on an "AS IS" BASIS,          *
19  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
20  * See the License for the specific language governing permissions and        *
21  * limitations under the License.                                             *
22  * -------------------------------------------------------------------------- */
23 
24 //=============================================================================
25 // INCLUDES
26 //=============================================================================
27 #include "XYFunctionInterface.h"
28 #include <OpenSim/Common/Constant.h>
29 #include <OpenSim/Common/Function.h>
30 #include <OpenSim/Common/GCVSpline.h>
31 #include <OpenSim/Common/LinearFunction.h>
32 #include <OpenSim/Common/MultiplierFunction.h>
33 #include <OpenSim/Common/PiecewiseConstantFunction.h>
34 #include <OpenSim/Common/PiecewiseLinearFunction.h>
35 #include <OpenSim/Common/SimmSpline.h>
36 #include <OpenSim/Common/StepFunction.h>
37 
38 
39 namespace OpenSim {
40 
isXYFunction(Function * f)41 bool XYFunctionInterface::isXYFunction(Function* f)
42 {
43     Function* func = f;
44     MultiplierFunction* mf = dynamic_cast<MultiplierFunction*>(f);
45     if (mf)
46         func = mf->getFunction();
47 
48     if (dynamic_cast<Constant*>(func) ||
49         dynamic_cast<StepFunction*>(func) ||
50         dynamic_cast<PiecewiseLinearFunction*>(func) ||
51         dynamic_cast<LinearFunction*>(func) ||
52         dynamic_cast<SimmSpline*>(func) ||
53         dynamic_cast<GCVSpline*>(func)||
54         dynamic_cast<PiecewiseConstantFunction*>(func))
55         return true;
56 
57     return false;
58 }
59 
XYFunctionInterface(Function * f)60 XYFunctionInterface::XYFunctionInterface(Function* f) :
61    _functionType(typeUndefined),
62     _constant(0),
63    _stepFunction(0),
64    _linearFunction(0),
65    _natCubicSpline(0),
66    _gcvSpline(0),
67    _mStepFunction(0),
68    _genericFunction(0)
69 {
70     Function* func;
71     MultiplierFunction* mf = dynamic_cast<MultiplierFunction*>(f);
72     if (mf) {
73         func = mf->getFunction();
74         _scaleFactor = mf->getScale();
75     } else {
76         func = f;
77         _scaleFactor = 1.0;
78     }
79 
80     _constant = dynamic_cast<Constant*>(func);
81     if (_constant) {
82         _functionType = typeConstant;
83         return;
84     }
85     _stepFunction = dynamic_cast<StepFunction*>(func);
86     if (_stepFunction) {
87         _functionType = typeStepFunction;
88         return;
89     }
90     _mStepFunction = dynamic_cast<PiecewiseConstantFunction*>(func);
91     if (_mStepFunction) {
92         _functionType = typePiecewiseConstantFunction;
93         return;
94     }
95     _piecewiseLinearFunction = dynamic_cast<PiecewiseLinearFunction*>(func);
96     if (_piecewiseLinearFunction) {
97         _functionType = typePiecewiseLinearFunction;
98         return;
99     }
100     _linearFunction = dynamic_cast<LinearFunction*>(func);
101     if (_linearFunction) {
102         _functionType = typeLinearFunction;
103         return;
104     }
105     _natCubicSpline = dynamic_cast<SimmSpline*>(func);
106     if (_natCubicSpline) {
107         _functionType = typeNatCubicSpline;
108         return;
109     }
110     _gcvSpline = dynamic_cast<GCVSpline*>(func);
111     if (_gcvSpline) {
112         _functionType = typeGCVSpline;
113         return;
114     }
115     _genericFunction = func;
116 
117 }
118 
isSpecifiedByControlPoints() const119 bool XYFunctionInterface::isSpecifiedByControlPoints() const
120 {
121     switch (_functionType)
122     {
123        case typeGCVSpline:
124        case typeNatCubicSpline:
125            return true;
126        case typeConstant:
127        case typePiecewiseConstantFunction:
128        case typePiecewiseLinearFunction:
129        case typeLinearFunction:
130        default:
131             return false;
132     }
133 }
getNumberOfPoints() const134 int XYFunctionInterface::getNumberOfPoints() const
135 {
136     switch (_functionType)
137     {
138         case typeConstant:
139             return 0;
140        case typePiecewiseConstantFunction:
141             return _mStepFunction->getNumberOfPoints();
142        case typePiecewiseLinearFunction:
143            return _piecewiseLinearFunction->getNumberOfPoints();
144        case typeLinearFunction:
145            return 2;
146         case typeNatCubicSpline:
147            return _natCubicSpline->getNumberOfPoints();
148        case typeGCVSpline:
149            return _gcvSpline->getNumberOfPoints();
150         default:
151             return 0;
152     }
153 }
154 
getXValues() const155 const double* XYFunctionInterface::getXValues() const
156 {
157     switch (_functionType)
158     {
159         case typeConstant:
160             return NULL;
161        case typeStepFunction:
162             return NULL;
163        case typePiecewiseConstantFunction:
164             return _mStepFunction->getXValues();
165        case typePiecewiseLinearFunction:
166            return _piecewiseLinearFunction->getXValues();
167        case typeLinearFunction:
168             {
169                 double* xValues = new double[2];
170                 xValues[0] = -1.0;
171                 xValues[1] = 1.0;
172                 return xValues; // possible memory leak
173             }
174         case typeNatCubicSpline:
175            return _natCubicSpline->getXValues();
176        case typeGCVSpline:
177            return _gcvSpline->getXValues();
178         default:
179             return 0;
180     }
181 }
182 
getYValues() const183 const double* XYFunctionInterface::getYValues() const
184 {
185     const double* yValues = NULL;
186     double* tmp = NULL;
187     int numPoints = getNumberOfPoints();
188 
189     switch (_functionType)
190     {
191         case typeConstant:
192             return NULL;
193        case typeStepFunction:
194             return NULL;;
195             break;
196        case typePiecewiseConstantFunction:
197             yValues = _mStepFunction->getYValues();
198             break;
199        case typePiecewiseLinearFunction:
200            yValues = _piecewiseLinearFunction->getYValues();
201             break;
202        case typeLinearFunction:
203            tmp = new double[2];
204             tmp[0] = _linearFunction->getCoefficients()[1] - _linearFunction->getCoefficients()[0];
205             tmp[1] = _linearFunction->getCoefficients()[1] + _linearFunction->getCoefficients()[0];
206             break;
207         case typeNatCubicSpline:
208            yValues = _natCubicSpline->getYValues();
209             break;
210        case typeGCVSpline:
211            yValues = _gcvSpline->getYValues();
212             break;
213         default:
214             return NULL;
215     }
216 
217     double* scaledY = new double[numPoints];
218     if (tmp){
219         scaledY[0] = tmp[0];
220         scaledY[1] = tmp[1];
221     }
222     else
223         memcpy(scaledY, yValues, numPoints*sizeof(double));
224     for (int i=0; i<numPoints; i++)
225         scaledY[i] *= _scaleFactor;
226 
227     if (tmp)
228         delete tmp;
229 
230     // possible memory leak
231     return scaledY;
232 }
233 
getX(int aIndex) const234 double XYFunctionInterface::getX(int aIndex) const
235 {
236     switch (_functionType)
237     {
238         case typeConstant:
239             return 0; //_constant->getX(aIndex);
240        case typeStepFunction:
241             return 0; //_stepFunction->getX(aIndex);
242         case typePiecewiseConstantFunction:
243             return _mStepFunction->getX(aIndex);
244         case typePiecewiseLinearFunction:
245            return _piecewiseLinearFunction->getX(aIndex);
246        case typeLinearFunction:
247            if (aIndex == 0)
248                 return -1.0;
249             else if (aIndex == 1)
250                 return 1.0;
251             else
252                 return 0.0;
253         case typeNatCubicSpline:
254            return _natCubicSpline->getX(aIndex);
255        case typeGCVSpline:
256            return _gcvSpline->getX(aIndex);
257         default:
258             return 0.0;
259     }
260 }
261 
getY(int aIndex) const262 double XYFunctionInterface::getY(int aIndex) const
263 {
264     switch (_functionType)
265     {
266         case typeConstant:
267             return _constant->getValue() * _scaleFactor;
268        case typeStepFunction:
269            return SimTK::NaN;
270        case typePiecewiseConstantFunction:
271            return _mStepFunction->getY(aIndex) * _scaleFactor;
272        case typePiecewiseLinearFunction:
273            return _piecewiseLinearFunction->getY(aIndex) * _scaleFactor;
274        case typeLinearFunction:
275            if (aIndex == 0)
276                 return (_linearFunction->getCoefficients()[1] - _linearFunction->getCoefficients()[0]) * _scaleFactor;
277             else if (aIndex == 1)
278                 return (_linearFunction->getCoefficients()[1] + _linearFunction->getCoefficients()[0]) * _scaleFactor;
279             else
280                 return 0.0;
281         case typeNatCubicSpline:
282            return _natCubicSpline->getY(aIndex) * _scaleFactor;
283        case typeGCVSpline:
284            return _gcvSpline->getY(aIndex) * _scaleFactor;
285         default:
286             return 0.0;
287     }
288 }
289 
setX(int aIndex,double aValue)290 void XYFunctionInterface::setX(int aIndex, double aValue)
291 {
292     switch (_functionType)
293     {
294         case typeConstant:
295             break;
296         case typePiecewiseConstantFunction:
297            _mStepFunction->setX(aIndex, aValue);
298            break;
299        case typePiecewiseLinearFunction:
300            _piecewiseLinearFunction->setX(aIndex, aValue);
301            break;
302        case typeLinearFunction:
303            break;
304         case typeNatCubicSpline:
305            _natCubicSpline->setX(aIndex, aValue);
306            break;
307        case typeGCVSpline:
308            _gcvSpline->setX(aIndex, aValue);
309            break;
310         default:
311             return;
312     }
313 }
314 
setY(int aIndex,double aValue)315 void XYFunctionInterface::setY(int aIndex, double aValue)
316 {
317     aValue /= _scaleFactor;
318 
319     switch (_functionType)
320     {
321         case typeConstant:
322             break;
323        case typePiecewiseConstantFunction:
324            _mStepFunction->setY(aIndex, aValue);
325            break;
326        case typePiecewiseLinearFunction:
327            _piecewiseLinearFunction->setY(aIndex, aValue);
328            break;
329         case typeLinearFunction:
330             break;
331         case typeNatCubicSpline:
332            _natCubicSpline->setY(aIndex, aValue);
333            break;
334        case typeGCVSpline:
335            _gcvSpline->setY(aIndex, aValue);
336            break;
337         default:
338             return;
339     }
340 }
341 
deletePoint(int aIndex)342 bool XYFunctionInterface::deletePoint(int aIndex)
343 {
344     switch (_functionType)
345     {
346         case typeConstant:
347             return true;
348        case typeStepFunction:
349            return false;
350        case typePiecewiseConstantFunction:
351            return _mStepFunction->deletePoint(aIndex);
352        case typePiecewiseLinearFunction:
353            return _piecewiseLinearFunction->deletePoint(aIndex);
354        case typeLinearFunction:
355            return false;
356         case typeNatCubicSpline:
357            return _natCubicSpline->deletePoint(aIndex);
358        case typeGCVSpline:
359            return _gcvSpline->deletePoint(aIndex);
360         default:
361             return true;
362     }
363 }
364 
deletePoints(const Array<int> & indices)365 bool XYFunctionInterface::deletePoints(const Array<int>& indices)
366 {
367     switch (_functionType)
368     {
369         case typeConstant:
370             return true;
371        case typeStepFunction:
372            return false;
373        case typePiecewiseConstantFunction:
374            return _mStepFunction->deletePoints(indices);
375        case typePiecewiseLinearFunction:
376            return _piecewiseLinearFunction->deletePoints(indices);
377        case typeLinearFunction:
378            return false;
379         case typeNatCubicSpline:
380            return _natCubicSpline->deletePoints(indices);
381        case typeGCVSpline:
382            return _gcvSpline->deletePoints(indices);
383         default:
384             return true;
385     }
386 }
387 
addPoint(double aX,double aY)388 int XYFunctionInterface::addPoint(double aX, double aY)
389 {
390     aY /= _scaleFactor;
391 
392     switch (_functionType)
393     {
394         case typeConstant:
395             return 1;
396        case typeStepFunction:
397            return 0;
398        case typePiecewiseConstantFunction:
399            return _mStepFunction->addPoint(aX, aY);
400        case typePiecewiseLinearFunction:
401            return _piecewiseLinearFunction->addPoint(aX, aY);
402        case typeLinearFunction:
403            return 0;
404         case typeNatCubicSpline:
405            return _natCubicSpline->addPoint(aX, aY);
406        case typeGCVSpline:
407            return _gcvSpline->addPoint(aX, aY);
408         default:
409             return 1;
410     }
411 }
412 
renderAsLineSegments(int aIndex)413 Array<XYPoint>* XYFunctionInterface::renderAsLineSegments(int aIndex)
414 {
415     if (_functionType == typeUndefined || _functionType == typeConstant ||
416         aIndex < 0 || aIndex >= getNumberOfPoints() - 1)
417         return NULL;
418 
419     Array<XYPoint>* xyPts = new Array<XYPoint>(XYPoint());
420     const double* x = getXValues();
421     const double* y = getYValues();
422 
423     if (_functionType == typeStepFunction)  {
424         xyPts->append(XYPoint(x[aIndex], y[aIndex]));
425         xyPts->append(XYPoint(x[aIndex+1], y[aIndex]));
426         xyPts->append(XYPoint(x[aIndex+1], y[aIndex+1]));
427     } else if (_functionType == typePiecewiseLinearFunction) {
428         xyPts->append(XYPoint(x[aIndex], y[aIndex]));
429         xyPts->append(XYPoint(x[aIndex+1], y[aIndex+1]));
430     } else if (_functionType == typeNatCubicSpline) {
431         // X sometimes goes slightly beyond the range due to roundoff error,
432         // so do the last point separately.
433         int numSegs = 20;
434         for (int i=0; i<numSegs-1; i++) {
435             double xValue = x[aIndex] + (double)i * (x[aIndex + 1] - x[aIndex]) / ((double)numSegs - 1.0);
436             xyPts->append(XYPoint(xValue, _natCubicSpline->calcValue(SimTK::Vector(1,xValue)) * _scaleFactor));
437         }
438         xyPts->append(XYPoint(x[aIndex + 1], _natCubicSpline->calcValue(SimTK::Vector(1,x[aIndex+1])) * _scaleFactor));
439     } else if (_functionType == typeGCVSpline) {
440         // X sometimes goes slightly beyond the range due to roundoff error,
441         // so do the last point separately.
442         int numSegs = 20;
443         for (int i=0; i<numSegs-1; i++) {
444             double xValue = x[aIndex] + (double)i * (x[aIndex + 1] - x[aIndex]) / ((double)numSegs - 1.0);
445             xyPts->append(XYPoint(xValue, _gcvSpline->calcValue(SimTK::Vector(1,xValue)) * _scaleFactor));
446         }
447         xyPts->append(XYPoint(x[aIndex + 1], _gcvSpline->calcValue(SimTK::Vector(1,x[aIndex + 1])) * _scaleFactor));
448     } else if (_functionType == typePiecewiseConstantFunction)  {
449         xyPts->append(XYPoint(x[aIndex], y[aIndex]));
450         xyPts->append(XYPoint(x[aIndex], y[aIndex+1]));
451         xyPts->append(XYPoint(x[aIndex+1], y[aIndex+1]));
452     } else if (_functionType == typeLinearFunction) {
453         xyPts->append(XYPoint(x[aIndex], y[aIndex]));
454         xyPts->append(XYPoint(x[aIndex+1], y[aIndex+1]));
455     }
456 
457     return xyPts;
458 }
459 
460 }   // namespace
461