PositionBasedDynamics/Simulation/CubicSDFCollisionDetection.cpp at python_bindings · InteractiveComputerGraphics/PositionBasedDynamics · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#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;
}