FunctionSampler

C++

Public Domain

Samples a function adaptively to resolve all sharp peaks

Download (right click, save as, rename as appropriate)

Embed

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
#ifndef FUNCTION_SAMPLER_1D_H
#define FUNCTION_SAMPLER_1D_H

#define _USE_MATH_DEFINES
#include <functional>
#include <list>
#include <set>
#include <map>
#include <cmath>
#include <limits>
#include <cassert>
#include <algorithm>

// Version 1.0 - 2009-05-14 - Original release

// Uncomment the following line to print out (lots!) of debugging
// output that shows the subdivision process.
// #define DEBUG_SAMPLER

#ifdef DEBUG_SAMPLER
# include <iostream>
#endif

namespace FunctionSampler1D{

// An instance of the sampler is created with a certain set of
// parameters:
//   PlotPoints        The initial number of uniformly spaced points
//                     to sample the function.
//   RangeThreshold    The fraction of the maxmimum y-extent below
//                     which subdividing should be avoided.
//   MaxBend           When a consecutive triplet of samples are
//                     fitted to a unit square, the maximum bend angle
//                     between the two line segments that will be
//                     tolerated before a subdivision will be
//                     performed.
//   MaxRecursion      The maximum level of bisection allowed.
// Instantiation of a Params structure will provide suitable default
// values.

// All actions happen on checkin

template <class FunctionValueType>
class FunctionSampler1D{
public:
	typedef FunctionValueType value_type;
	struct Params{
		size_t InitialPoints;
		double RangeThreshold;
		double MaxBend;
		size_t MaxRecursion;
		Params():
			InitialPoints(33),
			RangeThreshold(0.005),
			MaxBend(10.0*M_PI/180.0),
			MaxRecursion(20){}
	};
	struct FunctionSample{
		double x;
		FunctionValueType value;
		size_t recursion_level;
	};
	class CheckedOutSample{
		FunctionSample sample;
		size_t id;
	public:
		const int GetID() const{ return id; }
		const double& GetX() const{ return sample.x; }
		void SetValue(const FunctionValueType &val){ sample.value = val; }
		friend class FunctionSampler1D;
	};
private:
	double _x_start, _x_end;
	Params params;
	std::list<FunctionSample> samples;
	typedef typename std::list<FunctionSample>::iterator sample_list_iterator_t;
	typedef typename std::list<FunctionSample>::reverse_iterator sample_list_reverse_iterator_t;
	
	struct sample_list_iterator_comparator{
		bool operator()(const sample_list_iterator_t &a, const sample_list_iterator_t &b) const{
			return a->x < b->x;
		}
	};
	
	// samples in unrefined_samples need a point inserted BEFORE them
	typedef std::set<sample_list_iterator_t,sample_list_iterator_comparator> unrefined_set_t;
	mutable unrefined_set_t unrefined_samples;
	std::map<int, sample_list_iterator_t> checked_out_samples;
	
	size_t num_initial_points_checked_out;
	size_t id_counter;

	double dx, min_dx;
	double y_min, y_max;
public:
	FunctionSampler1D(
		double X0, double X1,
		const Params &Parameters
	):
		_x_start(X0), _x_end(X1),
		params(Parameters),
		num_initial_points_checked_out(0),
		id_counter(0)
	{
		if(params.InitialPoints < 3){ params.InitialPoints = 3; }
		if(_x_end < _x_start){ std::swap(_x_start, _x_end); }
		dx = (_x_end-_x_start) / (double)(params.InitialPoints-1);
		min_dx = dx*pow(0.5, (int)params.MaxRecursion);
		y_min = std::numeric_limits<double>::max();
		y_max = -std::numeric_limits<double>::max();
	}
	
	void Reset(){
		num_initial_points_checked_out = 0;
		unrefined_samples.clear();
		checked_out_samples.clear();
		samples.clear();
	}
	
	const std::list<FunctionSample>& GetValues() const{ return samples; }
	
