DensityEstimator.h

#ifndef PLANNING_DENSITY_ESTIMATOR_H
#define PLANNING_DENSITY_ESTIMATOR_H

#include <KrisLibrary/math/vector.h>
#include <KrisLibrary/geometry/GridSubdivision.h>
#include "PointLocation.h"

class DensityEstimatorBase 
{
public:
  virtual ~DensityEstimatorBase () {}
  virtual void Clear()=0;
  virtual void Add(const Math::Vector& x,void* data)=0;
  virtual void Remove(const Math::Vector& x,void* data)=0;
  virtual double Density(const Config& x)=0;
  virtual void* RandomNear(const Math::Vector& x) { return NULL; }
  virtual void* Random() { return NULL; }
  virtual void Random(Math::Vector& x) { }
};

class GridDensityEstimator : public DensityEstimatorBase
{
public:
  GridDensityEstimator();
  GridDensityEstimator(const std::vector<int>& mappedDims,Math::Real h);
  GridDensityEstimator(const std::vector<int>& mappedDims,const Math::Vector& h);
  void Randomize(int numSourceDims,int numMapped,const Vector& hsource);
  void Randomize(int numSourceDims,int numMapped,Real h);
  virtual void Clear();
  virtual void Add(const Math::Vector& x,void* data);
  virtual void Remove(const Math::Vector& x,void* data);
  virtual double Density(const Config& x);
  virtual void* RandomNear(const Math::Vector& x);
  virtual void* Random();
  
  std::vector<int> mappedDims;
  Math::Vector h;
  Geometry::GridSubdivision subdiv;

  //temporary
  Math::Vector temp;
  std::vector<Geometry::GridSubdivision::ObjectSet*> flattenedBuckets;
};

class MultiGridDensityEstimator : public DensityEstimatorBase
{
public:
  MultiGridDensityEstimator(int numDims,int numMappedDims,Real h);
  MultiGridDensityEstimator(int numDims,int numMappedDims,const Math::Vector& h);
  void Randomize();
  virtual void Clear();
  virtual void Add(const Math::Vector& x,void* data);
  virtual void Remove(const Math::Vector& x,void* data);
  virtual double Density(const Config& x);
  virtual void* RandomNear(const Math::Vector& x);
  virtual void* Random();

  int numDims,numMappedDims;
  Math::Vector h;
  std::vector<GridDensityEstimator> grids;
};

class KernelDensityEstimator : public DensityEstimatorBase
{
public:
  enum KernelType { KernelUniform, KernelGaussian, KernelTriangular};
  KernelDensityEstimator(Math::Real kernelRadius,Math::Real kernelTruncationFactor=3.0);
  virtual void Clear();
  virtual void Add(const Math::Vector& x,void* data);
  virtual void Remove(const Math::Vector& x,void* data);
  virtual double Density(const Config& x);
  virtual void* RandomNear(const Math::Vector& x);
  virtual void* Random();
  virtual void Random(Math::Vector& x);
  
  KernelType kernelType;
  Math::Real kernelRadius,kernelTruncationFactor;
  std::vector<Math::Vector> pointList;
  std::vector<void*> dataList;
  std::shared_ptr<PointLocationBase> pointLocation;
};


#endif