des.math.method.approx.interp source code

1 module des.math.method.approx.interp;
2 
3 import std.algorithm;
4 import std.exception;
5 import std.math;
6 import std.traits;
7 
8 version(unittest) import des.math.linear.vector;
9 
10 import des.math.combin;
11 public import des.math.basic.traits;
12 
13 ///
14 struct InterpolateTableData(T) if( hasBasicMathOp!T ) { float key; T val; }
15 
16 ///
17 auto lineInterpolate(T)( in InterpolateTableData!T[] tbl, float k, bool line_end=false )
18     if( hasBasicMathOp!T )
19 {
20     enforce( tbl.length > 1 );
21     size_t i = tbl.length - find!"a.key > b"( tbl, k ).length;
22     if( !line_end )
23     {
24         if( i <= 0 ) return tbl[0].val;
25         else if( i >= tbl.length ) return tbl[$-1].val;
26     }
27     else
28     {
29         if( i < 1 ) i = 1;
30         else if( i > tbl.length-1 ) i = tbl.length-1;
31     }
32 
33     auto a = tbl[i-1];
34     auto b = tbl[i];
35     return cast(T)( a.val + ( b.val - a.val ) * ( ( k - a.key ) / ( b.key - a.key ) ) );
36 }
37 
38 ///
39 unittest
40 {
41     alias InterpolateTableData!float TT;
42     auto tbl =
43         [
44         TT( 0, 10 ),
45         TT( 10, 18 ),
46         TT( 25, 20 ),
47         TT( 50, 13 ),
48         TT( 55, 25 )
49         ];
50 
51     assert( lineInterpolate( tbl, 0 ) == 10 );
52     assert( lineInterpolate( tbl, 5 ) == 14 );
53     assert( lineInterpolate( tbl, 10 ) == 18 );
54     assert( lineInterpolate( tbl, -10 ) == 10 );
55     assert( lineInterpolate( tbl, 80 ) == 25 );
56 }
57 
58 unittest
59 {
60     alias InterpolateTableData!double TD;
61     auto tbl =
62         [
63         TD( 0, 0 ),
64         TD( 1, 1 ),
65         TD( 2, 3 ),
66         TD( 3, 4 )
67         ];
68     assert( lineInterpolate( tbl, 5, true ) == 6 );
69     assert( lineInterpolate( tbl, -3, true ) == -3 );
70 }
71 
72 unittest
73 {
74     alias InterpolateTableData!vec3 TC;
75     auto tbl =
76         [
77         TC( 0, vec3(1,0,0) ),
78         TC( 1, vec3(0,1,0) ),
79         TC( 2, vec3(0,0,1) )
80         ];
81 
82     assert( lineInterpolate( tbl, -1 ) == vec3(1,0,0) );
83     assert( lineInterpolate( tbl, 0 ) == vec3(1,0,0) );
84     assert( lineInterpolate( tbl, 0.5 ) == vec3(0.5,0.5,0) );
85     assert( lineInterpolate( tbl, 3 ) == vec3(0,0,1) );
86 }
87 
88 @property bool canBezierInterpolate(T,F)()
89 { return is( typeof( T.init * F.init + T.init * F.init ) == T ) && isNumeric!F; }
90 
91 pure nothrow auto bezierInterpolation(T,F=float)( in T[] pts, F t )
92 if( canBezierInterpolate!(T,F) )
93 in
94 {
95     assert( t >= 0 && t <= 1 );
96     assert( pts.length > 0 );
97 }
98 body
99 {
100     auto N = pts.length-1;
101     auto omt = 1.0 - t;
102     T res = pts[0] * pow(omt,N) + pts[$-1] * pow(t,N);
103     for( auto i=1; i < N; i++ )
104         res = res + pts[i] * pow(t,i) * pow(omt,N-i) * combination(N,i);
105     return res;
106 }
107 
108 unittest
109 {
110     auto pts = [ vec2(0,0), vec2(2,2), vec2(4,0) ];
111     assert( bezierInterpolation( pts, 0.5 ) == vec2(2,1) );
112 }
113 
114 pure nothrow auto fixBezierSpline(T,F=float)( in T[] pts, size_t steps )
115 {
116     auto step = cast(F)(1.0) / cast(F)(steps-1);
117     auto res = new T[](steps);
118     for( auto i=0; i < steps; i++ )
119         res[i] = bezierInterpolation( pts, step * i );
120     return res;
121 }
122 
123 interface BezierSplineInterpolator(T,F=float)
124 if( canBezierInterpolate!(T,F) )
125 { T[] opCall( in T[] ); }
126 
127 class FixStepsBezierSplineInterpolator(T,F=float) : BezierSplineInterpolator!(T,F)
128 {
129     size_t steps;
130     this( size_t steps ) { this.steps = steps; }
131 
132     T[] opCall( in T[] pts )
133     { return fixBezierSpline!(T,F)( pts, steps ); }
134 }
135 
136 unittest
137 {
138     enum size_t len = 100;
139     auto fbi = new FixStepsBezierSplineInterpolator!(vec2)(len);
140     auto pts = [ vec2(0,0), vec2(2,2), vec2(4,0) ];
141     auto res = fbi( pts );
142     assert( res.length == len );
143 }
144 
145 /+ функция критеря должна быть функцией хевисада +/
146 auto criteriaBezierSpline(T,F=float)( in T[] pts, bool delegate(in T[], in T) criteria, F min_step=1e-5 )
147 if( canBezierInterpolate!(T,F) )
148 in
149 {
150     assert( pts.length > 1 );
151     assert( criteria !is null );
152     assert( min_step > 0 );
153     assert( min_step < cast(F)(.25) / cast(F)(pts.length-1) );
154 }
155 body
156 {
157     F step = cast(F)(.25) / cast(F)(pts.length-1);
158 
159     auto ret = [ bezierInterpolation( pts, 0 ), bezierInterpolation( pts, min_step ) ];
160 
161     auto t = min_step;
162     auto o = step;
163 
164     while( true )
165     {
166         if( 1-t < o ) o = 1-t;
167         auto buf = bezierInterpolation( pts, t+o );
168 
169         if( criteria( ret, buf ) )
170         {
171             t += o;
172             if( t >= 1 ) return ret ~ bezierInterpolation( pts, 1 );
173             continue;
174         }
175         else
176         {
177             o /= 2.0;
178             if( o > min_step ) continue;
179             else t += o;
180         }
181 
182         buf = bezierInterpolation( pts, t );
183 
184         if( t > 1 ) return ret ~ bezierInterpolation( pts, 1 );
185 
186         ret ~= buf;
187         o = step;
188     }
189 }
190 
191 /+ функция критеря должна быть функцией хевисада +/
192 auto filterBezierSpline(T,F=float)( in T[] pts, bool delegate(in T[], in T) criteria )
193 if( canBezierInterpolate!(T,F) )
194 in
195 {
196     assert( pts.length > 1 );
197     assert( criteria !is null );
198 }
199 body
200 {
201     auto ret = [ pts[0] ];
202 
203     for( auto i = 1; i < pts.length; i++ )
204     {
205         while( i < pts.length && criteria(ret, pts[i]) ) i++;
206 
207         if( ret[$-1] == pts[i-1] )
208             ret ~= pts[i];
209         else
210             ret ~= pts[i-1];
211     }
212 
213     if( ret[$-1] != pts[$-1] )
214         ret ~= pts[$-1];
215 
216     return ret;
217 }
218 
219 auto angleSplineCriteria(T)( float angle )
220     if( isSpecVector!(2,float,T) || isSpecVector!(3,float,T) )
221 {
222     auto cos_angle = cos( angle );
223     return ( in T[] accepted, in T newpoint )
224     {
225         if( accepted.length < 2 ) return false;
226 
227         auto cc = [ accepted[$-2], accepted[$-1], newpoint ];
228 
229         auto a = (cc[1]-cc[0]).e;
230         auto b = (cc[2]-cc[1]).e;
231 
232         return dot(a,b) >= cos_angle;
233     };
234 }
235 
236 auto lengthSplineCriteria(T)( float len )
237 if( isSpecVector!(2,float,T) || isSpecVector!(3,float,T) )
238 in{ assert( len > 0 ); } body
239 {
240     return ( in T[] accepted, in T newpoint )
241     { return (accepted[$-1] - newpoint).len2 <= len*len; };
242 }
243 
244 unittest
245 {
246     auto pts = [ vec2(0,0), vec2(5,2), vec2(-1,2), vec2(4,0) ];
247     auto pp = criteriaBezierSpline( pts, angleSplineCriteria!vec2(0.05) );
248 
249     assert( pp.length > pts.length );
250     assert( pp[0] == pts[0] );
251     assert( pp[$-1] == pts[$-1] );
252 }
253 
254 unittest
255 {
256     auto pts = [ vec2(0,0), vec2(5,2), vec2(-1,2), vec2(4,0) ];
257     auto pp = filterBezierSpline( fixBezierSpline( pts, 1000 ), lengthSplineCriteria!vec2(0.05) );
258 
259     assert( pp.length > pts.length );
260     assert( pp[0] == pts[0] );
261     assert( pp[$-1] == pts[$-1] );
262 }