simplex.h

Go to the documentation of this file.

/*
 *  Copyright (C) 2012 Josh Bialkowski (jbialk@mit.edu)
 *
 *  This file is part of mpblocks.
 *
 *  mpblocks 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.
 *
 *  mpblocks 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 mpblocks.  If not, see <http://www.gnu.org/licenses/>.
 */
#ifndef EDELSBRUNNER96_SIMPLEX_H_
#define EDELSBRUNNER96_SIMPLEX_H_

#include <array>
#include <bitset>
#include <cstdint>
#include <initializer_list>
#include <map>
#include <set>

#include <Eigen/Dense>

namespace edelsbrunner96 {
namespace simplex {


enum MarkedBits {
  ISC_OPENED, 
  ISC_MEMBER, 
  BFS_QUEUED, 
  RETIRED,    
  FEATURE_WALK, 
  LINE_WALK, 
  VISIBLE_HULL_WALK, 
  VISIBLE_HULL, 
  FUZZY_WALK, 
  REMOVED_FOR_INSERT, 
  NUM_BITS
};

}  // namespace simplex


template<class Traits>
struct SimplexBase {
  typedef typename Traits::Scalar       Scalar;
  typedef typename Traits::Point        Point;
  typedef typename Traits::PointRef     PointRef;
  typedef typename Traits::Simplex      Simplex;
  typedef typename Traits::SimplexRef   SimplexRef;
  typedef typename Traits::Storage      Storage;

  typedef std::bitset<simplex::NUM_BITS> MarkedVec;

  MarkedVec marked;       
  // TODO(bialkowski): I think version number is overkill and we can add a
  // "retired" flag to the bitset instead.
  uint16_t    version;      

  std::array<PointRef, Traits::NDim + 1> V;

  std::array<SimplexRef, Traits::NDim + 1> N;

  Point       c;            
  Scalar      r2;           

  SimplexBase();

  template <typename... PointRefs>
  void SetVertices(PointRefs... refs);

  template <typename Container>
  void SetVertices(Container refs);

  void SetVertices(std::initializer_list<PointRef> refs);


  SimplexRef NeighborAcross(PointRef q) const;


  void SetNeighborAcross(PointRef q, SimplexRef n);

  uint8_t IndexOf(PointRef v) const;

  Eigen::Matrix<typename Traits::Scalar, Traits::NDim, 1>
    CoordinatesOf(Storage& storage, const typename Traits::Point& xq) const;

  Eigen::Matrix<typename Traits::Scalar, Traits::NDim + 1, 1>
  BarycentricCoordinates(Storage& storage,
                         const typename Traits::Point& xq) const;

  bool Contains(Storage& storage, const typename Traits::Point& xq) const;

  void ComputeCenter(Storage& storage);
};

template <class Traits>
class SimplexFill {
 public:
  typedef typename Traits::Storage  Storage;
  typedef typename Traits::PointRef PointRef;
  typedef typename Traits::Simplex  Simplex;

  SimplexFill(Simplex& s)
      : index_(0),
        simplex_(s) {
  }

  void PushBack(PointRef p) {
    simplex_.V[index_++] = p;
  }

 private:
  uint8_t  index_;
  Simplex& simplex_;
};


template <class Traits, class Derived, class Iterator>
void BarycentricProjection(
    typename Traits::Storage& storage, const typename Traits::Point& x,
    Iterator begin, Iterator end,
    Eigen::MatrixBase<Derived>* L,
    typename Traits::Point* x_proj);


template <class Traits, class OutputIterator>
typename Traits::Scalar SimplexDistance(
    typename Traits::Storage& storage,
    const typename Traits::Point& x,
    const typename Traits::SimplexRef s_ref,
    typename Traits::Point* x_proj,
    OutputIterator V_out);

template <class Traits>
class ExhaustiveSimplexDistance {
 public:
  typedef typename Traits::Storage  Storage;
  typedef typename Traits::Scalar Scalar;
  typedef typename Traits::Point  Point;
  typedef typename Traits::PointRef  PointRef;

  ExhaustiveSimplexDistance() {
    V_feature_.reserve(Traits::NDim+1);
    V_best_feature_.reserve(Traits::NDim+1);
    min_distance_ = std::numeric_limits<Scalar>::max();
  }

  template <typename InputIterator>
  ExhaustiveSimplexDistance<Traits>& Compute(Storage& storage, const Point& x,
                                             InputIterator begin,
                                             InputIterator end) {
    for (InputIterator iter = begin; iter != end; iter++) {
      V_feature_.push_back(*iter);
      BarycentricProjection<Traits>(storage, x, V_feature_.begin(),
                                    V_feature_.end(), &L_, &x_proj_);
      if (L_.minCoeff() >= 0) {
        distance_ = (x_proj_ - x).squaredNorm();
        if (distance_ < min_distance_) {
          V_best_feature_ = V_feature_;
          x_best_proj_ = x_proj_;
          min_distance_ = distance_;
        }
      }

      Compute(storage, x, iter + 1, end);
      V_feature_.pop_back();
    }
    return *this;
  }

  template <typename OutputIterator>
  void GetResult(Scalar* distance, Point* x_proj, OutputIterator V_out) {
    *distance = std::sqrt(min_distance_);
    *x_proj = x_best_proj_;
    std::copy(V_best_feature_.begin(), V_best_feature_.end(), V_out);
  }

 private:
  std::vector<PointRef> V_feature_;
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, Traits::NDim + 1, 1> L_;
  Point  x_proj_;
  Scalar distance_;

  std::vector<PointRef> V_best_feature_;
  Point x_best_proj_;
  Scalar min_distance_;
};

}  // namespace edelsbrunner96

#endif  // EDELSBRUNNER96_SIMPLEX_H_