DistanceFieldCollisionDetection.cpp

#include "DistanceFieldCollisionDetection.h"
#include "Simulation/IDFactory.h"
#include "omp.h"

using namespace PBD;
using namespace Utilities;

int DistanceFieldCollisionDetection::DistanceFieldCollisionBox::TYPE_ID = IDFactory::getId();
int DistanceFieldCollisionDetection::DistanceFieldCollisionSphere::TYPE_ID = IDFactory::getId();
int DistanceFieldCollisionDetection::DistanceFieldCollisionTorus::TYPE_ID = IDFactory::getId();
int DistanceFieldCollisionDetection::DistanceFieldCollisionCylinder::TYPE_ID = IDFactory::getId();
int DistanceFieldCollisionDetection::DistanceFieldCollisionHollowSphere::TYPE_ID = IDFactory::getId();
int DistanceFieldCollisionDetection::DistanceFieldCollisionHollowBox::TYPE_ID = IDFactory::getId();
int DistanceFieldCollisionDetection::DistanceFieldCollisionObjectWithoutGeometry::TYPE_ID = IDFactory::getId();


DistanceFieldCollisionDetection::DistanceFieldCollisionDetection() :
	CollisionDetection()
{
}

DistanceFieldCollisionDetection::~DistanceFieldCollisionDetection()
{
}

