qpms/lattices2d.c

   1 #include "lattices.h"
   2 #include <assert.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5
   6 typedef struct {
   7   int i, j;
   8 } intcoord2_t;
   9
  10 static inline int sqi(int x) { return x*x; }
  11
  12 static inline double sqf(double x) { return x*x; }
  13
  14 void points2d_rordered_free(points2d_rordered_t *p) {
  15   free(p->rs);
  16   free(p->base);
  17   free(p->r_offsets);
  18   free(p);
  19 }
  20
  21 points2d_rordered_t *points2d_rordered_scale(const points2d_rordered_t *orig, const double f)
  22 {
  23   points2d_rordered_t *p = malloc(sizeof(points2d_rordered_t));
  24   if(0 == orig->nrs) { // orig is empty
  25     p->nrs = 0;
  26     p->rs = NULL;
  27     p->base = NULL;
  28     p->r_offsets = NULL;
  29     return p;
  30   }
  31   p->nrs = orig->nrs;
  32   p->rs = malloc(p->nrs*sizeof(double));
  33   p->r_offsets = malloc((p->nrs+1)*sizeof(ptrdiff_t));
  34
  35   const double af = fabs(f);
  36   for(size_t i = 0; i < p->nrs; ++i) {
  37     p->rs[i] = orig->rs[i] * af;
  38     p->r_offsets[i] = orig->r_offsets[i];
  39   }
  40   p->r_offsets[p->nrs] = orig->r_offsets[p->nrs];
  41   p->base = malloc(sizeof(point2d) * p->r_offsets[p->nrs]);
  42   for(size_t i = 0; i < p->r_offsets[p->nrs]; ++i)
  43     p->base[i] = point2d_fromxy(orig->base[i].x * f, orig->base[i].y * f);
  44   return p;
  45 }
  46
  47 ptrdiff_t points2d_rordered_locate_r(const points2d_rordered_t *p, const double r) {
  48   //if(p->r_rs[0] > r)
  49   //  return -1;
  50   //if(p->r_rs[p->nrs-1] < r)
  51   //  return p->nrs;
  52   ptrdiff_t lo = 0, hi = p->nrs-1, piv;
  53   while(lo < hi) {
  54     piv = (lo + hi + 1) / 2;
  55     if(p->rs[piv] > r) // the result will be less or equal
  56       hi = piv - 1;
  57     else
  58       lo = piv;
  59   }
  60   return lo;
  61 }
  62
  63 points2d_rordered_t points2d_rordered_annulus(const points2d_rordered_t *orig,
  64     double minr, bool inc_minr, double maxr, bool inc_maxr) {
  65   points2d_rordered_t p;
  66   ptrdiff_t imin, imax;
  67   imin = points2d_rordered_locate_r(orig, minr);
  68   imax = points2d_rordered_locate_r(orig, maxr);
  69   // TODO check
  70   if(imax >= orig->nrs) --imax;
  71   if(imax < 0) goto nothing;
  72   // END TODO
  73   if (!inc_minr && (orig->rs[imin] <= minr)) ++imin;
  74   if (!inc_maxr && (orig->rs[imax] >= maxr)) --imax;
  75   if (imax < imin) { // it's empty
  76 nothing:
  77     p.nrs = 0;
  78     p.base = NULL;
  79     p.rs = NULL;
  80     p.r_offsets = NULL;
  81   } else {
  82     p.base = orig->base;
  83     p.nrs = imax - imin + 1;
  84     p.rs = orig->rs + imin;
  85     p.r_offsets = orig->r_offsets + imin;
  86   }
  87   return p;
  88 }
  89
  90
  91 static inline double pr2(const point2d p) {
  92   return sqf(p.x) + sqf(p.y);
  93 }
  94
  95 static inline double prn(const point2d p) {
  96   return sqrt(pr2(p));
  97 }
  98
  99 static int point2d_cmp_by_r2(const void *p1, const void *p2) {
 100   const point2d *z1 = (point2d *) p1, *z2 = (point2d *) p2;
 101   double dif = pr2(*z1) - pr2(*z2);
 102   if(dif > 0) return 1;
 103   else if(dif < 0) return -1;
 104   else return 0;
 105 }
 106
 107 static points2d_rordered_t *points2d_rordered_frompoints_c(point2d *orig_base, const size_t nmemb,
 108     const double rtol, const double atol, bool copybase)
 109 {
 110   // TODO should the rtol and atol relate to |r| or r**2? (Currently: |r|)
 111   assert(rtol >= 0);
 112   assert(atol >= 0);
 113   points2d_rordered_t *p = malloc(sizeof(points2d_rordered_t));
 114   if(nmemb == 0) {
 115     p->nrs = 0;
 116     p->rs = NULL;
 117     p->base = NULL;
 118     p->r_offsets = NULL;
 119     return p;
 120   }
 121
 122   if (copybase) {
 123     p->base = malloc(nmemb * sizeof(point2d));
 124     memcpy(p->base, orig_base, nmemb * sizeof(point2d));
 125   } else
 126     p->base = orig_base;
 127
 128   qsort(p->base, nmemb, sizeof(point2d), point2d_cmp_by_r2);
 129
 130   // first pass: determine the number of "different" r's.
 131   size_t rcount = 0;
 132   double rcur = -INFINITY;
 133   double rcurmax = -INFINITY;
 134   for (size_t i = 0; i < nmemb; ++i)
 135     if ((rcur = prn(p->base[i])) > rcurmax) {
 136       ++rcount;
 137       rcurmax = rcur * (1 + rtol) + atol;
 138     }
 139
 140   p->nrs = rcount;
 141   // TODO check malloc return values
 142   p->rs = malloc(rcount * sizeof(double));
 143   p->r_offsets = malloc((rcount+1) * sizeof(ptrdiff_t));
 144
 145
 146   // second pass: fill teh rs;
 147   size_t ri = 0;
 148   size_t rcurcount = 0;
 149   rcur = prn(p->base[0]);
 150   rcurmax = rcur * (1 + rtol) + atol;
 151   double rcursum = 0;
 152   p->r_offsets[0] = 0;
 153   for (size_t i = 0; i < nmemb; ++i) {
 154     rcur = prn(p->base[i]);
 155     if (rcur > rcurmax) {
 156       p->rs[ri] = rcursum / (double) rcurcount; // average of the accrued r's within tolerance
 157       ++ri;
 158       p->r_offsets[ri] = i; //r_offsets[ri-1] + rcurcount (is the same)
 159       rcurcount = 0;
 160       rcursum = 0;
 161       rcurmax = rcur * (1 + rtol) + atol;
 162     }
 163     rcursum += rcur;
 164     ++rcurcount;
 165   }
 166   p->rs[ri] = rcursum / (double) rcurcount;
 167   p->r_offsets[rcount] = nmemb;
 168
 169   return p;
 170 }
 171
 172 points2d_rordered_t *points2d_rordered_frompoints(const point2d *orig_base, const size_t nmemb,
 173     const double rtol, const double atol)
 174 {
 175   return points2d_rordered_frompoints_c((point2d *)orig_base, nmemb, rtol, atol, true);
 176 }
 177
 178 points2d_rordered_t *points2d_rordered_shift(const points2d_rordered_t *orig, const  point2d shift,
 179     double rtol, double atol)
 180 {
 181   size_t n = (orig->nrs > 0) ?
 182       orig->r_offsets[orig->nrs] - orig->r_offsets[0] : 0;
 183
 184   point2d * shifted = malloc(n * sizeof(point2d));
 185
 186   for(size_t i = 0; i < n; ++i)
 187     shifted[i] = cart2_add(orig->base[i+orig->r_offsets[0]], shift);
 188
 189   return points2d_rordered_frompoints_c(shifted, n,rtol, atol, false);
 190 }
 191
 192 /*
 193  * EQUILATERAL TRIANGULAR LATTICE
 194  */
 195
 196
 197 /*
 198  * N. B. the possible radii (distances from origin) of the lattice points can be described as
 199  *
 200  *     r**2 / a**2 == i**2 + j**2 + i*j ,
 201  *
 202  * where i, j are integer indices describing steps along two basis vectors (which have
 203  * 60 degree angle between them).
 204  *
 205  * The plane can be divided into six sextants, characterized as:
 206  *
 207  * 0) i >= 0 && j >= 0,
 208  *    [a] i > 0,
 209  *    [b] j > 0,
 210  * 1) i <= 0 && {j >= 0} && i + j >= 0,
 211  *    [a] i + j > 0,
 212  *    [b] i < 0,
 213  * 2) {i <= 0} && j >= 0 && i + j <= 0,
 214  *    [a] j > 0,
 215  *    [b] i + j < 0,
 216  * 3) i <= 0 && j <= 0,
 217  *    [a] i < 0,
 218  *    [b] j < 0,
 219  * 4) i >= 0 && {j <= 0} && i + j <= 0,
 220  *    [a] i + j < 0,
 221  *    [b] i > 0,
 222  * 5) {i >= 0} && j <= 0 && i + j >= 0,
 223  *    [a] j < 0,
 224  *    [b] i + j > 0.
 225  *
 226  * The [a], [b] are two variants that uniquely assign the points at the sextant boundaries.
 227  * The {conditions} in braces are actually redundant.
 228  *
 229  * In each sextant, the "minimum steps from the origin" value is calculated as:
 230  * 0) i + j,
 231  * 1) j
 232  * 2) -i
 233  * 3) -i - j,
 234  * 4) -j,
 235  * 5) i.
 236  *
 237  * The "spider web" generation for s steps from the origin (s-th layer) goes as following (variant [a]):
 238  * 0) for (i = s, j = 0; i > 0; --i, ++j)
 239  * 1) for (i = 0, j = s; i + j > 0; --i)
 240  * 2) for (i = -s, j = s; j > 0; --j)
 241  * 3) for (i = -s, j = 0; i < 0; ++i, --j)
 242  * 4) for (i = 0, j = -s; i + j < 0; ++i)
 243  * 5) for (i = s, j = -s; j < 0; ++j)
 244  *
 245  *
 246  * Length of the s-th layer is 6*s for s >= 1. Size (number of lattice points) of the whole s-layer "spider web"
 247  * is therefore 3*s*(s+1), excluding origin.
 248  * The real area inside the web is (a*s)**2 * 3 * sqrt(3) / 2.
 249  * Area of a unit cell is a**2 * sqrt(3)/2.
 250  * Inside the web, but excluding the circumscribed circle, there is no more
 251  * than 3/4.*s*(s+1) + 6*s lattice cells (FIXME pretty stupid but safe estimate).
 252  *
 253  * s-th layer circumscribes a circle of radius a * s * sqrt(3)/2.
 254  *
 255  */
 256
 257
 258 typedef struct triangular_lattice_gen_privstuff_t {
 259   intcoord2_t *pointlist_base; // allocated memory for the point "buffer"
 260   size_t pointlist_capacity;
 261   // beginning and end of the point "buffer"
 262   // not 100% sure what type should I use here
 263   // (these are both relative to pointlist_base, due to possible realloc's)
 264   ptrdiff_t pointlist_beg, pointlist_n; // end of queue is at [(pointlist_beg+pointlist_n)%pointlist_capacity]
 265   int maxs; // the highest layer of the spider web generated (-1 by init, 0 is only origin (if applicable))
 266   // capacities of the arrays in ps
 267   size_t ps_rs_capacity;
 268   size_t ps_points_capacity; // this is the "base" array
 269   // TODO anything else?
 270 } triangular_lattice_gen_privstuff_t;
 271
 272 static inline int trilat_r2_ij(const int i, const int j) {
 273   return sqi(i) + sqi(j) + i*j;
 274 }
 275
 276 static inline int trilat_r2_coord(const intcoord2_t c) {
 277   return trilat_r2_ij(c.i, c.j);
 278 }
 279
 280 // version with offset (n.b. this is includes a factor of 3)
 281 static inline int trilat_3r2_ijs(const int i, const int j, const int s) {
 282   return 3*(sqi(i) + sqi(j) + i*j + j*s) + sqi(s);
 283 }
 284
 285 static inline int trilat_3r2_coord_s(const intcoord2_t c, const int s) {
 286   return trilat_3r2_ijs(c.i, c.j, s);
 287 }
 288
 289
 290
 291 // Classify points into sextants (variant [a] above)
 292 static int trilat_sextant_ij_a(const int i, const int j) {
 293   const int w = i + j;
 294   if (i >  0 && j >= 0) return  0;
 295   if (i <= 0 && w >  0) return  1;
 296   if (w <= 0 && j >  0) return  2;
 297   if (i <  0 && j <= 0) return  3;
 298   if (i >= 0 && w <  0) return  4;
 299   if (w >= 0 && j <  0) return  5;
 300   if (i == 0 && j == 0) return -1; // origin
 301   assert(0); // other options should be impossible
 302 }
 303
 304 static inline size_t tlgp_pl_end(const triangular_lattice_gen_privstuff_t *p) {
 305   return (p->pointlist_beg + p->pointlist_n) % p->pointlist_capacity;
 306 }
 307
 308 #if 0
 309 static inline void tlgpl_end_inc(triangular_lattice_gen_privstuff_t *p) {
 310   p->p_pointlist_n += 1;
 311 }
 312 #endif
 313
 314 // Puts a point to the end of the point queue
 315 static inline void trilatgen_pointlist_append_ij(triangular_lattice_gen_t *g, int i, int j) {
 316   intcoord2_t thepoint = {i, j};
 317   triangular_lattice_gen_privstuff_t *p = g->priv;
 318   assert(p->pointlist_n < p->pointlist_capacity);
 319
 320   // the actual addition
 321   p->pointlist_base[tlgp_pl_end(p)] = thepoint;
 322   p->pointlist_n += 1;
 323 }
 324
 325 // Arange the pointlist queue into a continuous chunk of memory, so that we can qsort() it
 326 static void trilatgen_pointlist_linearise(triangular_lattice_gen_t *g) {
 327   triangular_lattice_gen_privstuff_t *p = g->priv;
 328   assert(p->pointlist_n <= p->pointlist_capacity);
 329   if (p->pointlist_beg + p->pointlist_n <= p->pointlist_capacity)
 330     return;  // already linear, do nothing
 331   else if (p->pointlist_n == p->pointlist_capacity) { // full, therefore linear
 332     p->pointlist_beg = 0;
 333     return;
 334   } else { // non-linear; move "to the right"
 335     while (p->pointlist_beg < p->pointlist_capacity) {
 336       p->pointlist_base[tlgp_pl_end(p)] = p->pointlist_base[p->pointlist_beg];
 337       ++(p->pointlist_beg);
 338     }
 339     p->pointlist_beg = 0;
 340     return;
 341   }
 342 }
 343
 344 static inline intcoord2_t trilatgen_pointlist_first(const triangular_lattice_gen_t *g) {
 345   return g->priv->pointlist_base[g->priv->pointlist_beg];
 346 }
 347
 348 static inline void trilatgen_pointlist_deletefirst(triangular_lattice_gen_t *g) {
 349   triangular_lattice_gen_privstuff_t *p = g->priv;
 350   assert(p->pointlist_n > 0);
 351   ++p->pointlist_beg;
 352   if(p->pointlist_beg == p->pointlist_capacity)
 353     p->pointlist_beg = 0;
 354   --(p->pointlist_n);
 355 }
 356
 357 // TODO abort() and void or errorchecks and int?
 358 static int trilatgen_pointlist_extend_capacity(triangular_lattice_gen_t *g, size_t newcapacity) {
 359   triangular_lattice_gen_privstuff_t *p = g->priv;
 360   if (newcapacity <= p->pointlist_capacity)
 361     return 0;
 362
 363   trilatgen_pointlist_linearise(g);
 364
 365   QPMS_CRASHING_REALLOC(p->pointlist_base, newcapacity * sizeof(intcoord2_t));
 366   p->pointlist_capacity = newcapacity;
 367   return 0;
 368 }
 369
 370 // lower estimate for the number of lattice points inside the circumscribed hexagon, but outside the circle
 371 static inline size_t tlg_circumscribe_reserve(int maxs) {
 372   if (maxs <= 0)
 373     return 0;
 374   return 3*maxs*(maxs+1)/4 + 6*maxs;
 375 }
 376
 377 static inline size_t tlg_websize(int maxs) {
 378   if (maxs <= 0)
 379     return 0;
 380   else
 381     return 3*maxs*(maxs+1); // does not include origin point!
 382 }
 383
 384 static int trilatgen_ensure_pointlist_capacity(triangular_lattice_gen_t *g, int newmaxs) {
 385   return trilatgen_pointlist_extend_capacity(g,
 386         tlg_circumscribe_reserve(g->priv->maxs) // Space for those which are already in
 387       + tlg_websize(newmaxs) - tlg_websize(g->priv->maxs) // space for the new web layers
 388       + 1 // reserve for the origin
 389       );
 390 }
 391
 392 static int trilatgen_ensure_ps_rs_capacity(triangular_lattice_gen_t *g, int maxs) {
 393   if (maxs < 0)
 394     return 0;
 395
 396   size_t needed_capacity = 1 // reserve for origin
 397     + maxs*(maxs+1)/2; // stupid but safe estimate: number of points in a sextant of maxs-layered spider web
 398
 399   if (needed_capacity <= g->priv->ps_rs_capacity)
 400     return 0; // probably does not happen, but fuck it
 401
 402   QPMS_CRASHING_REALLOC(g->ps.rs, needed_capacity * sizeof(double));
 403   QPMS_CRASHING_REALLOC(g->ps.r_offsets, (needed_capacity + 1) * sizeof(ptrdiff_t));
 404
 405   g->priv->ps_rs_capacity = needed_capacity;
 406   return 0;
 407 }
 408
 409 static int trilatgen_ensure_ps_points_capacity(triangular_lattice_gen_t *g, int maxs) {
 410   if (maxs < 0)
 411     return 0;
 412   size_t needed_capacity = 1 /*res. for origin */ + tlg_websize(maxs) /* stupid but safe */;
 413   if(needed_capacity <= g->priv->ps_points_capacity)
 414     return 0;
 415
 416   QPMS_CRASHING_REALLOC(g->ps.base, needed_capacity * sizeof(point2d));
 417   g->priv->ps_points_capacity = needed_capacity;
 418   return 0;
 419 }
 420
 421 static int trilat_cmp_intcoord2_by_r2(const void *p1, const void *p2) {
 422   return trilat_r2_coord(*(const intcoord2_t *)p1) - trilat_r2_coord(*(const intcoord2_t *)p2);
 423 }
 424
 425
 426 static int trilat_cmp_intcoord2_by_3r2_plus1s(const void *p1, const void *p2) {
 427   return trilat_3r2_coord_s(*(const intcoord2_t *)p1, +1) - trilat_3r2_coord_s(*(const intcoord2_t *)p2, +1);
 428 }
 429
 430 static int trilat_cmp_intcoord2_by_3r2_minus1s(const void *p1, const void *p2) {
 431   return trilat_3r2_coord_s(*(const intcoord2_t *)p1, -1) - trilat_3r2_coord_s(*(const intcoord2_t *)p2, -1);
 432 }
 433
 434 static int trilat_cmp_intcoord2_by_3r2(const void *p1, const void *p2, void *sarg) {
 435   return trilat_3r2_coord_s(*(const intcoord2_t *)p1, *(int *)sarg) - trilat_3r2_coord_s(*(const intcoord2_t *)p2, *(int *)sarg);
 436 }
 437
 438 static void trilatgen_sort_pointlist(triangular_lattice_gen_t *g) {
 439   trilatgen_pointlist_linearise(g);
 440   triangular_lattice_gen_privstuff_t *p = g->priv;
 441   int (*compar)(const void *, const void *);
 442   switch (g->hexshift) {
 443     case 0:
 444       compar = trilat_cmp_intcoord2_by_r2;
 445       break;
 446     case -1:
 447       compar = trilat_cmp_intcoord2_by_3r2_minus1s;
 448       break;
 449     case 1:
 450       compar = trilat_cmp_intcoord2_by_3r2_plus1s;
 451       break;
 452     default:
 453       QPMS_WTF;
 454   }
 455   qsort(p->pointlist_base + p->pointlist_beg, p->pointlist_n, sizeof(intcoord2_t), compar);
 456 }
 457
 458 triangular_lattice_gen_t * triangular_lattice_gen_init(double a, TriangularLatticeOrientation ori, bool include_origin,
 459     int hexshift)
 460 {
 461   triangular_lattice_gen_t *g = malloc(sizeof(triangular_lattice_gen_t));
 462   g->a = a;
 463   g->hexshift = ((hexshift % 3)+3)%3; // reduce to the set {-1, 0, 1}
 464   if (2 == g->hexshift)
 465     g->hexshift = -1;
 466   g->orientation = ori;
 467   g->includes_origin = include_origin;
 468   g->ps.nrs = 0;
 469   g->ps.rs = NULL;
 470   g->ps.base = NULL;
 471   g->ps.r_offsets = NULL;
 472   g->priv = malloc(sizeof(triangular_lattice_gen_privstuff_t));
 473   g->priv->maxs = -1;
 474   g->priv->pointlist_capacity = 0;
 475   g->priv->pointlist_base = NULL;
 476   g->priv->pointlist_beg = 0;
 477   g->priv->pointlist_n = 0;
 478   g->priv->ps_rs_capacity = 0;
 479   g->priv->ps_points_capacity = 0;
 480   return g;
 481 }
 482
 483 void triangular_lattice_gen_free(triangular_lattice_gen_t *g) {
 484   free(g->ps.rs);
 485   free(g->ps.base);
 486   free(g->ps.r_offsets);
 487   free(g->priv->pointlist_base);
 488   free(g->priv);
 489   free(g);
 490 }
 491
 492 const points2d_rordered_t * triangular_lattice_gen_getpoints(const triangular_lattice_gen_t *g) {
 493   return &(g->ps);
 494 }
 495
 496 int triangular_lattice_gen_extend_to_r(triangular_lattice_gen_t * g, const double maxr) {
 497   return triangular_lattice_gen_extend_to_steps(g, maxr/g->a);
 498 }
 499
 500 int triangular_lattice_gen_extend_to_steps(triangular_lattice_gen_t * g, int maxsteps)
 501 {
 502   if (maxsteps <= g->priv->maxs)  // nothing needed
 503     return 0;
 504   // TODO FIXME: check for maximum possible maxsteps (not sure what it is)
 505   int err;
 506   err = trilatgen_ensure_pointlist_capacity(g, maxsteps
 507       + abs(g->hexshift) /*FIXME this is quite brainless addition, probably not even needed.*/);
 508   if(err) return err;
 509   err = trilatgen_ensure_ps_rs_capacity(g, maxsteps
 510       + abs(g->hexshift) /*FIXME this is quite brainless addition, probably not even needed.*/);
 511   if(err) return err;
 512   err = trilatgen_ensure_ps_points_capacity(g, maxsteps
 513       + abs(g->hexshift) /*FIXME this is quite brainless addition, probably not even needed.*/);
 514   if(err) return err;
 515
 516   if(g->includes_origin && g->priv->maxs < 0) // Add origin if not there yet
 517     trilatgen_pointlist_append_ij(g, 0, 0);
 518
 519   for (int s = g->priv->maxs + 1; s <= maxsteps; ++s) {
 520     int i, j;
 521     // now go along the spider web layer as indicated in the lenghthy comment above
 522     for (i = s, j = 0; i > 0; --i, ++j) trilatgen_pointlist_append_ij(g,i,j);
 523     for (i = 0, j = s; i + j > 0; --i) trilatgen_pointlist_append_ij(g,i,j);
 524     for (i = -s, j = s; j > 0; --j) trilatgen_pointlist_append_ij(g,i,j);
 525     for (i = -s, j = 0; i < 0; ++i, --j) trilatgen_pointlist_append_ij(g,i,j);
 526     for (i = 0, j = -s; i + j < 0; ++i) trilatgen_pointlist_append_ij(g,i,j);
 527     for (i = s, j = -s; j < 0; ++j) trilatgen_pointlist_append_ij(g,i,j);
 528   }
 529
 530   trilatgen_sort_pointlist(g);
 531
 532   // initialise first r_offset if needed
 533   if (0 == g->ps.nrs)
 534     g->ps.r_offsets[0] = 0;
 535
 536   //ted je potřeba vytahat potřebný počet bodů z fronty a naflákat je do ps.
 537   // FIXME pohlídat si kapacitu datových typů
 538   //int maxr2i = sqi(maxsteps) * 3 / 4;
 539   int maxr2i3 = sqi(maxsteps) * 9 / 4 + sqi(g->hexshift) - abs(3*maxsteps*g->hexshift);
 540   while (g->priv->pointlist_n > 0) { // This condition should probably be always true anyways.
 541     intcoord2_t coord = trilatgen_pointlist_first(g);
 542     //int r2i_cur = trilat_r2_coord(coord);
 543     //if(r2i_cur > maxr2i)
 544     int r2i3_cur = trilat_3r2_coord_s(coord, g->hexshift);
 545     if(r2i3_cur > maxr2i3)
 546       break;
 547     g->ps.rs[g->ps.nrs] = sqrt(/*r2i_cur*/ r2i3_cur/3.) * g->a;
 548     g->ps.r_offsets[g->ps.nrs+1] = g->ps.r_offsets[g->ps.nrs]; // the difference is the number of points on the circle
 549     while(1) {
 550       coord = trilatgen_pointlist_first(g);
 551       //if(r2i_cur != trilat_r2_coord(coord))
 552       if (r2i3_cur != trilat_3r2_coord_s(coord, g->hexshift))
 553         break;
 554       else {
 555         trilatgen_pointlist_deletefirst(g);
 556         point2d thepoint;
 557         switch (g->orientation) {
 558           case TRIANGULAR_HORIZONTAL:
 559             thepoint = point2d_fromxy((coord.i+.5*coord.j)*g->a, (M_SQRT3_2*coord.j + g->hexshift*M_1_SQRT3)*g->a);
 560             break;
 561           case TRIANGULAR_VERTICAL:
 562             thepoint = point2d_fromxy(-(M_SQRT3_2*coord.j + g->hexshift*M_1_SQRT3)*g->a, (coord.i+.5*coord.j)*g->a);
 563             break;
 564           default:
 565             QPMS_INVALID_ENUM(g->orientation);
 566         }
 567         g->ps.base[g->ps.r_offsets[g->ps.nrs+1]] = thepoint;
 568         ++(g->ps.r_offsets[g->ps.nrs+1]);
 569       }
 570     }
 571     ++(g->ps.nrs);
 572   }
 573   g->priv->maxs = maxsteps;
 574   return 0;
 575 }
 576
 577 honeycomb_lattice_gen_t *honeycomb_lattice_gen_init_h(double h, TriangularLatticeOrientation ori) {
 578   double a = M_SQRT3 * h;
 579   honeycomb_lattice_gen_t *g = honeycomb_lattice_gen_init_a(a, ori);
 580   g->h = h; // maybe it's not necessary as sqrt is "exact"
 581   return g;
 582 }
 583
 584 honeycomb_lattice_gen_t *honeycomb_lattice_gen_init_a(double a, TriangularLatticeOrientation ori) {
 585   honeycomb_lattice_gen_t *g = calloc(1, sizeof(honeycomb_lattice_gen_t)); // this already inits g->ps to zeros
 586   g->a = a;
 587   g->h = a * M_1_SQRT3;
 588   g->tg = triangular_lattice_gen_init(a, ori, true, 1);
 589   return g;
 590 }
 591
 592 void honeycomb_lattice_gen_free(honeycomb_lattice_gen_t *g) {
 593   free(g->ps.rs);
 594   free(g->ps.base);
 595   free(g->ps.r_offsets);
 596   triangular_lattice_gen_free(g->tg);
 597   free(g);
 598 }
 599
 600 int honeycomb_lattice_gen_extend_to_r(honeycomb_lattice_gen_t *g, double maxr) {
 601   return honeycomb_lattice_gen_extend_to_steps(g, maxr/g->a); /*CHECKME whether g->a is the correct denom.*/
 602 }
 603
 604 int honeycomb_lattice_gen_extend_to_steps(honeycomb_lattice_gen_t *g, const int maxsteps) {
 605   if (maxsteps <= g->tg->priv->maxs)  // nothing needed
 606     return 0;
 607   triangular_lattice_gen_extend_to_steps(g->tg, maxsteps);
 608
 609   QPMS_CRASHING_REALLOC(g->ps.rs, g->tg->ps.nrs * sizeof(double));
 610   QPMS_CRASHING_REALLOC(g->ps.r_offsets, (g->tg->ps.nrs+1) * sizeof(ptrdiff_t));
 611   QPMS_CRASHING_REALLOC(g->ps.base, 2 * (g->tg->ps.r_offsets[g->tg->ps.nrs]) * sizeof(point2d));
 612
 613   // Now copy (new) contents of g->tg->ps into g->ps, but with inverse copy of each point
 614   for (size_t ri = g->ps.nrs; ri <= g->tg->ps.nrs; ++ri)
 615     g->ps.r_offsets[ri] = g->tg->ps.r_offsets[ri] * 2;
 616   for (ptrdiff_t i_orig = g->tg->ps.r_offsets[g->ps.nrs]; i_orig < g->tg->ps.r_offsets[g->tg->ps.nrs]; ++i_orig) {
 617     point2d p = g->tg->ps.base[i_orig];
 618     g->ps.base[2*i_orig] = p;
 619     p.x *= -1; p.y *= -1;
 620     g->ps.base[2*i_orig + 1] = p;
 621   }
 622   g->ps.nrs = g->tg->ps.nrs;
 623   return 0;
 624 }
 625
 626
 627
 628 // THE NICE PART
 629
 630
 631 /*
 632  * Lagrange-Gauss reduction of a 2D basis.
 633  * The output shall satisfy |out1| <= |out2| <= |out2 - out1|
 634  */
 635 void l2d_reduceBasis(cart2_t b1, cart2_t b2, cart2_t *out1, cart2_t *out2){
 636   double B1 = cart2_dot(b1, b1);
 637   double mu = cart2_dot(b1, b2) / B1;
 638   b2 = cart2_substract(b2, cart2_scale(round(mu), b1));
 639   double B2 = cart2_dot(b2, b2);
 640   while(B2 < B1) {
 641     cart2_t b2t = b1;
 642     b1 = b2;
 643     b2 = b2t;
 644     B1 = B2;
 645     mu = cart2_dot(b1, b2) / B1;
 646     b2 = cart2_substract(b2, cart2_scale(round(mu), b1));
 647     B2 = cart2_dot(b2, b2);
 648   }
 649   *out1 = b1;
 650   *out2 = b2;
 651 }
 652
 653 void l3d_reduceBasis(const cart3_t in[3], cart3_t out[3]) {
 654   memcpy(out, in, 3*sizeof(cart3_t));
 655   QPMS_ENSURE_SUCCESS(qpms_reduce_lattice_basis((double *)out, 3, 3, 1.));
 656 }
 657
 658 /*
 659  * This gives the "ordered shortest triple" of base vectors (each pair from the triple
 660  * is a base) and there may not be obtuse angle between o1, o2 and between o2, o3
 661  */
 662 void l2d_shortestBase3(cart2_t b1, cart2_t b2, cart2_t *o1, cart2_t *o2, cart2_t *o3){
 663   l2d_reduceBasis(b1, b2, &b1, &b2);
 664   *o1 = b1;
 665   if (l2d_is_obtuse_r(b1, b2, 0)) {
 666     *o3 = b2;
 667     *o2 = cart2_add(b2, b1);
 668   } else {
 669     *o2 = b2;
 670     *o3 = cart2_substract(b2, b1);
 671   }
 672 }
 673
 674 // Determines whether angle between inputs is obtuse
 675 bool l2d_is_obtuse_r(cart2_t b1, cart2_t b2, double rtol) {
 676   const double B1 = cart2_normsq(b1);
 677   const double B2 = cart2_normsq(b2);
 678   const cart2_t b3 = cart2_substract(b2, b1);
 679   const double B3 = cart2_normsq(b3);
 680   const double eps = rtol * (B1 + B2); // TODO check what kind of quantity this should be. Maybe rtol should relate to lengths, not lengths**2
 681   return (B3 - B2 - B1 > eps);
 682 }
 683
 684
 685 /*
 686  * TODO doc
 687  * return value is 4 or 6.
 688  */
 689 int l2d_shortestBase46(const cart2_t i1, const cart2_t i2,  cart2_t *o1, cart2_t *o2, cart2_t *o3, cart2_t *o4, cart2_t *o5, cart2_t *o6, double rtol){
 690   cart2_t b1, b2, b3;
 691   l2d_reduceBasis(i1, i2, &b1, &b2);
 692   const double b1s = cart2_normsq(b1);
 693   const double b2s = cart2_normsq(b2);
 694   b3 = cart2_substract(b2, b1);
 695   const double b3s = cart2_normsq(b3);
 696   const double eps = rtol * (b1s + b2s); // TODO check the same as in l2d_is_obtuse_r
 697   if(fabs(b3s-b2s-b1s) < eps) {
 698     *o1 = b1; *o2 = b2; *o3 = cart2_scale(-1, b1); *o4 = cart2_scale(-1, b2);
 699     return 4;
 700   }
 701   else {
 702     if (b3s-b2s-b1s > eps) { //obtuse
 703       b3 = b2;
 704       b2 = cart2_add(b2, b1);
 705     }
 706     *o1 = b1; *o2 = b2; *o3 = b3;
 707     *o4 = cart2_scale(-1, b1);
 708     *o5 = cart2_scale(-1, b2);
 709     *o6 = cart2_scale(-1, b3);
 710     return 6;
 711   }
 712 }
 713
 714 /*
 715  * Given two basis vectors, returns 2D Bravais lattice type.
 716  */
 717 LatticeType2 l2d_classifyLattice(cart2_t b1, cart2_t b2, double rtol)
 718 {
 719   l2d_reduceBasis(b1, b2, &b1, &b2);
 720   cart2_t b3 = cart2_substract(b2, b1);
 721   double b1s = cart2_normsq(b1), b2s = cart2_normsq(b2), b3s = cart2_normsq(b3);
 722   double eps = rtol * (b2s + b1s); //FIXME what should eps be?
 723   // avoid obtuse angle between b1 and b2. TODO this should be yet tested
 724   // TODO use is_obtuse here?
 725   if (b3s - b2s - b1s > eps) {
 726     b3 = b2;
 727     b2 = cart2_add(b2, b1);
 728     // N.B. now the assumption |b3| >= |b2| is no longer valid
 729     // b3 = cart2_substract(b2, b1)
 730     b2s = cart2_normsq(b2);
 731     b3s = cart2_normsq(b3);
 732   }
 733   if (fabs(b2s-b1s) < eps || fabs(b2s - b3s) < eps) { // isoscele
 734     if (fabs(b3s-b1s) <  eps)
 735       return EQUILATERAL_TRIANGULAR;
 736     else if (fabs(b3s - 2*b1s))
 737       return SQUARE;
 738     else
 739       return RHOMBIC;
 740   } else if (fabs(b3s-b2s-b1s) < eps)
 741     return RECTANGULAR;
 742   else
 743     return OBLIQUE;
 744 }
 745
 746 LatticeFlags l2d_detectRightAngles(cart2_t b1, cart2_t b2, double rtol)
 747 {
 748   l2d_reduceBasis(b1, b2, &b1, &b2);
 749   cart2_t ht = cart2_substract(b2, b1);
 750   double b1s = cart2_normsq(b1), b2s = cart2_normsq(b2), hts = cart2_normsq(ht);
 751   double eps = rtol * (b2s + b1s); //FIXME what should eps be?
 752   if (hts - b2s - b1s <= eps)
 753     return ORTHOGONAL_01;
 754   else
 755     return NOT_ORTHOGONAL;
 756 }
 757
 758 LatticeFlags l3d_detectRightAngles(const cart3_t basis_nr[3], double rtol)
 759 {
 760   cart3_t b[3];
 761   l3d_reduceBasis(basis_nr, b);
 762   LatticeFlags res = NOT_ORTHOGONAL;
 763   for (int i = 0; i < 3; ++i) {
 764     cart3_t ba = b[i], bb = b[(i+1) % 3];
 765     cart3_t ht = cart3_substract(ba, bb);
 766     double bas = cart3_normsq(ba), bbs = cart3_normsq(ba), hts = cart3_normsq(ht);
 767     double eps = rtol * (bas + bbs);
 768     if (hts - bbs - bas <= eps)
 769       res |= ((LatticeFlags[]){ORTHOGONAL_01, ORTHOGONAL_12, ORTHOGONAL_02})[i];
 770   }
 771   return res;
 772 }
 773
 774 # if 0
 775 // variant
 776 int l2d_shortestBase46_arr(cart2_t i1, cart2_t i2,  cart2_t *oarr, double rtol);
 777
 778 // Determines whether angle between inputs is obtuse
 779 bool l2d_is_obtuse_r(cart2_t i1, cart2_t i2, double rtol);
 780
 781
 782 // Other functions in lattices2d.py: TODO?
 783 // range2D()
 784 // generateLattice()
 785 // generateLatticeDisk()
 786 // cutWS()
 787 // filledWS()
 788 // change_basis()
 789
 790 /*
 791  * Given basis vectors, returns the corners of the Wigner-Seits unit cell (W1, W2, -W1, W2)
 792  * for rectangular and square lattice or (w1, w2, w3, -w1, -w2, -w3) otherwise.
 793  */
 794 int l2d_cellCornersWS(cart2_t i1, cart2_t i2,  cart2_t *o1, cart2_t *o2, cart2_t *o3, cart2_t *o4, cart2_t *o5, cart2_t *o6, double rtol);
 795 // variant
 796 int l2d_cellCornersWS_arr(cart2_t i1, cart2_t i2,  cart2_t *oarr, double rtol);
 797
 798 #endif
 799
 800 // Reciprocal bases; returns 0 on success, TODO non-zero if b1 and b2 are parallel
 801 int l2d_reciprocalBasis1(cart2_t b1, cart2_t b2, cart2_t *rb1, cart2_t *rb2) {
 802   l2d_reduceBasis(b1, b2, &b1, &b2);
 803   const double det = b1.x * b2.y - b1.y * b2.x;
 804   if (!det) {
 805     rb1->x = rb1->y = rb2->x = rb2->y = NAN;
 806     return QPMS_ERROR; // TODO more specific error code
 807   } else {
 808     rb1->x =  b2.y / det;
 809     rb1->y = -b2.x / det;
 810     rb2->x = -b1.y / det;
 811     rb2->y =  b1.x / det;
 812     return QPMS_SUCCESS;
 813   }
 814 }
 815
 816 int l2d_reciprocalBasis2pi(cart2_t b1, cart2_t b2, cart2_t *rb1, cart2_t *rb2) {
 817   int retval = l2d_reciprocalBasis1(b1, b2, rb1, rb2);
 818   if (retval == QPMS_SUCCESS) {
 819     *rb1 = cart2_scale(2 * M_PI, *rb1);
 820     *rb2 = cart2_scale(2 * M_PI, *rb2);
 821   }
 822   return retval;
 823 };
 824
 825 // 3D reciprocal bases; returns (direct) unit cell volume. Assumes direct lattice basis already reduced.
 826 double l3d_reciprocalBasis1(const cart3_t db[3], cart3_t rb[3]) {
 827   double vol = cart3_tripleprod(db[0], db[1], db[2]);
 828   for(int i = 0; i < 3; ++i)
 829     rb[i] = cart3_divscale(cart3_vectorprod(db[(i+1) % 3], db[(i+2) % 3]), vol);
 830   return vol;
 831 }
 832
 833 double l3d_reciprocalBasis2pi(const cart3_t db[3], cart3_t rb[3]) {
 834   double vol = l3d_reciprocalBasis1(db, rb);
 835   for(int i = 1; i < 3; ++i)
 836     rb[i] = cart3_scale(2 * M_PI, rb[i]);
 837   return vol;
 838 }
 839
 840 // returns the radius of inscribed circle of a hexagon (or rectangle/square if applicable) created by the shortest base triple
 841 double l2d_hexWebInCircleRadius(cart2_t i1, cart2_t i2) {
 842   cart2_t b1, b2, b3;
 843   l2d_shortestBase3(i1, i2, &b1, &b2, &b3);
 844   const double r1 = cart2norm(b1), r2 = cart2norm(b2), r3 = cart2norm(b3);
 845   const double p = (r1+r2+r3)*0.5;
 846   return 2*sqrt(p*(p-r1)*(p-r2)*(p-r3))/r3; // CHECK is r3 guaranteed to be longest?
 847 }
 848
 849
 850 double l2d_unitcell_area(cart2_t b1, cart2_t b2) {
 851   l2d_reduceBasis(b1, b2, &b1, &b2);
 852   const double det = b1.x * b2.y - b1.y * b2.x;
 853   return fabs(det);
 854 }
 855
 856
 857 #if 0
 858 double qpms_emptylattice2_mode_nth(
 859                 cart2_t b1_rec,
 860                 cart2_t b2_rec,
 861                 double rtol,
 862                 cart2_t k,
 863                 double c,
 864                 size_t N
 865                 );
 866
 867 void qpms_emptylattice2_modes_maxindex(
 868                 double target_freqs[],
 869                 cart2_t b1_rec,
 870                 cart2_t b2_rec,
 871                 double rtol,
 872                 cart2_t k,
 873                 double c,
 874                 size_t maxindex
 875                 );
 876 #endif
 877
 878 static int dblcmp(const void *p1, const void *p2) {
 879   const double *x1 = (double *) p1, *x2 = (double *) p2;
 880   double dif = *x1 - *x2;
 881   if(dif > 0) return 1;
 882   else if (dif < 0) return -1;
 883   else return 0;
 884 }
 885
 886 size_t qpms_emptylattice2_modes_maxfreq(double **target_freqs,
 887   cart2_t b1, cart2_t b2, double rtol, cart2_t k,
 888   double c, double maxfreq)
 889 {
 890   QPMS_UNTESTED;
 891   l2d_reduceBasis(b1, b2, &b1, &b2);
 892   double maxk_safe = cart2norm(k) + maxfreq / c + cart2norm(b1) + cart2norm(b2); // This is an overkill
 893   size_t capacity = PGen_xyWeb_sizecap(b1, b2, rtol, k, 0, true, maxk_safe, true);
 894   cart2_t *Kpoints;
 895   QPMS_CRASHING_MALLOC(Kpoints, sizeof(cart2_t) * capacity);
 896   PGen Kgen = PGen_xyWeb_new(b1, b2, rtol, k, 0, true, maxk_safe, true);
 897
 898   size_t generated = 0;
 899   PGenReturnDataBulk rd;
 900   while((rd = PGen_fetch_cart2(&Kgen, capacity - generated, Kpoints + generated)).flags & PGEN_NOTDONE) {
 901     generated += rd.generated;
 902     if (capacity <= generated) {
 903       PGen_destroy(&Kgen);
 904       break;
 905     }
 906   }
 907
 908   double *thefreqs;
 909   QPMS_CRASHING_MALLOC(thefreqs, generated * sizeof(double));
 910   for(size_t i = 0; i < generated; ++i) thefreqs[i] = cart2norm(Kpoints[i]) * c;
 911
 912   free(Kpoints);
 913
 914   qsort(thefreqs, generated, sizeof(double), dblcmp);
 915   size_t count;
 916   bool hitmax = false;
 917   for(count = 0; count < generated; ++count)
 918     if(thefreqs[count] > maxfreq) {
 919       if(hitmax)
 920         break;
 921       else
 922         hitmax = true;
 923     }
 924
 925   *target_freqs = thefreqs;
 926   return count;
 927 }
 928
 929 void qpms_emptylattice2_modes_nearest(double target[2],
 930     cart2_t b1, cart2_t b2, double rtol,
 931     cart2_t k, double c, double omega)
 932 {
 933   QPMS_UNTESTED;
 934   double *freqlist;
 935   size_t n = qpms_emptylattice2_modes_maxfreq(&freqlist,
 936       b1, b2, rtol, k, c, omega);
 937   target[0] = (n > 1) ? freqlist[n-2] : NAN;
 938   target[1] = freqlist[n-1];
 939   free(freqlist);
 940 }
 941