-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
mesh_intersection.cc
454 lines (413 loc) · 20.4 KB
/
mesh_intersection.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
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
#include "drake/geometry/proximity/mesh_intersection.h"
#include <algorithm>
#include <cmath>
#include <functional>
#include <limits>
#include <memory>
#include <utility>
#include <vector>
#include "drake/common/default_scalars.h"
#include "drake/common/eigen_types.h"
#include "drake/geometry/geometry_ids.h"
#include "drake/geometry/proximity/bvh.h"
#include "drake/geometry/proximity/contact_surface_utility.h"
#include "drake/geometry/proximity/posed_half_space.h"
#include "drake/geometry/proximity/triangle_surface_mesh.h"
#include "drake/geometry/proximity/volume_mesh.h"
#include "drake/geometry/proximity/volume_mesh_field.h"
#include "drake/geometry/query_results/contact_surface.h"
#include "drake/math/rigid_transform.h"
namespace drake {
namespace geometry {
namespace internal {
// TODO(DamrongGuoy): Take care of double counting problem when a triangle of
// a surface mesh overlaps a triangular face shared by two tetrahedrons of a
// volume mesh. The fix will require several functions in the code to work
// together. These ideas should help:
// - Use Simulation of Simplicity with floating-point filter.
// - Book keeping the shared faces of the tetrahedrons in the volume mesh.
// - Require unique vertices in the surface mesh and in the volume mesh.
// TODO(DamrongGuoy): Take care of special cases when `p` lies on the
// plane of the half space to help with the double counting problem. Right
// now it is taken as being inside the half space.
// Instead of fcl::Halfspace(normal, distance), we might want to specify
// the half space using a pair (face, element), i.e., a triangular face of a
// tetrahedral element. It will identify the half space bounded by the plane
// of the triangular `face` that contains the tetrahedral `element`, and will
// enable symbolic perturbation in such a way that a point `p` on a shared
// face of two tetrahedrons will be considered inside one tetrahedron and
// outside another tetrahedron. Right now it will be considered inside both
// tetrahedrons.
// TODO(DamrongGuoy): Handle the case that the line is parallel to the plane.
template <typename T>
Vector3<T> CalcIntersection(const Vector3<T>& p_FA, const Vector3<T>& p_FB,
const PosedHalfSpace<T>& H_F) {
const T a = H_F.CalcSignedDistance(p_FA);
const T b = H_F.CalcSignedDistance(p_FB);
DRAKE_DEMAND(a != b);
const T wa = b / (b - a);
const T wb = T(1.0) - wa; // Enforce wa + wb = 1, i.e., wb = -a/(b-a)
const Vector3<T> intersection = wa * p_FA + wb * p_FB;
return intersection;
// Justification.
// 1. We set up the weights wa and wb such that wa + wb = 1, which
// guarantees that the linear combination is on the straight line
// through A and B.
// 2. We show that the H_F.signed_distance(wa * A + wb * B) is zero.
// Let H_F.signed_distance be sdf(P) = N.dot(P) + d.
// sdf(wa * A + wb * B)
// = N.dot(wa * A + wb * B) + d
// = wa * N.dot(A) + wb * N.dot(B) + d
// = b * N.dot(A)/(b - a) - a * N.dot(B)/(b - a) + d
// = (b * N.dot(A) - a * N.dot(B) + (b - a) * d) / (b - a)
// = (b * (N.dot(A) + d) - a * (N.dot(B) + d)) / (b-a)
// = (b * sdf(A) - a * sdf(B)) / (b-a)
// = (b * a - a * b) / (b-a)
// = 0 when a != b.
}
template <typename T>
void ClipPolygonByHalfSpace(const std::vector<Vector3<T>>& input_vertices_F,
const PosedHalfSpace<T>& H_F,
std::vector<Vector3<T>>* output_vertices_F) {
DRAKE_ASSERT(output_vertices_F != nullptr);
// Note: this is the inner loop of a modified Sutherland-Hodgman algorithm for
// clipping a polygon.
output_vertices_F->clear();
// Note: This code is correct for size < 3, but pointless so we make no effort
// to support it or test it.
const int size = static_cast<int>(input_vertices_F.size());
// TODO(SeanCurtis-TRI): If necessary, this can be made more efficient:
// eliminating the modulus and eliminating the redundant "inside" calculation
// on previous (by pre-determining previous and its "containedness" and then
// propagating current -> previous in each loop. Probably a desirable
// optimization as we need to make all of this work as cheap as possible.
for (int i = 0; i < size; ++i) {
const Vector3<T>& current = input_vertices_F[i];
const Vector3<T>& previous = input_vertices_F[(i - 1 + size) % size];
// Again, avoid doing classification on T-valued quantities so we don't
// waste computation in applying the chain rule.
const bool current_contained =
H_F.CalcSignedDistance(convert_to_double(current)) <= 0;
const bool previous_contained =
H_F.CalcSignedDistance(convert_to_double(previous)) <= 0;
if (current_contained) {
if (!previous_contained) {
// Current is inside and previous is outside. Compute the point where
// that edge enters the half space. This is a new vertex in the clipped
// polygon and must be included before current.
output_vertices_F->push_back(CalcIntersection(current, previous, H_F));
}
output_vertices_F->push_back(current);
} else if (previous_contained) {
// Current is outside and previous is inside. Compute the point where
// the edge exits the half space. This is a new vertex in the clipped
// polygon and is included *instead* of current.
output_vertices_F->push_back(CalcIntersection(current, previous, H_F));
}
}
}
template <typename T>
void RemoveNearlyDuplicateVertices(std::vector<Vector3<T>>* polygon) {
DRAKE_ASSERT(polygon != nullptr);
// TODO(SeanCurtis-TRI): The resulting polygon depends on the order of the
// inputs. Imagine I have vertices A, A', A'' (such that |X - X'| < eps.
// The sequence AA'A'' would be reduced to AA''
// The sequence A'A''A would be reduced to A'.
// The sequence A''AA' would be reduced to A''A.
// In all three cases, the exact same polygon is defined on input, but the
// output is different. This should be documented and/or fixed.
if (polygon->size() <= 1) return;
auto near = [](const Vector3<T>& p, const Vector3<T>& q) -> bool {
// TODO(SeanCurtis-TRI): This represents 5-6 bits of loss. Confirm that a
// tighter epsilon can't be used. This should probably be a function of the
// longest edge involved.
// Empirically we found that numeric_limits<double>::epsilon() 2.2e-16 is
// too small, especially when the objects are not axis-aligned.
const double kEpsSquared(1e-14 * 1e-14);
// We don't want to do this *classification* problem with generic T; it
// would be prohibitively expensive to propagate calculations to derivatives
// just to throw the work away.
return (convert_to_double(p) - convert_to_double(q)).squaredNorm() <
kEpsSquared;
};
// Remove consecutive vertices that are duplicated in the linear order. It
// will change "A,B,B,C,C,A" to "A,B,C,A". To close the cyclic order, we
// will check the first and the last vertices again near the end of the
// function.
// TODO(DamrongGuoy): This doesn't strictly satisfy the requirement of
// std::unique that the predicate represent an equivalence relation (i.e.,
// point A could be "near" points B and C, but that doesn't mean B and C are
// near each other). We need to figure out if that matters for this usage
// and, if not, document why here.
auto it = std::unique(polygon->begin(), polygon->end(), near);
polygon->resize(it - polygon->begin());
if (polygon->size() >= 3) {
// Check the first and the last vertices in the sequence. For example, given
// "A,B,C,A", we want "A,B,C".
if (near((*polygon)[0], *(polygon->rbegin()))) {
polygon->pop_back();
}
}
DRAKE_ASSERT(polygon->size() != 2 || !near((*polygon)[0], (*polygon)[1]));
}
template <typename MeshBuilder, typename BvType>
SurfaceVolumeIntersector<MeshBuilder, BvType>::~SurfaceVolumeIntersector() =
default;
template <typename MeshBuilder, typename BvType>
const std::vector<
Vector3<typename SurfaceVolumeIntersector<MeshBuilder, BvType>::T>>&
SurfaceVolumeIntersector<MeshBuilder, BvType>::ClipTriangleByTetrahedron(
int element, const VolumeMesh<double>& volume_M, int face,
const TriangleSurfaceMesh<double>& surface_N,
const math::RigidTransform<T>& X_MN) {
// Although polygon_M starts out pointing to polygon_[0] that is not an
// invariant in this function.
std::vector<Vector3<T>>* polygon_M = &(polygon_[0]);
// Initialize output polygon in M's frame from the triangular `face` of
// surface_N.
polygon_M->clear();
for (int i = 0; i < 3; ++i) {
const int v = surface_N.element(face).vertex(i);
const Vector3<T>& p_NV = surface_N.vertex(v).cast<T>();
polygon_M->push_back(X_MN * p_NV);
}
// Get the positions, in M's frame, of the four vertices of the tetrahedral
// `element` of volume_M. Because we are doing this in the M frame, we can
// leave the volume mesh's quantities as double-valued -- T-values will arise
// as we do transformed computations below.
Vector3<double> p_MVs[4];
for (int i = 0; i < 4; ++i) {
int v = volume_M.element(element).vertex(i);
p_MVs[i] = volume_M.vertex(v);
}
// Sets up the four half spaces associated with the four triangular faces of
// the tetrahedron. Assume the tetrahedron has the fourth vertex seeing the
// first three vertices in CCW order; for example, a tetrahedron of (Zero(),
// UnitX(), UnitY(), UnitZ()) (see the picture below) has this orientation.
//
// +Z
// |
// v3
// |
// |
// v0+------v2---+Y
// /
// /
// v1
// /
// +X
//
// This table encodes the four triangular faces of the tetrahedron in such
// a way that each right-handed face normal points outward from the
// tetrahedron, which is suitable for setting up the half space. Refer to
// the above picture.
const int faces[4][3] = {{1, 2, 3}, {0, 3, 2}, {0, 1, 3}, {0, 2, 1}};
// Although this assertion appears trivially true, its presence is protection
// for the subsequent code, which heavily relies on it being true, from any
// changes that may be applied to the previous code.
DRAKE_ASSERT(polygon_M == &(polygon_[0]));
std::vector<Vector3<T>>* in_M = polygon_M;
std::vector<Vector3<T>>* out_M = &(polygon_[1]);
for (auto& face_vertex : faces) {
const Vector3<T>& p_MA = p_MVs[face_vertex[0]].cast<T>();
const Vector3<T>& p_MB = p_MVs[face_vertex[1]].cast<T>();
const Vector3<T>& p_MC = p_MVs[face_vertex[2]].cast<T>();
// We'll allow the PosedHalfSpace to normalize our vector.
const Vector3<T> normal_M = (p_MB - p_MA).cross(p_MC - p_MA);
PosedHalfSpace<T> half_space_M(normal_M, p_MA);
// Intersects the output polygon by the half space of each face of the
// tetrahedron.
ClipPolygonByHalfSpace(*in_M, half_space_M, out_M);
std::swap(in_M, out_M);
}
polygon_M = in_M;
// TODO(DamrongGuoy): Remove the code below when ClipPolygonByHalfSpace()
// stops generating duplicate vertices. See the note in
// ClipPolygonByHalfSpace().
// Remove possible duplicate vertices from ClipPolygonByHalfSpace().
RemoveNearlyDuplicateVertices(polygon_M);
if (polygon_M->size() < 3) {
// RemoveNearlyDuplicateVertices() may have shrunk the polygon down to
// one or two vertices, so we empty the polygon.
polygon_M->clear();
}
// TODO(DamrongGuoy): Calculate area of the polygon. If it's too small,
// return an empty polygon.
// The output polygon could be at most a heptagon.
DRAKE_DEMAND(polygon_M->size() <= 7);
return *polygon_M;
}
template <typename MeshBuilder, typename BvType>
bool SurfaceVolumeIntersector<MeshBuilder, BvType>::
IsFaceNormalAlongPressureGradient(
const VolumeMeshFieldLinear<double, double>& volume_field_M,
const TriangleSurfaceMesh<double>& surface_N,
const math::RigidTransform<double>& X_MN, int tet_index,
int tri_index) {
const Vector3<double> grad_p_M = volume_field_M.EvaluateGradient(tet_index);
// TODO(SeanCurtis-TRI) If we pass in an *unnormalized* vector, we can
// reduce the number of square roots. Currently, we produce the *unit*
// gradient normal n̂ and the triangle face normal f̂ (we normalize this vector
// to correct for rounding error that creeps in from rotations). We use the
// unit vectors so that we can compute cos(θ) = f̂⋅n̂. However, we could
// compute the following instead:
//
// cos(θ) = f⃗/|f⃗|⋅n⃗/|n⃗|
// cos(θ) = f⃗⋅n⃗/(|f⃗||n⃗|)
// cos(θ) = f⃗⋅n⃗/√(|f⃗|²|n⃗|²)
//
// Two square roots become one square root.
return IsFaceNormalInNormalDirection(grad_p_M.normalized(), surface_N,
tri_index, X_MN.rotation());
}
template <typename MeshBuilder, typename BvType>
void SurfaceVolumeIntersector<MeshBuilder, BvType>::SampleVolumeFieldOnSurface(
const VolumeMeshFieldLinear<double, double>& volume_field_M,
const Bvh<BvType, VolumeMesh<double>>& bvh_M,
const TriangleSurfaceMesh<double>& surface_N,
const Bvh<Obb, TriangleSurfaceMesh<double>>& bvh_N,
const math::RigidTransform<T>& X_MN,
const bool filter_face_normal_along_field_gradient) {
// Builds the intersection mesh represented in M's frame.
MeshBuilder builder_M;
const math::RigidTransform<double>& X_MN_d = convert_to_double(X_MN);
std::vector<std::pair<int, int>> candidate_tet_tri_pairs;
bvh_M.Collide(bvh_N, X_MN_d,
[&candidate_tet_tri_pairs](
int tet_index, int tri_index) -> BvttCallbackResult {
candidate_tet_tri_pairs.emplace_back(tet_index, tri_index);
return BvttCallbackResult::Continue;
});
for (const auto& [tet_index, tri_index] : candidate_tet_tri_pairs) {
CalcContactPolygon(volume_field_M, surface_N, X_MN, X_MN_d, &builder_M,
filter_face_normal_along_field_gradient, tet_index,
tri_index);
}
if (builder_M.num_faces() == 0) return;
std::tie(mesh_M_, field_M_) = builder_M.MakeMeshAndField();
}
template <typename MeshBuilder, typename BvType>
void SurfaceVolumeIntersector<MeshBuilder, BvType>::CalcContactPolygon(
const VolumeMeshFieldLinear<double, double>& volume_field_M,
const TriangleSurfaceMesh<double>& surface_N,
const math::RigidTransform<T>& X_MN,
const math::RigidTransform<double>& X_MN_d, MeshBuilder* builder_M,
const bool filter_face_normal_along_field_gradient, const int tet_index,
const int tri_index) {
const VolumeMesh<double>& vol_mesh_M = volume_field_M.mesh();
if (filter_face_normal_along_field_gradient) {
if (!this->IsFaceNormalAlongPressureGradient(
volume_field_M, surface_N, X_MN_d, tet_index, tri_index)) {
return;
}
}
// TODO(SeanCurtis-TRI): This redundantly transforms surface mesh vertex
// positions. Specifically, each vertex will be transformed M times (once
// per tetrahedron). Even with broadphase culling, this vertex will get
// transformed once for each tet-tri pair where the tri is incidental
// to the vertex and the tet-tri pair can't be conservatively culled.
// This is O(mn), where m is the number of faces incident to the vertex
// and n is the number of tet BVs that overlap this triangle BV. However,
// if the broadphase culling determines the surface and volume are
// disjoint regions, *no* vertices will be transformed. Unclear what the
// best balance for best average performance.
const std::vector<Vector3<T>>& polygon_vertices_M =
this->ClipTriangleByTetrahedron(tet_index, vol_mesh_M, tri_index,
surface_N, X_MN);
if (polygon_vertices_M.size() < 3) return;
// Add the vertices to the builder (with corresponding field values) and
// construct index-based polygon representation.
polygon_vertex_indices_.clear();
for (const auto& p_MV : polygon_vertices_M) {
polygon_vertex_indices_.push_back(builder_M->AddVertex(
p_MV, volume_field_M.EvaluateCartesian(tet_index, p_MV)));
}
const Vector3<T> nhat_M =
X_MN.rotation() * surface_N.face_normal(tri_index).template cast<T>();
// The gradient of the field in the volume mesh is *also* the
// gradient of the field on the contact surface. That's not
// generally true, but is for the special case of compliant-rigid case.
// So, we use it both for AddPolygon() as grad_e_MN_M and as grad_e_M when
// we store it per-face below.
const Vector3<T> grad_e_MN_M = volume_field_M.EvaluateGradient(tet_index);
const int num_new_faces =
builder_M->AddPolygon(polygon_vertex_indices_, nhat_M, grad_e_MN_M);
// TODO(SeanCurtis-TRI) This represents a *second* iteration on the vertices
// of the polygon. Rolling this into the builder would allow us to reduce
// the loops.
// Store the constituent gradient values from the volume mesh for
// each face in the contact surface.
for (int i = 0; i < num_new_faces; ++i) {
this->grad_eM_Ms_.push_back(grad_e_MN_M);
}
}
template <typename T>
std::unique_ptr<ContactSurface<T>>
ComputeContactSurfaceFromSoftVolumeRigidSurface(
const GeometryId id_S, const VolumeMeshFieldLinear<double, double>& field_S,
const Bvh<Obb, VolumeMesh<double>>& bvh_S,
const math::RigidTransform<T>& X_WS, const GeometryId id_R,
const TriangleSurfaceMesh<double>& mesh_R,
const Bvh<Obb, TriangleSurfaceMesh<double>>& bvh_R,
const math::RigidTransform<T>& X_WR,
HydroelasticContactRepresentation representation) {
auto process_intersection =
[&X_WS, id_S,
id_R](auto&& intersector_in) -> std::unique_ptr<ContactSurface<T>> {
if (intersector_in.has_intersection()) {
// TODO(DamrongGuoy): Compute the mesh and field with the quantities
// expressed in World frame by construction so that we can delete these
// two transforming methods.
// Transform the mesh from the S frame to the world frame. This entails:
// 1. Transforming the surface's vertices.
// 2. Allowing the LinearField e_SR a chance to re-express its cached
// gradient values.
// 3. Re-expressing the gradients of the volume-mesh's field
// (grad_eS_S) in the world frame (grad_eS_W).
intersector_in.mutable_mesh().TransformVertices(X_WS);
intersector_in.mutable_field().Transform(X_WS);
std::vector<Vector3<T>>& grad_eS_S = intersector_in.mutable_grad_eM_M();
for (auto& grad_eSi_S : grad_eS_S) {
grad_eSi_S = X_WS.rotation() * grad_eSi_S;
}
// The contact surface is documented as having the normals pointing *out*
// of the second surface and into the first. This mesh intersection
// creates a surface mesh with normals pointing out of the rigid surface,
// so we make sure the ids are ordered so that the rigid is the second id.
return std::make_unique<ContactSurface<T>>(
id_S, id_R, intersector_in.release_mesh(),
intersector_in.release_field(),
std::make_unique<std::vector<Vector3<T>>>(std::move(grad_eS_S)),
nullptr);
}
return nullptr;
};
// Compute the transformation from the rigid frame to the soft frame.
const math::RigidTransform<T> X_SR = X_WS.InvertAndCompose(X_WR);
if (representation == HydroelasticContactRepresentation::kTriangle) {
SurfaceVolumeIntersector<TriMeshBuilder<T>, Obb> intersector;
intersector.SampleVolumeFieldOnSurface(field_S, bvh_S, mesh_R, bvh_R, X_SR);
return process_intersection(intersector);
} else {
// Polygon.
SurfaceVolumeIntersector<PolyMeshBuilder<T>, Obb> intersector;
intersector.SampleVolumeFieldOnSurface(field_S, bvh_S, mesh_R, bvh_R, X_SR);
return process_intersection(intersector);
}
}
// Hydroelastics use Obb for the bounding volumes of tetrahedra.
template class SurfaceVolumeIntersector<TriMeshBuilder<double>, Obb>;
template class SurfaceVolumeIntersector<TriMeshBuilder<AutoDiffXd>, Obb>;
template class SurfaceVolumeIntersector<PolyMeshBuilder<double>, Obb>;
template class SurfaceVolumeIntersector<PolyMeshBuilder<AutoDiffXd>, Obb>;
// TODO(xuchenhan-tri): Allow for AutoDiffXd when deformable simulation
// supports it in general.
// Deformables use Aabb for the bounding volumes of deformable tetrahedra.
template class SurfaceVolumeIntersector<PolyMeshBuilder<double>, Aabb>;
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
(&CalcIntersection<T>, &ClipPolygonByHalfSpace<T>,
&RemoveNearlyDuplicateVertices<T>,
&ComputeContactSurfaceFromSoftVolumeRigidSurface<T>))
} // namespace internal
} // namespace geometry
} // namespace drake