source: issm/trunk-jpl/src/m/contrib/morlighem/ronne/InterpFromGrid.cpp@ 23874

Last change on this file since 23874 was 23874, checked in by Mathieu Morlighem, 6 years ago

NEW: added InterpFromGrid too
File size: 14.9 KB
Line 
1/*Written by Mathieu Morlighem April 19th 2019*/
2
3/*includes*/
4#include <mex.h>
5#include <pthread.h>
6#include <math.h> //for isnan
7#include <cstring> // for strcmp
8#define f(m,n)\
9 data[n*dataM+m] //Warning: matrix is transposed!
10
11/*Inputs{{{*/
12#define DATAX (mxArray*)prhs[0]
13#define DATAY (mxArray*)prhs[1]
14#define DATA (mxArray*)prhs[2]
15#define INTERPX (mxArray*)prhs[3]
16#define INTERPY (mxArray*)prhs[4]
17#define METHOD (mxArray*)prhs[5]
18/*}}}*/
19/*Outputs{{{*/
20#define INTERP (mxArray**)&plhs[0]
21/*}}}*/
22/*threading structs{{{*/
23typedef struct{
24 void* usr;
25 int my_thread;
26 int num_threads;
27} pthread_handle;
28
29typedef struct{
30 int dataM;
31 int dataN;
32 double* datax;
33 double* datay;
34 double* data;
35 int interpN;
36 double* interpx;
37 double* interpy;
38 double* interp;
39 int method;
40} AppStruct; /*}}}*/
41/*Prototypes{{{*/
42void FetchMatrixPointer(double** pmatrix,int *pM,int *pN,const mxArray* dataref);
43void FetchVectorPointer(double** pvector,int *pN,const mxArray* dataref);
44void FetchString(char** pstring,const mxArray* dataref);
45void WriteMatrix(mxArray** pdataref,double* matrix,int M,int N);
46void WriteVector(mxArray** pdataref,double* vector,int N);
47void* InterpFromGridt(void* vpthread_handle);
48void LaunchThread(void* function(void*), void* usr,int num_threads);
49bool binary_search_increasing(int* pindex,double target,double* list,int n);
50bool binary_search_decreasing(int* pindex,double target,double* list,int n);
51void dataderivatives(double* A,double* x,double* y,double* data,int M,int N, int m0, int m1,int m2,int m3, int n0, int n1,int n2,int n3);
52/*}}}*/
53
54void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]){/*{{{*/
55
56 double *datax = NULL;
57 double *datay = NULL;
58 double *data = NULL;
59 int dataM,dataN;
60 double *interpx = NULL;
61 double *interpy = NULL;
62 double *interp = NULL;
63 int interpM,interpN;
64
65 int num_threads = 20;
66 int test1,test2,test3,test4;
67 int method = 1; // 0 = nearest, 1 = bilinear, 2 = bicubic
68
69 /*Check arguments to avoid crash*/
70 if(nlhs>1 || (nrhs<5 || nrhs>6)) mexErrMsgTxt("Wrong usage");
71
72 /*Get variables from matlab to C*/
73 FetchVectorPointer(&datax,&dataN,DATAX);
74 FetchVectorPointer(&datay,&dataM,DATAY);
75 FetchMatrixPointer(&data ,&test1,&test2,DATA);
76 FetchMatrixPointer(&interpx,&interpM,&interpN,INTERPX);
77 FetchMatrixPointer(&interpy,&test3,&test4,INTERPY);
78 if(!dataM*dataN) mexErrMsgTxt("data is empty");
79 if(!interpM*interpN) mexErrMsgTxt("no interpolation requested");
80 if(test1!=dataM) mexErrMsgTxt("x should have as many elements as there are columns in the data");
81 if(test2!=dataN) mexErrMsgTxt("y should have as many elements as there are lines in the data");
82 if(test3!=interpM) mexErrMsgTxt("interpolation locations (x,y) do not have the same size");
83 if(test4!=interpN) mexErrMsgTxt("interpolation locations (x,y) do not have the same size");
84 if(nrhs==6){
85 char* method_string = NULL;
86 FetchString(&method_string,METHOD);
87 if(strcmp(method_string,"nearest")==0) method = 0;
88 else if(strcmp(method_string,"linear")==0) method = 1;
89 else if(strcmp(method_string,"cubic")==0) method = 2;
90 else{
91 mexErrMsgTxt("Method not supported yet");
92 }
93
94 mxFree(method_string);
95 }
96
97 /*Check inputs*/
98 if(true){
99 for(int i=0;i<interpM*interpN;i++){
100 if(isnan(interpx[i])) mexErrMsgTxt("NaN found in interpx");
101 if(isnan(interpy[i])) mexErrMsgTxt("NaN found in interpy");
102 }
103 }
104 if(method==3){
105 if(datax[1]-datax[0]<0) mexErrMsgTxt("x needs to be increasing for cubic interpolation");
106 if(datay[1]-datay[0]<0) mexErrMsgTxt("y needs to be increasing for cubic interpolation");
107 }
108
109 /*Allocate output*/
110 interp=(double*)mxMalloc(interpM*interpN*sizeof(double));
111
112 /*Multithreaded core*/
113 AppStruct usr;
114 usr.dataM = dataM;
115 usr.dataN = dataN;
116 usr.datax = datax;
117 usr.datay = datay;
118 usr.data = data;
119 usr.interpN = interpM*interpN;
120 usr.interpx = interpx;
121 usr.interpy = interpy;
122 usr.interp = interp;
123 usr.method = method;
124 LaunchThread(InterpFromGridt,(void*)&usr,num_threads);
125
126 /*Write output vector*/
127 WriteMatrix(INTERP,interp,interpM,interpN);
128
129 /*Clean-up and return*/
130 /*Do not erase pointers!*/
131 return;
132}/*}}}*/
133
134/*InterpFromGridt{{{*/
135void* InterpFromGridt(void* vpthread_handle){
136
137 /*recover this thread info*/
138 pthread_handle *handle = (pthread_handle*)vpthread_handle;
139 int my_thread = handle->my_thread;
140 int num_threads = handle->num_threads;
141
142 /*Recover struct*/
143 AppStruct *usr = (AppStruct*)handle->usr;
144 int dataM = usr->dataM;
145 int dataN = usr->dataN;
146 double *datax = usr->datax;
147 double *datay = usr->datay;
148 double *data = usr->data;
149 int interpN = usr->interpN;
150 double *interpx = usr->interpx;
151 double *interpy = usr->interpy;
152 double *interp = usr->interp;
153 int method = usr->method;
154
155 /*Intermediary*/
156 double xprime,yprime;
157 double x,y,x0,x1,x2,x3,y0,y1,y2,y3;
158 double Q11,Q12;
159 double Q21,Q22;
160 double A[16];
161 int m,n,m0,m1,m2,m3,n0,n1,n2,n3;
162 int oldm=-1,oldn=-1;
163
164 /*Is our matrix inverted?*/
165 bool invertx = (datax[1]-datax[0])<0 ? true:false;
166 bool inverty = (datay[1]-datay[0])<0 ? true:false;
167
168 for(int idx=my_thread;idx<interpN;idx+=num_threads){
169
170 x=interpx[idx];
171 y=interpy[idx];
172
173 /*Find indices m and n into y and x, for which y(m)<=y_grids<=y(m+1) and x(n)<=x_grid<=x(n+1)*/
174 if(invertx) binary_search_decreasing(&n,x,datax,dataN);
175 else binary_search_increasing(&n,x,datax,dataN);
176 if(inverty) binary_search_decreasing(&m,y,datay,dataM);
177 else binary_search_increasing(&m,y,datay,dataM);
178
179 if(n>=0 && n<dataN && m>=0 && m<dataM){
180
181 /* Q12 Q22
182 * y2 x---------+-----x
183 * | | |
184 * | |P |
185 * |---------+-----|
186 * | | |
187 * | | |
188 * y1 x---------+-----x Q21
189 * x1 x2
190 *
191 */
192 if(invertx){
193 n1=n+1; n2=n;
194 }
195 else{
196 n1=n; n2=n+1;
197 }
198 if(inverty){
199 m1=m+1; m2=m;
200 }
201 else{
202 m1=m; m2=m+1;
203 }
204
205 x1 = datax[n1]; x2 = datax[n2];
206 y1 = datay[m1]; y2 = datay[m2];
207
208 if(method==0){
209 /*Nearest neighbor interpolation*/
210 if(x > (x1+x2)/2.){
211 if(y > (y1+y2)/2.)
212 interp[idx] = f(m2,n2);
213 else
214 interp[idx] = f(m1,n2);
215 }
216 else{
217 if(y > (y1+y2)/2.)
218 interp[idx] = f(m2,n1);
219 else
220 interp[idx] = f(m1,n1);
221 }
222 continue;
223 }
224 else if(method==1){
225 /*Bilinear interpolation*/
226 if(Q11==-9999 || Q12==-9999 || Q21==-9999 || Q22==-9999){
227 interp[idx] = -9999;
228 continue;
229 }
230
231 interp[idx] =
232 +f(m1,n1)*(x2-x)*(y2-y)/((x2-x1)*(y2-y1))
233 +f(m1,n2)*(x-x1)*(y2-y)/((x2-x1)*(y2-y1))
234 +f(m2,n1)*(x2-x)*(y-y1)/((x2-x1)*(y2-y1))
235 +f(m2,n2)*(x-x1)*(y-y1)/((x2-x1)*(y2-y1));
236 }
237 else{
238 /*Bicubic interpolation*/
239 if(invertx){n0=n+2; n3=n-1;}
240 else{ n0=n-1; n3=n+2; }
241 if(inverty){ m0=m+2; m3=m-1; }
242 else{ m0=m-1; m3=m+2; }
243
244 if(n0<0 || n3>=dataN || m0<0 || m3>=dataM){
245 interp[idx] = -9999;
246 continue;
247 }
248
249 /*Local coordinates (between 0 and 1)*/
250 xprime = (x - datax[n1])/(datax[n2]-datax[n1]);
251 yprime = (y - datay[m1])/(datay[m2]-datay[m1]);
252
253 /*Get derivatives at current pixel*/
254 if(oldm!=m || oldn!=n){
255 dataderivatives(&A[0],datax,datay,data,dataM,dataN,m0,m1,m2,m3,n0,n1,n2,n3);
256 oldm = m;
257 oldn = n;
258 }
259
260 double a00 = A[0];
261 double a10 = A[4];
262 double a20 = -3*A[0]+3*A[1]-2*A[4]-A[5];
263 double a30 = 2*A[0]-2*A[1]+A[4]+A[5];
264 double a01 = A[8];
265 double a11 = A[12];
266 double a21 = -3*A[8]+3*A[9]-2*A[12]-A[13];
267 double a31 = 2*A[8]-2*A[9]+A[12]+A[13];
268 double a02 = -3*A[0]+3*A[2]-2*A[8]-A[10];
269 double a12 = -3*A[4]+3*A[6]-2*A[12]-A[14];
270 double a22 = 9*A[0]-9*A[1]-9*A[2]+9*A[3]+6*A[4]+3*A[5]-6*A[6]-3*A[7]+6*A[8]-6*A[9]+3*A[10]-3*A[11]+4*A[12]+2*A[13]+2*A[14]+A[15];
271 double a32 =-6*A[0]+6*A[1]+6*A[2]-6*A[3]-3*A[4]-3*A[5]+3*A[6]+3*A[7]-4*A[8]+4*A[9]-2*A[10]+2*A[11]-2*A[12]-2*A[13]-A[14]-A[15];
272 double a03 = 2*A[0]-2*A[2]+A[8]+A[10];
273 double a13 = 2*A[4]-2*A[6]+A[12]+A[14];
274 double a23 =-6*A[0]+6*A[1]+6*A[2]-6*A[3]-4*A[4]-2*A[5]+4*A[6]+2*A[7]-3*A[8]+3*A[9]-3*A[10]+3*A[11]-2*A[12]-A[13]-2*A[14]-A[15] ;
275 double a33 = 4*A[0]-4*A[1]-4*A[2]+4*A[3]+2*A[4]+2*A[5]-2*A[6]-2*A[7]+2*A[8]-2*A[9]+2*A[10]-2*A[11]+A[12]+A[13]+A[14]+A[15];
276
277 x1= xprime;
278 x2= x1*x1;
279 x3= x2*x1;
280 y1= yprime;
281 y2= y1*y1;
282 y3= y2*y1;
283 interp[idx] = (a00+a01*y1+a02*y2+a03*y3)+(a10+a11*y1+a12*y2+a13*y3)*x1+(a20+a21*y1+a22*y2+a23*y3)*x2+(a30+a31*y1+a32*y2+a33*y3)*x3;
284 }
285 }
286 else{
287 interp[idx] = -9999.;
288 }
289 }
290 //if(my_thread==0) printf("\r interpolation progress = %5.1f%%\n",100.);
291
292 return NULL;
293}/*}}}*/
294/*binary_search_increasing {{{*/
295bool binary_search_increasing(int* pindex,double target,double* list,int n){
296
297 /*output*/
298 int index; //index, if found
299 bool found=false; //found=0 if target is not found, 1 otherwise.
300
301 /*intermediary*/
302 int n0 = 0;
303 int n1 = int(n/2);
304 //int n1 = int((target-list[0])/(list[1]-list[0]));
305 int n2 = n-1;
306
307 if(target<list[n0]){
308 found = true;
309 index = -1;
310 }
311 else if(target>list[n2]){
312 found = true;
313 index = n;
314 }
315 else{
316 while(!found){
317 /*did we find the target?*/
318 if(list[n1]<=target && list[n1+1]>=target){
319 found = true;
320 index = n1;
321 break;
322 }
323 if(target < list[n1]){
324 n2 = n1;
325 n1 = n0 + int((n2-n0)/2);
326 }
327 else{
328 n0 = n1;
329 n1 = n0 + int((n2-n0)/2);
330 }
331 }
332 }
333
334 /*Assign output pointers:*/
335 *pindex=index;
336
337 /*Return result: */
338 return found;
339}/*}}}*/
340/*binary_search_decreasing{{{*/
341bool binary_search_decreasing(int* pindex,double target,double* list,int n){
342
343 /*output*/
344 int index; //index, if found
345 bool found=false; //found=0 if target is not found, 1 otherwise.
346
347 /*intermediary*/
348 int n0 = 0;
349 int n1 = int(n/2);
350 //int n1 = int((target-list[0])/(list[0]-list[1]));
351 int n2 = n-1;
352
353 if (target>list[n0]){
354 found = true;
355 index = -1;
356 }
357 else if(target<list[n2]){
358 found = true;
359 index = n;
360 }
361 else{
362 while(!found){
363 /*did we find the target?*/
364 if(list[n1]>=target && list[n1+1]<=target){
365 found = true;
366 index = n1;
367 break;
368 }
369 if(target > list[n1]){
370 n2 = n1;
371 n1 = n0 + int((n2-n0)/2);
372 }
373 else{
374 n0 = n1;
375 n1 = n0 + int((n2-n0)/2);
376 }
377 }
378 }
379
380 /*Assign output pointers:*/
381 *pindex=index;
382
383 /*Return result: */
384 return found;
385}/*}}}*/
386/*dataderivatives{{{*/
387void dataderivatives(double* A,double* x,double* y,double* data,int dataM,int dataN,
388 int m0, int m1,int m2,int m3, int n0, int n1,int n2,int n3){
389
390 /* i+1 + +-------+ f(1,1)
391 * | | |
392 * | |f(0,0) |
393 * i + +-------+ f(1,0)
394 * +--+-------+-----> x
395 * j j+1
396 */
397
398
399 /*Function at corners*/
400 A[0] = f(m1,n1); // f(0,0)
401 A[1] = f(m1,n2); // f(1,0)
402 A[2] = f(m2,n1); // f(0,1)
403 A[3] = f(m2,n2); // f(1,1)
404
405 /*x component of the gradient*/
406 A[4] = .5*(f(m1,n2) - f(m1,n0));///(x[n2]-x[n0]); // dfdx(0,0)
407 A[5] = .5*(f(m1,n3) - f(m1,n1));///(x[n3]-x[n1]); // dfdx(1,0)
408 A[6] = .5*(f(m2,n2) - f(m2,n0));///(x[n2]-x[n0]); // dfdx(0,1)
409 A[7] = .5*(f(m2,n3) - f(m2,n1));///(x[n3]-x[n1]); // dfdx(1,1)
410
411 /*y component of the gradient*/
412 A[ 8] = .5*(f(m2,n1) - f(m0,n1));///(y[m2]-y[m0]); // dfdy(0,0)
413 A[ 9] = .5*(f(m2,n2) - f(m0,n2));///(y[m2]-y[m0]); // dfdy(1,0)
414 A[10] = .5*(f(m3,n1) - f(m1,n1));///(y[m3]-y[m1]); // dfdy(0,1)
415 A[11] = .5*(f(m3,n2) - f(m1,n2));///(y[m3]-y[m1]); // dfdy(1,1)
416
417 /*cross-component of the gradient*/
418 A[12] = .25*( (f(m2,n2) - f(m2,n0)) - (f(m0,n2) - f(m0,n0)) );///( (x[n2]-x[n0])*(y[m2]-y[m0]) ); // d2f/dxdy (0,0)
419 A[13] = .25*( (f(m2,n3) - f(m2,n1)) - (f(m0,n3) - f(m0,n1)) );///( (x[n3]-x[n1])*(y[m2]-y[m0]) ); // d2f/dxdy (1,0)
420 A[14] = .25*( (f(m3,n2) - f(m3,n0)) - (f(m1,n2) - f(m1,n0)) );///( (x[n2]-x[n0])*(y[m3]-y[m1]) ); // d2f/dxdy (0,1)
421 A[15] = .25*( (f(m3,n3) - f(m3,n1)) - (f(m1,n3) - f(m1,n1)) );///( (x[n3]-x[n1])*(y[m3]-y[m1]) ); // d2f/dxdy (1,1)
422}/*}}}*/
423/*LaunchThread{{{*/
424void LaunchThread(void* function(void*), void* usr,int num_threads){
425
426 int i;
427 int *status = NULL;
428 pthread_t *threads = NULL;
429 pthread_handle *handles = NULL;
430
431 /*dynamically allocate: */
432 threads=(pthread_t*)mxMalloc(num_threads*sizeof(pthread_t));
433 handles=(pthread_handle*)mxMalloc(num_threads*sizeof(pthread_handle));
434
435 for(i=0;i<num_threads;i++){
436 handles[i].usr=usr;
437 handles[i].my_thread =i;
438 handles[i].num_threads=num_threads;
439 }
440
441 if(num_threads==1){
442 function(handles);
443 }
444 else{
445 for(i=0;i<num_threads;i++){
446 if(pthread_create(threads+i,NULL,function,(void*)(handles+i))){
447 mexErrMsgTxt("pthread_create error");
448 }
449 }
450 for(i=0;i<num_threads;i++){
451 if(pthread_join(threads[i],(void**)&status)){
452 mexErrMsgTxt("pthread_join error");
453 }
454 }
455 }
456
457 /*Free ressources:*/
458 mxFree(threads);
459 mxFree(handles);
460}/*}}}*/
461/*FetchMatrixPointer {{{*/
462void FetchMatrixPointer(double** pmatrix,int *pM,int *pN,const mxArray* dataref){
463
464 double *matrix=NULL;
465 double *values=NULL;
466 int N,M;
467
468 if(mxIsEmpty(dataref) ){
469 M=N=0;
470 matrix=NULL;
471 }
472 else if (mxIsDouble(dataref) ){
473 M=mxGetM(dataref);
474 N=mxGetN(dataref);
475 matrix=(double*)mxGetPr(dataref);
476 }
477 else{
478 mexErrMsgTxt("matrix type not supported");
479 }
480
481 *pmatrix=matrix;
482 if (pN)*pN=N;
483 if (pM)*pM=M;
484}/*}}}*/
485/*FetchVectorPointer {{{*/
486void FetchVectorPointer(double** pvector,int *pN,const mxArray* dataref){
487
488 double *vector=NULL;
489 double *values=NULL;
490 int N;
491
492 if(mxIsEmpty(dataref) ){
493 N=0;
494 vector=NULL;
495 }
496 else if (mxIsDouble(dataref) ){
497 if(mxGetM(dataref)!=1 && mxGetN(dataref)!=1){
498 mexErrMsgTxt("input is a matrix and not a vector");
499 }
500 N=mxGetN(dataref)*mxGetM(dataref);
501 vector=(double*)mxGetPr(dataref);
502 }
503 else{
504 mexErrMsgTxt("vector type not supported");
505 }
506
507 *pvector=vector;
508 if (pN)*pN=N;
509}/*}}}*/
510/*FetchString{{{*/
511void FetchString(char** pstring,const mxArray* dataref){
512
513 char* outstring=NULL;
514
515 /*Ok, the string should be coming directly from the matlab workspace: */
516 if (!mxIsClass(dataref,"char")){
517 mexErrMsgTxt("input data_type is not a string!");
518 }
519 else{
520 /*Recover the string:*/
521 int stringlen;
522
523 stringlen = mxGetM(dataref)*mxGetN(dataref)+1;
524 outstring = (char*)mxMalloc(stringlen*sizeof(char));
525 mxGetString(dataref,outstring,stringlen);
526 }
527
528 /*Assign output pointers:*/
529 *pstring=outstring;
530 return;
531}/*}}}*/
532/*WriteMatrix {{{*/
533void WriteMatrix(mxArray** pdataref,double* matrix,int M,int N){
534
535 mxArray* dataref=NULL;
536
537 if(matrix){
538 /*data is a double* pointer. set pointer and invert sizes*/
539 dataref = mxCreateDoubleMatrix(0,0,mxREAL);
540 mxSetM(dataref,(mwSize)M);
541 mxSetN(dataref,(mwSize)N);
542 mxSetPr(dataref,(double*)matrix);
543 }
544 else{
545 dataref = mxCreateDoubleScalar(0.0);
546 }
547 *pdataref=dataref;
548}
549/*}}}*/
550/*WriteVector {{{*/
551void WriteVector(mxArray** pdataref,double* vector,int N){
552
553 mxArray* dataref=NULL;
554
555 if(vector){
556 /*data is a double* pointer. Copy into a vector: */
557 dataref = mxCreateDoubleMatrix(0,0,mxREAL);
558 mxSetM(dataref,(mwSize)N);
559 mxSetN(dataref,(mwSize)1);
560 mxSetPr(dataref,(double*)vector);
561 }
562 else{
563 dataref = mxCreateDoubleScalar(0.0);
564 }
565 *pdataref=dataref;
566}
567/*}}}*/
Note: See TracBrowser for help on using the repository browser.