-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
make_mesh_field.cc
97 lines (85 loc) · 3.36 KB
/
make_mesh_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
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
#include "drake/geometry/proximity/make_mesh_field.h"
#include <cmath>
#include <limits>
#include <set>
#include <utility>
#include <vector>
#include "drake/common/default_scalars.h"
#include "drake/common/eigen_types.h"
#include "drake/common/extract_double.h"
#include "drake/geometry/proximity/calc_distance_to_surface_mesh.h"
#include "drake/geometry/proximity/triangle_surface_mesh.h"
#include "drake/geometry/proximity/volume_to_surface_mesh.h"
namespace drake {
namespace geometry {
namespace internal {
namespace {
template <typename T>
TriangleSurfaceMesh<double> ConvertVolumeToSurfaceMeshDouble(
const VolumeMesh<T>& volume_mesh, std::vector<int>* boundary_vertices) {
TriangleSurfaceMesh<T> surface =
ConvertVolumeToSurfaceMeshWithBoundaryVertices(volume_mesh,
boundary_vertices);
if constexpr (std::is_same_v<T, double>) {
return surface;
} else {
// Scalar conversion from T (supposedly AutoDiffXd) to double.
std::vector<Vector3<double>> vertices_double;
vertices_double.reserve(surface.vertices().size());
for (const Vector3<T>& p_MV : surface.vertices()) {
vertices_double.emplace_back(ExtractDoubleOrThrow(p_MV));
}
std::vector<SurfaceTriangle> triangles = surface.triangles();
return {std::move(triangles), std::move(vertices_double)};
}
}
} // namespace
template <typename T>
VolumeMeshFieldLinear<T, T> MakeVolumeMeshPressureField(
const VolumeMesh<T>* mesh_M, const T& hydroelastic_modulus) {
DRAKE_DEMAND(hydroelastic_modulus > T(0));
DRAKE_DEMAND(mesh_M != nullptr);
std::vector<int> boundary_vertices;
// The subscript _d is for the scalar type double.
TriangleSurfaceMesh<double> surface_d =
ConvertVolumeToSurfaceMeshDouble(*mesh_M, &boundary_vertices);
// TODO(DamrongGuoy): Check whether there could be numerical roundings that
// cause a vertex on the boundary to have a non-zero value. Consider
// initializing pressure_values to zeros and skip the computation for
// boundary vertices.
std::vector<T> pressure_values;
T max_value(std::numeric_limits<double>::lowest());
// First round, it's actually unsigned distance, not pressure values yet.
const Bvh<Obb, TriangleSurfaceMesh<double>> bvh(surface_d);
auto boundary_iter = boundary_vertices.begin();
for (int v = 0; v < ssize(mesh_M->vertices()); ++v) {
if (boundary_iter != boundary_vertices.end() && *boundary_iter == v) {
++boundary_iter;
pressure_values.push_back(0);
continue;
}
const Vector3<T>& p_MV = mesh_M->vertex(v);
const Vector3<double> p_MV_d = ExtractDoubleOrThrow(p_MV);
T pressure = internal::CalcDistanceToSurfaceMesh(p_MV_d, surface_d, bvh);
pressure_values.push_back(pressure);
if (max_value < pressure) {
max_value = pressure;
}
}
if (max_value <= T(0)) {
throw std::runtime_error(
"MakeVolumeMeshPressureField(): "
"the computed max distance to boundary among "
"all mesh vertices is non-positive. Perhaps "
"the mesh lacks interior vertices.");
}
for (T& p : pressure_values) {
p = hydroelastic_modulus * p / max_value;
}
return {std::move(pressure_values), mesh_M, true};
}
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
(&MakeVolumeMeshPressureField<T>))
} // namespace internal
} // namespace geometry
} // namespace drake