Geant4  10.01.p01
c2_function.icc
Go to the documentation of this file.
1 //
2 // ********************************************************************
3 // * License and Disclaimer *
4 // * *
5 // * The Geant4 software is copyright of the Copyright Holders of *
6 // * the Geant4 Collaboration. It is provided under the terms and *
7 // * conditions of the Geant4 Software License, included in the file *
8 // * LICENSE and available at http://cern.ch/geant4/license . These *
9 // * include a list of copyright holders. *
10 // * *
11 // * Neither the authors of this software system, nor their employing *
12 // * institutes,nor the agencies providing financial support for this *
13 // * work make any representation or warranty, express or implied, *
14 // * regarding this software system or assume any liability for its *
15 // * use. Please see the license in the file LICENSE and URL above *
16 // * for the full disclaimer and the limitation of liability. *
17 // * *
18 // * This code implementation is the result of the scientific and *
19 // * technical work of the GEANT4 collaboration. *
20 // * By using, copying, modifying or distributing the software (or *
21 // * any work based on the software) you agree to acknowledge its *
22 // * use in resulting scientific publications, and indicate your *
23 // * acceptance of all terms of the Geant4 Software license. *
24 // ********************************************************************
25 //
26 //
27 /**
28  * \file electromagnetic/TestEm7/include/c2_function.icc
29  * \brief Provides code for the general c2_function algebra which supports
30  * fast, flexible operations on piecewise-twice-differentiable functions
31  *
32  * \author Created by R. A. Weller and Marcus H. Mendenhall on 7/9/05.
33  * \author Copyright 2005 __Vanderbilt University__. All rights reserved.
34  *
35  * \version c2_function.cc 488 2012-04-04 11:30:21Z marcus
36  */
37 
38  // $Id$
39  //
40 
41 #include <iostream>
42 #include <vector>
43 #include <algorithm>
44 #include <cstdlib>
45 #include <numeric>
46 #include <functional>
47 #include <iterator>
48 #include <cmath>
49 #include <limits>
50 #include <sstream>
51 
52 template <typename float_type> const std::string c2_function<float_type>::cvs_file_vers() const
53 { return "c2_function.cc 488 2012-04-04 11:30:21Z marcus "; }
54 
55 // find a pre-bracketed root of a c2_function, which is a MUCH easier job than general root finding
56 // since the derivatives are known exactly, and smoothness is guaranteed.
57 // this searches for f(x)=value, to make life a little easier than always searching for f(x)=0
58 template <typename float_type> float_type c2_function<float_type>::find_root(float_type lower_bracket, float_type upper_bracket,
59  float_type start, float_type value, int *error,
60  float_type *final_yprime, float_type *final_yprime2) const throw(c2_exception)
61 {
62  // find f(x)=value within the brackets, using the guarantees of smoothness associated with a c2_function
63  // can use local f(x)=a*x**2 + b*x + c and solve quadratic to find root, then iterate
64 
65  float_type yp, yp2; // we will make unused pointers point here, to save null checks later
66  if (!final_yprime) final_yprime=&yp;
67  if (!final_yprime2) final_yprime2=&yp2;
68 
69  float_type ftol=5*(std::numeric_limits<float_type>::epsilon()*std::abs(value)+std::numeric_limits<float_type>::min());
70  float_type xtol=5*(std::numeric_limits<float_type>::epsilon()*(std::abs(upper_bracket)+std::abs(lower_bracket))+std::numeric_limits<float_type>::min());
71 
72  float_type root=start; // start looking in the middle
73  if(error) *error=0; // start out with error flag set to OK, if it is expected
74 
75  float_type c, b;
76 
77  if(!root_info) {
78  root_info=new struct c2_root_info;
79  root_info->inited=false;
80  }
81  // this new logic is to keep track of where we were before, and lower the number of
82  // function evaluations if we are searching inside the same bracket as before.
83  // Since this root finder has, very often, the bracket of the entire domain of the function,
84  // this makes a big difference, especially to c2_inverse_function
85  if(!root_info->inited || upper_bracket != root_info->upper.x || lower_bracket != root_info->lower.x) {
86  root_info->upper.x=upper_bracket;
87  fill_fblock(root_info->upper);
88  root_info->lower.x=lower_bracket;
89  fill_fblock(root_info->lower);
90  root_info->inited=true;
91  }
92 
93  float_type clower=root_info->lower.y-value;
94  if(!clower) {
95  *final_yprime=root_info->lower.yp;
96  *final_yprime2=root_info->lower.ypp;
97  return lower_bracket;
98  }
99 
100  float_type cupper=root_info->upper.y-value;
101  if(!cupper) {
102  *final_yprime=root_info->upper.yp;
103  *final_yprime2=root_info->upper.ypp;
104  return upper_bracket;
105  }
106  const float_type lower_sign = (clower < 0) ? -1 : 1;
107 
108  if(lower_sign*cupper >0) {
109  // argh, no sign change in here!
110  if(error) { *error=1; return 0.0; }
111  else {
112  std::ostringstream outstr;
113  outstr << "unbracketed root in find_root at xlower= " << lower_bracket << ", xupper= " << upper_bracket;
114  outstr << ", value= " << value << ": bailing";
115  throw c2_exception(outstr.str().c_str());
116  }
117  }
118 
119  float_type delta=upper_bracket-lower_bracket; // first error step
120  c=value_with_derivatives(root, final_yprime, final_yprime2)-value; // compute initial values
121  b=*final_yprime; // make a local copy for readability
122  increment_evaluations();
123 
124  while(
125  std::abs(delta) > xtol && // absolute x step check
126  std::abs(c) > ftol && // absolute y tolerance
127  std::abs(c) > xtol*std::abs(b) // comparison to smallest possible Y step from derivative
128  )
129  {
130  float_type a=(*final_yprime2)/2; // second derivative is 2*a
131  float_type disc=b*b-4*a*c;
132  // std::cout << std::endl << "find_root_debug a,b,c,d " << a << " " << b << " " << c << " " << disc << std::endl;
133 
134  if(disc >= 0) {
135  float_type q=-0.5*((b>=0)?(b+std::sqrt(disc)):(b-std::sqrt(disc)));
136  if(q*q > std::abs(a*c)) delta=c/q; // since x1=q/a, x2=c/q, x1/x2=q^2/ac, this picks smaller step
137  else delta=q/a;
138  root+=delta;
139  }
140 
141  if(disc < 0 || root<lower_bracket || root>upper_bracket ||
142  std::abs(delta) >= 0.5*(upper_bracket-lower_bracket)) {
143  // if we jump out of the bracket, or aren't converging well, bisect
144  root=0.5*(lower_bracket+upper_bracket);
145  delta=upper_bracket-lower_bracket;
146  }
147  c=value_with_derivatives(root, final_yprime, final_yprime2)-value; // compute initial values
148  if(c2_isnan(c)) {
149  bad_x_point=root;
150  return c; // return the nan if a computation failed
151  }
152  b=*final_yprime; // make a local copy for readability
153  increment_evaluations();
154 
155  // now, close in bracket on whichever side this still brackets
156  if(c*lower_sign < 0.0) {
157  cupper=c;
158  upper_bracket=root;
159  } else {
160  clower=c;
161  lower_bracket=root;
162  }
163  // std::cout << "find_root_debug x, y, dx " << root << " " << c << " " << delta << std::endl;
164  }
165  return root;
166 }
167 
168 /* def partial_integrals(self, xgrid):
169  Return the integrals of a function between the sampling points xgrid. The sum is the definite integral.
170  This method uses an exact integration of the polynomial which matches the values and derivatives at the
171  endpoints of a segment. Its error scales as h**6, if the input functions really are smooth.
172  This could very well be used as a stepper for adaptive Romberg integration.
173  For InterpolatingFunctions, it is likely that the Simpson's rule integrator is sufficient.
174  #the weights come from an exact mathematica solution to the 5th order polynomial with the given values & derivatives
175  #yint = (y0+y1)*dx/2 + dx^2*(yp0-yp1)/10 + dx^3 * (ypp0+ypp1)/120 )
176 */
177 
178 // the recursive part of the integrator is agressively designed to minimize copying of data... lots of pointers
179 template <typename float_type> float_type c2_function<float_type>::integrate_step(c2_integrate_recur &rb) const throw(c2_exception)
180 {
181  std::vector< recur_item > &rb_stack=*rb.rb_stack; // heap-based stack of data for recursion
182  rb_stack.clear();
183 
184  recur_item top;
185  top.depth=0; top.done=false; top.f0index=0; top.f2index=0; top.step_sum=0;
186 
187  // push storage for our initial elements
188  rb_stack.push_back(top);
189  rb_stack.back().f1=*rb.f0;
190  rb_stack.back().done=true; // this element will never be evaluated further
191 
192  rb_stack.push_back(top);
193  rb_stack.back().f1=*rb.f1;
194  rb_stack.back().done=true; // this element will never be evaluated further
195 
196  if(!rb.inited) {
197  switch(rb.derivs) {
198  case 0:
199  rb.eps_scale=0.1; rb.extrap_coef=16; break;
200  case 1:
201  rb.eps_scale=0.1; rb.extrap_coef=64; break;
202  case 2:
203  rb.eps_scale=0.02; rb.extrap_coef=1024; break;
204  default:
205  throw c2_exception("derivs must be 0, 1 or 2 in partial_integrals");
206  }
207 
208  rb.extrap2=1.0/(rb.extrap_coef-1.0);
209  rb.dx_tolerance=10.0*std::numeric_limits<float_type>::epsilon();
210  rb.abs_tol_min=10.0*std::numeric_limits<float_type>::min();
211  rb.inited=true;
212  }
213 
214  // now, push our first real element
215  top.f0index=0; // left element is stack[0]
216  top.f2index=1; // right element is stack[1]
217  top.abs_tol=rb.abs_tol;
218  rb_stack.push_back(top);
219 
220  while(rb_stack.size() > 2) {
221  recur_item &back=rb_stack.back();
222  if(back.done) {
223  float_type sum=back.step_sum;
224  rb_stack.pop_back();
225  rb_stack.back().step_sum+=sum; // bump our sum up to the parent
226  continue;
227  }
228  back.done=true;
229 
230  c2_fblock<float_type> &f0=rb_stack[back.f0index].f1, &f2=rb_stack[back.f2index].f1;
231  c2_fblock<float_type> &f1=back.f1; // will hold new middle values
232  size_t f1index=rb_stack.size()-1; // our current offset
233  float_type abs_tol=back.abs_tol;
234 
235  f1.x=0.5*(f0.x + f2.x); // center of interval
236  float_type dx2=0.5*(f2.x - f0.x);
237 
238  // check for underflow on step size, which prevents us from achieving specified accuracy.
239  if(std::abs(dx2) < std::abs(f1.x)*rb.dx_tolerance || std::abs(dx2) < rb.abs_tol_min) {
240  std::ostringstream outstr;
241  outstr << "Step size underflow in adaptive_partial_integrals at depth=" << back.depth << ", x= " << f1.x;
242  throw c2_exception(outstr.str().c_str());
243  }
244 
245  fill_fblock(f1);
246  if(c2_isnan(f1.y)) {
247  bad_x_point=f1.x;
248  return f1.y; // can't go any further if a nan has appeared
249  }
250 
251  bool yptrouble=f0.ypbad || f2.ypbad || f1.ypbad;
252  bool ypptrouble=f0.yppbad || f2.yppbad || f1.yppbad;
253 
254  // select the real derivative count based on whether we are at a point where derivatives exist
255  int derivs = std::min(rb.derivs, (yptrouble||ypptrouble)?(yptrouble?0:1):2);
256 
257  if(!back.depth) { // top level, total has not been initialized yet
258  switch(derivs) { // create estimate of next lower order for first try
259  case 0:
260  back.previous_estimate=(f0.y+f2.y)*dx2; break;
261  case 1:
262  back.previous_estimate=(f0.y+4.0*f1.y+f2.y)*dx2/3.0; break;
263  case 2:
264  back.previous_estimate=( (14*f0.y + 32*f1.y + 14*f2.y) + 2*dx2 * (f0.yp - f2.yp) ) * dx2 /30.; break;
265  default:
266  back.previous_estimate=0.0; // just to suppress missing default warnings
267  }
268  }
269 
270  float_type left, right;
271 
272  // pre-compute constants so all multiplies use a small dynamic range
273  // note the forced csting of the denominators to assure full precision for (e.g.) long double
274  // constants for 0 derivative integrator
275  static const float_type c0c1=5./((float_type)12.), c0c2=8./((float_type)12.), c0c3=-1./((float_type)12.);
276  // constants for 1 derivative integrator
277  static const float_type c1c1=101./((float_type)240.), c1c2=128./((float_type)240.), c1c3=11./((float_type)240.),
278  c1c4=13./((float_type)240.), c1c5=-40./((float_type)240.), c1c6=-3./((float_type)240.);
279  // constants for 2 derivative integrator
280  static const float_type c2c1=169./((float_type)40320.), c2c2=1024./ ((float_type)40320.), c2c3=-41./((float_type)40320.),
281  c2c4=2727./((float_type)40320.), c2c5=-5040./((float_type)40320.), c2c6=423./((float_type)40320.),
282  c2c7=17007./((float_type)40320.), c2c8=24576./((float_type)40320.), c2c9=-1263./((float_type)40320.);
283 
284  switch(derivs) {
285  case 2:
286  // use ninth-order estimates for each side, from full set of all values (!) (Thanks, Mathematica!)
287  left= ( ( (c2c1*f0.ypp + c2c2*f1.ypp + c2c3*f2.ypp)*dx2 +
288  (c2c4*f0.yp + c2c5*f1.yp + c2c6*f2.yp) )*dx2 +
289  (c2c7*f0.y + c2c8*f1.y + c2c9*f2.y) )* dx2;
290  right= ( ( (c2c1*f2.ypp + c2c2*f1.ypp + c2c3*f0.ypp)*dx2 -
291  (c2c4*f2.yp + c2c5*f1.yp + c2c6*f0.yp) )*dx2 +
292  (c2c7*f2.y + c2c8*f1.y + c2c9*f0.y) )* dx2;
293  // std::cout << f0.x << " " << f1.x << " " << f2.x << std::endl ;
294  // std::cout << f0.y << " " << f1.y << " " << f2.y << " " << left << " " << right << " " << total << std::endl ;
295  break;
296  case 1:
297  left= ( (c1c1*f0.y + c1c2*f1.y + c1c3*f2.y) + dx2*(c1c4*f0.yp + c1c5*f1.yp + c1c6*f2.yp) ) * dx2 ;
298  right= ( (c1c1*f2.y + c1c2*f1.y + c1c3*f0.y) - dx2*(c1c4*f2.yp + c1c5*f1.yp + c1c6*f0.yp) ) * dx2 ;
299  break;
300  case 0:
301  left= (c0c1*f0.y + c0c2*f1.y + c0c3*f2.y)*dx2;
302  right= (c0c1*f2.y + c0c2*f1.y + c0c3*f0.y)*dx2;
303  break;
304  default:
305  left=right=0.0; // suppress warnings about missing default
306  break;
307  }
308 
309  float_type lrsum=left+right;
310 
311  bool extrapolate=back.depth && rb.extrapolate && (derivs==rb.derivs); // only extrapolate if no trouble with derivs
312  float_type eps=std::abs(back.previous_estimate-lrsum)*rb.eps_scale;
313  if(extrapolate) eps*=rb.eps_scale;
314 
315  if(rb.adapt && eps > abs_tol && eps > std::abs(lrsum)*rb.rel_tol) {
316  // tolerance not met, subdivide & recur
317  if(abs_tol > rb.abs_tol_min) abs_tol=abs_tol*0.5; // each half has half the error budget
318  top.abs_tol=abs_tol;
319  top.depth=back.depth+1;
320 
321  // save the last things we need from back before a push happens, in case
322  // the push causes a reallocation and moves the whole stack.
323  size_t f0index=back.f0index, f2index=back.f2index;
324 
325  top.f0index=f1index; top.f2index=f2index; // insert pointers to right side data into our recursion block
326  top.previous_estimate=right;
327  rb_stack.push_back(top);
328 
329  top.f0index=f0index; top.f2index=f1index; // insert pointers to left side data into our recursion block
330  top.previous_estimate=left;
331  rb_stack.push_back(top);
332 
333  } else if(extrapolate) {
334  // extrapolation only happens on leaf nodes, where the tolerance was met.
335  back.step_sum+=(rb.extrap_coef*lrsum - back.previous_estimate)*rb.extrap2;
336  } else {
337  back.step_sum+=lrsum;
338  }
339  }
340  return rb_stack.back().step_sum; // last element on the stack holds the sum
341 }
342 
343 template <typename float_type> bool c2_function<float_type>::check_monotonicity(
344  const std::vector<float_type> &data, const char message[]) const throw(c2_exception)
345 {
346  size_t np=data.size();
347  if(np < 2) return false; // one point has no direction!
348 
349  bool rev=(data[1] < data[0]); // which way do data point?
350  size_t i;
351 
352  if(!rev) for(i = 2; i < np && (data[i-1] < data[i]) ; i++) { }
353  else for(i = 2; i < np &&(data[i-1] > data[i]) ; i++) { }
354 
355  if(i != np) throw c2_exception(message);
356 
357  return rev;
358 }
359 
360 template <typename float_type> void c2_function<float_type>::set_sampling_grid(const std::vector<float_type> &grid) throw(c2_exception)
361 {
362  bool rev=this->check_monotonicity(grid, "set_sampling_grid: sampling grid not monotonic");
363 
364  if(!sampling_grid || no_overwrite_grid) sampling_grid=new std::vector<float_type>;
365  (*sampling_grid)=grid; no_overwrite_grid=0;
366 
367  if(rev) std::reverse(sampling_grid->begin(), sampling_grid->end()); // make it increasing
368 }
369 
370 template <typename float_type> void c2_function<float_type>::
371  get_sampling_grid(float_type amin, float_type amax, std::vector<float_type> &grid) const
372 {
373  std::vector<float_type> *result=&grid;
374  result->clear();
375 
376  if( !(sampling_grid) || !(sampling_grid->size()) || (amax <= sampling_grid->front()) || (amin >= sampling_grid->back()) ) {
377  // nothing is known about the function in this region, return amin and amax
378  result->push_back(amin);
379  result->push_back(amax);
380  } else {
381  std::vector<float_type> &sg=*sampling_grid; // just a shortcut
382  int np=sg.size();
383  int klo=0, khi=np-1, firstindex=0, lastindex=np-1;
384 
385  result->push_back(amin);
386 
387  if(amin > sg.front() ) {
388  // hunt through table for position bracketing starting point
389  while(khi-klo > 1) {
390  int kk=(khi+klo)/2;
391  if(sg[kk] > amin) khi=kk;
392  else klo=kk;
393  }
394  khi=klo+1;
395  // khi now points to first point definitively beyond our first point, or last point of array
396  firstindex=khi;
397  khi=np-1; // restart upper end of search
398  }
399 
400  if(amax < sg.back()) {
401  // hunt through table for position bracketing starting point
402  while(khi-klo > 1) {
403  int kk=(khi+klo)/2;
404  if(sg[kk] > amax) khi=kk;
405  else klo=kk;
406  }
407  khi=klo+1;
408  // khi now points to first point definitively beyond our last point, or last point of array
409  lastindex=klo;
410  }
411 
412  int initsize=result->size();
413  result->resize(initsize+(lastindex-firstindex+2));
414  std::copy(sg.begin()+firstindex, sg.begin()+lastindex+1, result->begin()+initsize);
415  result->back()=amax;
416 
417  // this is the unrefined sampling grid... now check for very close points on front & back and fix if needed.
418  preen_sampling_grid(result);
419  }
420 }
421 
422 template <typename float_type> void c2_function<float_type>::preen_sampling_grid(std::vector<float_type> *result) const
423 {
424  // this is the unrefined sampling grid... now check for very close points on front & back and fix if needed.
425  if(result->size() > 2) { // may be able to prune dangerously close points near the ends if there are at least 3 points
426  bool deleteit=false;
427  float_type x0=(*result)[0], x1=(*result)[1];
428  float_type dx1=x1-x0;
429 
430  float_type ftol=10.0*(std::numeric_limits<float_type>::epsilon()*(std::abs(x0)+std::abs(x1))+std::numeric_limits<float_type>::min());
431  if(dx1 < ftol) deleteit=true;
432  float_type dx2=(*result)[2]-x0;
433  if(dx1/dx2 < 0.1) deleteit=true; // endpoint is very close to internal interesting point
434 
435  if(deleteit) result->erase(result->begin()+1); // delete redundant interesting point
436  }
437 
438  if(result->size() > 2) { // may be able to prune dangerously close points near the ends if there are at least 3 points
439  bool deleteit=false;
440  int pos=result->size()-3;
441  float_type x0=(*result)[pos+1], x1=(*result)[pos+2];
442  float_type dx1=x1-x0;
443 
444  float_type ftol=10.0*(std::numeric_limits<float_type>::epsilon()*(std::abs(x0)+std::abs(x1))+std::numeric_limits<float_type>::min());
445  if(dx1 < ftol) deleteit=true;
446  float_type dx2=x1-(*result)[pos];
447  if(dx1/dx2 < 0.1) deleteit=true; // endpoint is very close to internal interesting point
448 
449  if(deleteit) result->erase(result->end()-2); // delete redundant interesting point
450  }
451 }
452 
453 template <typename float_type> void c2_function<float_type>::
454  refine_sampling_grid(std::vector<float_type> &grid, size_t refinement) const
455 {
456  size_t np=grid.size();
457  size_t count=(np-1)*refinement + 1;
458  float_type dxscale=1.0/refinement;
459 
460  std::vector<float_type> result(count);
461 
462  for(size_t i=0; i<(np-1); i++) {
463  float_type x=grid[i];
464  float_type dx=(grid[i+1]-x)*dxscale;
465  for(size_t j=0; j<refinement; j++, x+=dx) result[i*refinement+j]=x;
466  }
467  result.back()=grid.back();
468  grid=result; // copy the expanded grid back to the input
469 }
470 
471 template <typename float_type> float_type c2_function<float_type>::integral(float_type amin, float_type amax, std::vector<float_type> *partials,
472  float_type abs_tol, float_type rel_tol, int derivs, bool adapt, bool extrapolate) const throw(c2_exception)
473 {
474  if(amin==amax) {
475  if(partials) partials->clear();
476  return 0.0;
477  }
478  std::vector<float_type> grid;
479  get_sampling_grid(amin, amax, grid);
480  float_type intg=partial_integrals(grid, partials, abs_tol, rel_tol, derivs, adapt, extrapolate);
481  return intg;
482 }
483 
484 template <typename float_type> c2_function<float_type> &c2_function<float_type>::normalized_function(float_type amin, float_type amax, float_type norm)
485  const throw(c2_exception)
486 {
487  float_type intg=integral(amin, amax);
488  return *new c2_scaled_function_p<float_type>(*this, norm/intg);
489 }
490 
491 template <typename float_type> c2_function<float_type> &c2_function<float_type>::square_normalized_function(float_type amin, float_type amax, float_type norm)
492  const throw(c2_exception)
493 {
494  c2_ptr<float_type> mesquared((*new c2_quadratic_p<float_type>(0., 0., 0., 1.))(*this));
495 
496  std::vector<float_type> grid;
497  get_sampling_grid(amin, amax, grid);
498  float_type intg=mesquared->partial_integrals(grid);
499 
500  return *new c2_scaled_function_p<float_type>(*this, std::sqrt(norm/intg));
501 }
502 
503 template <typename float_type> c2_function<float_type> &c2_function<float_type>::square_normalized_function(
504  float_type amin, float_type amax, const c2_function<float_type> &weight, float_type norm)
505  const throw(c2_exception)
506 {
507  c2_ptr<float_type> weighted((*new c2_quadratic_p<float_type>(0., 0., 0., 1.))(*this) * weight);
508 
509  std::vector<float_type> grid;
510  get_sampling_grid(amin, amax, grid);
511  float_type intg=weighted->partial_integrals(grid);
512 
513  return *new c2_scaled_function_p<float_type>(*this, std::sqrt(norm/intg));
514 }
515 
516 template <typename float_type> float_type c2_function<float_type>::partial_integrals(
517  std::vector<float_type> xgrid, std::vector<float_type> *partials,
518  float_type abs_tol, float_type rel_tol, int derivs, bool adapt, bool extrapolate)
519  const throw(c2_exception)
520 {
521  int np=xgrid.size();
522 
523  c2_fblock<float_type> f0, f2;
524  struct c2_integrate_recur rb;
525  rb.rel_tol=rel_tol;
526  rb.extrapolate=extrapolate;
527  rb.adapt=adapt;
528  rb.derivs=derivs;
529  std::vector< recur_item > rb_stack;
530  rb_stack.reserve(20); // enough for most operations
531  rb.rb_stack=&rb_stack;
532  rb.inited=false;
533  float_type dx_inv=1.0/std::abs(xgrid.back()-xgrid.front());
534 
535  if(partials) partials->resize(np-1);
536 
537  float_type sum=0.0;
538 
539  f2.x=xgrid[0];
540  fill_fblock(f2);
541  if(c2_isnan(f2.y)) {
542  bad_x_point=f2.x;
543  return f2.y; // can't go any further if a nan has appeared
544  }
545 
546  for(int i=0; i<np-1; i++) {
547  f0=f2; // copy upper bound to lower before computing new upper bound
548 
549  f2.x=xgrid[i+1];
550  fill_fblock(f2);
551  if(c2_isnan(f2.y)) {
552  bad_x_point=f2.x;
553  return f2.y; // can't go any further if a nan has appeared
554  }
555 
556  rb.abs_tol=abs_tol*std::abs(f2.x-f0.x)*dx_inv; // distribute error tolerance over whole domain
557  rb.f0=&f0; rb.f1=&f2;
558  float_type ps=integrate_step(rb);
559  sum+=ps;
560  if(partials) (*partials)[i]=ps;
561  if(c2_isnan(ps)) break; // NaN stops integration
562  }
563  return sum;
564 }
565 
566 // generate a sampling grid at points separated by dx=5, which is intentionally
567 // incommensurate with pi and 2*pi so grid errors are somewhat randomized
568 template <typename float_type> void c2_sin_p<float_type>::
569  get_sampling_grid(float_type amin, float_type amax, std::vector<float_type> &grid) const
570 {
571  grid.clear();
572  for(; amin < amax; amin+=5.0) grid.push_back(amin);
573  grid.push_back(amax);
574  this->preen_sampling_grid(&grid);
575 }
576 
577 template <typename float_type> float_type c2_function_transformation<float_type>::evaluate(
578  float_type xraw,
579  float_type y, float_type yp0, float_type ypp0,
580  float_type *yprime, float_type *yprime2) const
581 {
582  y=Y.fHasStaticTransforms ? Y.pOut(y) : Y.fOut(y);
583 
584  if(yprime || yprime2) {
585 
586  float_type yp, yp2;
587  if(X.fHasStaticTransforms && Y.fHasStaticTransforms) {
588  float_type fpi=1.0/Y.pInPrime(y);
589  float_type gp=X.pInPrime(xraw);
590  // from Mathematica Dt[InverseFunction[f][y[g[x]]], x]
591  yp=gp*yp0*fpi; // transformed derivative
592  yp2=(gp*gp*ypp0 + X.pInDPrime(xraw)*yp0 - Y.pInDPrime(y)*yp*yp )*fpi;
593  } else {
594  float_type fpi=1.0/Y.fInPrime(y);
595  float_type gp=X.fInPrime(xraw);
596  // from Mathematica Dt[InverseFunction[f][y[g[x]]], x]
597  yp=gp*yp0*fpi; // transformed derivative
598  yp2=(gp*gp*ypp0 + X.fInDPrime(xraw)*yp0 - Y.fInDPrime(y)*yp*yp )*fpi;
599  }
600  if(yprime) *yprime=yp;
601  if(yprime2) *yprime2=yp2;
602  }
603  return y;
604 }
605 
606 // The constructor
607 template <typename float_type> interpolating_function_p<float_type> & interpolating_function_p<float_type>::load(
608  const std::vector<float_type> &x, const std::vector<float_type> &f,
609  bool lowerSlopeNatural, float_type lowerSlope,
610  bool upperSlopeNatural, float_type upperSlope,
611  bool splined
612  ) throw(c2_exception)
613 {
614  c2_ptr<float_type> keepme(*this);
615  X= x;
616  F= f;
617 
618  // Xraw is useful in some of the arithmetic operations between interpolating functions
619  Xraw=x;
620 
621  this->set_domain(std::min(Xraw.front(), Xraw.back()),std::max(Xraw.front(), Xraw.back()));
622 
623  if(x.size() != f.size()) {
624  throw c2_exception("interpolating_function::init() -- x & y inputs are of different size");
625  }
626 
627  size_t np=X.size(); // they are the same now, so lets take a short cut
628 
629  if(np < 2) {
630  throw c2_exception("interpolating_function::init() -- input < 2 elements ");
631  }
632 
633  bool xraw_rev=this->check_monotonicity(Xraw,
634  "interpolating_function::init() non-monotonic raw x input"); // which way does raw X point? sampling grid MUST be increasing
635 
636  if(!xraw_rev) { // we can use pointer to raw X values if they are in the right order
637  this->set_sampling_grid_pointer(Xraw); // our intial grid of x values is certainly a good guess for 'interesting' points
638  } else {
639  this->set_sampling_grid(Xraw); // make a copy of it, and assure it is in right order
640  }
641 
642  if(fTransform.X.fTransformed) { // check if X scale is nonlinear, and if so, do transform
643  if(!lowerSlopeNatural) lowerSlope /= fTransform.X.fInPrime(X[0]);
644  if(!upperSlopeNatural) upperSlope /= fTransform.X.fInPrime(X[np-1]);
645  for(size_t i=0; i<np; i++) X[i]=fTransform.X.fIn(X[i]);
646  }
647  if(fTransform.Y.fTransformed) { // check if Y scale is nonlinear, and if so, do transform
648  if(!lowerSlopeNatural) lowerSlope *= fTransform.Y.fInPrime(F[0]);
649  if(!upperSlopeNatural) upperSlope *= fTransform.Y.fInPrime(F[np-1]);
650  for(size_t i=0; i<np; i++) F[i]=fTransform.Y.fIn(F[i]);
651  }
652 
653  xInverted=this->check_monotonicity(X,
654  "interpolating_function::init() non-monotonic transformed x input");
655 
656  if(splined) spline(lowerSlopeNatural, lowerSlope, upperSlopeNatural, upperSlope);
657  else y2.assign(np,0.0);
658 
659  lastKLow=0;
660  keepme.release_for_return();
661  return *this;
662 }
663 
664 
665 // The constructor
666 template <typename float_type> interpolating_function_p<float_type> & interpolating_function_p<float_type>::load_pairs(
667  std::vector<std::pair<float_type, float_type> > &data,
668  bool lowerSlopeNatural, float_type lowerSlope,
669  bool upperSlopeNatural, float_type upperSlope,
670  bool splined
671  ) throw(c2_exception)
672 {
673  c2_ptr<float_type> keepme(*this);
674 
675  size_t np=data.size();
676  if(np < 2) {
677  throw c2_exception("interpolating_function::init() -- input < 2 elements ");
678  }
679 
680  // sort into ascending order
681  std::sort(data.begin(), data.end(), comp_pair);
682 
683  std::vector<float_type> xtmp, ytmp;
684  xtmp.reserve(np);
685  ytmp.reserve(np);
686  for (size_t i=0; i<np; i++) {
687  xtmp.push_back(data[i].first);
688  ytmp.push_back(data[i].second);
689  }
690  this->load(xtmp, ytmp, lowerSlopeNatural, lowerSlope, upperSlopeNatural, upperSlope, splined);
691 
692  keepme.release_for_return();
693  return *this;
694 }
695 
696 template <typename float_type> interpolating_function_p<float_type> &
697  interpolating_function_p<float_type>::load_random_generator_function(
698  const std::vector<float_type> &bincenters, const c2_function<float_type> &binheights)
699  throw(c2_exception)
700 {
701  c2_ptr<float_type> keepme(*this);
702 
703  std::vector<float_type> integral;
704  c2_const_ptr<float_type> keepit(binheights); // manage function... not really needed here, but always safe.
705  // integrate from first to last bin in original order, leaving results in integral
706  // ask for relative error of 1e-6 on each bin, with absolute error set to 0 (since we don't know the data scale).
707  float_type sum=binheights.partial_integrals(bincenters, &integral, 0.0, 1e-6);
708  // the integral vector now has partial integrals... it must be accumulated by summing
709  integral.insert(integral.begin(), 0.0); // integral from start to start is 0
710  float_type scale=1.0/sum;
711  for(size_t i=1; i<integral.size(); i++) integral[i]=integral[i]*scale + integral[i-1];
712  integral.back()=1.0; // force exact value on boundary
713 
714  this->load(integral, bincenters,
715  false, 1.0/(scale*binheights(bincenters.front() )),
716  false, 1.0/(scale*binheights(bincenters.back() ))
717  ); // use integral as x axis in inverse function
718  keepme.release_for_return();
719  return *this;
720 }
721 
722 template <typename float_type> interpolating_function_p<float_type> &
723  interpolating_function_p<float_type>::load_random_generator_bins(
724  const std::vector<float_type> &bins, const std::vector<float_type> &binheights, bool splined)
725  throw(c2_exception)
726 {
727  c2_ptr<float_type> keepme(*this);
728 
729  size_t np=binheights.size();
730  std::vector<float_type> integral(np+1), bin_edges(np+1);
731 
732  // compute the integral based on estimates of the bin edges from the given bin centers...
733  // except for bin 0 & final bin, the edge of a bin is halfway between then center of the
734  // bin and the center of the previous/next bin.
735  // This gives width[n] = (center[n+1]+center[n])/2 - (center[n]+center[n-1])/2 = (center[n+1]-center[n-1])/2
736  // for the edges, assume a bin of width (center[1]-center[0]) or (center[np-1]-center[np-2])
737  // be careful that absolute values are used in case data are reversed.
738 
739  if(bins.size() == binheights.size()+1) {
740  bin_edges=bins; // edges array was passed in
741  } else if (bins.size() == binheights.size()) {
742  bin_edges.front()=bins[0] - (bins[1]-bins[0])*0.5; // edge bin
743  for(size_t i=1; i<np; i++) {
744  bin_edges[i]=(bins[i]+bins[i-1])*0.5;
745  }
746  bin_edges.back()=bins[np-1] + (bins[np-1]-bins[np-2])*0.5; // edge bin
747  } else {
748  throw c2_exception("inconsistent bin vectors passed to load_random_generator_bins");
749  }
750 
751  float_type running_sum=0.0;
752  for(size_t i=0; i<np; i++) {
753  integral[i]=running_sum;
754  if(!binheights[i]) throw c2_exception("empty bin passed to load_random_generator_bins");
755  running_sum+=binheights[i]*std::abs(bin_edges[i+1]-bin_edges[i]);
756  }
757  float_type scale=1.0/running_sum;
758  for(size_t i=0; i<np; i++) integral[i]*=scale;
759  integral.back()=1.0; // force exactly correct value on boundary
760  this->load(integral, bin_edges,
761  false, 1.0/(scale*binheights.front()),
762  false, 1.0/(scale*binheights.back()),
763  splined); // use integral as x axis in inverse function
764  keepme.release_for_return();
765  return *this;
766 }
767 
768 
769 // The spline table generator
770 template <typename float_type> void interpolating_function_p<float_type>::spline(
771  bool lowerSlopeNatural, float_type lowerSlope,
772  bool upperSlopeNatural, float_type upperSlope
773  ) throw(c2_exception)
774 {
775 // construct spline tables here.
776  // this code is a re-translation of the pythonlabtools spline algorithm from pythonlabtools.sourceforge.net
777  size_t np=X.size();
778  std::vector<float_type> u(np), dy(np-1), dx(np-1), dxi(np-1), dx2i(np-2), siga(np-2), dydx(np-1);
779 
780  std::transform(X.begin()+1, X.end(), X.begin(), dx.begin(), std::minus<float_type>() ); // dx=X[1:] - X [:-1]
781  for(size_t i=0; i<dxi.size(); i++) dxi[i]=1.0/dx[i]; // dxi = 1/dx
782  for(size_t i=0; i<dx2i.size(); i++) dx2i[i]=1.0/(X[i+2]-X[i]);
783 
784  std::transform(F.begin()+1, F.end(), F.begin(), dy.begin(), std::minus<float_type>() ); // dy = F[i+1]-F[i]
785  std::transform(dx2i.begin(), dx2i.end(), dx.begin(), siga.begin(), std::multiplies<float_type>()); // siga = dx[:-1]*dx2i
786  std::transform(dxi.begin(), dxi.end(), dy.begin(), dydx.begin(), std::multiplies<float_type>()); // dydx=dy/dx
787 
788  // u[i]=(y[i+1]-y[i])/float(x[i+1]-x[i]) - (y[i]-y[i-1])/float(x[i]-x[i-1])
789  std::transform(dydx.begin()+1, dydx.end(), dydx.begin(), u.begin()+1, std::minus<float_type>() ); // incomplete rendering of u = dydx[1:]-dydx[:-1]
790 
791  y2.resize(np,0.0);
792 
793  if(lowerSlopeNatural) {
794  y2[0]=u[0]=0.0;
795  } else {
796  y2[0]= -0.5;
797  u[0]=(3.0*dxi[0])*(dy[0]*dxi[0] -lowerSlope);
798  }
799 
800  for(size_t i=1; i < np -1; i++) { // the inner loop
801  float_type sig=siga[i-1];
802  float_type p=sig*y2[i-1]+2.0;
803  y2[i]=(sig-1.0)/p;
804  u[i]=(6.0*u[i]*dx2i[i-1] - sig*u[i-1])/p;
805  }
806 
807  float_type qn, un;
808 
809  if(upperSlopeNatural) {
810  qn=un=0.0;
811  } else {
812  qn= 0.5;
813  un=(3.0*dxi[dxi.size()-1])*(upperSlope- dy[dy.size()-1]*dxi[dxi.size()-1] );
814  }
815 
816  y2[np-1]=(un-qn*u[np-2])/(qn*y2[np-2]+1.0);
817  for (size_t k=np-1; k != 0; k--) y2[k-1]=y2[k-1]*y2[k]+u[k-1];
818 }
819 
820 template <typename float_type> interpolating_function_p<float_type> &interpolating_function_p<float_type>::sample_function(
821  const c2_function<float_type> &func,
822  float_type amin, float_type amax, float_type abs_tol, float_type rel_tol,
823  bool lowerSlopeNatural, float_type lowerSlope,
824  bool upperSlopeNatural, float_type upperSlope
825  ) throw(c2_exception)
826 {
827  c2_ptr<float_type> keepme(*this);
828 
829  const c2_transformation<float_type> &XX=fTransform.X, &YY=fTransform.Y; // shortcuts
830 
831  // set up our params to look like the samplng function for now
832  sampler_function=func;
833  std::vector<float_type> grid;
834  func.get_sampling_grid(amin, amax, grid);
835  size_t gsize=grid.size();
836  if(XX.fTransformed) for(size_t i=0; i<gsize; i++) grid[i]=XX.fIn(grid[i]);
837  this->set_sampling_grid_pointer(grid);
838 
839  // float_type amin1=fXin(amin), amax1=fXin(amax); // bounds in transformed space
840  // get a list of points needed in transformed space, directly into our tables
841  this->adaptively_sample(grid.front(), grid.back(), 8*abs_tol, 8*rel_tol, 0, &X, &F);
842  // clear the sampler function now, since otherwise our value_with_derivatives is broken
843  sampler_function.unset_function();
844 
845  xInverted=this->check_monotonicity(X,
846  "interpolating_function::init() non-monotonic transformed x input");
847 
848  size_t np=X.size();
849 
850  // Xraw is useful in some of the arithmetic operations between interpolating functions
851  if(!XX.fTransformed) Xraw=X;
852  else {
853  Xraw.resize(np);
854  for (size_t i=1; i<np-1; i++) Xraw[i]=XX.fOut(X[i]);
855  Xraw.front()=amin;
856  Xraw.back()=amax;
857  }
858 
859  bool xraw_rev=this->check_monotonicity(Xraw,
860  "interpolating_function::init() non-monotonic raw x input");
861  // which way does raw X point? sampling grid MUST be increasing
862 
863  if(!xraw_rev) { // we can use pointer to raw X values if they are in the right order
864  this->set_sampling_grid_pointer(Xraw);
865  // our intial grid of x values is certainly a good guess for 'interesting' points
866  } else {
867  this->set_sampling_grid(Xraw); // make a copy of it, and assure it is in right order
868  }
869 
870  if(XX.fTransformed) { // check if X scale is nonlinear, and if so, do transform
871  if(!lowerSlopeNatural) lowerSlope /= XX.fInPrime(amin);
872  if(!upperSlopeNatural) upperSlope /= XX.fInPrime(amax);
873  }
874  if(YY.fTransformed) { // check if Y scale is nonlinear, and if so, do transform
875  if(!lowerSlopeNatural) lowerSlope *= YY.fInPrime(func(amin));
876  if(!upperSlopeNatural) upperSlope *= YY.fInPrime(func(amax));
877  }
878  // note that each of ends has 3 points with two equal gaps, since they were obtained by bisection
879  // so the step sizes are easy to get
880  // the 'natural slope' option for sampled functions has a different meaning than
881  // for normal splines. In this case, the derivative is adjusted to make the
882  // second derivative constant on the last two points at each end
883  // which is consistent with the error sampling technique we used to get here
884  if(lowerSlopeNatural) {
885  float_type hlower=X[1]-X[0];
886  lowerSlope=0.5*(-F[2]-3*F[0]+4*F[1])/hlower;
887  lowerSlopeNatural=false; // it's not the usual meaning of natural any more
888  }
889  if(upperSlopeNatural) {
890  float_type hupper=X[np-1]-X[np-2];
891  upperSlope=0.5*(F[np-3]+3*F[np-1]-4*F[np-2])/hupper;
892  upperSlopeNatural=false; // it's not the usual meaning of natural any more
893  }
894  this->set_domain(amin, amax);
895 
896  spline(lowerSlopeNatural, lowerSlope, upperSlopeNatural, upperSlope);
897  lastKLow=0;
898  keepme.release_for_return();
899  return *this;
900 }
901 
902 // This function is the reason for this class to exist
903 // it computes the interpolated function, and (if requested) its proper first and second derivatives including all coordinate transforms
904 template <typename float_type> float_type interpolating_function_p<float_type>::value_with_derivatives(
905  float_type x, float_type *yprime, float_type *yprime2) const throw(c2_exception)
906 {
907  if(sampler_function.valid()) {
908  // if this is non-null, we are sampling data for later, so just return raw function
909  // however, transform it into our sampling space, first.
910  if(yprime) *yprime=0;
911  if(yprime2) *yprime2=0;
912  sampler_function->increment_evaluations();
913  return fTransform.Y.fIn(sampler_function(fTransform.X.fOut(x))); // derivatives are completely undefined
914  }
915 
916  if(x < this->xmin() || x > this->xmax()) {
917  std::ostringstream outstr;
918  outstr << "Interpolating function argument " << x << " out of range " << this->xmin() << " -- " << this ->xmax() << ": bailing";
919  throw c2_exception(outstr.str().c_str());
920  }
921 
922  float_type xraw=x;
923 
924  if(fTransform.X.fTransformed) x=fTransform.X.fHasStaticTransforms?
925  fTransform.X.pIn(x) : fTransform.X.fIn(x); // save time by explicitly testing for identity function here
926 
927  int klo=0, khi=X.size()-1;
928 
929  if(khi < 0) throw c2_exception("Uninitialized interpolating function being evaluated");
930 
931  const float_type *XX=&X[lastKLow]; // make all fast checks short offsets from here
932 
933  if(!xInverted) { // select search depending on whether transformed X is increasing or decreasing
934  if((XX[0] <= x) && (XX[1] >= x) ) { // already bracketed
935  klo=lastKLow;
936  } else if((XX[1] <= x) && (XX[2] >= x)) { // in next bracket to the right
937  klo=lastKLow+1;
938  } else if(lastKLow > 0 && (XX[-1] <= x) && (XX[0] >= x)) { // in next bracket to the left
939  klo=lastKLow-1;
940  } else { // not bracketed, not close, start over
941  // search for new KLow
942  while(khi-klo > 1) {
943  int kk=(khi+klo)/2;
944  if(X[kk] > x) khi=kk;
945  else klo=kk;
946  }
947  }
948  } else {
949  if((XX[0] >= x) && (XX[1] <= x) ) { // already bracketed
950  klo=lastKLow;
951  } else if((XX[1] >= x) && (XX[2] <= x)) { // in next bracket to the right
952  klo=lastKLow+1;
953  } else if(lastKLow > 0 && (XX[-1] >= x) && (XX[0] <= x)) { // in next bracket to the left
954  klo=lastKLow-1;
955  } else { // not bracketed, not close, start over
956  // search for new KLow
957  while(khi-klo > 1) {
958  int kk=(khi+klo)/2;
959  if(X[kk] < x) khi=kk;
960  else klo=kk;
961  }
962  }
963  }
964 
965  khi=klo+1;
966  lastKLow=klo;
967 
968  float_type h=X[khi]-X[klo];
969 
970  float_type a=(X[khi]-x)/h;
971  float_type b=1.0-a;
972  float_type ylo=F[klo], yhi=F[khi], y2lo=y2[klo], y2hi=y2[khi];
973  float_type y=a*ylo+b*yhi+((a*a*a-a)*y2lo+(b*b*b-b)*y2hi)*(h*h)/6.0;
974 
975  float_type yp0=0; // the derivative in interpolating table coordinates
976  float_type ypp0=0; // second derivative
977 
978  if(yprime || yprime2) {
979  yp0=(yhi-ylo)/h+((3*b*b-1)*y2hi-(3*a*a-1)*y2lo)*h/6.0; // the derivative in interpolating table coordinates
980  ypp0=b*y2hi+a*y2lo; // second derivative
981  }
982 
983  if(fTransform.isIdentity) {
984  if(yprime) *yprime=yp0;
985  if(yprime2) *yprime2=ypp0;
986  return y;
987  } else return fTransform.evaluate(xraw, y, yp0, ypp0, yprime, yprime2);
988 }
989 
990 template <typename float_type> void interpolating_function_p<float_type>::set_lower_extrapolation(float_type bound)
991 {
992  int kl = 0 ;
993  int kh=kl+1;
994  float_type xx=fTransform.X.fIn(bound);
995  float_type h0=X[kh]-X[kl];
996  float_type h1=xx-X[kl];
997  float_type yextrap=F[kl]+((F[kh]-F[kl])/h0 - h0*(y2[kl]+2.0*y2[kh])/6.0)*h1+y2[kl]*h1*h1/2.0;
998 
999  X.insert(X.begin(), xx);
1000  F.insert(F.begin(), yextrap);
1001  y2.insert(y2.begin(), y2.front()); // duplicate first or last element
1002  Xraw.insert(Xraw.begin(), bound);
1003  if (bound < this->fXMin) this->fXMin=bound; // check for reversed data
1004  else this->fXMax=bound;
1005 
1006  //printf("%10.4f %10.4f %10.4f %10.4f %10.4f\n", bound, xx, h0, h1, yextrap);
1007  //for(int i=0; i<X.size(); i++) printf("%4d %10.4f %10.4f %10.4f %10.4f \n", i, Xraw[i], X[i], F[i], y2[i]);
1008 }
1009 
1010 template <typename float_type> void interpolating_function_p<float_type>::set_upper_extrapolation(float_type bound)
1011 {
1012  int kl = X.size()-2 ;
1013  int kh=kl+1;
1014  float_type xx=fTransform.X.fIn(bound);
1015  float_type h0=X[kh]-X[kl];
1016  float_type h1=xx-X[kl];
1017  float_type yextrap=F[kl]+((F[kh]-F[kl])/h0 - h0*(y2[kl]+2.0*y2[kh])/6.0)*h1+y2[kl]*h1*h1/2.0;
1018 
1019  X.insert(X.end(), xx);
1020  F.insert(F.end(), yextrap);
1021  y2.insert(y2.end(), y2.back()); // duplicate first or last element
1022  Xraw.insert(Xraw.end(), bound);
1023  if (bound < this->fXMin) this->fXMin=bound; // check for reversed data
1024  else this->fXMax=bound;
1025  //printf("%10.4f %10.4f %10.4f %10.4f %10.4f\n", bound, xx, h0, h1, yextrap);
1026  //for(int i=0; i<X.size(); i++) printf("%4d %10.4f %10.4f %10.4f %10.4f \n", i, Xraw[i], X[i], F[i], y2[i]);
1027 }
1028 
1029 // return a new interpolating_function which is the unary function of an existing interpolating_function
1030 // can also be used to generate a resampling of another c2_function on a different grid
1031 // by creating a=interpolating_function(x,x)
1032 // and doing b=a.unary_operator(c) where c is a c2_function (probably another interpolating_function)
1033 
1034 template <typename float_type> interpolating_function_p<float_type>&
1035  interpolating_function_p<float_type>::unary_operator(const c2_function<float_type> &source) const
1036 {
1037  size_t np=X.size();
1038  std::vector<float_type>yv(np);
1039  c2_ptr<float_type> comp(source(*this));
1040  float_type yp0, yp1, ypp;
1041 
1042  for(size_t i=1; i<np-1; i++) {
1043  yv[i]=source(fTransform.Y.fOut(F[i])); // copy pointwise the function of our data values
1044  }
1045 
1046  yv.front()=comp(Xraw.front(), &yp0, &ypp); // get derivative at front
1047  yv.back()= comp(Xraw.back(), &yp1, &ypp); // get derivative at back
1048 
1049  interpolating_function_p &copy=clone();
1050  copy.load(this->Xraw, yv, false, yp0, false, yp1);
1051 
1052  return copy;
1053 }
1054 
1055 template <typename float_type> void
1056 interpolating_function_p<float_type>::get_data(std::vector<float_type> &xvals, std::vector<float_type> &yvals) const throw()
1057 {
1058 
1059  xvals=Xraw;
1060  yvals.resize(F.size());
1061 
1062  for(size_t i=0; i<F.size(); i++) yvals[i]=fTransform.Y.fOut(F[i]);
1063 }
1064 
1065 template <typename float_type> interpolating_function_p<float_type> &
1066  interpolating_function_p<float_type>::binary_operator(const c2_function<float_type> &rhs,
1067  const c2_binary_function<float_type> *combining_stub) const
1068 {
1069  size_t np=X.size();
1070  std::vector<float_type> yv(np);
1071  c2_constant_p<float_type> fval(0);
1072  float_type yp0, yp1, ypp;
1073 
1074  c2_const_ptr<float_type> stub(*combining_stub); // manage ownership
1075 
1076  for(size_t i=1; i<np-1; i++) {
1077  fval.reset(fTransform.Y.fOut(F[i])); // update the constant function pointwise
1078  yv[i]=combining_stub->combine(fval, rhs, Xraw[i], (float_type *)0, (float_type *)0); // compute rhs & combine without derivatives
1079  }
1080 
1081  yv.front()=combining_stub->combine(*this, rhs, Xraw.front(), &yp0, &ypp); // get derivative at front
1082  yv.back()= combining_stub->combine(*this, rhs, Xraw.back(), &yp1, &ypp); // get derivative at back
1083 
1084  interpolating_function_p &copy=clone();
1085  copy.load(this->Xraw, yv, false, yp0, false, yp1);
1086 
1087  return copy;
1088 }
1089 
1090 template <typename float_type> c2_inverse_function_p<float_type>::c2_inverse_function_p(const c2_function<float_type> &source)
1091  : c2_function<float_type>(), func(source)
1092 {
1093  float_type l=source.xmin();
1094  float_type r=source.xmax();
1095  start_hint=(l+r)*0.5; // guess that we start in the middle
1096  // compute our domain assuming the function is monotonic so its values on its domain boundaries are our domain
1097  float_type ly=source(l);
1098  float_type ry=source(r);
1099  if (ly > ry) {
1100  float_type t=ly; ly=ry; ry=t;
1101  }
1102  this->set_domain(ly, ry);
1103 }
1104 
1105 template <typename float_type> float_type c2_inverse_function_p<float_type>::value_with_derivatives(
1106  float_type x, float_type *yprime, float_type *yprime2
1107  ) const throw(c2_exception)
1108 {
1109  float_type l=this->func->xmin();
1110  float_type r=this->func->xmax();
1111  float_type yp, ypp;
1112  float_type y=this->func->find_root(l, r, get_start_hint(x), x, 0, &yp, &ypp);
1113  start_hint=y;
1114  if(yprime) *yprime=1.0/yp;
1115  if(yprime2) *yprime2=-ypp/(yp*yp*yp);
1116  return y;
1117 }
1118 
1119 //accumulated_histogram starts with binned data, generates the integral, and generates a piecewise linear interpolating_function
1120 //If drop_zeros is true, it merges empty bins together before integration
1121 //Note that the resulting interpolating_function is guaranteed to be increasing (if drop_zeros is false)
1122 // or stricly increasing (if drop_zeros is true)
1123 //If inverse_function is true, it drop zeros, integrates, and returns the inverse function which is useful
1124 // for random number generation based on the input distribution.
1125 //If normalize is true, the big end of the integral is scaled to 1.
1126 //If the data are passed in reverse order (large X first), the integral is carried out from the big end,
1127 // and then the data are reversed to the result in in increasing X order.
1128 template <typename float_type> accumulated_histogram<float_type>::accumulated_histogram(
1129  const std::vector<float_type>binedges, const std::vector<float_type> binheights,
1130  bool normalize, bool inverse_function, bool drop_zeros)
1131 {
1132 
1133  int np=binheights.size();
1134 
1135  std::vector<float_type> be, bh;
1136  if(drop_zeros || inverse_function) { //inverse functions cannot have any zero bins or they have vertical sections
1137  if(binheights[0] || !inverse_function) { // conserve lower x bound if not an inverse function
1138  be.push_back(binedges[0]);
1139  bh.push_back(binheights[0]);
1140  }
1141  for(int i=1; i<np-1; i++) {
1142  if(binheights[i]) {
1143  be.push_back(binedges[i]);
1144  bh.push_back(binheights[i]);
1145  }
1146  }
1147  if(binheights[np-1] || !inverse_function) {
1148  bh.push_back(binheights[np-1]);
1149  be.push_back(binedges[np-1]);
1150  be.push_back(binedges[np]); // push both sides of the last bin if needed
1151  }
1152  np=bh.size(); // set np to compressed size of bin array
1153  } else {
1154  be=binedges;
1155  bh=binheights;
1156  }
1157  std::vector<float_type> cum(np+1, 0.0);
1158  for(int i=1; i<=np; i++) cum[i]=bh[i]*(be[i]-be[i-1])+cum[i-1]; // accumulate bins, leaving bin 0 as 0
1159  if(be[1] < be[0]) { // if bins passed in backwards, reverse them
1160  std::reverse(be.begin(), be.end());
1161  std::reverse(cum.begin(), cum.end());
1162  for(unsigned int i=0; i<cum.size(); i++) cum[i]*=-1; // flip sign on reversed data
1163  }
1164  if(normalize) {
1165  float_type mmm=1.0/std::max(cum[0], cum[np]);
1166  for(int i=0; i<=np; i++) cum[i]*=mmm;
1167  }
1168  if(inverse_function) interpolating_function_p<float_type>(cum, be); // use cum as x axis in inverse function
1169  else interpolating_function_p<float_type>(be, cum); // else use lower bin edge as x axis
1170  std::fill(this->y2.begin(), this->y2.end(), 0.0); // clear second derivatives, to we are piecewise linear
1171 }
1172 
1173 template <typename float_type> c2_piecewise_function_p<float_type>::c2_piecewise_function_p()
1174 : c2_function<float_type>(), lastKLow(-1)
1175 {
1176  this->sampling_grid=new std::vector<float_type>; // this always has a smapling grid
1177 }
1178 
1179 template <typename float_type> c2_piecewise_function_p<float_type>::~c2_piecewise_function_p()
1180 {
1181 }
1182 
1183 template <typename float_type> float_type c2_piecewise_function_p<float_type>::value_with_derivatives(
1184  float_type x, float_type *yprime, float_type *yprime2
1185  ) const throw(c2_exception)
1186 {
1187 
1188  size_t np=functions.size();
1189  if(!np) throw c2_exception("attempting to evaluate an empty piecewise function");
1190 
1191  if(x < this->xmin() || x > this->xmax()) {
1192  std::ostringstream outstr;
1193  outstr << "piecewise function argument " << x << " out of range " << this->xmin() << " -- " << this->xmax();
1194  throw c2_exception(outstr.str().c_str());
1195  }
1196 
1197  int klo=0;
1198 
1199  if(lastKLow >= 0 && functions[lastKLow]->xmin() <= x && functions[lastKLow]->xmax() > x) {
1200  klo=lastKLow;
1201  } else {
1202  int khi=np;
1203  while(khi-klo > 1) {
1204  int kk=(khi+klo)/2;
1205  if(functions[kk]->xmin() > x) khi=kk;
1206  else klo=kk;
1207  }
1208  }
1209  lastKLow=klo;
1210  return functions[klo]->value_with_derivatives(x, yprime, yprime2);
1211 }
1212 
1213 template <typename float_type> void c2_piecewise_function_p<float_type>::append_function(
1214  const c2_function<float_type> &func) throw(c2_exception)
1215 {
1216  c2_const_ptr<float_type> keepfunc(func); // manage function before we can throw any exceptions
1217  if(functions.size()) { // check whether there are any gaps to fill, etc.
1218  const c2_function<float_type> &tail=functions.back();
1219  float_type x0=tail.xmax();
1220  float_type x1=func.xmin();
1221  if(x0 < x1) {
1222  // must insert a connector if x0 < x1
1223  float_type y0=tail(x0);
1224  float_type y1=func(x1);
1225  c2_function<float_type> &connector=*new c2_linear_p<float_type>(x0, y0, (y1-y0)/(x1-x0));
1226  connector.set_domain(x0,x1);
1227  functions.push_back(c2_const_ptr<float_type>(connector));
1228  this->sampling_grid->push_back(x1);
1229  } else if(x0>x1) throw c2_exception("function domains not increasing in c2_piecewise_function");
1230  }
1231  functions.push_back(keepfunc);
1232  // extend our domain to include all known functions
1233  this->set_domain(functions.front()->xmin(), functions.back()->xmax());
1234  // extend our sampling grid with the new function's grid, with the first point dropped to avoid duplicates
1235  std::vector<float_type> newgrid;
1236  func.get_sampling_grid(func.xmin(), func.xmax(), newgrid);
1237  this->sampling_grid->insert(this->sampling_grid->end(), newgrid.begin()+1, newgrid.end());
1238 }
1239 
1240 template <typename float_type> c2_connector_function_p<float_type>::c2_connector_function_p(
1241  float_type x0, const c2_function<float_type> &f0, float_type x2, const c2_function<float_type> &f2,
1242  bool auto_center, float_type y1)
1243  : c2_function<float_type>()
1244 {
1245  c2_const_ptr<float_type> left(f0), right(f2); // make sure if these are unowned, they get deleted
1246  c2_fblock<float_type> fb0, fb2;
1247  fb0.x=x0;
1248  f0.fill_fblock(fb0);
1249  fb2.x=x2;
1250  f2.fill_fblock(fb2);
1251  init(fb0, fb2, auto_center, y1);
1252 }
1253 
1254 template <typename float_type> c2_connector_function_p<float_type>::c2_connector_function_p(
1255  float_type x0, float_type y0, float_type yp0, float_type ypp0,
1256  float_type x2, float_type y2, float_type yp2, float_type ypp2,
1257  bool auto_center, float_type y1)
1258  : c2_function<float_type>()
1259 {
1260  c2_fblock<float_type> fb0, fb2;
1261  fb0.x=x0; fb0.y=y0; fb0.yp=yp0; fb0.ypp=ypp0;
1262  fb2.x=x2; fb2.y=y2; fb2.yp=yp2; fb2.ypp=ypp2;
1263  init(fb0, fb2, auto_center, y1);
1264 }
1265 
1266 template <typename float_type> c2_connector_function_p<float_type>::c2_connector_function_p(
1267  const c2_fblock<float_type> &fb0,
1268  const c2_fblock<float_type> &fb2,
1269  bool auto_center, float_type y1)
1270  : c2_function<float_type>()
1271 {
1272  init(fb0, fb2, auto_center, y1);
1273 }
1274 
1275 template <typename float_type> void c2_connector_function_p<float_type>::init(
1276  const c2_fblock<float_type> &fb0,
1277  const c2_fblock<float_type> &fb2,
1278  bool auto_center, float_type y1)
1279 {
1280  float_type dx=(fb2.x-fb0.x)/2.0;
1281  fhinv=1.0/dx;
1282 
1283  // scale derivs to put function on [-1,1] since mma solution is done this way
1284  float_type yp0=fb0.yp*dx;
1285  float_type yp2=fb2.yp*dx;
1286  float_type ypp0=fb0.ypp*dx*dx;
1287  float_type ypp2=fb2.ypp*dx*dx;
1288 
1289  float_type ff0=(8*(fb0.y + fb2.y) + 5*(yp0 - yp2) + ypp0 + ypp2)*0.0625;
1290  if(auto_center) y1=ff0; // forces ff to be 0 if we are auto-centering
1291 
1292  // y[x_] = y1 + x (a + b x) + x [(x-1) (x+1)] (c + d x) + x (x-1)^2 (x+1)^2 (e + f x)
1293  // y' = a + 2 b x + d x [(x+1)(x-1)] + (c + d x)(3x^2-1) + f x [(x+1)(x-1)]^2 + (e + f x)[(x+1)(x-1)](5x^2-1)
1294  // y'' = 2 b + 6x(c + d x) + 2d(3x^2-1) + 4x(e + f x)(5x^2-3) + 2f(x^2-1)(5x^2-1)
1295  fy1=y1;
1296  fa=(fb2.y - fb0.y)*0.5;
1297  fb=(fb0.y + fb2.y)*0.5 - y1;
1298  fc=(yp2+yp0-2.*fa)*0.25;
1299  fd=(yp2-yp0-4.*fb)*0.25;
1300  fe=(ypp2-ypp0-12.*fc)*0.0625;
1301  ff=(ff0 - y1);
1302  this->set_domain(fb0.x, fb2.x); // this is where the function is valid
1303 }
1304 
1305 template <typename float_type> c2_connector_function_p<float_type>::~c2_connector_function_p()
1306 {
1307 }
1308 
1309 template <typename float_type> float_type c2_connector_function_p<float_type>::value_with_derivatives(
1310  float_type x, float_type *yprime, float_type *yprime2
1311  ) const throw(c2_exception)
1312 {
1313  float_type x0=this->xmin(), x2=this->xmax();
1314  float_type dx=(x-(x0+x2)*0.5)*fhinv;
1315  float_type q1=(x-x0)*(x-x2)*fhinv*fhinv; // exactly vanish all bits at both ends
1316  float_type q2=dx*q1;
1317 
1318  float_type r1=fa+fb*dx;
1319  float_type r2=fc+fd*dx;
1320  float_type r3=fe+ff*dx;
1321 
1322  float_type y=fy1+dx*r1+q2*r2+q1*q2*r3;
1323 
1324  if(yprime || yprime2) {
1325  float_type q3=3*q1+2;
1326  float_type q4=5*q1+4;
1327  if(yprime) *yprime=(fa+2*fb*dx+fd*q2+r2*q3+ff*q1*q2+q1*q4*r3)*fhinv;
1328  if(yprime2) *yprime2=2*(fb+fd*q3+3*dx*r2+ff*q1*q4+r3*(2*dx*(5*q1+2)))*fhinv*fhinv;
1329  }
1330  return y;
1331 }
1332 
1333 // the recursive part of the sampler is agressively designed to minimize copying of data... lots of pointers
1334 template <typename float_type> void c2_function<float_type>::sample_step(c2_sample_recur &rb) const throw(c2_exception)
1335 {
1336  std::vector< recur_item > &rb_stack=*rb.rb_stack; // heap-based stack of data for recursion
1337  rb_stack.clear();
1338 
1339  recur_item top;
1340  top.depth=0; top.done=false; top.f0index=0; top.f2index=0;
1341 
1342  // push storage for our initial elements
1343  rb_stack.push_back(top);
1344  rb_stack.back().f1=*rb.f0;
1345  rb_stack.back().done=true;
1346 
1347  rb_stack.push_back(top);
1348  rb_stack.back().f1=*rb.f1;
1349  rb_stack.back().done=true;
1350 
1351  if(!rb.inited) {
1352  rb.dx_tolerance=10.0*std::numeric_limits<float_type>::epsilon();
1353  rb.abs_tol_min=10.0*std::numeric_limits<float_type>::min();
1354  rb.inited=true;
1355  }
1356 
1357  // now, push our first real element
1358  top.f0index=0; // left element is stack[0]
1359  top.f2index=1; // right element is stack[1]
1360  rb_stack.push_back(top);
1361 
1362  while(rb_stack.size() > 2) {
1363  recur_item &back=rb_stack.back();
1364  if(back.done) {
1365  rb_stack.pop_back();
1366  continue;
1367  }
1368  back.done=true;
1369 
1370  c2_fblock<float_type> &f0=rb_stack[back.f0index].f1, &f2=rb_stack[back.f2index].f1;
1371  c2_fblock<float_type> &f1=back.f1; // will hold new middle values
1372  size_t f1index=rb_stack.size()-1; // our current offset
1373 
1374  // std::cout << "processing: " << rb_stack.size() << " " <<
1375  // (&back-&rb_stack.front()) << " " << back.depth << " " << f0.x << " " << f2.x << std::endl;
1376 
1377  f1.x=0.5*(f0.x + f2.x); // center of interval
1378  float_type dx2=0.5*(f2.x - f0.x);
1379 
1380  // check for underflow on step size, which prevents us from achieving specified accuracy.
1381  if(std::abs(dx2) < std::abs(f1.x)*rb.dx_tolerance || std::abs(dx2) < rb.abs_tol_min) {
1382  std::ostringstream outstr;
1383  outstr << "Step size underflow in adaptive_sampling at depth=" << back.depth << ", x= " << f1.x;
1384  throw c2_exception(outstr.str().c_str());
1385  }
1386 
1387  fill_fblock(f1);
1388 
1389  if(c2_isnan(f1.y) || f1.ypbad || f1.yppbad) {
1390  // can't go any further if a nan has appeared
1391  bad_x_point=f1.x;
1392  throw c2_exception("NaN encountered while sampling function");
1393  }
1394 
1395  float_type eps;
1396  if(rb.derivs==2) {
1397  // this is code from connector_function to compute the value at the midpoint
1398  // it is re-included here to avoid constructing a complete c2connector
1399  // just to find out if we are close enough
1400  float_type ff0=(8*(f0.y + f2.y) + 5*(f0.yp - f2.yp)*dx2 + (f0.ypp+f2.ypp)*dx2*dx2)*0.0625;
1401  // we are converging as at least x**5 and bisecting, so real error on final step is smaller
1402  eps=std::abs(ff0-f1.y)/32.0;
1403  } else {
1404  // there are two tolerances to meet... the shift in the estimate of the actual point,
1405  // and the difference between the current points and the extremum
1406  // build all the coefficients needed to construct the local parabola
1407  float_type ypcenter, ypp;
1408  if (rb.derivs==1) {
1409  // linear extrapolation error using exact derivs
1410  eps = (std::abs(f0.y+f0.yp*dx2-f1.y)+std::abs(f2.y-f2.yp*dx2-f1.y))*0.125;
1411  ypcenter=2*f1.yp*dx2; // first deriv scaled so this interval is on [-1,1]
1412  ypp=2*(f2.yp-f0.yp)*dx2*dx2; // second deriv estimate scaled so this interval is on [-1,1]
1413  } else {
1414  // linear interpolation error without derivs if we are at top level
1415  // or 3-point parabolic interpolation estimates from previous level, if available
1416  ypcenter=(f2.y-f0.y)*0.5; // derivative estimate at center
1417  ypp=(f2.y+f0.y-2*f1.y); // second deriv estimate
1418  if(back.depth==0) eps=std::abs((f0.y+f2.y)*0.5 - f1.y)*2; // penalize first step
1419  else eps=std::abs(f1.y-back.previous_estimate)*0.25;
1420  }
1421  float_type ypleft=ypcenter-ypp; // derivative at left edge
1422  float_type ypright=ypcenter+ypp; // derivative at right edge
1423  float_type extremum_eps=0;
1424  if((ypleft*ypright) <=0) // y' changes sign if we have an extremum
1425  {
1426  // compute position and value of the extremum this way
1427  float_type xext=-ypcenter/ypp;
1428  float_type yext=f1.y + xext*ypcenter + 0.5*xext*xext*ypp;
1429  // and then find the the smallest offset of it from a point, looking in the left or right side
1430  if(xext <=0) extremum_eps=std::min(std::abs(f0.y-yext), std::abs(f1.y-yext));
1431  else extremum_eps=std::min(std::abs(f2.y-yext), std::abs(f1.y-yext));
1432  }
1433  eps=std::max(eps, extremum_eps); // if previous shot was really bad, keep trying
1434  }
1435 
1436  if(eps < rb.abs_tol || eps < std::abs(f1.y)*rb.rel_tol) {
1437  if(rb.out) {
1438  // we've met the tolerance, and are building a function, append two connectors
1439  rb.out->append_function(
1440  *new c2_connector_function_p<float_type>(f0, f1, true, 0.0)
1441  );
1442  rb.out->append_function(
1443  *new c2_connector_function_p<float_type>(f1, f2, true, 0.0)
1444  );
1445  }
1446  if(rb.xvals && rb.yvals) {
1447  rb.xvals->push_back(f0.x);
1448  rb.xvals->push_back(f1.x);
1449  rb.yvals->push_back(f0.y);
1450  rb.yvals->push_back(f1.y);
1451  // the value at f2 will get pushed in the next segment... it is not forgotten
1452  }
1453  } else {
1454  top.depth=back.depth+1; // increment depth counter
1455 
1456  // save the last things we need from back before a push happens, in case
1457  // the push causes a reallocation and moves the whole stack.
1458  size_t f0index=back.f0index, f2index=back.f2index;
1459  float_type left=0, right=0;
1460  if(rb.derivs==0) {
1461  // compute three-point parabolic interpolation estimate of right-hand and left-hand midpoint
1462  left=(6*f1.y + 3*f0.y - f2.y) * 0.125;
1463  right=(6*f1.y + 3*f2.y - f0.y) * 0.125;
1464  }
1465 
1466  top.f0index=f1index; top.f2index=f2index; // insert pointers to right side data into our recursion block
1467  top.previous_estimate=right;
1468  rb_stack.push_back(top);
1469 
1470  top.f0index=f0index; top.f2index=f1index; // insert pointers to left side data into our recursion block
1471  top.previous_estimate=left;
1472  rb_stack.push_back(top);
1473  }
1474  }
1475 }
1476 
1477 template <typename float_type> c2_piecewise_function_p<float_type> *
1478  c2_function<float_type>::adaptively_sample(
1479  float_type amin, float_type amax,
1480  float_type abs_tol, float_type rel_tol,
1481  int derivs, std::vector<float_type> *xvals, std::vector<float_type> *yvals) const throw(c2_exception)
1482 {
1483  c2_fblock<float_type> f0, f2;
1484  c2_sample_recur rb;
1485  std::vector< recur_item > rb_stack;
1486  rb_stack.reserve(20); // enough for most operations
1487  rb.rb_stack=&rb_stack;
1488  rb.out=0;
1489  if(derivs==2) rb.out=new c2_piecewise_function_p<float_type>();
1490  c2_ptr<float_type> pieces(*rb.out); // manage this function, if any, so it deletes on an exception
1491  rb.rel_tol=rel_tol;
1492  rb.abs_tol=abs_tol;
1493  rb.xvals=xvals;
1494  rb.yvals=yvals;
1495  rb.derivs=derivs;
1496  rb.inited=false;
1497 
1498  if(xvals && yvals) {
1499  xvals->clear();
1500  yvals->clear();
1501  }
1502 
1503  // create xgrid as a automatic-variable copy of the sampling grid so the exception handler correctly
1504  // disposes of it.
1505  std::vector<float_type> xgrid;
1506  get_sampling_grid(amin, amax, xgrid);
1507  int np=xgrid.size();
1508 
1509  f2.x=xgrid[0];
1510  fill_fblock(f2);
1511  if(c2_isnan(f2.y) || f2.ypbad || f2.yppbad) {
1512  // can't go any further if a nan has appeared
1513  bad_x_point=f2.x;
1514  throw c2_exception("NaN encountered while sampling function");
1515  }
1516 
1517  for(int i=0; i<np-1; i++) {
1518  f0=f2; // copy upper bound to lower before computing new upper bound
1519 
1520  f2.x=xgrid[i+1];
1521  fill_fblock(f2);
1522  if(c2_isnan(f2.y) || f2.ypbad || f2.yppbad) {
1523  // can't go any further if a nan has appeared
1524  bad_x_point=f2.x;
1525  throw c2_exception("NaN encountered while sampling function");
1526  }
1527 
1528  rb.f0=&f0; rb.f1=&f2;
1529  sample_step(rb);
1530  }
1531  if(xvals && yvals) { // push final point in vector
1532  xvals->push_back(f2.x);
1533  yvals->push_back(f2.y);
1534  }
1535 
1536  if(rb.out) rb.out->set_sampling_grid(xgrid); // reflect old sampling grid, which still should be right
1537  pieces.release_for_return(); // unmanage the piecewise_function so we can return it
1538  return rb.out;
1539 }