distance.h

Go to the documentation of this file.

/** @file distance.h
 */
/**
# Computation of a distance field from a discretised curve or surface
 
The goal is to compute the (signed) minimal distance from a set of
oriented segments (2d) or triangles (3D) to any point on the
grid. This signed distance function is also dynamically updated when
the mesh is refined or coarsened. 
 
The scheme is robust and will give results even for inconsistent
surface representations (i.e. surfaces with holes, manifold egdes,
incompatible orientations etc...). Faces do not need to be properly
connected i.e. the scheme works also for "triangle soups".
 
We need a few utility function such as computation of the minimal
distance between a point and a segment or a point and a triangle.
*/
 
#include <stdint.h>
#include "PointTriangle.h"
 
/**
The 3D triangulated surfaces are defined using the (binary) [STL
format](https://en.wikipedia.org/wiki/STL_%28file_format%29) which can
be exported from most CAD modelling programs. The *input_stl()*
function reads such a file a returns an array of triplets of vertex
coordinates defining the triangles. */
 
trace
coord * input_stl (FILE * fp)
{
  Array * a = array_new();
  char tag[6];
 
  if (fgets (tag, 6, fp) != tag) {
    fprintf (stderr, "input_stl(): error reading tag\n");
    exit(1);
  }
  rewind (fp);
  if (!strcmp (tag, "solid")) { /* ASCII file */
    fprintf (stderr, "input_stl(): ASCII STL not implemented yet "
       "(use binary instead)\n");
    exit(1);
  }
  else { /* binary file */
    uint32_t nf;
    char header[80];
    unsigned i;
 
    if (fread (header, sizeof (char), 80, fp) != 80) {
      fprintf (stderr, "Input file is not a valid STL file\n"
         "stdin: incomplete header\n");
      exit (1);
    }
    if (fread (&nf, sizeof (uint32_t), 1, fp) != 1) {
      fprintf (stderr, "Input file is not a valid STL file\n"
         "stdin: missing number of facets\n");
      exit (1);
    }
    i = nf;
    while (i > 0) {
      float x = 0, y = 0, z = 0;
      /* dim: (x == Delta, y == Delta, z == Delta */);
      unsigned j;
      uint16_t attbytecount;
 
      if (fread (&x, sizeof (float), 1, fp) != 1) {
  fprintf (stderr, "Input file is not a valid STL file\n"
         "stdin: missing normal x-coordinate\n");
  exit (1);
      }
      if (fread (&y, sizeof (float), 1, fp) != 1) {
  fprintf (stderr, "Input file is not a valid STL file\n"
         "stdin: missing normal y-coordinate\n");
  exit (1);
      }
      if (fread (&z, sizeof (float), 1, fp) != 1) {
  fprintf (stderr, "Input file is not a valid STL file\n"
         "stdin: missing normal z-coordinate\n");
  exit (1);
      }
 
      for (j = 0; j < 3; j++) {
  if (fread (&x, sizeof (float), 1, fp) != 1) {
    fprintf (stderr, "Input file is not a valid STL file\n"
       "stdin: missing vertex x-coordinate\n");
    exit (1);
  }
  if (fread (&y, sizeof (float), 1, fp) != 1) {
    fprintf (stderr, "Input file is not a valid STL file\n"
       "stdin: missing vertex y-coordinate\n");
    exit (1);
  }
  if (fread (&z, sizeof (float), 1, fp) != 1) {
    fprintf (stderr, "Input file is not a valid STL file\n"
       "stdin: missing vertex z-coordinate\n");
    exit (1);
  }
  coord p = {x,y,z};
  array_append (a, &p, sizeof(coord));
      }
 
      if (fread (&attbytecount, sizeof (uint16_t), 1, fp) != 1) {
  fprintf (stderr, "Input file is not a valid STL file\n"
         "stdin: missing attribute byte count\n");
  exit (1);
      }
      i--;
    }
  }
  coord p = {nodata};
  array_append (a, &p, sizeof(coord));
  return (coord *) array_shrink (a);
}
 
/**
In 2D dimensions, the file format is that used by gnuplot i.e. pairs
of 2D vertex coordinates separated by blank lines. An easy way to
create these files is to use a vector graphics editing program
(e.g. inkscape) and save the curve as an *.eps* file, then convert it
to gnuplot format using
 
~~~bash
pstoedit -f gnuplot -flat 0.1 file.eps file.gnu
~~~
 
The function below reads such a file and returns an array of pairs of
coordinates. */
 
