-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
deformable_contact.cc
145 lines (129 loc) · 5.61 KB
/
deformable_contact.cc
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
#include "drake/geometry/query_results/deformable_contact.h"
#include <utility>
namespace drake {
namespace geometry {
namespace internal {
using multibody::contact_solvers::internal::PartialPermutation;
namespace {
// TODO(xuchenhan-tri): Consider moving this function to a header file for full
// unit testing.
/* Extends a partial permutation to a full permutation.
@pre permutation != nullptr. */
void ExtendToFullPermutation(PartialPermutation* permutation) {
for (int i = 0; i < static_cast<int>(permutation->domain_size()); ++i) {
/* The call to permutation.push() only conditionally adds i. */
permutation->push(i);
}
}
} // namespace
ContactParticipation::ContactParticipation(int num_vertices)
: participation_(num_vertices, false) {}
void ContactParticipation::Participate(
const std::unordered_set<int>& vertices) {
for (int v : vertices) {
DRAKE_DEMAND(0 <= v && v < static_cast<int>(participation_.size()));
if (!participation_[v]) {
++num_vertices_in_contact_;
participation_[v] = true;
}
}
}
PartialPermutation ContactParticipation::CalcVertexPermutation() const {
/* Build the partial permutation. */
PartialPermutation permutation = CalcVertexPartialPermutation();
ExtendToFullPermutation(&permutation);
return permutation;
}
PartialPermutation ContactParticipation::CalcVertexPartialPermutation() const {
int permuted_vertex_index = 0;
std::vector<int> permuted_vertex_indexes(participation_.size(), -1);
for (int v = 0; v < static_cast<int>(participation_.size()); ++v) {
if (participation_[v]) {
permuted_vertex_indexes[v] = permuted_vertex_index++;
}
}
return PartialPermutation(std::move(permuted_vertex_indexes));
}
PartialPermutation ContactParticipation::CalcDofPermutation() const {
PartialPermutation permutation = CalcDofPartialPermutation();
ExtendToFullPermutation(&permutation);
return permutation;
}
PartialPermutation ContactParticipation::CalcDofPartialPermutation() const {
/* Build the partial permutation. */
int permuted_vertex_index = 0;
std::vector<int> permuted_dof_indexes(3 * participation_.size(), -1);
for (int v = 0; v < static_cast<int>(participation_.size()); ++v) {
if (participation_[v]) {
permuted_dof_indexes[3 * v] = 3 * permuted_vertex_index;
permuted_dof_indexes[3 * v + 1] = 3 * permuted_vertex_index + 1;
permuted_dof_indexes[3 * v + 2] = 3 * permuted_vertex_index + 2;
++permuted_vertex_index;
}
}
return PartialPermutation(std::move(permuted_dof_indexes));
}
template <typename T>
DeformableContactSurface<T>::DeformableContactSurface(
GeometryId id_A, GeometryId id_B, PolygonSurfaceMesh<T> contact_mesh_W,
std::vector<T> signed_distances,
std::vector<Vector4<int>> contact_vertex_indexes_A,
std::vector<Vector4<T>> barycentric_coordinates_A,
std::optional<std::vector<Vector4<int>>> contact_vertex_indexes_B,
std::optional<std::vector<Vector4<T>>> barycentric_coordinates_B)
: id_A_(id_A),
id_B_(id_B),
contact_mesh_W_(std::move(contact_mesh_W)),
signed_distances_(std::move(signed_distances)),
contact_vertex_indexes_A_(std::move(contact_vertex_indexes_A)),
barycentric_coordinates_A_(std::move(barycentric_coordinates_A)),
contact_vertex_indexes_B_(std::move(contact_vertex_indexes_B)),
barycentric_coordinates_B_(std::move(barycentric_coordinates_B)) {
const int num_contact_points = contact_mesh_W_.num_faces();
DRAKE_DEMAND(num_contact_points ==
static_cast<int>(signed_distances_.size()));
DRAKE_DEMAND(num_contact_points ==
static_cast<int>(barycentric_coordinates_A_.size()));
DRAKE_DEMAND(num_contact_points ==
static_cast<int>(contact_vertex_indexes_A_.size()));
DRAKE_DEMAND(contact_vertex_indexes_B_.has_value() ==
barycentric_coordinates_B_.has_value());
if (contact_vertex_indexes_B_.has_value()) {
DRAKE_DEMAND(num_contact_points ==
static_cast<int>(barycentric_coordinates_B_->size()));
DRAKE_DEMAND(num_contact_points ==
static_cast<int>(contact_vertex_indexes_B_->size()));
}
nhats_W_.reserve(num_contact_points);
contact_points_W_.reserve(num_contact_points);
for (int i = 0; i < num_contact_points; ++i) {
nhats_W_.emplace_back(contact_mesh_W_.face_normal(i));
contact_points_W_.emplace_back(contact_mesh_W_.element_centroid(i));
}
}
template <typename T>
void DeformableContact<T>::AddDeformableRigidContactSurface(
GeometryId deformable_id, GeometryId rigid_id,
const std::unordered_set<int>& participating_vertices,
PolygonSurfaceMesh<T> contact_mesh_W, std::vector<T> signed_distances,
std::vector<Vector4<int>> contact_vertex_indexes,
std::vector<Vector4<T>> barycentric_coordinates) {
const auto iter = contact_participations_.find(deformable_id);
DRAKE_THROW_UNLESS(iter != contact_participations_.end());
DRAKE_DEMAND(static_cast<int>(signed_distances.size()) ==
contact_mesh_W.num_faces());
DRAKE_DEMAND(static_cast<int>(contact_vertex_indexes.size()) ==
contact_mesh_W.num_faces());
DRAKE_DEMAND(static_cast<int>(barycentric_coordinates.size()) ==
contact_mesh_W.num_faces());
iter->second.Participate(participating_vertices);
contact_surfaces_.emplace_back(
deformable_id, rigid_id, std::move(contact_mesh_W),
std::move(signed_distances), std::move(contact_vertex_indexes),
std::move(barycentric_coordinates), std::nullopt, std::nullopt);
}
template class DeformableContactSurface<double>;
template class DeformableContact<double>;
} // namespace internal
} // namespace geometry
} // namespace drake