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
|
// 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 <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 pt_on_q; // point of collision on q
Eigen::Vector3d q_n; // face normal
VFCollisionPair();
};
class Collision {
public:
// Obstacle data created in set_obstacles
struct ObstacleData {
Eigen::MatrixXd V0, V1;
Eigen::MatrixXi F;
std::vector<Eigen::AlignedBox<double,3> > aabbs;
AABBTree<double,3> tree;
} obsdata;
struct Settings {
double floor_z;
bool test_floor;
Settings() :
floor_z(-0.5),
// floor_z(-std::numeric_limits<double>::max()),
test_floor(true)
{}
} settings;
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 surface mesh,
// perform discrete collision and create
// a vertex-face collision pair if colliding.
// Also adds collision pairs if below floor.
static void detect_discrete_vf(
const Eigen::Vector3d &pt,
int pt_idx,
bool pt_is_obs,
const AABBTree<double,3> *mesh_tree,
const Eigen::MatrixXd *mesh_x,
const Eigen::MatrixXi *mesh_tris,
bool mesh_is_obs,
std::vector<VFCollisionPair> *pairs);
};
// Collision detection against multiple meshes
class EmbeddedMeshCollision : public Collision {
public:
EmbeddedMeshCollision(const EmbeddedMeshData *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 EmbeddedMeshData *mesh;
// 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); }
};
/*
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_
|