trace
coord * input_xy (FILE * fp)
{
  Array * a = array_new();
  coord p = {0}, last, * la = NULL;
  while (!feof(fp)) {
    if (fscanf (fp, "%lf %lf", &p.x, &p.y) == 2) {
      if (la) {
  array_append (a, la, sizeof(coord));
  array_append (a, &p, sizeof(coord));
      }
      last = p, la = &last;
    }
    else {
      int c;
      while ((c = fgetc(fp)) != EOF && c != '\n');
      la = NULL;
    }
  }
  p.x = nodata;
  array_append (a, &p, sizeof(coord));
  return (coord *) array_shrink (a);
}
 
/**
This function computes the coordinates of the bounding box of a set of
segments or triangles. */
 
void bounding_box (coord * p, coord * min, coord * max)
{
  for (int _d = 0; _d < dimension; _d++)
    (*min).x = HUGE, (*max).x = - HUGE;
  while (p->x != nodata) {
    for (int _d = 0; _d < dimension; _d++) {
      if ((*p).x < (*min).x)
  (*min).x = (*p).x;
      if ((*p).x > (*max).x)
  (*max).x = (*p).x;
    }
    p++;
  }
}
  
/**
An extra field, holding a pointer to the elements (segments or
triangles) intersecting the neighborhood of the cell, is associated
with the distance function. The neighborhood is a sphere centered on
the cell center and with a diameter \f$3\Delta\f$. */
 
attribute {
  scalar surface;
}
 
#define double_to_pointer(d) (*((void **) &(d)))
#define BSIZE 3. // if larger than 1, cells overlap
 
/**
To compute the minimal distance and its sign, we need to store
information on the closest (2 in 2D, 12 in 3D) elements. */
 
typedef struct {
  double d2; // minimal distance
  coord * v; // the element
  int type;  // 0,1,2: vertex, 3: edge, 4: face
} closest_t;
 
#if dimension == 2
#  define ND 2
#else // dimension == 3
#  define ND 12
static double vertex_angle (coord * p, int type)
{
  if (type >= 3) // edge or face
    return pi;
  coord * v = p + type, * v1 = p + (type + 1)%3, * v2 = p + (type + 2)%3;
  coord vv1 = vecdiff (*v1, *v), vv2 = vecdiff(*v2,*v);
  return acos(vecdot(vv1,vv2)/sqrt(vecdot(vv1,vv1)*vecdot(vv2,vv2)));
}
 
static coord face_normal (coord * q, int type)
{
  coord ab = vecdiff(*(q+1),*q), ac = vecdiff(*(q+2),*q);
  coord n = vecdotproduct(ab,ac);
  double nn = sqrt(vecdot(n,n));
  assert (nn > 0.);
  nn = vertex_angle(q, type)/nn;
  for (int _d = 0; _d < dimension; _d++)
    n.x *= nn;
  return n;
}
#endif // dimension == 3
 