void DistanceFieldCollisionDetection::collisionDetection(SimulationModel &model)
{
	model.resetContacts();
	const SimulationModel::RigidBodyVector &rigidBodies = model.getRigidBodies();
	const SimulationModel::TriangleModelVector &triModels = model.getTriangleModels();
	const SimulationModel::TetModelVector &tetModels = model.getTetModels();
	const ParticleData &pd = model.getParticles();

	std::vector < std::pair<unsigned int, unsigned int>> coPairs;
	for (unsigned int i = 0; i < m_collisionObjects.size(); i++)
	{
		CollisionDetection::CollisionObject *co1 = m_collisionObjects[i];
		for (unsigned int k = 0; k < m_collisionObjects.size(); k++)
		{
			CollisionDetection::CollisionObject *co2 = m_collisionObjects[k];
			if ((i != k))
			{
				// ToDo: self collisions for deformables
				coPairs.push_back({ i, k });
			}
		}
	}

	//omp_set_num_threads(1);
	std::vector<std::vector<ContactData> > contacts_mt;	
#ifdef _DEBUG
	const unsigned int maxThreads = 1;
#else
	const unsigned int maxThreads = omp_get_max_threads();
#endif
	contacts_mt.resize(maxThreads);

	#pragma omp parallel default(shared)
	{
		// Update BVHs
		#pragma omp for schedule(static)  
		for (int i = 0; i < (int)m_collisionObjects.size(); i++)
		{
			CollisionDetection::CollisionObject *co = m_collisionObjects[i];
			updateAABB(model, co);
			if (isDistanceFieldCollisionObject(co))
			{
				if (co->m_bodyType == CollisionDetection::CollisionObject::TriangleModelCollisionObjectType) 
				{
					DistanceFieldCollisionObject *sco = (DistanceFieldCollisionObject*)co;

					TriangleModel* tm = triModels[co->m_bodyIndex];
					const unsigned int offset = tm->getIndexOffset();
					const IndexedFaceMesh& mesh = tm->getParticleMesh();
					const unsigned int numVert = mesh.numVertices();
					sco->m_bvh.init(&pd.getPosition(offset), numVert);
					sco->m_bvh.update();
				}
				else if (co->m_bodyType == CollisionDetection::CollisionObject::TetModelCollisionObjectType)
				{
					TetModel* tm = tetModels[co->m_bodyIndex];
					const unsigned int offset = tm->getIndexOffset();
					const IndexedTetMesh& mesh = tm->getParticleMesh();
					const unsigned int numVert = mesh.numVertices();

					DistanceFieldCollisionObject *sco = (DistanceFieldCollisionObject*)co;
					sco->m_bvh.init(&pd.getPosition(offset), numVert);
					sco->m_bvhTets.updateVertices(&pd.getPosition(offset));
					sco->m_bvhTets0.updateVertices(&pd.getPosition(offset));

					sco->m_bvh.update();
					sco->m_bvhTets.update();
				}
			}
		}

		#pragma omp for schedule(static)
		for (int i = 0; i < (int)coPairs.size(); i++)
		{
			std::pair<unsigned int, unsigned int> &coPair = coPairs[i];
			CollisionDetection::CollisionObject *co1 = m_collisionObjects[coPair.first];
			CollisionDetection::CollisionObject *co2 = m_collisionObjects[coPair.second];

			if (((co2->m_bodyType != CollisionDetection::CollisionObject::RigidBodyCollisionObjectType) &&
				(co2->m_bodyType != CollisionDetection::CollisionObject::TetModelCollisionObjectType)) ||
				!isDistanceFieldCollisionObject(co1) ||
				!isDistanceFieldCollisionObject(co2) ||
				!AABB::intersection(co1->m_aabb, co2->m_aabb))
				continue;


			if ((co1->m_bodyType == CollisionDetection::CollisionObject::RigidBodyCollisionObjectType) &&
				(co2->m_bodyType == CollisionDetection::CollisionObject::RigidBodyCollisionObjectType) &&
				((DistanceFieldCollisionObject*) co1)->m_testMesh)
			{
				RigidBody *rb1 = rigidBodies[co1->m_bodyIndex];				
				RigidBody *rb2 = rigidBodies[co2->m_bodyIndex];
				const Real restitutionCoeff = rb1->getRestitutionCoeff() * rb2->getRestitutionCoeff();
				const Real frictionCoeff = rb1->getFrictionCoeff() + rb2->getFrictionCoeff();
				collisionDetectionRigidBodies(rb1, (DistanceFieldCollisionObject*)co1, rb2, (DistanceFieldCollisionObject*)co2,
					restitutionCoeff, frictionCoeff
					, contacts_mt
					);
			}
			else if ((co1->m_bodyType == CollisionDetection::CollisionObject::TriangleModelCollisionObjectType) &&
					(co2->m_bodyType == CollisionDetection::CollisionObject::RigidBodyCollisionObjectType) &&
					((DistanceFieldCollisionObject*)co1)->m_testMesh)
			{
				TriangleModel *tm = triModels[co1->m_bodyIndex];
				RigidBody *rb2 = rigidBodies[co2->m_bodyIndex];
				const unsigned int offset = tm->getIndexOffset();
				const IndexedFaceMesh &mesh = tm->getParticleMesh();
				const unsigned int numVert = mesh.numVertices();
				const Real restitutionCoeff = tm->getRestitutionCoeff() * rb2->getRestitutionCoeff();
				const Real frictionCoeff = tm->getFrictionCoeff() + rb2->getFrictionCoeff();
				collisionDetectionRBSolid(pd, offset, numVert, (DistanceFieldCollisionObject*)co1, rb2, (DistanceFieldCollisionObject*)co2,
					restitutionCoeff, frictionCoeff
					, contacts_mt
					);
			}
			else if ((co1->m_bodyType == CollisionDetection::CollisionObject::TetModelCollisionObjectType) && 
					(co2->m_bodyType == CollisionDetection::CollisionObject::RigidBodyCollisionObjectType) &&
					((DistanceFieldCollisionObject*)co1)->m_testMesh)
			{
				TetModel *tm = tetModels[co1->m_bodyIndex];
				RigidBody *rb2 = rigidBodies[co2->m_bodyIndex];
				const unsigned int offset = tm->getIndexOffset();
				const IndexedTetMesh &mesh = tm->getParticleMesh();
				const unsigned int numVert = mesh.numVertices();
				const Real restitutionCoeff = tm->getRestitutionCoeff() * rb2->getRestitutionCoeff();
				const Real frictionCoeff = tm->getFrictionCoeff() + rb2->getFrictionCoeff();
				collisionDetectionRBSolid(pd, offset, numVert, (DistanceFieldCollisionObject*)co1, rb2, (DistanceFieldCollisionObject*)co2,
					restitutionCoeff, frictionCoeff
					, contacts_mt
					);
			}
 			else if ((co1->m_bodyType == CollisionDetection::CollisionObject::TetModelCollisionObjectType) &&
 				(co2->m_bodyType == CollisionDetection::CollisionObject::TetModelCollisionObjectType) &&
 				((DistanceFieldCollisionObject*)co1)->m_testMesh)
 			{
 				TetModel *tm1 = tetModels[co1->m_bodyIndex];
 				TetModel *tm2 = tetModels[co2->m_bodyIndex];
 				const unsigned int offset = tm1->getIndexOffset();
 				const IndexedTetMesh &mesh = tm1->getParticleMesh();
 				const unsigned int numVert = mesh.numVertices();
 				const Real restitutionCoeff = tm1->getRestitutionCoeff() * tm2->getRestitutionCoeff();
 				const Real frictionCoeff = tm1->getFrictionCoeff() + tm2->getFrictionCoeff();
 				collisionDetectionSolidSolid(pd, offset, numVert, (DistanceFieldCollisionObject*)co1, tm2, (DistanceFieldCollisionObject*)co2,
 					restitutionCoeff, frictionCoeff
 					, contacts_mt
 				);
 			}
		}
	}

	for (unsigned int i = 0; i < contacts_mt.size(); i++)
	{
		for (unsigned int j = 0; j < contacts_mt[i].size(); j++)
		{
			if (contacts_mt[i][j].m_type == 1)
				addParticleRigidBodyContact(contacts_mt[i][j].m_index1, contacts_mt[i][j].m_index2,
					contacts_mt[i][j].m_cp1, contacts_mt[i][j].m_cp2, contacts_mt[i][j].m_normal,
					contacts_mt[i][j].m_dist, contacts_mt[i][j].m_restitution, contacts_mt[i][j].m_friction);
			else if (contacts_mt[i][j].m_type == 0)
				addRigidBodyContact(contacts_mt[i][j].m_index1, contacts_mt[i][j].m_index2,
					contacts_mt[i][j].m_cp1, contacts_mt[i][j].m_cp2, contacts_mt[i][j].m_normal,
					contacts_mt[i][j].m_dist, contacts_mt[i][j].m_restitution, contacts_mt[i][j].m_friction);
			else if (contacts_mt[i][j].m_type == 2)
			{
				addParticleSolidContact(contacts_mt[i][j].m_index1, contacts_mt[i][j].m_index2,
					contacts_mt[i][j].m_elementIndex2, contacts_mt[i][j].m_bary2,
					contacts_mt[i][j].m_cp1, contacts_mt[i][j].m_cp2, contacts_mt[i][j].m_normal,					
					contacts_mt[i][j].m_dist, contacts_mt[i][j].m_restitution, contacts_mt[i][j].m_friction);
			}
		}
	}
}

