level-set/src/FastMarching.hpp

/*
    GNU Octave level-set package.
    Copyright (C) 2013-2014  Daniel Kraft <d@domob.eu>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef FASTMARCHING_HPP
#define FASTMARCHING_HPP

/*

Implementation of the fast marching method to numerically solve the
Eikonal equation:

  | grad u | = f

f is assumed to be positive, and also assumed to include the grid spacing
already (so that 1 is used as unit distance between grid points).
For solving problems with changing sign of f,
the domain has to be split into parts with constant sign of f, and if f
is negative, a corresponding transformation on f and u has to be performed
to make f positive.

The base grid is rectangular and assumed to "hold all" of the domain,
but it is not necessary to set F on the full grid to meaningful values.
Only for cells which are not initially marked as "alive" are the
neighbours actually ever accessed, and points outside the domain of
interest can be set NULL.  Those will be assumed to be always
too far away to influence interior points, so in particular if f -> infinity
towards the boundary we can really ignore those points.

Each grid cell is represented by a pointed-to object, so that we can easily
store pointers to those objects in the grid to access them by indices and
find their neighbours, and also in the heap structure used to find the
next "narrow band" point to operate on.

Additionally, it is possible to extend a function g known on the domain boundary
"along" the normal direction, such that

  grad u * grad g = 0.

*/

#include "Heap.hpp"

#include <cassert>
#include <limits>
#include <map>
#include <vector>

namespace fastMarching
{

class Entry;
class Grid;

/** Type used for real numbers.  */
typedef double realT;

/** Define value used to represent "very large" values.  */
static const realT LARGE_DIST = std::numeric_limits<realT>::max ();

/**
 * Type used to number entries.  Signed values are allowed in order to allow
 * -1 to be a "valid" index.  The grid takes care of out-of-range values.
 */
typedef int indexT;

/**
 * Full set of indices.  To be precise, this contains N indexT grouped
 * together, where N is the space dimension we solve the equation in.
 */
typedef std::vector<indexT> IndexTuple;

/** Type used to number the dimensions.  */
typedef IndexTuple::size_type dimensionT;

/** Type used for the heap.  */
typedef Heap<Entry*, bool (*)(const Entry*, const Entry*)> heapT;

/**
 * Method to issue warnings.  This is not implemented in FastMarching.cpp
 * but must be provided by the caller.  That way, it can use Octave's
 * warning infrastructure but still make the FastMarching code itself
 * independent of Octave.
 * @param id An ID for the warning.
 * @param fmt The (printf-like) message template.
 * @param ... Other arguments for printf-like formatting.
 */
void issueWarning (const std::string& id, const std::string& fmt, ...);

/* ************************************************************************** */
/* Update equation class.  */

/**
 * Represent and solve the quadratic equation necessary in the fast marching
 * step for updating the distance of a point from given neighbours.  This
 * is also useful for initialising the narrow-band entries from the level-set
 * function, and thus available as global class.
 */
class UpdateEquation
{

private:

  class EqnEntry;

  /**
   * Map space dimensions to the currently best entry along the given
   * dimension.  It may be null if there's not yet an acceptable entry
   * there, and there may be multiple entries added for the same dimension
   * (on both sides of our point).  In this case, only the closer one is
   * relevant, and will be kept in the map.
   */
  typedef std::map<dimensionT, EqnEntry*> entryMapT;

  /** The actual map.  */
  entryMapT entries;

  /**
   * Remember whether one of the g values was being absent.  If that's
   * the case, we can't calculate g from the equation.  This is enforced.
   */
  bool gMissing;

  /**
   * When solving, keep track of the actually used and sorted neighbours
   * for calculating g later on.
   */
  mutable std::vector<const EqnEntry*> sortedEntries;

  /** Store calculated distance value here.  */
  mutable realT u;

public:

  /**
   * Construct it empty.
   */
  inline UpdateEquation ()
    : entries (), gMissing(false), sortedEntries(), u(0.0)
  {}