static void update_distance (Point point, coord ** i, scalar d)
{
  scalar surface = d.surface;
  Array * a = array_new();
  coord c = {x,y,z}, closest = {0};
  closest_t q[ND];
  for (int i = 0; i < ND; i++)
    q[i].d2 = HUGE;
  int nd = 0;
  double r2 = sq(BSIZE*Delta/2.);
  bool first = (level == 0);
  while (*i) {
    coord * p = *i;
#if dimension == 2
    coord r;
    double s, d2 = PointSegmentDistance (&c, p, p + 1, &r, &s);
#elif dimension == 3
    double s, t, d2 = PointTriangleDistance (&c, p, p + 1, p + 2, &s, &t);
#endif
    // keep pointers/distances/types of up to ND closest elements
    for (int i = 0; i < ND; i++)
      if (d2 < q[i].d2) {
  for (int j = ND - 1; j > i; j--)
    q[j] = q[j-1];
  q[i].d2 = d2, q[i].v = p;
#if dimension == 2
  // vertices
  if (s == 0.)
    q[i].type = 0;
  else if (s == 1.)
    q[i].type = 1;
  else
    // edge
    q[i].type = 3;
  if (i == 0)
    closest = r;
#elif dimension == 3
  // vertices
  if (s == 0. && t == 0.)
    q[i].type = 0;
  else if (s == 1. && t == 0.)
    q[i].type = 1;
  else if (s == 0. && t == 1.)
    q[i].type = 2;
  else if (s == 0. || t == 0. || s + t == 1.)
    // edge
    q[i].type = 3;
  else
    // face
    q[i].type = 4;
  if (i == 0)
    for (int _d = 0; _d < dimension; _d++)
      closest.x = ((*q[0].v).x*(1. - s - t) + s*(*(q[0].v+1)).x +
       t*(*(q[0].v+2)).x);
#endif // dimension == 3
  if (i >= nd)
    nd = i + 1;
  break;
      }
    // add elements which are close enough to the local list
    if (d2 < r2 || first)
      array_append (a, &p, sizeof(coord *));
    first = false, i++;
  }
  if (a->len) {
    // set surface[] to list, ended with NULL
    coord * p = NULL;
    array_append (a, &p, sizeof(coord *));
    p = (coord *) array_shrink (a);
    assert (sizeof(real) >= sizeof(void *));
    memcpy (&surface[], &p, sizeof(void *));
 
    int orient;
#if dimension == 2
    if (q[0].type == 3)
      // edge
      orient = PointSegmentOrientation (&c, q[0].v, q[0].v + 1);
    else {
      // vertex
      if (nd == 1) // a single vertex, cannot find sign
  // get sign from parent
  orient = sign(bilinear (point, d));
      else { // two vertices
  orient = PointSegmentOrientation (&c, q[0].v, q[0].v + 1);
  if (orient != PointSegmentOrientation (&c, q[1].v, q[1].v + 1)) {
    coord n = {0};
    for (int i = 0; i < 2; i++) {
#if DEBUG     
      fprintf (stderr, "q %g %g %g %g\n",
         x, y, q[i].v->x - x, q[i].v->y - y);
#endif
      coord ab = vecdiff(*(q[i].v + 1),*q[i].v);
      double nn = sqrt(vecdot(ab,ab));
      assert (nn > 0.);
      n.x -= ab.y/nn, n.y += ab.x/nn;
    }
#if DEBUG
    fprintf (stderr, "vertex %g %g %g %g %g %g %g %g\n",
       x, y, closest.x - x, closest.y - y,
       closest.x, closest.y, n.x, n.y);
#endif
    coord diff = vecdiff(closest,c);
    orient = sign(vecdot(n,diff));
  }
      }
    }
#elif dimension == 3
    if (q[0].type < 3) {
      coord n = face_normal (q[0].v, q[0].type), diff = vecdiff(closest,c);
      int nv = 1;
      orient = sign(vecdot(n,diff));
      for (int i = 1; i < nd && q[i].type < 3; i++)
  if (vecdist2 (closest, *(q[i].v + q[i].type)) < sq(1e-6)) {
    coord n1 = face_normal (q[i].v, q[i].type);
    for (int _d = 0; _d < dimension; _d++)
      n.x += n1.x;
    nv++;
    if (orient > -2 && sign(vecdot(n1,diff)) != orient)
      orient = -2;
  }
      if (nv < 3) // less than 3 vertices, cannot find sign
  // get sign from parent
  orient = sign(bilinear (point, d));
      else if (orient == -2) {
  // the vertices do not have the same orientation
  // get the proper orientation from the pseudo-normal n
  orient = sign(vecdot(n,diff));
#if DEBUG
  fprintf (stderr, "vertex %g %g %g %g %g %g %d %g %g %g %g %g %g %d\n",
     x, y, z, closest.x - x, closest.y - y, closest.z - z, orient,
     closest.x, closest.y, closest.z, n.x, n.y, n.z, nv);
#endif
      }
    }
    else if (q[0].type == 3) {
      // edge
      if (nd == 1 || q[1].type != 3) // a single edge, cannot find sign
  // get sign from parent
  orient = sign(bilinear (point, d));
      else { // two edges
  orient = PointTriangleOrientation (&c, q[0].v, q[0].v+1, q[0].v+2);
  if (orient !=
      PointTriangleOrientation (&c, q[1].v, q[1].v+1, q[1].v+2)) {
    coord n1 = face_normal (q[0].v, 3), n2 = face_normal (q[1].v, 3), n;
    for (int _d = 0; _d < dimension; _d++)
      n.x = n1.x + n2.x;
    coord diff = vecdiff(closest,c);
    orient = sign(vecdot(n,diff));
#if DEBUG
    fprintf (stderr, "edge %g %g %g %g %g %g %d %g %g %g %g %g %g\n",
       x, y, z, closest.x - x, closest.y - y, closest.z - z, orient,
       closest.x, closest.y, closest.z, n.x, n.y, n.z);
#endif
  }
#if DEBUG
  else
    fprintf (stderr, "edge %g %g %g %g %g %g %d\n",
       x, y, z, closest.x - x, closest.y - y, closest.z - z, orient);
#endif
      }
    }
    else { // face
#if DEBUG
      fprintf (stderr, "face %g %g %g %g %g %g\n",
         x, y, z, closest.x - x, closest.y - y, closest.z - z);
#endif
      orient = PointTriangleOrientation (&c, q[0].v, q[0].v+1, q[0].v+2);
    }
#endif // dimension == 3
    d[] = sqrt (q[0].d2)*orient; 
  }
  else { // !a->len
    free (a);
    surface[] = 0.;
    if (level > 0)
      d[] = bilinear (point, d);
    else
      d[] = 0.;
  }
}
 