void DistanceFieldCollisionDetection::collisionDetectionRigidBodies(RigidBody *rb1, DistanceFieldCollisionObject *co1, RigidBody *rb2, DistanceFieldCollisionObject *co2, 
	const Real restitutionCoeff, const Real frictionCoeff
	, std::vector<std::vector<ContactData> > &contacts_mt
	)
{
	if ((rb1->getMass() == 0.0) && (rb2->getMass() == 0.0))
		return;

	const VertexData &vd = rb1->getGeometry().getVertexData();

	const Vector3r &com2 = rb2->getPosition();

	// remove the rotation of the main axis transformation that is performed
	// to get a diagonal inertia tensor since the distance function is 
	// evaluated in local coordinates
	//
	// transformation world to local:
	// p_local = R_initial^T ( R_MAT R^T (p_world - x) - x_initial + x_MAT)
	// 
	// transformation local to:
	// p_world = R R_MAT^T (R_initial p_local + x_initial - x_MAT) + x
	//
	const Matrix3r &R = rb2->getTransformationR();
	const Vector3r &v1 = rb2->getTransformationV1();
	const Vector3r &v2 = rb2->getTransformationV2();

	const PointCloudBSH &bvh = ((DistanceFieldCollisionDetection::DistanceFieldCollisionObject*) co1)->m_bvh;
	std::function<bool(unsigned int, unsigned int)> predicate = [&](unsigned int node_index, unsigned int depth)
	{
		const BoundingSphere &bs = bvh.hull(node_index);
		const Vector3r &sphere_x = bs.x();
		const Vector3r sphere_x_w = rb1->getRotation() * sphere_x + rb1->getPosition();

		AlignedBox3r box3f;
		box3f.extend(co2->m_aabb.m_p[0]);
		box3f.extend(co2->m_aabb.m_p[1]);
		const Real dist = box3f.exteriorDistance(sphere_x_w);

		// Test if center of bounding sphere intersects AABB
		if (dist < bs.r())
		{
			// Test if distance of center of bounding sphere to collision object is smaller than the radius
			const Vector3r x = R * (sphere_x_w - com2) + v1;
			const double dist2 = co2->distance(x.template cast<double>(), m_tolerance);
			if (dist2 == std::numeric_limits<double>::max())
				return true;
			if (dist2 < bs.r())
				return true;
		}
		return false;
	};
	std::function<void(unsigned int, unsigned int)> cb = [&](unsigned int node_index, unsigned int depth)
	{
		auto const& node = bvh.node(node_index);
		if (!node.is_leaf())
			return;

		for (auto i = node.begin; i < node.begin + node.n; ++i)
		{
			unsigned int index = bvh.entity(i);
			const Vector3r &x_w = vd.getPosition(index);
			const Vector3r x = R * (x_w - com2) + v1;
			Vector3r cp, n;
			Real dist;
			if (co2->collisionTest(x, m_tolerance, cp, n, dist))
			{
				const Vector3r cp_w = R.transpose() * cp + v2;
				const Vector3r n_w = R.transpose() * n;

#ifdef _DEBUG
				int tid = 0;
#else
				int tid = omp_get_thread_num();
#endif			

				contacts_mt[tid].push_back({ 0, co1->m_bodyIndex, co2->m_bodyIndex, x_w, cp_w, n_w, dist, restitutionCoeff, frictionCoeff });
			}
		}
	};
	bvh.traverse_depth_first(predicate, cb);
}