	void CheckIn(const CheckedOutSample &sample){
		double y_val = double(sample.sample.value);
		if(y_val < y_min){ y_min = y_val; }
		if(y_val > y_max){ y_max = y_val; }
		
		if(sample.id < params.InitialPoints){
#if defined(DEBUG_SAMPLER)
			std::cerr << "* Checking in an initial sample, id = " << sample.id << std::endl;
#endif
			// We need to figure out where to insert it
			for(sample_list_iterator_t i = samples.begin(); i != samples.end(); ++i){
				if(i->x > sample.sample.x){
					CheckForRefinement(samples.insert(i, sample.sample));
					return;
				}
			}
#if defined(DEBUG_SAMPLER)
			std::cerr << "*  Inserting at end" << std::endl;
#endif
			CheckForRefinement(samples.insert(samples.end(), sample.sample));
		}else{
#if defined(DEBUG_SAMPLER)
			std::cerr << "* Checking in a sample, id = " << sample.id << std::endl;
#endif
			CheckForRefinement(samples.insert(checked_out_samples[sample.id], sample.sample));
			checked_out_samples.erase(sample.id);
		}
	}
	
	bool Done() const{
		if(id_counter < params.InitialPoints){ return false; }
		FilterUnrefinedSamples();
		return (unrefined_samples.empty());
	}
	
	bool CanCheckOut() const{
		if(num_initial_points_checked_out < params.InitialPoints){
			return true;
		}else{
			if(unrefined_samples.empty()){ return false; }
			else{
				FilterUnrefinedSamples();
				return !unrefined_samples.empty();
			}
		}
	}
	
