-
Notifications
You must be signed in to change notification settings - Fork 1.3k
/
make_mesh_from_vtk.cc
52 lines (44 loc) · 1.74 KB
/
make_mesh_from_vtk.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
#include "drake/geometry/proximity/make_mesh_from_vtk.h"
#include <string>
#include <utility>
#include <vector>
#include "drake/common/default_scalars.h"
#include "drake/common/eigen_types.h"
#include "drake/geometry/proximity/vtk_to_volume_mesh.h"
namespace drake {
namespace geometry {
namespace internal {
template <typename T>
VolumeMesh<T> MakeVolumeMeshFromVtk(const std::filesystem::path& filename,
double scale) {
VolumeMesh<double> read_mesh = ReadVtkToVolumeMesh(filename, scale);
for (int e = 0; e < read_mesh.num_elements(); ++e) {
if (read_mesh.CalcTetrahedronVolume(e) <= 0.) {
throw std::runtime_error(fmt::format(
"MakeVolumeMeshFromVtk('{}', {}): "
"The {}-th tetrahedron (index start at 0) with "
"vertices {}, {}, {}, {} has non-positive volume, "
"so you might want to switch two consecutive vertices.",
filename.string(), scale, e, read_mesh.element(e).vertex(0),
read_mesh.element(e).vertex(1), read_mesh.element(e).vertex(2),
read_mesh.element(e).vertex(3)));
}
}
if constexpr (std::is_same_v<T, double>) {
return read_mesh;
} else {
static_assert(std::is_same_v<T, AutoDiffXd>);
std::vector<Vector3<T>> mesh_vertices;
mesh_vertices.reserve(read_mesh.num_vertices());
for (const Vector3<double>& p_MV : read_mesh.vertices()) {
mesh_vertices.emplace_back(p_MV);
}
std::vector<VolumeElement> mesh_elements = read_mesh.tetrahedra();
return {std::move(mesh_elements), std::move(mesh_vertices)};
}
}
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
(&MakeVolumeMeshFromVtk<T>))
} // namespace internal
} // namespace geometry
} // namespace drake