void DistanceFieldCollisionDetection::collisionDetectionRBSolid(const ParticleData &pd, const unsigned int offset, const unsigned int numVert,
	DistanceFieldCollisionObject *co1, RigidBody *rb2, DistanceFieldCollisionObject *co2, 
	const Real restitutionCoeff, const Real frictionCoeff
	, std::vector<std::vector<ContactData> > &contacts_mt
	)
{
	const Vector3r &com2 = rb2->getPosition();

	// remove the rotation of the main axis transformation that is performed
	// to get a diagonal inertia tensor since the distance function is 
	// evaluated in local coordinates
	//
	// transformation world to local:
	// p_local = R_initial^T ( R_MAT R^T (p_world - x) - x_initial + x_MAT)
	// 
	// transformation local to:
	// p_world = R R_MAT^T (R_initial p_local + x_initial - x_MAT) + x
	//
	const Matrix3r &R = rb2->getTransformationR();
	const Vector3r &v1 = rb2->getTransformationV1();
	const Vector3r &v2 = rb2->getTransformationV2();

	const PointCloudBSH &bvh = ((DistanceFieldCollisionDetection::DistanceFieldCollisionObject*) co1)->m_bvh;

	std::function<bool(unsigned int, unsigned int)> predicate = [&](unsigned int node_index, unsigned int depth)
	{
		const BoundingSphere &bs = bvh.hull(node_index);
		const Vector3r &sphere_x_w = bs.x();

		AlignedBox3r box3f;
		box3f.extend(co2->m_aabb.m_p[0]);
		box3f.extend(co2->m_aabb.m_p[1]);
		const Real dist = box3f.exteriorDistance(sphere_x_w);

		// Test if center of bounding sphere intersects AABB
		if (dist < bs.r())
		{
			// Test if distance of center of bounding sphere to collision object is smaller than the radius
			const Vector3r x = R * (sphere_x_w - com2) + v1;
			const double dist2 = co2->distance(x.template cast<double>(), m_tolerance);
			if (dist2 == std::numeric_limits<double>::max())
				return true;
			if (dist2 < bs.r())
				return true;
		}
		return false;
	};

	std::function<void(unsigned int, unsigned int)> cb = [&](unsigned int node_index, unsigned int depth)
	{
		auto const& node = bvh.node(node_index);
		if (!node.is_leaf())
			return;

		for (auto i = node.begin; i < node.begin + node.n; ++i)
		{
			unsigned int index = bvh.entity(i) + offset;
			const Vector3r &x_w = pd.getPosition(index);
			const Vector3r x = R * (x_w - com2) + v1;
			Vector3r cp, n;
			Real dist;
			if (co2->collisionTest(x, m_tolerance, cp, n, dist))
			{
				const Vector3r cp_w = R.transpose() * cp + v2;
				const Vector3r n_w = R.transpose() * n;

#ifdef _DEBUG
				int tid = 0;
#else
				int tid = omp_get_thread_num();
#endif			
				contacts_mt[tid].push_back({ 1, index, co2->m_bodyIndex, x_w, cp_w, n_w, dist, restitutionCoeff, frictionCoeff });
			}
		}
	};

	bvh.traverse_depth_first(predicate, cb);
}