  /**
   * Destruct and free all memory held in the entries map.
   */
  ~UpdateEquation ();

  // No copying.
  UpdateEquation (const UpdateEquation&) = delete;
  UpdateEquation& operator= (const UpdateEquation&) = delete;

  /**
   * Add a new entry.  The entry is characterised by the dimension along
   * which it neighbours the cell being calculated, the distance (grid spacing)
   * to it and the distance value at the neighbour.  Using this function
   * without a parameter for g flags the equation as missing g values,
   * which means that getExtendedFunction does not work.
   * @param d Dimension along which the point neighbours the centre.
   * @param h Grid spacing to the neighbour.
   * @param u Value of the distance function at the neighbour.
   */
  inline void
  add (dimensionT d, realT h, realT u)
  {
    add (d, h, u, 0.0);
    gMissing = true;
  }

  /**
   * Add a new entry together with g.
   * @param d Dimension along which the point neighbours the centre.
   * @param h Grid spacing to the neighbour.
   * @param u Value of the distance function at the neighbour.
   * @param g Value of the extended function at the neighbour.
   */
  void add (dimensionT d, realT h, realT u, realT g);

  /**
   * Calculate the new distance value solving the quadratic equation.
   * @param f Speed value at the current cell.
   * @return Distance value at the centre cell.
   */
  realT solve (realT f) const;

  /**
   * Calculate the value of the extended function at the centre.  This
   * can only be used if all g values were given for all entries, and
   * if solve() was already called.
   * @return The value of the extended function at the centre.
   * @throws std::logic_error if the equation was built without all g's.
   * @throws std::logic_error if solve() was not yet called.
   */
  realT getExtendedFunction () const;

};

/**
 * The equation entry class.  This simply holds some data.
 */
class UpdateEquation::EqnEntry
{

public:

  /** The neighbour's distance on the grid.  */
  realT h;

  /** The neighbour's distance value.  */
  realT u;

  /** The neighbour's extended function value.  */
  realT g;

  /* Note that we don't need the dimension, since this is already part
     of the map in UpdateEquation anyway.  */

  /**
   * Construct it given all the data.
   * @param mh Grid distance.
   * @param mu Distance value.
   * @param mg Extended function value.
   */
  inline EqnEntry (realT mh, realT mu, realT mg)
    : h(mh), u(mu), g(mg)
  {}

  // No copying or default constructor, since it is not needed.
  EqnEntry () = delete;
  EqnEntry (const EqnEntry&) = delete;
  EqnEntry& operator= (const EqnEntry&) = delete;

  /**
   * Get the "effective distance" that this point would imply for the
   * centre, which is the distance if only this point would be there.  It
   * is used for deciding which one of two neighbours along a single dimension
   * is the one to use, and also for sorting later on.
   * @return The effective distance, which is u + h.
   */
  inline realT
  getEffDistance () const
  {
    /* Note:  The roles of u and h in the quadratic equation being solved
       are not entirely the same, so it is not clear whether this
       "effective distance" is really the right criterion to decide upon which
       point to use.  However, in the cases actually used, either h is the
       same along a dimension (grid spacing) or u is zero everywhere (initial
       distances from the level-set function).  For those, this concept should
       work at least.  */

    return u + h;
  }

  /**
   * Compare two entry pointers for sorting by distance.  If the distances
   * are equal, we compare the pointers just to get a criterion to avoid
   * false classification as being equal (which might confuse the sorting
   * algorithm).
   * @param a First pointer.
   * @param b Second pointer.
   * @return True iff a < b.
   */
  static inline bool
  compare (const EqnEntry* a, const EqnEntry* b)
  {
    const realT diff = a->getEffDistance () - b->getEffDistance ();
    if (diff != 0.0)
      return (diff < 0.0);

    return (a < b);
  }

};

/* ************************************************************************** */
/* Single cell entry.  */

/**
 * This class represents all data stored for a single grid point.  It also
 * has the ability to do cell-related calculations, and update itself solving
 * the respective quadratic equation.
 */
class Entry
{

public:

  /** Possible states of a cell during processing.  */
  enum State
  {
    ALIVE,
    NARROW_BAND,
    FAR_AWAY
  };

private:

  /** Value of f at the given point.  */
  const realT f;

  /** Current value of u.  */
  realT u;
  /** Current value of the extended function g.  */
  realT g;

  /** Current state.  */
  State state;

  /**
   * Index of this grid cell.  This is used to find the neighbours
   * in the updating calculation.
   */
  const IndexTuple coord;

  /** Entry into the heap in case we're in narrow band.  */
  heapT::Entry heapEntry;

  /**
   * Construct the cell entry given the data.
   * @param s Initial state.
   * @param c Coordinate in the grid.
   * @param mf Value of f at this position.
   * @param mu Distance value.
   * @param mg Value for extended function.
   */
  inline Entry (State s, const IndexTuple& c, realT mf, realT mu, realT mg)
    : f(mf), u(mu), g(mg), state(s), coord(c), heapEntry(nullptr)
  {
    assert (s == ALIVE || s == FAR_AWAY);
  }

public:

  /* No copying or default constructor.  */
  Entry () = delete;
  Entry (const Entry& o) = delete;
  Entry& operator= (const Entry& o) = delete;

  /**
   * Construct the entry for an initially alive cell.
   * @param c Coordinate for it.
   * @param u Initial distance.
   * @param g Initial function value.
   */
  static inline
  Entry* newAlive (const IndexTuple& c, realT u, realT g)
  {
    /* f is not necessary, set to dummy value.  */
    return new Entry (ALIVE, c, 0.0, u, g);
  }

  /**
   * Construct the entry for an initially far away cell.
   * @param c Coordinate for it.
   * @param f Value of f.
   */
  static inline
  Entry* newFarAway (const IndexTuple& c, realT f)
  {
    /* g0 is not necessary (will be overwritten), set to dummy value.  */
    return new Entry (FAR_AWAY, c, f, LARGE_DIST, 0.0);
  }

  /**
   * Get current value of u.
   * @return Current value of u for this cell.
   */
  inline realT
  getDistance () const
  {
    return u;
  }

  /**
   * Get value of extended function g.
   * @return Current value of g for this cell.
   */
  inline realT
  getFunction () const
  {
    return g;
  }

  /**
   * Get index coordinate.  This is used by the grid to put the cell into
   * the correct place when initialising.
   * @return This entry's coordinate.
   */
  inline const IndexTuple&
  getCoordinate () const
  {
    return coord;
  }

  /**
   * Get the current state.
   * @return The current state.
   */
  inline State
  getState () const
  {
    return state;
  }

  /**
   * Set state to alive.
   * @see setNarrowBand
   */
  void setAlive ();

  /**
   * Set state to narrow band, adding also the heap entry.
   * @param e Heap entry.
   * @see setState
   */
  void setNarrowBand (const heapT::Entry& e);

  /**
   * Update distance value from the neighbours.  They are found
   * using our coordinates and the given grid, and the heap is
   * automatically bubbled for the change to take place.
   * This may raise assertion failures under certain conditions.
   * @param grid The grid to use to access neighbours.
   */
  void update (const Grid& grid);

  /**
   * Compare two elements as they should be compared for the heap
   * data structure.  Since the heap has the largest element first and
   * that should correspond to smallest distance, return ordering
   * induced by ">" on u.  Because the heap
   * will be based on pointers to entries, this function also works on them.
   * @param a First entry.
   * @param b Second entry.
   * @return True iff a should be accessed later than b, which is equivalent
   *         to a.u > b.u.
   */
  static inline bool
  compareForHeap (const Entry* a, const Entry* b)
  {
    assert (a && b);
    return a->u > b->u;
  }

};

/* ************************************************************************** */
/* Full grid.  */

/**
 * This class represents a full grid, which also keeps track of the high-level
 * data structures during the solve.
 */
class Grid
{

private:

  /** Size of the grid.  */
  const IndexTuple size;