	// Returns false if could not check out a sample
	bool CheckOut(CheckedOutSample &sample, bool do_expensive_checks = true){
		if(num_initial_points_checked_out < params.InitialPoints){
			sample.sample.x = _x_start+(double)num_initial_points_checked_out*dx;
			sample.sample.recursion_level = 0;
			sample.id = id_counter++;
			++num_initial_points_checked_out;
			return true;
		}
#if defined(DEBUG_SAMPLER)
		std::cerr << "* Checking out non-initial point" << std::endl;
#endif		
		// At this point, we want to pull a point out of unrefined_samples
		if(unrefined_samples.size() < 1){ return false; }
		sample_list_iterator_t icur = *(unrefined_samples.begin());
#if defined(DEBUG_SAMPLER)
			std::cerr << "*  Pulled out x = " << icur->x << std::endl;
#endif
		unrefined_samples.erase(unrefined_samples.begin());
		assert(icur != samples.begin());
		if((!do_expensive_checks) || (do_expensive_checks && CheckSampleForRefinement(icur))){
			sample_list_iterator_t iprev = icur; --iprev;
			
			sample.sample.x = 0.5*(icur->x + iprev->x);
			sample.sample.recursion_level = icur->recursion_level +1;
			sample.id = id_counter++;
			checked_out_samples[sample.id] = icur;
#if defined(DEBUG_SAMPLER)
			std::cerr << "*  new x = " << sample.sample.x << ", id = " << sample.id << std::endl;
#endif
			return true;
		}
		return false;
	}
private:
	void FilterUnrefinedSamples() const{
		while(!unrefined_samples.empty() && !CheckSampleForRefinement(*(unrefined_samples.begin()))){
			unrefined_samples.erase(unrefined_samples.begin());
		}
	}
	void CheckForRefinement(sample_list_iterator_t iter){
		sample_list_iterator_t iter_prev = iter, iter_next = iter;
		if(iter != samples.begin()){ --iter_prev; }
		if(iter != samples.end()  ){ ++iter_next; }

		sample_list_iterator_t icurr = iter_prev;
		do{
			CheckSampleForRefinement(icurr, true);
			++icurr;
		}while(icurr != iter_next);
	}
	bool CheckSampleForRefinement(sample_list_iterator_t icurr, bool do_add = false) const{
		if(samples.begin() == icurr || samples.end() == icurr){ return false; }
		sample_list_iterator_t iprev = icurr; --iprev;
		sample_list_iterator_t inext = icurr; ++inext;
		if(inext == samples.end()){ return false; }
		if(iprev == samples.begin()){ return false; }
		if(icurr->recursion_level >= params.MaxRecursion){ return false; }
		
		double yp, y0, yn;
		double xp, x0, xn;
		yp = double(iprev->value); xp = iprev->x;
		y0 = double(icurr->value); x0 = icurr->x;
		yn = double(inext->value); xn = inext->x;
		
		// Deal with NaNs
		if(y0 != y0){ y0 = 0; }
		if(yp != yp){ yp = 0; }
		if(yn != yn){ yn = 0; }

#if defined(DEBUG_SAMPLER)
		std::cerr << "* x0="  << x0 << ", y0=" << y0 << std::endl;
		std::cerr << "*  xp=" << xp << ", xn=" << xn << std::endl;
		std::cerr << "*  yp=" << yp << ", yn=" << yn << std::endl;
#endif

		if( ((xn-x0) < min_dx) && ((x0-xp) < min_dx)){
#if defined(DEBUG_SAMPLER)
			std::cerr << "*  MaxRecursion reached: "
					  << (xn-x0) << ", " << (x0-xp)
					  << " < " << min_dx << std::endl;
#endif
		}else{
			// Fit to square
			double local_y_max = std::max(yp, y0);
				   local_y_max = std::max(local_y_max, yn);
			double local_y_min = std::min(yp, y0);
				   local_y_min = std::min(local_y_min, yn);
			double local_x_max = std::max(xp, x0);
				   local_x_max = std::max(local_x_max, xn);
			double local_x_min = std::min(xp, x0);
				   local_x_min = std::min(local_x_min, xn);
			double dx0 = (x0-xp)/(local_x_max-local_x_min);
			double dx1 = (xn-x0)/(local_x_max-local_x_min);
			double dy0 = (y0==yp) ? 0 : (y0-yp)/(local_y_max-local_y_min);
			double dy1 = (yn==y0) ? 0 : (yn-y0)/(local_y_max-local_y_min);
			// Find the cosine of the angle between segments
			// 0->p and p->n using dot product
			double cosq = (dx0*dx1 + dy0*dy1)
						  / std::sqrt((dx0*dx0 + dy0*dy0) * (dx1*dx1 + dy1*dy1));
			
			// If the function is sufficiently "flat" (small y-variation)
			// at this point, then don't bother refining it.
			if( (std::abs(y0-yp) < params.RangeThreshold * (y_max - y_min)) &&
				(std::abs(yn-y0) < params.RangeThreshold * (y_max - y_min)) ){
#if defined(DEBUG_SAMPLER)
				std::cerr << "*  Flat enough" << std::endl;
#endif
			}else{
			
#if defined(DEBUG_SAMPLER)
				std::cerr << "*  cosq=" << cosq << std::endl;
#endif
				double min_dx_allowed = std::numeric_limits<double>::epsilon()*(_x_end-_x_start);
				if((xn - x0) < min_dx_allowed || (x0 - xp) < min_dx_allowed){
#if defined(DEBUG_SAMPLER)
					std::cerr << "*  Resolution too fine" << std::endl;
#endif
				}else{
					if((cosq < cos(params.MaxBend)) || (dx1 > 3*dx0) || (dx0 > 3*dx1)){
						if(x0-xp > xn-x0){
#if defined(DEBUG_SAMPLER)
							std::cerr << "*  Need refining between xp and x0" << std::endl;
#endif
							if(do_add){ unrefined_samples.insert(icurr); }
							return true;
						}else{
#if defined(DEBUG_SAMPLER)
							std::cerr << "*  Need refining between x0 and xn" << std::endl;
#endif
							if(do_add){ unrefined_samples.insert(inext); }
							return true;
						}
					}
				}
			}
		}
		return false;
	}
};

}; // end namespace FunctionSampler1D

#endif // FUNCTION_SAMPLER_1D_H