text: #include "CubicSDFCollisionDetection.h"
  #include "Simulation/IDFactory.h"
  #include "Eigen/Dense"
  
  using namespace PBD;
  
  int CubicSDFCollisionDetection::CubicSDFCollisionObject::TYPE_ID = IDFactory::getId();
  
  CubicSDFCollisionDetection::CubicSDFCollisionDetection() :
  	DistanceFieldCollisionDetection()
  {	
  }
  
  CubicSDFCollisionDetection::~CubicSDFCollisionDetection()
  {
  }
  
  bool CubicSDFCollisionDetection::isDistanceFieldCollisionObject(CollisionObject *co) const
  {
   	return DistanceFieldCollisionDetection::isDistanceFieldCollisionObject(co) ||
  		(co->getTypeId() == CubicSDFCollisionDetection::CubicSDFCollisionObject::TYPE_ID);
  }
  
  void CubicSDFCollisionDetection::addCubicSDFCollisionObject(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const std::string &sdfFile, const Vector3r &scale, const bool testMesh, const bool invertSDF)
  {
  	CubicSDFCollisionDetection::CubicSDFCollisionObject *co = new CubicSDFCollisionDetection::CubicSDFCollisionObject();
  	co->m_bodyIndex = bodyIndex;
  	co->m_bodyType = bodyType;
  	co->m_sdfFile = sdfFile;
  	co->m_scale = scale;
  	co->m_sdf = std::make_shared<Grid>(co->m_sdfFile);
   	co->m_bvh.init(vertices, numVertices);
   	co->m_bvh.construct();
  	co->m_testMesh = testMesh;
  	co->m_invertSDF = 1.0;
  	if (invertSDF)
  		co->m_invertSDF = -1.0;
  	m_collisionObjects.push_back(co);
  }
  
  void PBD::CubicSDFCollisionDetection::addCubicSDFCollisionObject(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, GridPtr sdf, const Vector3r &scale, const bool testMesh /*= true*/, const bool invertSDF /*= false*/)
  {
  	CubicSDFCollisionDetection::CubicSDFCollisionObject *co = new CubicSDFCollisionDetection::CubicSDFCollisionObject();
  	co->m_bodyIndex = bodyIndex;
  	co->m_bodyType = bodyType;
  	co->m_sdfFile = "";
  	co->m_scale = scale;
  	co->m_sdf = sdf;
  	co->m_bvh.init(vertices, numVertices);
  	co->m_bvh.construct();
  	co->m_testMesh = testMesh;
  	co->m_invertSDF = 1.0;
  	if (invertSDF)
  		co->m_invertSDF = -1.0;
  	m_collisionObjects.push_back(co);
  }
  
  CubicSDFCollisionDetection::CubicSDFCollisionObject::CubicSDFCollisionObject()
  {
  }
  
  CubicSDFCollisionDetection::CubicSDFCollisionObject::~CubicSDFCollisionObject()
  {
  }
  
  double CubicSDFCollisionDetection::CubicSDFCollisionObject::distance(const Eigen::Vector3d &x, const Real tolerance)
  {
  	const Eigen::Vector3d scaled_x = x.cwiseProduct(m_scale.template cast<double>().cwiseInverse());
  	const double dist = m_sdf->interpolate(0, scaled_x);
  	if (dist == std::numeric_limits<double>::max())
  		return dist;
  	return m_invertSDF * m_scale[0]*dist - tolerance;
  }
  
  bool CubicSDFCollisionDetection::CubicSDFCollisionObject::collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist)
  {
  	const Vector3r scaled_x = x.cwiseProduct(m_scale.cwiseInverse());
  
  	Eigen::Vector3d normal;	
  	double d = m_sdf->interpolate(0, scaled_x.template cast<double>(), &normal);
  	if (d == std::numeric_limits<Real>::max())
  		return false;
  	dist = static_cast<Real>(m_invertSDF * d - tolerance);
  
  	normal = m_invertSDF * normal;
  	if (dist < maxDist)
  	{
  		normal.normalize();
  		n = normal.template cast<Real>();
  
  		cp = (scaled_x - dist * n);
  		cp = cp.cwiseProduct(m_scale);
  
  		return true;
  	}
  	return false;
  }
  
  