void DistanceFieldCollisionDetection::collisionDetectionSolidSolid(const ParticleData &pd, const unsigned int offset, const unsigned int numVert,
	DistanceFieldCollisionObject *co1, TetModel *tm2, DistanceFieldCollisionObject *co2,
	const Real restitutionCoeff, const Real frictionCoeff
	, std::vector<std::vector<ContactData> > &contacts_mt
)
{
	const PointCloudBSH &bvh1 = ((DistanceFieldCollisionDetection::DistanceFieldCollisionObject*) co1)->m_bvh;
	const TetMeshBSH &bvh2 = ((DistanceFieldCollisionDetection::DistanceFieldCollisionObject*) co2)->m_bvhTets;
	const unsigned int *indices = tm2->getParticleMesh().getTets().data();
	const unsigned int offset2 = tm2->getIndexOffset();


	// callback function for BVH which is called if a leaf node in the point cloud BVH
	// has a collision with a leaf node in the tet BVH
	std::function<void(unsigned int, unsigned int)> cb = [&](unsigned int node_index1, unsigned int node_index2)
	{
		auto const& node1 = bvh1.node(node_index1);
		auto const& node2 = bvh2.node(node_index2);

		// loop over all primitives (points, tets) in the leaf nodes
		for (auto i = node1.begin; i < node1.begin + node1.n; ++i)
		{
			for (auto j = node2.begin; j < node2.begin + node2.n; ++j)
			{
				// Get sample point
				unsigned int index = bvh1.entity(i) + offset;
				const Vector3r &x_w = pd.getPosition(index);

				// Get tet
				const unsigned int tetIndex = bvh2.entity(j);
				const Vector3r &x0 = pd.getPosition(indices[4 * tetIndex] + offset2);
				const Vector3r &x1 = pd.getPosition(indices[4 * tetIndex + 1] + offset2);
				const Vector3r &x2 = pd.getPosition(indices[4 * tetIndex + 2] + offset2);
				const Vector3r &x3 = pd.getPosition(indices[4 * tetIndex + 3] + offset2);

				// Compute barycentric coordinates of point in tet
				Matrix3r A;
				A.col(0) = x1 - x0;
				A.col(1) = x2 - x0;
				A.col(2) = x3 - x0;
				Vector3r bary = A.inverse() * (x_w - x0);

				// check if point lies in tet using barycentric coordinates
 				if ((bary[0] >= 0.0) && (bary[1] >= 0.0) && (bary[2] >= 0.0) && 
 					(bary[0] + bary[1] + bary[2] <= 1.0))
				{
					// use barycentric coordinates to determine position of the point in the reference space of the tet
 					const Vector3r &X0 = pd.getPosition0(indices[4 * tetIndex] + offset2);
					const Vector3r &X1 = pd.getPosition0(indices[4 * tetIndex + 1] + offset2);
					const Vector3r &X2 = pd.getPosition0(indices[4 * tetIndex + 2] + offset2);
					const Vector3r &X3 = pd.getPosition0(indices[4 * tetIndex + 3] + offset2);

					Matrix3r A0;
					A0.col(0) = X1 - X0;
					A0.col(1) = X2 - X0;
					A0.col(2) = X3 - X0;

					// point in reference space of the tet
					const Vector3r X = X0 + A0 * bary;

					Vector3r cp_l, n_l;
					Real dist;

					// apply inverse initial transform to transform the point in the space of the 
					// signed distance field
					const Vector3r X_l = (tm2->getInitialR().transpose() * (X - tm2->getInitialX()));

					// perform collision test with distance field to get closest point on surface
					//if (co2->collisionTest(X_l, m_tolerance, cp_l, n_l, dist))
					if (co2->collisionTest(X_l, 0.0, cp_l, n_l, dist))
					{
						unsigned int cp_tetIndex; 
						Vector3r cp_bary;

						// transform closest point on surface back to the reference space of the tet model
						const Vector3r cp0 = (tm2->getInitialR() * cp_l + tm2->getInitialX());

						// find the tet which contains the resulting point
						if (findRefTetAt(pd, tm2, co2, cp0, cp_tetIndex, cp_bary))
						{
							// if we are in another tet, update matrix A
							Vector3r cp_w;
							if (cp_tetIndex != tetIndex)
							{
								const Vector3r &x0 = pd.getPosition(indices[4 * cp_tetIndex] + offset2);
								const Vector3r &x1 = pd.getPosition(indices[4 * cp_tetIndex + 1] + offset2);
								const Vector3r &x2 = pd.getPosition(indices[4 * cp_tetIndex + 2] + offset2);
								const Vector3r &x3 = pd.getPosition(indices[4 * cp_tetIndex + 3] + offset2);
								A.col(0) = x1 - x0;
								A.col(1) = x2 - x0;
								A.col(2) = x3 - x0;
								
								// compute world space contact point in body 2	
								cp_w = x0 + A * cp_bary;
							}
							else 
								// compute world space contact point in body 2	
								cp_w = x0 + A * cp_bary;							

#ifdef _DEBUG
							int tid = 0;
#else
							int tid = omp_get_thread_num();
#endif	

							Vector3r n_w = cp_w - x_w;

							// normalize normal vector
	 						const Real dist = (x_w - cp_w).norm();
							if (dist > 1.0e-6)
								n_w /= dist;

	 						contacts_mt[tid].push_back({ 2, index, co2->m_bodyIndex, x_w, cp_w, n_w, dist, restitutionCoeff, frictionCoeff, tetIndex, cp_tetIndex, bary, cp_bary });
						}
					}
				}
			}
		}
	};

	BVHTest::traverse(bvh1, bvh2, cb);

}

bool DistanceFieldCollisionDetection::isDistanceFieldCollisionObject(CollisionObject *co) const
{
	return (co->getTypeId() == DistanceFieldCollisionDetection::DistanceFieldCollisionBox::TYPE_ID) ||
		(co->getTypeId() == DistanceFieldCollisionDetection::DistanceFieldCollisionSphere::TYPE_ID) ||
		(co->getTypeId() == DistanceFieldCollisionDetection::DistanceFieldCollisionTorus::TYPE_ID) ||
		(co->getTypeId() == DistanceFieldCollisionDetection::DistanceFieldCollisionCylinder::TYPE_ID) ||
		(co->getTypeId() == DistanceFieldCollisionDetection::DistanceFieldCollisionHollowSphere::TYPE_ID) ||
		(co->getTypeId() == DistanceFieldCollisionDetection::DistanceFieldCollisionHollowBox::TYPE_ID) ||
		(co->getTypeId() == DistanceFieldCollisionDetection::DistanceFieldCollisionObjectWithoutGeometry::TYPE_ID);
}

