-
Notifications
You must be signed in to change notification settings - Fork 391
Expand file tree
/
Copy pathDistanceFieldCollisionDetection.h
More file actions
171 lines (141 loc) · 7.07 KB
/
Copy pathDistanceFieldCollisionDetection.h
File metadata and controls
171 lines (141 loc) · 7.07 KB
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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
#ifndef _SIMPLECOLLISIONDETECTION_H
#define _SIMPLECOLLISIONDETECTION_H
#include "Common/Common.h"
#include "Simulation/CollisionDetection.h"
#include "AABB.h"
#include "BoundingSphereHierarchy.h"
namespace PBD
{
/** Distance field collision detection. */
class DistanceFieldCollisionDetection : public CollisionDetection
{
public:
struct DistanceFieldCollisionObject : public CollisionObject
{
bool m_testMesh;
Real m_invertSDF;
PointCloudBSH m_bvh;
TetMeshBSH m_bvhTets;
TetMeshBSH m_bvhTets0;
DistanceFieldCollisionObject() { m_testMesh = true; m_invertSDF = 1.0; }
virtual ~DistanceFieldCollisionObject() {}
virtual bool collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist = 0.0);
virtual void approximateNormal(const Eigen::Vector3d &x, const Real tolerance, Vector3r &n);
virtual double distance(const Eigen::Vector3d &x, const Real tolerance) = 0;
void initTetBVH(const Vector3r *vertices, const unsigned int numVertices, const unsigned int *indices, const unsigned int numTets, const Real tolerance);
};
struct DistanceFieldCollisionObjectWithoutGeometry : public DistanceFieldCollisionObject
{
static int TYPE_ID;
virtual ~DistanceFieldCollisionObjectWithoutGeometry() {}
virtual int &getTypeId() const { return TYPE_ID; }
virtual bool collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist = 0.0) { return false; }
virtual double distance(const Eigen::Vector3d &x, const Real tolerance) { return 0.0; }
};
struct DistanceFieldCollisionBox : public DistanceFieldCollisionObject
{
Vector3r m_box;
static int TYPE_ID;
virtual ~DistanceFieldCollisionBox() {}
virtual int &getTypeId() const { return TYPE_ID; }
virtual double distance(const Eigen::Vector3d &x, const Real tolerance);
};
struct DistanceFieldCollisionSphere : public DistanceFieldCollisionObject
{
Real m_radius;
static int TYPE_ID;
virtual ~DistanceFieldCollisionSphere() {}
virtual int &getTypeId() const { return TYPE_ID; }
virtual bool collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist = 0.0);
virtual double distance(const Eigen::Vector3d &x, const Real tolerance);
};
struct DistanceFieldCollisionTorus : public DistanceFieldCollisionObject
{
Vector2r m_radii;
static int TYPE_ID;
virtual ~DistanceFieldCollisionTorus() {}
virtual int &getTypeId() const { return TYPE_ID; }
virtual double distance(const Eigen::Vector3d &x, const Real tolerance);
};
struct DistanceFieldCollisionCylinder : public DistanceFieldCollisionObject
{
Vector2r m_dim;
static int TYPE_ID;
virtual ~DistanceFieldCollisionCylinder() {}
virtual int &getTypeId() const { return TYPE_ID; }
virtual double distance(const Eigen::Vector3d &x, const Real tolerance);
};
struct DistanceFieldCollisionHollowSphere : public DistanceFieldCollisionObject
{
Real m_radius;
Real m_thickness;
static int TYPE_ID;
virtual ~DistanceFieldCollisionHollowSphere() {}
virtual int &getTypeId() const { return TYPE_ID; }
virtual bool collisionTest(const Vector3r &x, const Real tolerance, Vector3r &cp, Vector3r &n, Real &dist, const Real maxDist = 0.0);
virtual double distance(const Eigen::Vector3d &x, const Real tolerance);
};
struct DistanceFieldCollisionHollowBox : public DistanceFieldCollisionObject
{
Vector3r m_box;
Real m_thickness;
static int TYPE_ID;
virtual ~DistanceFieldCollisionHollowBox() {}
virtual int &getTypeId() const { return TYPE_ID; }
virtual double distance(const Eigen::Vector3d &x, const Real tolerance);
};
struct ContactData
{
char m_type;
unsigned int m_index1;
unsigned int m_index2;
Vector3r m_cp1;
Vector3r m_cp2;
Vector3r m_normal;
Real m_dist;
Real m_restitution;
Real m_friction;
// Test
unsigned int m_elementIndex1;
unsigned int m_elementIndex2;
Vector3r m_bary1;
Vector3r m_bary2;
};
protected:
void collisionDetectionRigidBodies(RigidBody *rb1, DistanceFieldCollisionObject *co1, RigidBody *rb2, DistanceFieldCollisionObject *co2,
const Real restitutionCoeff, const Real frictionCoeff
, std::vector<std::vector<ContactData> > &contacts_mt
);
void 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
);
void 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
);
bool findRefTetAt(const ParticleData &pd, TetModel *tm, const DistanceFieldCollisionDetection::DistanceFieldCollisionObject *co, const Vector3r &X,
unsigned int &tetIndex, Vector3r &barycentricCoordinates);
public:
DistanceFieldCollisionDetection();
virtual ~DistanceFieldCollisionDetection();
virtual void collisionDetection(SimulationModel &model);
virtual bool isDistanceFieldCollisionObject(CollisionObject *co) const;
void addCollisionBox(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector3r &box, const bool testMesh = true, const bool invertSDF = false);
void addCollisionSphere(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Real radius, const bool testMesh = true, const bool invertSDF = false);
void addCollisionTorus(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector2r &radii, const bool testMesh = true, const bool invertSDF = false);
void addCollisionObjectWithoutGeometry(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const bool testMesh);
void 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 = true, const bool invertSDF = false);
void 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 = true, const bool invertSDF = false);
/** Add collision cylinder
*
* @param bodyIndex index of corresponding body
* @param bodyType type of corresponding body
* @param dim (radius, height) of cylinder
*/
void addCollisionCylinder(const unsigned int bodyIndex, const unsigned int bodyType, const Vector3r *vertices, const unsigned int numVertices, const Vector2r &dim, const bool testMesh = true, const bool invertSDF = false);
};
}
#endif