Line data Source code
1 : /* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 : Copyright (c) 2012-2018 The plumed team
3 : (see the PEOPLE file at the root of the distribution for a list of names)
4 :
5 : See http://www.plumed.org for more information.
6 :
7 : This file is part of plumed, version 2.
8 :
9 : plumed is free software: you can redistribute it and/or modify
10 : it under the terms of the GNU Lesser General Public License as published by
11 : the Free Software Foundation, either version 3 of the License, or
12 : (at your option) any later version.
13 :
14 : plumed is distributed in the hope that it will be useful,
15 : but WITHOUT ANY WARRANTY; without even the implied warranty of
16 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 : GNU Lesser General Public License for more details.
18 :
19 : You should have received a copy of the GNU Lesser General Public License
20 : along with plumed. If not, see <http://www.gnu.org/licenses/>.
21 : +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
22 : #include "CubicInterpolation.h"
23 :
24 : namespace PLMD {
25 :
26 0 : CInterpolation::CInterpolation( const std::vector<unsigned>& dd, const std::vector<double>& fmin, const std::vector<double>& fmax ) :
27 0 : bold(0)
28 : {
29 0 : plumed_assert( fmin.size()==dd.size() && fmax.size()==dd.size() );
30 :
31 0 : np.resize( dd.size() ); stride.resize( dd.size() ); unsigned totalp=1;
32 0 : for(unsigned i=0; i<dd.size(); ++i) { np[i]=dd[i]; stride[dd.size()-1-i]=totalp; totalp*=np[i]; }
33 :
34 : unsigned ii,kk;
35 0 : splinepoints.resize( totalp, np.size() );
36 0 : std::vector<double> delr( np.size() );
37 0 : for(unsigned j=0; j<np.size(); ++j) delr[j] = ( fmax[j] - fmin[j] )/static_cast<double>(np[j]-1);
38 :
39 0 : for(unsigned i=0; i<totalp; ++i) {
40 0 : ii=i;
41 0 : for(unsigned j=0; j<np.size(); ++j) {
42 0 : kk=std::floor( double(ii) / double(stride[j]) );
43 0 : ii-=kk*stride[j];
44 0 : splinepoints(i,j)=fmin[j] + kk*delr[j];
45 : }
46 0 : plumed_assert(ii==0);
47 : }
48 0 : lb.resize( np.size() ); ub.resize( np.size() );
49 0 : }
50 :
51 0 : CInterpolation::~CInterpolation() {
52 0 : splinepoints.resize(0,0); lb.resize(0); ub.resize(0); np.resize(0); stride.resize(0);
53 0 : }
54 :
55 0 : void CInterpolation::getNumbersOfPoints( std::vector<unsigned>& nspline ) const {
56 0 : nspline.resize( np.size() );
57 0 : for(unsigned i=0; i<np.size(); ++i) nspline[i]=np[i];
58 0 : }
59 :
60 0 : unsigned CInterpolation::findBox( const std::vector<double>& pos ) {
61 : plumed_dbg_massert( pos.size()==np.size(), "position size does not match the size of the grid");
62 :
63 0 : unsigned jold, ccf_box, bnew=0;
64 0 : for(unsigned i=0; i<np.size(); ++i) {
65 0 : jold=static_cast<int>( std::floor( double(bold)/double(stride[i]) ) );
66 0 : bold-=jold*stride[i];
67 0 : ccf_box=search1( i, pos[i], jold );
68 0 : bnew+=ccf_box;
69 : }
70 0 : plumed_dbg_assert( bold==0 ); bold=bnew;
71 0 : for(unsigned i=0; i<np.size(); ++i) { lb[i]=splinepoints(bold,i); ub[i]=splinepoints(bold+stride[i],i); }
72 0 : return bold;
73 : }
74 :
75 0 : unsigned CInterpolation::search1( const unsigned& kk, const double& x, const unsigned& jold ) const {
76 0 : int inc=stride[kk], jl=jold*stride[kk], ju=(jold+1)*stride[kk], jm;
77 0 : if ( x>=splinepoints(jl,kk) && x<splinepoints( ju, kk ) ) return jl;
78 : else {
79 0 : if( x>=splinepoints(jl, kk ) ) {
80 : while(true) {
81 0 : ju=jl+inc;
82 0 : if( x<splinepoints( ju, kk ) ) break;
83 0 : else if( ju>=(np[kk]-1)*inc ) {ju=(np[kk]-1)*inc; break; }
84 0 : jl=ju;
85 : }
86 : }
87 : else {
88 0 : ju=jl;
89 : while(true) {
90 0 : jl=jl-inc;
91 0 : if( x>=splinepoints( jl, kk ) ) break;
92 0 : else if( jl<=0 ) { jl=0; break; }
93 0 : ju=jl;
94 : }
95 : }
96 : }
97 0 : while( ju-jl>inc ) {
98 0 : jm = (ju+jl) / (2*inc) ;
99 0 : if ( x>splinepoints(jm*inc,kk) ) jl=jm*inc; else ju=jm*inc;
100 : }
101 : plumed_dbg_assert( jl%stride[kk]==0 && ju==jl+stride[kk] );
102 0 : return jl;
103 : }
104 :
105 0 : InterpolateCubic::InterpolateCubic( const std::vector<unsigned>& dd, const std::vector<double>& fmin, const std::vector<double>& fmax ) :
106 0 : CInterpolation(dd,fmin,fmax)
107 : {
108 0 : plumed_massert(np.size()==1,"should be one dimensional data");
109 0 : clist.resize( 4*np[0] );
110 0 : }
111 :
112 0 : void InterpolateCubic::set_table( const std::vector<Value>& ff ) {
113 0 : plumed_assert( getNumberOfSplinePoints()==ff.size() );
114 0 : plumed_assert( ff[0].getNumberOfDerivatives()==1 );
115 :
116 : double d1, norm; unsigned pij;
117 0 : for(unsigned i=0; i<np[0]-1; ++i) {
118 0 : d1 = getPointSpacing( 0, i );
119 0 : norm=(d1*d1)/6.0; pij=i*4;
120 0 : clist[pij]=ff[i].get(); pij++;
121 0 : clist[pij]=ff[i+1].get(); pij++;
122 0 : clist[pij]=ff[i].getDerivative(0)*norm; pij++;
123 0 : clist[pij]=ff[i+1].getDerivative(0)*norm;
124 : }
125 0 : }
126 :
127 0 : double InterpolateCubic::get_fdf( const std::vector<double>& pos ) {
128 : plumed_dbg_assert( pos.size()==1 );
129 :
130 0 : unsigned mybox=findBox( pos );
131 0 : double d1=ub[0] - lb[0];
132 0 : double b=( pos[0] - lb[0] ) / d1, a=( ub[0] - pos[0] ) / d1;
133 :
134 0 : double *cbase=&clist[(mybox*4)+3], *c3=cbase-1, *c2=c3-1, *c1=c2-1;
135 0 : double f=a*(*c1) + b*(*c2) + (a*a*a-a)*(*c3) + (b*b*b-b)*(*cbase);
136 0 : return f;
137 : }
138 :
139 0 : InterpolateBicubic::InterpolateBicubic( const std::vector<unsigned>& dd, const std::vector<double>& fmin, const std::vector<double>& fmax ) :
140 0 : CInterpolation(dd,fmin,fmax)
141 : {
142 0 : plumed_massert(np.size()==2,"should be two dimensional data");
143 : static int wt_d[16*16]=
144 : { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
145 : 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
146 : -3, 0, 0, 3, 0, 0, 0, 0,-2, 0, 0,-1, 0, 0, 0, 0,
147 : 2, 0, 0,-2, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0,
148 : 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
149 : 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
150 : 0, 0, 0, 0,-3, 0, 0, 3, 0, 0, 0, 0,-2, 0, 0,-1,
151 : 0, 0, 0, 0, 2, 0, 0,-2, 0, 0, 0, 0, 1, 0, 0, 1,
152 : -3, 3, 0, 0,-2,-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
153 : 0, 0, 0, 0, 0, 0, 0, 0,-3, 3, 0, 0,-2,-1, 0, 0,
154 : 9,-9, 9,-9, 6, 3,-3,-6, 6,-6,-3, 3, 4, 2, 1, 2,
155 : -6, 6,-6, 6,-4,-2, 2, 4,-3, 3, 3,-3,-2,-1,-1,-2,
156 : 2,-2, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
157 : 0, 0, 0, 0, 0, 0, 0, 0, 2,-2, 0, 0, 1, 1, 0, 0,
158 : -6, 6,-6, 6,-3,-3, 3, 3,-4, 4, 2,-2,-2,-2,-1,-1,
159 : 4,-4, 4,-4, 2, 2,-2,-2, 2,-2,-2, 2, 1, 1, 1, 1
160 : };
161 :
162 : // This is to set up the coefficient matrix
163 0 : unsigned l=0; wt.resize(16,16); t1.resize(16); t2.resize(16);
164 0 : for (unsigned i=0; i<16; i++) for (unsigned j=0; j<16; j++) { wt(i,j)=wt_d[l++]; }
165 : // Resize everything
166 0 : dcross.resize( np[0], np[1] ); clist.resize( np[0] * np[1] * 4 * 4 );
167 0 : }
168 :
169 0 : void InterpolateBicubic::set_table( const std::vector<Value>& ff ) {
170 0 : plumed_assert( getNumberOfSplinePoints()==ff.size() );
171 0 : plumed_assert( ff[0].getNumberOfDerivatives()==2 );
172 :
173 0 : dcross=0.0; unsigned iplus, iminus;
174 0 : for(unsigned i=1; i<np[0]-1; ++i) {
175 0 : iplus=(i+1)*stride[0]; iminus=(i-1)*stride[0];
176 0 : for(unsigned j=1; j<np[1]-1; ++j) {
177 0 : dcross(i,j) = ( ff[iplus+j+1].get() + ff[iminus+j-1].get() - ff[iplus+j-1].get() - ff[iminus+j+1].get() ) /
178 0 : getCrossTermDenominator( i, j );
179 : }
180 : }
181 :
182 0 : double d1, d2; Matrix<double> tc(4,4);
183 0 : std::vector<double> y(4), dy1(4), dy2(4), d2y12(4);
184 :
185 0 : unsigned pij=0; unsigned ipos;
186 0 : for (unsigned i=0; i<np[0]-1; ++i) {
187 0 : ipos=i*stride[0]; d1 = getPointSpacing( 0, i );
188 0 : for (unsigned j=0; j<np[1]-1; ++j) {
189 0 : d2 = getPointSpacing( 1, j );
190 0 : y[0] = ff[ipos+j].get(); y[1] = ff[ipos+stride[0]+j].get(); y[2] = ff[ipos+stride[0]+j+1].get(); y[3] = ff[ipos+j+1].get();
191 0 : dy1[0] = ff[ipos+j].getDerivative(0); dy1[1] = ff[ipos+stride[0]+j].getDerivative(0);
192 0 : dy1[2] = ff[ipos+stride[0]+j+1].getDerivative(0); dy1[3] = ff[ipos+j+1].getDerivative(0);
193 0 : dy2[0] = ff[ipos+j].getDerivative(1); dy2[1] = ff[ipos+stride[0]+j].getDerivative(1);
194 0 : dy2[2] = ff[ipos+stride[0]+j+1].getDerivative(1); dy2[3] = ff[ipos+j+1].getDerivative(1);
195 0 : d2y12[0] = dcross( i, j ); d2y12[1] = dcross( i+1, j ); d2y12[2] = dcross( i+1, j+1 ); d2y12[3] = dcross( i, j+1 );
196 0 : IBicCoeff( y, dy1, dy2, d2y12, d1, d2, tc);
197 :
198 0 : pij=( ipos+j )*16;
199 0 : for(unsigned k=0; k<4; ++k) { for(unsigned n=0; n<4; ++n) { clist[pij++]=tc(k,n); } }
200 : }
201 0 : }
202 0 : }
203 :
204 0 : void InterpolateBicubic::IBicCoeff( const std::vector<double>& y, const std::vector<double>& dy1, const std::vector<double>& dy2,
205 : const std::vector<double>& d2y12, const double& d1, const double& d2, Matrix<double>& c ) {
206 0 : double xx, d1d2=d1*d2;
207 0 : for(unsigned i=0; i<4; i++) { t1[i] = y[i]; t1[i+4] = dy1[i]*d1; t1[i+8] = dy2[i]*d2; t1[i+12] = d2y12[i]*d1d2; }
208 0 : for(unsigned i=0; i<16; i++) { xx=0.0; for(unsigned k=0; k<16; k++) { xx += wt(i,k)*t1[k]; } t2[i]=xx; }
209 0 : unsigned l=0; for(unsigned i=0; i<4; i++) { for(unsigned j=0; j<4; j++) { c(i,j)=t2[l++]; } }
210 0 : }
211 :
212 0 : double InterpolateBicubic::get_fdf( const std::vector<double>& pos ) {
213 :
214 : plumed_dbg_assert( pos.size()==2 );
215 0 : unsigned mybox=findBox( pos );
216 0 : double d1 = ub[0] - lb[0], d2 = ub[1] - lb[1];
217 0 : double t = (pos[0] - lb[0]) / d1, u = (pos[1] - lb[1]) / d2;
218 :
219 : //faster access by pointer arithmetic (dirty dirty dirty)
220 0 : double *cbase=&clist[(mybox+1)*16-1], *c3, *c2, *c1, *c0;
221 :
222 0 : double f=0.;
223 0 : for (int i=3; i>=0; i--) { // Note to self - this has to be an int as unsigned cannot be less than zero - duh!!
224 0 : c3=cbase; c2=c3-1; c1=c2-1; c0=c1-1; cbase=c0-1;
225 0 : f= t*f + ( ( (*c3)*u + (*c2) )*u + (*c1) )*u + (*c0);
226 : }
227 0 : delete cbase; delete c3; delete c2; delete c1; delete c0;
228 0 : return f;
229 : }
230 :
231 2523 : }
|
