1 #ifndef SimTK_SIMMATH_GCVSPL_UTIL_H_
2 #define SimTK_SIMMATH_GCVSPL_UTIL_H_
3
4 /* -------------------------------------------------------------------------- *
5 * Simbody(tm): SimTKmath *
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: *
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 #include "SimTKcommon.h"
28 #include "simmath/internal/common.h"
29
30 // These are global functions.
31 int SimTK_gcvspl_(const SimTK::Real *, const SimTK::Real *, int *, const SimTK::Real *, const SimTK::Real *, int *, int *,
32 int *, int *, SimTK::Real *, SimTK::Real *, int *, SimTK::Real *, int *);
33 SimTK::Real SimTK_splder_(int *, int *, int *, SimTK::Real *, const SimTK::Real *, const SimTK::Real *, int *, SimTK::Real *, int);
34
35 namespace SimTK {
36
37 /**
38 * This class provides entry points for using the GCVSPL algorithm in terms of SimTK data types.
39 * In most cases, you should use the SplineFitter class rather than invoking this class directly.
40 * For details, see Woltring, H.J. (1986), A FORTRAN package for generalized, cross-validatory
41 * spline smoothing and differentiation. Advances in Engineering Software 8(2):104-113.
42 */
43
44 class SimTK_SIMMATH_EXPORT GCVSPLUtil {
45 public:
46 static void gcvspl(const Vector& x, const Vector& y, const Vector& wx, Real wy, int m, int md, Real val, Vector& c, Vector& wk, int& ier);
47 template <int K>
48 static void gcvspl(const Vector& x, const Vector_<Vec<K> >& y, const Vector& wx, Vec<K> wy, int m, int md, Real val, Vector_<Vec<K> >& c, Vector& wk, int& ier);
49 static Real splder(int derivOrder, int degree, Real t, const Vector& x, const Vector& coeff);
50 template <int K>
51 static Vec<K> splder(int derivOrder, int degree, Real t, const Vector& x, const Vector_<Vec<K> >& coeff);
52 };
53
54 template <int K>
gcvspl(const Vector & x,const Vector_<Vec<K>> & y,const Vector & wx,Vec<K> wy,int degree,int md,Real val,Vector_<Vec<K>> & c,Vector & wk,int & ier)55 void GCVSPLUtil::gcvspl(const Vector& x, const Vector_<Vec<K> >& y, const Vector& wx, Vec<K> wy, int degree, int md, Real val, Vector_<Vec<K> >& c, Vector& wk, int& ier) {
56 SimTK_APIARGCHECK_ALWAYS(degree > 0 && degree%2==1, "GCVSPLUtil", "gcvspl", "degree must be positive and odd");
57 SimTK_APIARGCHECK_ALWAYS(y.size() >= x.size(), "GCVSPLUtil", "gcvspl", "y is shorter than x");
58 SimTK_APIARGCHECK_ALWAYS(wx.size() >= x.size(), "GCVSPLUtil", "gcvspl", "wx and x must be the same size");
59 SimTK_APIARGCHECK_ALWAYS(x.hasContiguousData(), "GCVSPLUtil", "gcvspl", "x must have contiguous storage (i.e. not be a view)");
60 SimTK_APIARGCHECK_ALWAYS(wk.hasContiguousData(), "GCVSPLUtil", "gcvspl", "wk must have contiguous storage (i.e. not be a view)");
61
62 // Create various temporary variables.
63
64 int m = (degree+1)/2;
65 int n = x.size();
66 int ny = y.size();
67 Vector yvec(ny*K);
68 int index = 0;
69 for (int j = 0; j < K; ++j)
70 for (int i = 0; i < ny; ++i)
71 yvec[index++] = y[i][j];
72 int nc = n*K;
73 Vector cvec(nc);
74 wk.resize(6*(m*n+1)+n);
75 int k = K;
76
77 // Invoke GCV.
78
79 SimTK_gcvspl_(&x[0], &yvec[0], &ny, &wx[0], &wy[0], &m, &n, &k, &md, &val, &cvec[0], &n, &wk[0], &ier);
80 if (ier != 0) {
81 SimTK_APIARGCHECK_ALWAYS(n >= 2*m, "GCVSPLUtil", "gcvspl", "Too few data points");
82 SimTK_APIARGCHECK_ALWAYS(ier != 2, "GCVSPLUtil", "gcvspl", "The values in x must be strictly increasing");
83 SimTK_APIARGCHECK_ALWAYS(ier == 0, "GCVSPLUtil", "gcvspl", "GCVSPL returned an error code");
84 }
85 c.resize(n);
86 index = 0;
87 for (int j = 0; j < K; ++j)
88 for (int i = 0; i < n; ++i)
89 c[i][j] = cvec[index++];
90 }
91
92 template <int K>
splder(int derivOrder,int degree,Real t,const Vector & x,const Vector_<Vec<K>> & coeff)93 Vec<K> GCVSPLUtil::splder(int derivOrder, int degree, Real t, const Vector& x, const Vector_<Vec<K> >& coeff) {
94 assert(derivOrder >= 0);
95 assert(t >= x[0] && t <= x[x.size()-1]);
96 assert(x.size() == coeff.size());
97 assert(degree > 0 && degree%2==1);
98 assert(x.hasContiguousData());
99
100 // Create various temporary variables.
101
102
103 Vec<K> result;
104 int m = (degree+1)/2;
105 int n = x.size();
106 int interval = (int) ceil(n*(t-x[0])/(x[n-1]-x[0]));
107
108 const int MaxCheapM = 32;
109 Real qbuf[2*MaxCheapM];
110
111 Real *q = qbuf; // tentatively
112 if (m > MaxCheapM)
113 q = new Real[2*m]; // use heap instead; don't forget to delete
114
115 int offset = (int) (&coeff[1][0]-&coeff[0][0]);
116
117 // Evaluate the spline one component at a time.
118
119 for (int i = 0; i < K; ++i)
120 result[i] = SimTK_splder_(&derivOrder, &m, &n, &t, &x[0], &coeff[0][i], &interval, q, offset);
121
122 if (m > MaxCheapM)
123 delete[] q;
124
125 return result;
126 }
127
128 } // namespace SimTK
129
130 #endif // SimTK_SIMMATH_GCVSPL_UTIL_H_
131
132
133