grid_search.cc 17.6 KB
Newer Older
1
2
3
4
#if defined(HAVE_CONFIG_H)
#include "config.h"
#endif

Uwe Schulzweida's avatar
Uwe Schulzweida committed
5
6
#include <stdio.h>
#include <stdlib.h>
Uwe Schulzweida's avatar
Uwe Schulzweida committed
7
#include <float.h>
Uwe Schulzweida's avatar
Uwe Schulzweida committed
8
9
#include <math.h>

Uwe Schulzweida's avatar
Uwe Schulzweida committed
10
#include "cdo_int.h"
11
#include "dmemory.h"
12
#include "util.h"
13
#include "grid.h"
Uwe Schulzweida's avatar
Uwe Schulzweida committed
14
#include "grid_search.h"
15
16
17
18


#define  PI       M_PI
#define  PI2      (2.0*PI)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
19
20


Uwe Schulzweida's avatar
Uwe Schulzweida committed
21
22
static int gridsearch_method_nn = GS_KDTREE;

23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
static
double cdo_default_search_radius(void)
{
  extern double gridsearch_radius;

  double search_radius = gridsearch_radius;

  if ( search_radius <    0. ) search_radius = 0.;
  if ( search_radius >  180. ) search_radius = 180.;

  search_radius = search_radius*DEG2RAD;

  return search_radius;
}

38
39
static inline
void LLtoXYZ_f(double lon, double lat, float *restrict xyz)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
40
41
{
   double cos_lat = cos(lat);
42
43
44
   xyz[0] = cos_lat * cos(lon);
   xyz[1] = cos_lat * sin(lon);
   xyz[2] = sin(lat);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
45
46
}

47
48
static inline
void LLtoXYZ_kd(double lon, double lat, kdata_t *restrict xyz)
49
50
51
52
53
54
55
{
   double cos_lat = cos(lat);
   xyz[0] = KDATA_SCALE(cos_lat * cos(lon));
   xyz[1] = KDATA_SCALE(cos_lat * sin(lon));
   xyz[2] = KDATA_SCALE(sin(lat));
}

56
static constexpr
57
58
59
60
61
float square(const float x)
{
  return x*x;
}

