-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
make_box_field.cc
62 lines (53 loc) · 2.42 KB
/
make_box_field.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
#include "drake/geometry/proximity/make_box_field.h"
#include <utility>
#include <vector>
#include "drake/common/default_scalars.h"
#include "drake/common/eigen_types.h"
#include "drake/common/unused.h"
#include "drake/geometry/proximity/distance_to_point_callback.h"
namespace drake {
namespace geometry {
namespace internal {
template <typename T>
VolumeMeshFieldLinear<T, T> MakeBoxPressureField(
const Box& box, const VolumeMesh<T>* mesh_B,
const T hydroelastic_modulus) {
DRAKE_DEMAND(hydroelastic_modulus > T(0));
const Vector3<double> half_size = box.size() / 2.0;
const double min_half_size = half_size.minCoeff();
// TODO(DamrongGuoy): Switch to a better implementation in the future. The
// current implementation has a number of limitations:
// 1. For simplicity, we use a scaling of distance to boundary, which is
// not differentiable at points equally far from two or more faces of
// the box.
// 2. Implicitly we impose the rigid core on the medial axis. In the
// future, we will consider a rigid core as an offset of the box or
// other shapes.
// 3. We do not have a mechanism to define a pressure field using barrier
// functions. One possibility is to generate the mesh in offset
// layers, and define linear pressure fields in each offset with
// different elastic modulus.
std::vector<T> pressure_values;
pressure_values.reserve(mesh_B->num_vertices());
for (const Vector3<T>& r_BV : mesh_B->vertices()) {
// N is for the nearest point of V on the boundary of the box,
// and grad_B is the gradient vector of the signed distance function
// of the box at V, expressed in frame B.
const auto [r_BN, grad_B, is_V_on_edge_or_vertex] =
point_distance::DistanceToPoint<T>::ComputeDistanceToBox(half_size,
r_BV);
unused(is_V_on_edge_or_vertex);
T signed_distance = grad_B.dot(r_BV - r_BN);
// Map signed_distance ∈ [-min_half_size, 0] to extent e ∈ [0, 1],
// -min_half_size ⇝ 1, 0 ⇝ 0.
T extent = -signed_distance / T(min_half_size);
pressure_values.push_back(hydroelastic_modulus * extent);
}
return VolumeMeshFieldLinear<T, T>(std::move(pressure_values), mesh_B);
}
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
&MakeBoxPressureField<T>
))
} // namespace internal
} // namespace geometry
} // namespace drake