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
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
|
// Copyright Matt Overby 2020.
// Distributed under the MIT License.
#ifndef ADMMPD_COLLISION_H_
#define ADMMPD_COLLISION_H_
#include "admmpd_bvh.h"
#include "admmpd_types.h"
#include "admmpd_embeddedmesh.h"
#include <set>
namespace admmpd {
struct VFCollisionPair {
int p_idx; // point
int p_is_obs; // 0 or 1
int q_idx; // face, or -1 if floor
int q_is_obs; // 0 or 1
Eigen::Vector3d q_pt; // pt of collision (if q obs)
Eigen::Vector3d q_bary; // barys of collision (if q !obs)
VFCollisionPair();
};
class Collision {
public:
struct Settings {
double collision_eps;
double floor_z;
bool test_floor;
bool self_collision;
Settings() :
collision_eps(1e-10),
floor_z(-std::numeric_limits<double>::max()),
test_floor(true),
self_collision(false)
{}
} settings;
struct ObstacleData {
bool has_obs() const { return F.rows()>0; }
Eigen::MatrixXd V;
Eigen::MatrixXi F;
AABBTree<double,3> tree;
std::vector<Eigen::AlignedBox<double,3> > leaves;
} obsdata;
virtual ~Collision() {}
// Performs collision detection.
// Returns the number of active constraints.
virtual int detect(
const Eigen::MatrixXd *x0,
const Eigen::MatrixXd *x1) = 0;
// Appends the per-vertex graph of dependencies
// for constraints (ignores obstacles).
virtual void graph(
std::vector<std::set<int> > &g) = 0;
// Set the soup of obstacles for this time step.
// I don't really like having to switch up interface style, but we'll
// do so here to avoid copies that would happen in admmpd_api.
virtual void set_obstacles(
const float *v0,
const float *v1,
int nv,
const unsigned int *faces,
int nf);
// Special case for floor since it's common.
virtual void set_floor(double z) { settings.floor_z=z; }
virtual double get_floor() const { return settings.floor_z; }
// Linearize the constraints and return Jacobian.
virtual void linearize(
const Eigen::MatrixXd *x,
std::vector<Eigen::Triplet<double> > *trips,
std::vector<double> *d) = 0;
// Given a point and a mesh, perform
// discrete collision detection.
std::pair<bool,VFCollisionPair>
detect_against_obs(
const Eigen::Vector3d &pt,
const ObstacleData *obs) const;
// Given a point and a mesh, perform
// discrete collision detection.
// std::pair<bool,VFCollisionPair>
// detect_point_against_mesh(
// int pt_idx,
// bool pt_is_obs,
// const Eigen::Vector3d &pt,
// bool mesh_is_obs,
// const EmbeddedMesh *emb_mesh,
// const Eigen::MatrixXd *mesh_tets_x,
// const AABBTree<double,3> *mesh_tets_tree) const;
};
// Collision detection against multiple meshes
class EmbeddedMeshCollision : public Collision {
public:
EmbeddedMeshCollision(const EmbeddedMesh *mesh_) : mesh(mesh_){}
// Performs collision detection and stores pairs
int detect(
const Eigen::MatrixXd *x0,
const Eigen::MatrixXd *x1);
void graph(
std::vector<std::set<int> > &g);
// Linearizes the collision pairs about x
// for the constraint Kx=l
void linearize(
const Eigen::MatrixXd *x,
std::vector<Eigen::Triplet<double> > *trips,
std::vector<double> *d);
protected:
// A ptr to the embedded mesh data
const EmbeddedMesh *mesh;
// Pairs are compute on detect
std::vector<Eigen::Vector2i> vf_pairs; // index into per_vertex_pairs
std::vector<std::vector<VFCollisionPair> > per_vertex_pairs;
// Updates the tetmesh BVH for self collisions.
// Called by detect()
// TODO
void update_bvh(
const Eigen::MatrixXd *x0,
const Eigen::MatrixXd *x1);
};
/*
class TetMeshCollision : public Collision {
public:
TetMeshCollision(const TetMeshData *mesh_) :
mesh(mesh_),
floor_z(-std::numeric_limits<double>::max())
{}
// Performs collision detection.
// Returns the number of active constraints.
int detect(
const Eigen::MatrixXd *x0,
const Eigen::MatrixXd *x1);
// Special case for floor since it's common.
void set_floor(double z)
{
floor_z = z;
}
// Linearize the constraints and return Jacobian tensor.
// Constraints are linearized about x for constraint
// K x = l
void jacobian(
const Eigen::MatrixXd *x,
std::vector<Eigen::Triplet<double> > *trips_x,
std::vector<Eigen::Triplet<double> > *trips_y,
std::vector<Eigen::Triplet<double> > *trips_z,
std::vector<double> *l) = 0;
protected:
const TetMeshData *mesh;
double floor_z;
// Pairs are compute on detect
std::vector<VFCollisionPair> vf_pairs;
// Updates the tetmesh BVH for self collisions.
// Called by detect()
// TODO
void update_bvh(
const Eigen::MatrixXd *x0,
const Eigen::MatrixXd *x1)
{ (void)(x0); (void)(x1); }
};
*/
} // namespace admmpd
#endif // ADMMPD_COLLISION_H_
|