62
static constexpr
63
64
65
66
67
float distance(const float *restrict a, const float *restrict b)
{
  return (square((a[0]-b[0]))+square((a[1]-b[1]))+square((a[2]-b[2])));
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
68

69
void gridsearch_set_method(const char *methodstr)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
70
{
Uwe Schulzweida's avatar
Uwe Schulzweida committed
71
  if      ( strcmp(methodstr, "kdtree")  == 0 ) gridsearch_method_nn = GS_KDTREE;
72
  else if ( strcmp(methodstr, "kdsph")   == 0 ) gridsearch_method_nn = GS_KDSPH;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
73
74
  else if ( strcmp(methodstr, "nearpt3") == 0 ) gridsearch_method_nn = GS_NEARPT3;
  else if ( strcmp(methodstr, "full")    == 0 ) gridsearch_method_nn = GS_FULL;
75
  else
76
    cdoAbort("gridsearch method %s not available!", methodstr);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
77
78
79
}


80
struct gridsearch *gridsearch_create_reg2d(bool lcyclic, size_t nx, size_t ny, const double *restrict lons, const double *restrict lats)
81
{
82
  struct gridsearch *gs = (struct gridsearch *) Calloc(1, sizeof(struct gridsearch));
83
84
85
86

  gs->nx = nx;
  gs->ny = ny;

87
88
89
90
  unsigned nxm = nx;
  if ( lcyclic ) nxm++;

  double *reg2d_center_lon = (double *) Malloc(nxm*sizeof(double));
91
  double *reg2d_center_lat = (double *) Malloc(ny*sizeof(double));
Uwe Schulzweida's avatar
Uwe Schulzweida committed
92

93
  memcpy(reg2d_center_lon, lons, nxm*sizeof(double));
94
95
  memcpy(reg2d_center_lat, lats, ny*sizeof(double));

96
97
98
99
  double *coslon = (double *) Malloc(nx*sizeof(double));
  double *sinlon = (double *) Malloc(nx*sizeof(double));
  double *coslat = (double *) Malloc(ny*sizeof(double));
  double *sinlat = (double *) Malloc(ny*sizeof(double));
100

101
  for ( size_t n = 0; n < nx; ++n )
102
103
104
105
106
107
108
    {
      double rlon = lons[n];
      if ( rlon > PI2 ) rlon -= PI2;
      if ( rlon < 0   ) rlon += PI2;
      coslon[n] = cos(rlon);
      sinlon[n] = sin(rlon);
    }
109
  for ( size_t n = 0; n < ny; ++n )
110
111
112
113
114
115
116
117
118
119
120
121
122
    {
      coslat[n] = cos(lats[n]);
      sinlat[n] = sin(lats[n]);
    }

  gs->reg2d_center_lon = reg2d_center_lon;
  gs->reg2d_center_lat = reg2d_center_lat;

  gs->coslon = coslon;
  gs->sinlon = sinlon;
  gs->coslat = coslat;
  gs->sinlat = sinlat;

123
124
  gs->search_radius = cdo_default_search_radius();

125
126
  return gs;
}
Uwe Schulzweida's avatar
Uwe Schulzweida committed
127

128

129
struct kdNode *gs_create_kdtree(size_t n, const double *restrict lons, const double *restrict lats)
130
{
131
  struct kd_point *pointlist = (struct kd_point *) Malloc(n * sizeof(struct kd_point));  
132
  // see  example_cartesian.c
133
  if ( cdoVerbose ) printf("kdtree lib init 3D: n=%zu  nthreads=%d\n", n, ompNumThreads);
134
  kdata_t min[3], max[3];
135
136
  min[0] = min[1] = min[2] =  1e9;
  max[0] = max[1] = max[2] = -1e9;
137
  kdata_t *restrict point;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
138
139
140
#if defined(HAVE_OPENMP4)
#pragma omp simd
#endif
141
  for ( size_t i = 0; i < n; i++ ) 
142
143
    {
      point = pointlist[i].point;
144
      LLtoXYZ_kd(lons[i], lats[i], point);
145
      for ( size_t j = 0; j < 3; ++j )
146
147
148
149
150
151
152
153
        {
          min[j] = point[j] < min[j] ? point[j] : min[j];
          max[j] = point[j] > max[j] ? point[j] : max[j];
        }
      pointlist[i].index = i;
    }

  struct kdNode *kdt = kd_buildTree(pointlist, n, min, max, 3, ompNumThreads);
154
  if ( pointlist ) Free(pointlist);
155
156
157
158

  return kdt;
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
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
struct kdNode *gs_create_kdsph(size_t n, const double *restrict lons, const double *restrict lats)
{
  struct kd_point *pointlist = (struct kd_point *) Malloc(n * sizeof(struct kd_point)); // kd_point contains 3d point
  // see  example_cartesian.c
  if ( cdoVerbose ) printf("kdtree lib spherical init: n=%zu  nthreads=%d\n", n, ompNumThreads);
  kdata_t min[2], max[2];
  min[0] = min[1] =  1e9;
  max[0] = max[1] = -1e9;
  kdata_t *restrict point;
#if defined(HAVE_OPENMP4)
#pragma omp simd
#endif
  for ( size_t i = 0; i < n; i++ ) 
    {
      point = pointlist[i].point;
      point[0] = lons[i];
      point[1] = lats[i];
      point[2] = 0; // dummy
      for ( size_t j = 0; j < 2; ++j )
        {
          min[j] = point[j] < min[j] ? point[j] : min[j];
          max[j] = point[j] > max[j] ? point[j] : max[j];
        }
      pointlist[i].index = i;
    }

  struct kdNode *kdt = kd_sph_buildTree(pointlist, n, min, max, ompNumThreads);
  if ( pointlist ) Free(pointlist);

  return kdt;
}


Uwe Schulzweida's avatar
Uwe Schulzweida committed
193
void gs_destroy_nearpt3(struct gsNear *near)
194
{
Uwe Schulzweida's avatar
Uwe Schulzweida committed
195
196
  if ( near )
    {
197
#if defined(ENABLE_NEARPT3)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
198
      if ( near->nearpt3 ) nearpt3_destroy(near->nearpt3);
199
#endif
Uwe Schulzweida's avatar
Uwe Schulzweida committed
200
201
      if ( near->pts )
        {
202
203
          Free(near->pts[0]);
          Free(near->pts);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
204
205
        }

206
      Free(near);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
207
208
209
    }
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
210

211
struct gsNear *gs_create_nearpt3(size_t n, const double *restrict lons, const double *restrict lats)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
212
{
213
  struct gsNear *near = (struct gsNear *) Calloc(1, sizeof(struct gsNear));
Uwe Schulzweida's avatar
Uwe Schulzweida committed
214

215
216
  Coord_T **p = (Coord_T **) Malloc(n*sizeof(Coord_T *));
  p[0] = (Coord_T *) Malloc(3*n*sizeof(Coord_T));
217
  for ( size_t i = 1; i < n; i++ ) p[i] = p[0] + i*3;
218
219
220

  float point[3];

Uwe Schulzweida's avatar
Uwe Schulzweida committed
221
222
223
#if defined(HAVE_OPENMP4)
#pragma omp simd
#endif
224
  for ( size_t i = 0; i < n; i++ )
225
226
227
    {
      LLtoXYZ_f(lons[i], lats[i], point);

228
229
230
      p[i][0] = NPT3SCALE(point[0]);
      p[i][1] = NPT3SCALE(point[1]);
      p[i][2] = NPT3SCALE(point[2]);
231
232
    }

Uwe Schulzweida's avatar
Uwe Schulzweida committed
233
  near->n = n;
234
235
  near->plons = lons;
  near->plats = lats;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
236
  near->pts = p;
237
#if defined(ENABLE_NEARPT3)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
238
  near->nearpt3 = nearpt3_preprocess(n, p);
239
240
241
242
#else
  cdoAbort("nearpt3 support not compiled in!");
#endif
  
Uwe Schulzweida's avatar
Uwe Schulzweida committed
243
244
245
246
247
248
249
250
251
252
  return near;
}


void gs_destroy_full(struct gsFull *full)
{
  if ( full )
    {
      if ( full->pts )
        {
253
254
          Free(full->pts[0]);
          Free(full->pts);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
255
256
        }

257
      Free(full);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
258
259
260
261
    }
}


262
struct gsFull *gs_create_full(size_t n, const double *restrict lons, const double *restrict lats)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
263
{
264
  struct gsFull *full = (struct gsFull *) Calloc(1, sizeof(struct gsFull));
Uwe Schulzweida's avatar
Uwe Schulzweida committed
265

266
267
  float **p = (float **) Malloc(n*sizeof(float *));
  p[0] = (float *) Malloc(3*n*sizeof(float));
268
  for ( size_t i = 1; i < n; i++ ) p[i] = p[0] + i*3;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
269

Uwe Schulzweida's avatar
Uwe Schulzweida committed
270
271
272
#if defined(HAVE_OPENMP4)
#pragma omp simd
#endif
273
  for ( size_t i = 0; i < n; i++ )
Uwe Schulzweida's avatar
Uwe Schulzweida committed
274
    {
275
      LLtoXYZ_f(lons[i], lats[i], p[i]);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
276
277
    }
  
278
279
280
  full->n = n;
  full->plons = lons;
  full->plats = lats;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
281
282
283
  full->pts = p;

  return full;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
284
285
286
}


287
struct gridsearch *gridsearch_create(size_t n, const double *restrict lons, const double *restrict lats)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
288
{
289
  struct gridsearch *gs = (struct gridsearch *) Calloc(1, sizeof(struct gridsearch));
290

291
292
293
  gs->n = n;
  if ( n == 0 ) return gs;

294
  gs->kdt  = gs_create_kdtree(n, lons, lats);
295

296
297
  gs->search_radius = cdo_default_search_radius();

298
299
300
301
  return gs;
}


302
struct gridsearch *gridsearch_create_nn(size_t n, const double *restrict lons, const double *restrict lats)
303
{
304
  struct gridsearch *gs = (struct gridsearch *) Calloc(1, sizeof(struct gridsearch));
305
306
307
308
309

  gs->method_nn = gridsearch_method_nn;
  gs->n = n;
  if ( n == 0 ) return gs;

Uwe Schulzweida's avatar
Uwe Schulzweida committed
310
  if      ( gs->method_nn == GS_KDTREE  ) gs->kdt  = gs_create_kdtree(n, lons, lats);
311
  else if ( gs->method_nn == GS_KDSPH   ) gs->kdt  = gs_create_kdsph(n, lons, lats);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
312
313
  else if ( gs->method_nn == GS_NEARPT3 ) gs->near = gs_create_nearpt3(n, lons, lats);
  else if ( gs->method_nn == GS_FULL    ) gs->full = gs_create_full(n, lons, lats);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
314

315
316
  gs->search_radius = cdo_default_search_radius();

Uwe Schulzweida's avatar
Uwe Schulzweida committed
317
318
319
320
321
322
  return gs;
}


void gridsearch_delete(struct gridsearch *gs)
{
323
324
  if ( gs )
    {
325
326
      if ( gs->kdt ) kd_destroyTree(gs->kdt);
      
327
328
      if ( gs->reg2d_center_lon ) Free(gs->reg2d_center_lon);
      if ( gs->reg2d_center_lat ) Free(gs->reg2d_center_lat);
329

330
331
332
333
      if ( gs->coslat ) Free(gs->coslat);
      if ( gs->coslon ) Free(gs->coslon);
      if ( gs->sinlat ) Free(gs->sinlat);
      if ( gs->sinlon ) Free(gs->sinlon);
334

Uwe Schulzweida's avatar
Uwe Schulzweida committed
335
336
      if ( gs->near ) gs_destroy_nearpt3(gs->near);
      if ( gs->full ) gs_destroy_full(gs->full);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
337

338
      Free(gs);
339
    }
Uwe Schulzweida's avatar
Uwe Schulzweida committed
340
341
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
static
double gs_set_range(double *prange)
{
  double range;

  if ( prange )
    range = *prange;
  else
    range = SQR(2 * M_PI);     /* This has to be bigger than the presumed
                                * maximum distance to the NN but smaller
                                * than once around the sphere. The content
                                * of this variable is replaced with the
                                * distance to the NN squared. */
  return range;
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
358

Uwe Schulzweida's avatar
Uwe Schulzweida committed
359
kdNode *gs_nearest_kdtree(kdNode *kdt, double lon, double lat, double *prange)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
360
{
Uwe Schulzweida's avatar
Uwe Schulzweida committed
361
  if ( kdt == NULL ) return NULL;
362
  
Uwe Schulzweida's avatar
Uwe Schulzweida committed
363
  float range0 = gs_set_range(prange);
364
  kdata_t range = KDATA_SCALE(range0);
365

366
367
  kdata_t point[3];
  LLtoXYZ_kd(lon, lat, point);
368

Uwe Schulzweida's avatar
Uwe Schulzweida committed
369
  kdNode *node = kd_nearest(kdt, point, &range, 3);
370

371
372
373
  float frange = KDATA_INVSCALE(range);
  if ( !(frange < range0) ) node = NULL;
  if ( prange ) *prange = frange;
374

Uwe Schulzweida's avatar
Uwe Schulzweida committed
375
376
377
378
  return node;
}


379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
kdNode *gs_nearest_kdsph(kdNode *kdt, double lon, double lat, double *prange)
{
  if ( kdt == NULL ) return NULL;
  
  float range0 = gs_set_range(prange);
  kdata_t range = KDATA_SCALE(range0);

  kdata_t point[2];
  point[0] = lon;
  point[1] = lat;

  kdNode *node = kd_nearest(kdt, point, &range, 3);

  float frange = KDATA_INVSCALE(range);
  if ( !(frange < range0) ) node = NULL;
  if ( prange ) *prange = frange;

  return node;
}


Uwe Schulzweida's avatar
Uwe Schulzweida committed
400
unsigned gs_nearest_nearpt3(struct gsNear *near, double lon, double lat, double *prange)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
401
{
402
  size_t index = GS_NOT_FOUND;
403
  if ( near == NULL ) return index;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
404
  
405
#if defined(ENABLE_NEARPT3)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
406
  float range0 = gs_set_range(prange);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
407

408
  float point[3];
Uwe Schulzweida's avatar
Uwe Schulzweida committed
409
410
  LLtoXYZ_f(lon, lat, point);

411
  Coord_T q[3];
412
413
414
  q[0] = NPT3SCALE(point[0]);
  q[1] = NPT3SCALE(point[1]);
  q[2] = NPT3SCALE(point[2]);
415

416
  int closestpt = nearpt3_query(near->nearpt3, q);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
417

418
419
420
421
422
423
424
425
426
427
428
429
  if ( closestpt >= 0 )
    {
      float point0[3];
      LLtoXYZ_f(near->plons[closestpt], near->plats[closestpt], point0);
      
      float range = distance(point, point0);
      if ( range < range0 )
        {
           index = (unsigned) closestpt;
           *prange = range;
        }
    }
430
431
432
433
#else
  UNUSED(lon);
  UNUSED(lat);
  UNUSED(prange);
434
#endif
Uwe Schulzweida's avatar
Uwe Schulzweida committed
435
436
437
438

  return index;
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
439

440
size_t gs_nearest_full(struct  gsFull *full, double lon, double lat, double *prange)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
441
{
442
  size_t index = GS_NOT_FOUND;
443
  if ( full == NULL ) return index;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
444
445
446
  
  float range0 = gs_set_range(prange);

447
  float point[3];
Uwe Schulzweida's avatar
Uwe Schulzweida committed
448
449
  LLtoXYZ_f(lon, lat, point);

450
  size_t n = full->n;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
451
  float **pts = full->pts;
452
  size_t closestpt = n;
453
  float dist = FLT_MAX;
454
  for ( size_t i = 0; i < n; i++ )
Uwe Schulzweida's avatar
Uwe Schulzweida committed
455
    {
456
      float d = distance(point, pts[i]);
457
      if ( closestpt >=n || d < dist || (d<=dist && i < closestpt) )
Uwe Schulzweida's avatar
Uwe Schulzweida committed
458
459
460
461
462
463
        {
          dist = d;
          closestpt = i;
        }
    }

464
  if ( closestpt < n )
Uwe Schulzweida's avatar
Uwe Schulzweida committed
465
    {
466
467
468
      if ( dist < range0 )
        {
          *prange = dist;
469
          index = closestpt;
470
        }
Uwe Schulzweida's avatar
Uwe Schulzweida committed
471
472
473
474
475
    }
  
  return index;
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
476

477
size_t gridsearch_nearest(struct gridsearch *gs, double lon, double lat, double *prange)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
478
{
479
  size_t index = GS_NOT_FOUND;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
480

Uwe Schulzweida's avatar
Uwe Schulzweida committed
481
  if ( gs )
Uwe Schulzweida's avatar
Uwe Schulzweida committed
482
    {
Uwe Schulzweida's avatar
Uwe Schulzweida committed
483
484
      if ( gs->method_nn == GS_KDTREE )
        {
Uwe Schulzweida's avatar
Uwe Schulzweida committed
485
          kdNode *node = gs_nearest_kdtree(gs->kdt, lon, lat, prange);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
486
487
          if ( node ) index = (int) node->index;
        }
488
489
490
491
492
      else if ( gs->method_nn == GS_KDSPH )
        {
          kdNode *node = gs_nearest_kdsph(gs->kdt, lon, lat, prange);
          if ( node ) index = (int) node->index;
        }
Uwe Schulzweida's avatar
Uwe Schulzweida committed
493
494
      else if ( gs->method_nn == GS_NEARPT3 )
        {
Uwe Schulzweida's avatar
Uwe Schulzweida committed
495
496
497
498
499
          index = gs_nearest_nearpt3(gs->near, lon, lat, prange);
        }
      else if ( gs->method_nn == GS_FULL )
        {
          index = gs_nearest_full(gs->full, lon, lat, prange);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
500
        }
501
502
503
504
      else
        {
          cdoAbort("gridsearch_nearest::method_nn undefined!");
        }
Uwe Schulzweida's avatar
Uwe Schulzweida committed
505
506
507
    }

  return index;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
508
509
510
}


511
struct pqueue *gridsearch_qnearest(struct gridsearch *gs, double lon, double lat, double *prange, size_t nnn)
512
513
514
{
  if ( gs->kdt == NULL ) return NULL;
  
515
  kdata_t point[3];
Uwe Schulzweida's avatar
Uwe Schulzweida committed
516
  float range0 = gs_set_range(prange);
517
  kdata_t range = KDATA_SCALE(range0);
518
  struct pqueue *result = NULL;
519

520
  LLtoXYZ_kd(lon, lat, point);
521

522
523
524
525
  if ( gs )
    {
      result = kd_qnearest(gs->kdt, point, &range, nnn, 3);
      // printf("range %g %g %g %p\n", lon, lat, range, node);
526

527
      float frange = KDATA_INVSCALE(range);
528
      /*
529
      if ( !(frange < range0) )
530
531
532
533
534
535
536
537
538
539
        {
          if ( result )
            {
              struct resItem *p;
              while ( pqremove_min(result, &p) ) Free(p); // Free the result node taken from the heap
              Free(result->d); // free the heap
              Free(result);    // and free the heap information structure
            }
          result = NULL;
        }
540
      */
541
      if ( prange ) *prange = frange;
542
543
    }
  
544
545
  return result;
}
546
547
548
549
550

#define  BIGNUM   1.e+20
#define  TINY     1.e-14

static
551
void knn_store_distance(size_t nadd, double distance, size_t num_neighbors, size_t *restrict nbr_add, double *restrict nbr_dist)
552
553
554
555
556
557
558
559
560
561
562
{
  if ( num_neighbors == 1 )
    {
      if ( distance < nbr_dist[0] || (distance <= nbr_dist[0] && nadd < nbr_add[0]) )
	{
	  nbr_add[0]  = nadd;
	  nbr_dist[0] = distance;
	}
    }
  else
    {
563
      for ( size_t nchk = 0; nchk < num_neighbors; ++nchk )
564
565
566
	{
	  if ( distance < nbr_dist[nchk] || (distance <= nbr_dist[nchk] && nadd < nbr_add[nchk]) )
	    {
567
	      for ( size_t n = num_neighbors-1; n > nchk; --n )
568
569
570
571
572
573
574
575
576
577
578
579
580
		{
		  nbr_add[n]  = nbr_add[n-1];
		  nbr_dist[n] = nbr_dist[n-1];
		}
	      nbr_add[nchk]  = nadd;
	      nbr_dist[nchk] = distance;
	      break;
	    }
	}
    }
}

static
581
void knn_check_distance(size_t num_neighbors, const size_t *restrict nbr_add, double *restrict nbr_dist)
582
583
{
  // If distance is zero, set to small number
584
  for ( size_t nchk = 0; nchk < num_neighbors; ++nchk )
585
    if ( nbr_add[nchk] != GS_NOT_FOUND && nbr_dist[nchk] <= 0. ) nbr_dist[nchk] = TINY;
586
587
588
589
590
591
}


void gridsearch_knn_init(struct gsknn *knn)
{
  unsigned ndist = knn->ndist;
592
  size_t *restrict add = knn->add;
593
594
  double *restrict dist = knn->dist;

595
  for ( size_t i = 0; i < ndist; ++i )
596
    {
597
      add[i]  = GS_NOT_FOUND;
598
599
600
601
      dist[i] = BIGNUM;
    }
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
602

603
struct gsknn *gridsearch_knn_new(size_t size)
604
605
606
607
608
{
  struct gsknn *knn = (struct gsknn *) Malloc(sizeof(struct gsknn));
  
  knn->ndist   = size;
  knn->size    = size;
609
  knn->mask    = (bool*) Malloc(size*sizeof(bool));     // mask at nearest neighbors
610
  knn->add     = (size_t*) Malloc(size*sizeof(size_t)); // source address at nearest neighbors
Uwe Schulzweida's avatar
Uwe Schulzweida committed
611
  knn->dist    = (double*) Malloc(size*sizeof(double)); // angular distance of the nearest neighbors
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
  knn->tmpadd  = NULL;
  knn->tmpdist = NULL;

  gridsearch_knn_init(knn);

  return knn;
}


void gridsearch_knn_delete(struct gsknn *knn)
{
  if ( knn )
    {
      knn->size = 0;
      if ( knn->dist    ) Free(knn->dist);
      if ( knn->add     ) Free(knn->add);
      if ( knn->tmpdist ) Free(knn->tmpdist);
      if ( knn->tmpadd  ) Free(knn->tmpadd);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
630
      if ( knn->mask    ) Free(knn->mask);
631
632
633
634
635
      Free(knn);
    }
}


636
size_t gridsearch_knn(struct gridsearch *gs, struct gsknn *knn, double plon, double plat)
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
{
  /*
    Output variables:

    int nbr_add[num_neighbors]     ! address of each of the closest points
    double nbr_dist[num_neighbors] ! distance to each of the closest points

    Input variables:

    double plat,         ! latitude  of the search point
    double plon,         ! longitude of the search point
  */

  double search_radius = gs->search_radius;

  // Initialize distance and address arrays
  gridsearch_knn_init(knn);

655
656
  size_t num_neighbors = knn->size;
  size_t *restrict nbr_add = knn->add;
657
658
  double *restrict nbr_dist = knn->dist;

659
  size_t ndist = num_neighbors;
660
661
662
  // check some more points if distance is the same use the smaller index (nadd)
  if ( ndist > 8 ) ndist += 8;
  else             ndist *= 2; 
663
  if ( ndist > gs->n ) ndist = gs->n;
664

665
  if ( knn->tmpadd  == NULL ) knn->tmpadd  = (size_t*) Malloc(ndist*sizeof(size_t));
666
667
  if ( knn->tmpdist == NULL ) knn->tmpdist = (double*) Malloc(ndist*sizeof(double));

668
  size_t *adds = knn->tmpadd;
669
670
671
672
673
  double *dist = knn->tmpdist;
  
  const double range0 = SQR(search_radius);
  double range = range0;

674
  size_t j = 0;
675
676
677

  if ( num_neighbors == 1 )
    {
678
      size_t nadd = gridsearch_nearest(gs, plon, plat, &range);
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
      if ( nadd != GS_NOT_FOUND )
        {
          //if ( range < range0 )
            {
              dist[j] = sqrt(range);
              adds[j] = nadd;
              j++;
            }
        }
    }
  else
    {
      struct pqueue *gs_result = gridsearch_qnearest(gs, plon, plat, &range, ndist);
      if ( gs_result )
        {
694
          size_t nadd;
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
          struct resItem *p;
          while ( pqremove_min(gs_result, &p) )
            {
              nadd  = p->node->index;
              range = p->dist_sq;
              Free(p); // Free the result node taken from the heap

              if ( range < range0 )
                {
                  dist[j] = sqrt(range);
                  adds[j] = nadd;
                  j++;
                }
            }
          Free(gs_result->d); // free the heap
          Free(gs_result);    // and free the heap information structure
        }
    }

  ndist = j;
715
  size_t max_neighbors = (ndist < num_neighbors) ? ndist : num_neighbors;
716
717
718
719
720
721
722
723
724
725
726
727

  for ( j = 0; j < ndist; ++j )
    knn_store_distance(adds[j], dist[j], max_neighbors, nbr_add, nbr_dist);

  knn_check_distance(max_neighbors, nbr_add, nbr_dist);

  if ( ndist > num_neighbors ) ndist = num_neighbors;

  knn->ndist = ndist;

  return ndist;
} // gridsearch_knn