void DistanceFieldCollisionDetection::addCollisionBox(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector3r &box, const bool testMesh, const bool invertSDF)
{
	DistanceFieldCollisionDetection::DistanceFieldCollisionBox *cf = new DistanceFieldCollisionDetection::DistanceFieldCollisionBox();
	cf->m_bodyIndex = bodyIndex;
	cf->m_bodyType = bodyType;
	// distance function requires 0.5*box 
	cf->m_box = 0.5*box;
	cf->m_bvh.init(vertices, numVertices);
	cf->m_bvh.construct();
	cf->m_testMesh = testMesh;
	if (invertSDF)
		cf->m_invertSDF = -1.0;
	m_collisionObjects.push_back(cf);
}

void DistanceFieldCollisionDetection::addCollisionSphere(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Real radius, const bool testMesh, const bool invertSDF)
{
	DistanceFieldCollisionDetection::DistanceFieldCollisionSphere *cs = new DistanceFieldCollisionDetection::DistanceFieldCollisionSphere();
	cs->m_bodyIndex = bodyIndex;
	cs->m_bodyType = bodyType;
	cs->m_radius = radius;
	cs->m_bvh.init(vertices, numVertices);
	cs->m_bvh.construct();
	cs->m_testMesh = testMesh;
	if (invertSDF)
		cs->m_invertSDF = -1.0;
	m_collisionObjects.push_back(cs);
}

void DistanceFieldCollisionDetection::addCollisionTorus(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector2r &radii, const bool testMesh, const bool invertSDF)
{
	DistanceFieldCollisionDetection::DistanceFieldCollisionTorus *ct = new DistanceFieldCollisionDetection::DistanceFieldCollisionTorus();
	ct->m_bodyIndex = bodyIndex;
	ct->m_bodyType = bodyType;
	ct->m_radii = radii;
	ct->m_bvh.init(vertices, numVertices);
	ct->m_bvh.construct();
	ct->m_testMesh = testMesh;
	if (invertSDF)
		ct->m_invertSDF = -1.0;
	m_collisionObjects.push_back(ct);
}

void DistanceFieldCollisionDetection::addCollisionCylinder(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector2r &dim, const bool testMesh, const bool invertSDF)
{
	DistanceFieldCollisionDetection::DistanceFieldCollisionCylinder *ct = new DistanceFieldCollisionDetection::DistanceFieldCollisionCylinder();
	ct->m_bodyIndex = bodyIndex;
	ct->m_bodyType = bodyType;
	ct->m_dim = dim;
	// distance function uses height/2
	ct->m_dim[1] *= 0.5;
	ct->m_bvh.init(vertices, numVertices);
	ct->m_bvh.construct();
	ct->m_testMesh = testMesh;
	if (invertSDF)
		ct->m_invertSDF = -1.0;
	m_collisionObjects.push_back(ct);
}

void DistanceFieldCollisionDetection::addCollisionHollowSphere(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Real radius, const Real thickness, const bool testMesh, const bool invertSDF)
{
	DistanceFieldCollisionDetection::DistanceFieldCollisionHollowSphere *cs = new DistanceFieldCollisionDetection::DistanceFieldCollisionHollowSphere();
	cs->m_bodyIndex = bodyIndex;
	cs->m_bodyType = bodyType;
	cs->m_radius = radius;
	cs->m_thickness = thickness;
	cs->m_bvh.init(vertices, numVertices);
	cs->m_bvh.construct();
	cs->m_testMesh = testMesh;
	if (invertSDF)
		cs->m_invertSDF = -1.0;
	m_collisionObjects.push_back(cs);
}

void DistanceFieldCollisionDetection::addCollisionHollowBox(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector3r &box, const Real thickness, const bool testMesh, const bool invertSDF)
{
	DistanceFieldCollisionDetection::DistanceFieldCollisionHollowBox *cf = new DistanceFieldCollisionDetection::DistanceFieldCollisionHollowBox();
	cf->m_bodyIndex = bodyIndex;
	cf->m_bodyType = bodyType;
	// distance function requires 0.5*box 
	cf->m_box = 0.5*box;
	cf->m_thickness = thickness;
	cf->m_bvh.init(vertices, numVertices);
	cf->m_bvh.construct();
	cf->m_testMesh = testMesh;
	if (invertSDF)
		cf->m_invertSDF = -1.0;
	m_collisionObjects.push_back(cf);
}

