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
|
// Copyright Matt Overby 2020.
// Distributed under the MIT License.
#include "admmpd_collision.h"
#include "BLI_assert.h"
namespace admmpd {
using namespace Eigen;
void EmbeddedMeshCollision::set_obstacles(
const float *v0,
const float *v1,
int nv,
const int *faces,
int nf)
{
if (obs_V0.rows() != nv)
obs_V0.resize(nv,3);
if (obs_V1.rows() != nv)
obs_V1.resize(nv,3);
for (int i=0; i<nv; ++i)
{
for (int j=0; j<3; ++j)
{
obs_V0(i,j) = v0[i*3+j];
obs_V1(i,j) = v1[i*3+j];
}
}
if (obs_F.rows() != nf)
{
obs_F.resize(nf,3);
obs_aabbs.resize(nf);
}
for (int i=0; i<nf; ++i)
{
obs_aabbs[i].setEmpty();
for (int j=0; j<3; ++j)
{
int fj = faces[i*3+j];
obs_F(i,j) = fj;
obs_aabbs[i].extend(obs_V0.row(fj).transpose());
obs_aabbs[i].extend(obs_V1.row(fj).transpose());
}
}
obs_tree.init(obs_aabbs);
} // end add obstacle
void EmbeddedMeshCollision::detect(const Eigen::MatrixXd *x0, const Eigen::MatrixXd *x1)
{
/*
// First, update the positions of the embedded vertex
// and perform collision detection against obstacles
int n_ev = emb_V0.rows();
std::vector<int> colliding(n_ev,0);
for (int i=0; i<n_ev; ++i)
{
int t = vtx_to_tet->operator[](i);
BLI_assert(t >= 0);
BLI_assert(t < tets->rows());
RowVector4i tet = tets->row(t);
Vector4d bary = emb_barys->row(i);
// emb_V0.row(i) =
// bary[0] * x0->row(tet[0]) +
// bary[1] * x0->row(tet[1]) +
// bary[2] * x0->row(tet[2]) +
// bary[3] * x0->row(tet[3]);
Vector3d pt =
bary[0] * x1->row(tet[0]) +
bary[1] * x1->row(tet[1]) +
bary[2] * x1->row(tet[2]) +
bary[3] * x1->row(tet[3]);
// emb_V1.row(i) =
// Check if we are inside the mesh.
// If so, find the nearest face in the rest pose.
PointInTriangleMeshTraverse<double> pt_in_mesh(pt, &obs_V1, &obs_F);
obs_tree.traverse(pt_in_mesh);
if (pt_in_mesh.output.num_hits() % 2 == 1)
{
// Find
// hit = true;
}
// colliding[i] = hit;
}
// Only bother with self collision if it
// is not colliding with an obstacle.
// This is only useful for discrete tests.
*/
} // end emb collision detect
/*
void FloorCollider::detect(const Eigen::MatrixXd *x)
{
(void)(x);
// Can just do detection in jacobian I guess.
}
void FloorCollider::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)
{
const double floor_z = -2.0;
int nx = x->rows();
for (int i=0; i<nx; ++i)
{
Eigen::Vector3d xi = x->row(i);
if (xi[2]>floor_z)
continue;
int c_idx = l->size();
trips_z->emplace_back(c_idx,i,1.0);
l->emplace_back(floor_z);
}
} // end floor collider Jacobian
*/
} // namespace admmpd
|