  /**
   * Store pointers to the grid entries "by index" here.  This is the
   * multi-dimensional array mapped to a single dimension.  Some entries
   * may be NULL, which means that those are outside of the domain
   * we're interested in.
   */
  std::vector<Entry*> entries;

  /** Keep track of points currently in "narrow band" state.  */
  heapT narrowBand;

  /**
   * Calculate index into entries for given coordinates.
   * @param c Coordinates in indices.
   * @return Single index into entries or -1 if c is out of range.
   */
  indexT lineariseIndex (const IndexTuple& c) const;

  /**
   * Perform a single update step on the currently best next narrow-band
   * entry.  It is removed and set alive, and its neighbours are then
   * added into the narrow-band if not yet there and updated.
   */
  void update ();

  /**
   * Recalculate neighbours of a given entry.  This is used both for
   * update and initialisation of narrow band.
   * @param center Recalculate this entry's neighbours.
   */
  void recalculateNeighbours (const Entry& center);

  /**
   * Get grid cell by index, mutable version for internal use.
   * @param c Index coordinate tuple.
   * @return The corresponding entry pointer.
   * @see get (const IndexTuple&)
   */
  inline Entry*
  get (const IndexTuple& c)
  {
    const indexT ind = lineariseIndex (c);
    if (ind < 0)
      return nullptr;
    return entries[ind];
  }

  /**
   * Internal routine used for iterate(), that is called recursively
   * to perform the required number of nested loops.
   * @param cur Current index tuple, will be built up recursively.
   * @param depth Current depth in the recursion.
   * @param f The call-back to call for each cell index.
   */
  template<typename F>
    void iterate (IndexTuple& cur, dimensionT depth, const F& f) const;

public:

  /* No copying or default constructor.  */
  Grid () = delete;
  Grid (const Grid& o) = delete;
  Grid& operator= (const Grid& o) = delete;

  /**
   * Construct the grid with a given size.  The entries will be empty
   * and will still have to be filled in.
   * @param s Size to use.
   */
  Grid (const IndexTuple& s);

  /**
   * Destructor, freeing all memory.
   */
  ~Grid ();

  /**
   * Initialise a given grid cell by the given entry.  The coordinate
   * where to put the entry is taken from the object itself, and an
   * error is raised if that entry is already filled in.  The entries
   * state must be either "alive" or "far away", narrow band is
   * automatically initialised later on.
   * @param e The new entry.  Ownership is taken over.
   * @throws std::invalid_argument if something is wrong with e.
   */
  void setInitial (Entry* e);

  /**
   * Get grid cell by index.  Returned is the pointer, which may be NULL
   * in case this cell is outside the domain or even the index range given
   * outside the size.
   * @param c Index coordinate tuple.
   * @return The corresponding entry pointer.
   */
  inline const Entry*
  get (const IndexTuple& c) const
  {
    const indexT ind = lineariseIndex (c);
    if (ind < 0)
      return nullptr;
    return entries[ind];
  }

  /**
   * Perform the calculation, updating all narrow-band entries until
   * all of them are alive.  It is necessary to have narrow-band already
   * filled in, meaning that far-away entries not reached by the propagation
   * from any narrow-band will not be updated!
   */
  void perform ();

  /**
   * Iterate over all cells of the given grid and call the provided
   * call-back routine with the coordinate as IndexTuple.
   * @param f The call-back to call for each cell index.
   */
  template<typename F>
    inline void
    iterate (const F& f) const
  {
    IndexTuple cur(size.size ());
    iterate (cur, 0, f);
  }

  /**
   * Iterate over all neighbours of a given index.  For each neighbour that
   * is still on the grid, call the provided function with its index.
   * @param c The centre coordinate.
   * @param f The call-back to call for each neighbour.
   */
  template<typename F>
    void iterateNeighbours (const IndexTuple& c, const F& f) const;

};

/* ************************************************************************** */

} // Namespace fastMarching.

/* Include template implementations.  */
#include "FastMarching.tpp"

#endif /* Header guard.  */