void DistanceFieldCollisionDetection::addCollisionObjectWithoutGeometry(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const bool testMesh)
{
	DistanceFieldCollisionObjectWithoutGeometry *co = new DistanceFieldCollisionObjectWithoutGeometry();
	co->m_bodyIndex = bodyIndex;
	co->m_bodyType = bodyType;
	co->m_bvh.init(vertices, numVertices);
	co->m_bvh.construct();
	co->m_testMesh = testMesh;
	co->m_invertSDF = 1.0;
	m_collisionObjects.push_back(co);
}

double DistanceFieldCollisionDetection::DistanceFieldCollisionBox::distance(const Eigen::Vector3d &x, const Real tolerance)
{
	const Eigen::Vector3d box_d = m_box.template cast<double>();
	const Eigen::Vector3d x_d = x.template cast<double>();
	const Eigen::Vector3d d(fabs(x_d.x()) - box_d.x(), fabs(x_d.y()) - box_d.y(), fabs(x_d.z()) - box_d.z());
	const Eigen::Vector3d max_d(std::max(d.x(), 0.0), std::max(d.y(), 0.0), std::max(d.z(), 0.0));
	return m_invertSDF*(std::min(std::max(d.x(), std::max(d.y(), d.z())), 0.0) + max_d.norm()) - static_cast<double>(tolerance);
}

double DistanceFieldCollisionDetection::DistanceFieldCollisionSphere::distance(const Eigen::Vector3d &x, const Real tolerance)
{
	const Eigen::Vector3d d = x.template cast<double>();
	const double dl = d.norm();
	return m_invertSDF*(dl - static_cast<double>(m_radius)) - static_cast<double>(tolerance);
}

bool DistanceFieldCollisionDetection::DistanceFieldCollisionSphere::collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist)
{
	const Vector3r d = x;
	const Real dl = d.norm();
	dist = m_invertSDF*(dl - m_radius) - tolerance;
	if (dist < maxDist)
	{
		if (dl < 1.e-6)
			n.setZero();
		else
			n = m_invertSDF * d / dl;

		cp = ((m_radius+tolerance) * n);
		return true;
	}
	return false;
}

double DistanceFieldCollisionDetection::DistanceFieldCollisionTorus::distance(const Eigen::Vector3d &x, const Real tolerance)
{
	const Eigen::Vector2d radii_d = m_radii.template cast<double>();
	const Eigen::Vector2d q(Vector2r(x.x(), x.z()).norm() - radii_d.x(), x.y());
	return m_invertSDF*(q.norm() - radii_d.y()) - tolerance;
}

double DistanceFieldCollisionDetection::DistanceFieldCollisionCylinder::distance(const Eigen::Vector3d &x, const Real tolerance)
{
	const double l = sqrt(x.x()*x.x() + x.z()*x.z());
	const Eigen::Vector2d d = Eigen::Vector2d(fabs(l), fabs(x.y())) - m_dim.template cast<double>();
	const Eigen::Vector2d max_d(std::max(d.x(), 0.0), std::max(d.y(), 0.0));
	return m_invertSDF*(std::min(std::max(d.x(), d.y()), 0.0) + max_d.norm()) - static_cast<double>(tolerance);
}


double DistanceFieldCollisionDetection::DistanceFieldCollisionHollowSphere::distance(const Eigen::Vector3d &x, const Real tolerance)
{
	const Eigen::Vector3d d = x.template cast<double>();
	const double dl = d.norm();
	return m_invertSDF*(fabs(dl - static_cast<double>(m_radius)) - static_cast<double>(m_thickness)) - static_cast<double>(tolerance);
}

bool DistanceFieldCollisionDetection::DistanceFieldCollisionHollowSphere::collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist)
{
	const Vector3r d = x;
	const Real dl = d.norm();
	dist = m_invertSDF*(fabs(dl - m_radius) - m_thickness) - tolerance;
	if (dist < maxDist)
	{
		if (dl < 1.e-6)
			n.setZero();
		else if (dl < m_radius)
			n = -m_invertSDF*d / dl;
		else
			n = m_invertSDF*d / dl;

		cp = x - dist * n;
		return true;
	}
	return false;
}

double DistanceFieldCollisionDetection::DistanceFieldCollisionHollowBox::distance(const Eigen::Vector3d &x, const Real tolerance)
{
	const Eigen::Vector3d box_d = m_box.template cast<double>();
	const Eigen::Vector3d x_d = x.template cast<double>();
	const Eigen::Vector3d d = x_d.cwiseAbs() - box_d;
	const Eigen::Vector3d max_d = d.cwiseMax(Eigen::Vector3d(0.0, 0.0, 0.0));
	return m_invertSDF * (fabs(std::min(d.maxCoeff(), 0.0) + max_d.norm()) - m_thickness) - static_cast<double>(tolerance);
}