#undef ND
 
static void refine_distance (Point point, scalar d)
{
  scalar surface = d.surface;
  if (surface[] == 0.)
    for (int _c = 0; _c < 4; _c++) /* foreach_child */ {
      surface[] = 0.;
      d[] = bilinear (point, d);
    }
  else {
    coord ** ap = (coord **) double_to_pointer (surface[]);
    int s = 0;
    for (int _c = 0; _c < 4; _c++) /* foreach_child */ {
      update_distance (point, ap, d);
      s += sign(d[]);
    }
 
    /**
    To increase robustness to inconsistent input, we check whether all
    children are included within the minimum distance sphere. If this
    is the case then the children and parent must have the same
    orientation. We enforce this, using the "average" orientation. */
 
    if (fabs(d[]) > sqrt(dimension)/4.*Delta) {
      if (abs(s) != 1 << dimension) {
  s = sign(s);
  for (int _c = 0; _c < 4; _c++) /* foreach_child */
    d[] = s*fabs(d[]);
      }
      if (sign(d[]) != sign(s))
  d[] = - d[];
    }
  }
}
 
static void restriction_distance (Point point, scalar d) {}
 
static void coarsen_distance (Point point, scalar d) {
  scalar surface = d.surface;
  for (int _c = 0; _c < 4; _c++) /* foreach_child */
    free (double_to_pointer (surface[]));
}
 
static void delete_distance (scalar d) {
  scalar surface = d.surface;
  for (int _l = 0; _l < 0; _l++)
    free (*((void **)double_to_pointer (surface[])));
  for (int l = 0; l <= depth(); l++)
    for (int _l = 0; _l < l; _l++)
      free (double_to_pointer (surface[]));
  delete ({surface});
}
 
trace
void distance (scalar d, coord * p)
{
  scalar surface = d.surface;
  if (surface.i)
    delete_distance (d);
  surface = {0} /* new scalar */;
  surface.restriction = no_restriction;
#if TREE
  surface.refine = no_restriction; // handled by refine_distance()
  set_prolongation (surface, no_restriction);
  set_prolongation (d, refine_bilinear);
  d.refine = refine_distance;  
  d.coarsen = coarsen_distance;
#endif
  d.surface = surface;
  d.delete = delete_distance;
  d.restriction = restriction_distance;
 
  Array * a = array_new();
  while (p->x != nodata) {
#if dimension == 3
    // filter degenerate triangles
    coord ab = vecdiff(*(p+1),*p), ac = vecdiff(*(p+2),*p);
    coord n = vecdotproduct(ab,ac);
    if (vecdot(n,n) > 0.)
#endif
      array_append (a, &p, sizeof (coord *));
    p += dimension;
  }
  p = NULL;
  array_append (a, &p, sizeof (coord *));
  p = (coord *) array_shrink (a);
 
  for (int _l = 0; _l < 0, noauto; _l++)
    update_distance (point, (coord **) p, d);
  free (p);
  
  boundary_level ({d}, 0);
  for (int l = 0; l < depth(); l++) {
    for (int _l = 0; _l < l; _l++)
      refine_distance (point, d);
    boundary_level ({d}, l + 1);
  }
}