-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
make_capsule_mesh.cc
188 lines (161 loc) · 8.37 KB
/
make_capsule_mesh.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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
#include "drake/geometry/proximity/make_capsule_mesh.h"
#include <cmath>
#include "drake/common/default_scalars.h"
#include "drake/geometry/proximity/meshing_utilities.h"
#include "drake/geometry/proximity/proximity_utilities.h"
namespace drake {
namespace geometry {
namespace internal {
template <typename T>
VolumeMesh<T> MakeCapsuleVolumeMesh(const Capsule& capsule,
double resolution_hint) {
// Z coordinates of the endpoints of the medial axis.
const double medial_top_z = capsule.length() / 2.;
const double medial_bottom_z = -medial_top_z;
// Z coordinates of the poles of the caps.
const double top_z = medial_top_z + capsule.radius();
const double bottom_z = -top_z;
// Let n be the number of vertices on each circular rim of the capsule.
// The total number of vertices, V = 2n⌊n/2⌋ + 4
// The total number of tetrahedra, F = 4n(⌊n/2⌋ - 1) + 5n
// We need to be able to index both the vertices and tetrahedra with type safe
// indices, thus the absolute maximum number of tetrahedra is INT_MAX. However
// we choose a more reasonable maximum of 1 million tetrahedra.
//
// F = 4n(⌊n/2⌋ - 1) + 5n ≤ MAX
// F ≤ 4n(n/2 - 1) + 5n ≤ MAX
// 2n² + n - MAX ≤ 0
// n ≤ ((√ 8 MAX + 1) - 1) / 4
// n ≤ ⌊ 706.86... ⌋ for MAX = 1'000'000
// n ≤ 706
// The mesh will have a minimum of 3 vertices on its rim.
const int num_vertices_per_circle = static_cast<int>(
std::clamp(2. * M_PI * capsule.radius() / resolution_hint, 3.0, 706.0));
// Each vertex on the rim corresponds to a line of longitude on the
// hemispheres of the caps. num_circles_per_cap is how many lines of latitude
// to subdivide each cap into.
const int num_circles_per_cap = num_vertices_per_circle / 2;
std::vector<Vector3<T>> mesh_vertices;
// 2 caps * number of circles per cap * number of verts per circle
// + the two medial vertices and the two cap pole vertices.
mesh_vertices.reserve(2 * num_circles_per_cap * num_vertices_per_circle + 4);
// Add the cap pole vertices and the medial vertices.
int medial_top = static_cast<int>(mesh_vertices.size());
mesh_vertices.emplace_back(0, 0, medial_top_z);
int medial_bottom = static_cast<int>(mesh_vertices.size());
mesh_vertices.emplace_back(0, 0, medial_bottom_z);
int top = static_cast<int>(mesh_vertices.size());
mesh_vertices.emplace_back(0, 0, top_z);
int bottom = static_cast<int>(mesh_vertices.size());
mesh_vertices.emplace_back(0, 0, bottom_z);
// Storage for the vertex indices of each cap.
std::vector<int> top_cap(num_circles_per_cap * num_vertices_per_circle);
std::vector<int> bottom_cap(num_circles_per_cap * num_vertices_per_circle);
// The vertices of the caps are spaced in an equal subdivision of the sphere
// with spherical coordinates. Vertex (i, j) will have spherical coordinates:
// (r, θ, φ) = (radius, π - i*theta_step, j*phi_step) where θ is the polar
// angle and φ the azimuthal angle. One of the peculiarities of this
// parameterization is the anisotropic behavior of contact patch resolution
// at different points of the sphere. For instance, the pole vertices have
// a degree of `num_vertices_per_circle` while all other vertices on the
// sphere have a degree of 4. Thus a shallow contact with a plane at and
// tangent to the pole will produce a much denser contact surface than that
// produced by the same type of shallow contact at a point at the equator
// of the sphere. It is unclear at this moment whether that artifact has an
// effect on the simulation.
const double theta_step = M_PI_2 / num_circles_per_cap;
const double phi_step = 2 * M_PI / num_vertices_per_circle;
// Subdivide the sphere and partition into top and bottom caps.
for (int i = 0; i < num_circles_per_cap; ++i) {
const double theta = M_PI_2 - i * theta_step;
const double s = sin(theta);
// Offset the z value of each cap.
const double top_circle_z = capsule.radius() * cos(theta) + medial_top_z;
const double bottom_circle_z = -top_circle_z;
for (int j = 0; j < num_vertices_per_circle; ++j) {
const double phi = j * phi_step;
const double x = capsule.radius() * s * cos(phi);
const double y = capsule.radius() * s * sin(phi);
top_cap[num_vertices_per_circle * i + j] =
static_cast<int>(mesh_vertices.size());
mesh_vertices.emplace_back(x, y, top_circle_z);
bottom_cap[num_vertices_per_circle * i + j] =
static_cast<int>(mesh_vertices.size());
mesh_vertices.emplace_back(x, y, bottom_circle_z);
}
}
std::vector<VolumeElement> mesh_elements;
// Each quadrilateral of each hemispherical cap is the base of a pyramid
// whose top vertex is the center of the sphere of the cap (one of the medial
// vertices.) The pyramids are split into tetrahedra in such a way that they
// conform to each other. That is to say they share common faces where they
// meet by the appropriate choice of diagonal. The quadrilateral faces of the
// pyramid are: [(i,j), (i, j+1), (i+1, j+1), (i+1, j)] where those vertices
// are given counterclockwise when viewed from outside of the capsule.
//
// (i+1,j) ______
// o +
// ／ \ ＼
// / \ \ (one slice of the top cap)
// (i,j) o---------o M₁ +
// i indexes latitude and j indexes longitude.
for (int i = 0; i < num_circles_per_cap - 1; ++i) {
for (int j = 0; j < num_vertices_per_circle; ++j) {
// Connect to the next longitude, wrapping around the circle.
const int j1 = (j + 1) % num_vertices_per_circle;
// The arguments to SplitPyramidToTetrahedra are given in CLOCKWISE order
// for the base.
Append(SplitPyramidToTetrahedra(
top_cap[(i + 1) * num_vertices_per_circle + j],
top_cap[(i + 1) * num_vertices_per_circle + j1],
top_cap[i * num_vertices_per_circle + j1],
top_cap[i * num_vertices_per_circle + j], medial_top),
&mesh_elements);
Append(
SplitPyramidToTetrahedra(
bottom_cap[i * num_vertices_per_circle + j],
bottom_cap[i * num_vertices_per_circle + j1],
bottom_cap[(i + 1) * num_vertices_per_circle + j1],
bottom_cap[(i + 1) * num_vertices_per_circle + j], medial_bottom),
&mesh_elements);
}
}
// The vertices on the last circle of latitude on each cap all connect to the
// pole vertices forming a circular cap divided into triangular wedges. Each
// of those wedges connects to the corresponding medial vertex, forming a
// single tetrahedra.
//
// The two equatorial circles of each cap connect corresponding vertices to
// each other, forming the cylindrical middle portion of the capsule. Each
// quadrilateral face of the cylinder connects to the two medial vertices,
// forming a triangular prism. Each prism is then subdivided into 3 tetrahedra
// such that adjacent prisms are conforming.
// Offset into the index vector of the last circle of the caps.
const int last_circle_offset =
(num_circles_per_cap - 1) * num_vertices_per_circle;
for (int j = 0; j < num_vertices_per_circle; ++j) {
const int j1 = (j + 1) % num_vertices_per_circle;
// Add tetrahedra of the top and bottom caps.
// Vertices are oriented such that the signed volume of the tetrahedra
// is positive, assuming the right hand rule.
mesh_elements.emplace_back(top, top_cap[last_circle_offset + j1],
top_cap[last_circle_offset + j], medial_top);
mesh_elements.emplace_back(bottom, bottom_cap[last_circle_offset + j],
bottom_cap[last_circle_offset + j1],
medial_bottom);
// Add the cylindrical prisms.
// Vertices are passed in the order: bottom triangular cap (oriented
// inwards) and then top triangular cap (oriented outwards).
Append(SplitTriangularPrismToTetrahedra(medial_bottom, bottom_cap[j],
bottom_cap[j1], medial_top,
top_cap[j], top_cap[j1]),
&mesh_elements);
}
return {std::move(mesh_elements), std::move(mesh_vertices)};
}
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
&MakeCapsuleVolumeMesh<T>
))
} // namespace internal
} // namespace geometry
} // namespace drake