void DistanceFieldCollisionDetection::DistanceFieldCollisionObject::approximateNormal(const Eigen::Vector3d &x, const Real tolerance, Vector3r &n)
{
	// approximate gradient
	double eps = 1.e-6;
	n.setZero();
	Eigen::Vector3d xTmp = x;
	for (unsigned int j = 0; j < 3; j++)
	{
		xTmp[j] += eps;

		double e_p, e_m;
		e_p = distance(xTmp, tolerance);
		xTmp[j] = x[j] - eps;
		e_m = distance(xTmp, tolerance);
		xTmp[j] = x[j];

		double res = (e_p - e_m) * (1.0 / (2.0*eps));

		n[j] = static_cast<Real>(res);
	}

	const Real norm2 = n.squaredNorm();
	if (norm2 < 1.e-6)
		n.setZero();
	else
		n = n / sqrt(norm2);
}


bool DistanceFieldCollisionDetection::DistanceFieldCollisionObject::collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist)
{
	const Real t_d = static_cast<Real>(tolerance);
	dist = static_cast<Real>(distance(x.template cast<double>(), t_d));
	if (dist < maxDist)
	{
		// approximate gradient
		const Eigen::Vector3d x_d = x.template cast<double>();

		approximateNormal(x_d, t_d, n);

		cp = (x - dist * n);
		return true;
	}
	return false;
}

void DistanceFieldCollisionDetection::DistanceFieldCollisionObject::initTetBVH(const Vector3r *vertices, const unsigned int numVertices, const unsigned int *indices, const unsigned int numTets, const Real tolerance)
{
	if (m_bodyType == CollisionDetection::CollisionObject::TetModelCollisionObjectType)
	{
		m_bvhTets.init(vertices, numVertices, indices, numTets, tolerance);
		m_bvhTets.construct();

		// ToDo: copy constructor
		m_bvhTets0.init(vertices, numVertices, indices, numTets, 0.0);
		m_bvhTets0.construct();

	}
}

bool DistanceFieldCollisionDetection::findRefTetAt(const ParticleData &pd, TetModel *tm, const DistanceFieldCollisionDetection::DistanceFieldCollisionObject *co, const Vector3r &X,
	unsigned int &tetIndex, Vector3r &barycentricCoordinates)
{
 	const TetMeshBSH &bvh0 = ((DistanceFieldCollisionDetection::DistanceFieldCollisionObject*) co)->m_bvhTets0;
 	const unsigned int *indices = tm->getParticleMesh().getTets().data();
 	const unsigned int offset = tm->getIndexOffset();
	std::vector<Vector3r> bary;
	std::vector<unsigned int> tets;
	bary.reserve(100);
	tets.reserve(100);

	std::function<bool(unsigned int, unsigned int)> predicate = [&](unsigned int node_index, unsigned int depth)
 	{
 		const BoundingSphere &bs = bvh0.hull(node_index);
		return bs.contains(X);
 	};
 	std::function<void(unsigned int, unsigned int)> cb = [&](unsigned int node_index, unsigned int depth)
 	{
 		auto const& node = bvh0.node(node_index);
 		if (!node.is_leaf())
 			return;
 
 		for (auto i = node.begin; i < node.begin + node.n; ++i)
 		{
			const unsigned int tetIndex = bvh0.entity(i);

 			// use barycentric coordinates to determine position in reference space
 			const Vector3r &X0 = pd.getPosition0(indices[4 * tetIndex] + offset);
 			const Vector3r &X1 = pd.getPosition0(indices[4 * tetIndex + 1] + offset);
 			const Vector3r &X2 = pd.getPosition0(indices[4 * tetIndex + 2] + offset);
 			const Vector3r &X3 = pd.getPosition0(indices[4 * tetIndex + 3] + offset);

 			// Compute barycentric coordinates of point in tet
 			Matrix3r A;
			A.col(0) = X1 - X0;
			A.col(1) = X2 - X0;
			A.col(2) = X3 - X0;
 			bary.push_back(A.inverse() * (X - X0));
			tets.push_back(tetIndex);
 		}
 	};

 	bvh0.traverse_depth_first(predicate, cb);

	if (bary.size() == 0)
		return false;

	// find best set of barycentric coordinates
	unsigned int index = 0;
	Real minError = REAL_MAX;
	for (unsigned int i = 0; i < bary.size(); i++)
	{
		// Determine if barycentric coordinates are negative and add distance to 0 as error
		Real error = std::max(static_cast<Real>(0.0), -bary[i][0]);
		error += std::max(static_cast<Real>(0.0), -bary[i][1]);
		error += std::max(static_cast<Real>(0.0), -bary[i][2]);

		// Determine if sum of barycentric coordinates is larger than one and add distance to 1 as error
		error += std::max(static_cast<Real>(0.0), bary[i][0] + bary[i][1] + bary[i][2] - static_cast<Real>(1.0));

		if (error < minError)
		{
			minError = error;
			index = i;
		}
	}
	barycentricCoordinates = bary[index];
	tetIndex = tets[index];
	return true;
}