-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
mesh_plane_intersection.h
187 lines (166 loc) · 9.32 KB
/
mesh_plane_intersection.h
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
#pragma once
#include <memory>
#include <optional>
#include <unordered_map>
#include <vector>
#include "drake/common/sorted_pair.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/plane.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 {
/* Intersects a tetrahedron with a plane; the resulting polygon is added to
polygon_vertices. An optional parameter, cut_edges, is provided to store,
for each vertex in polygon_vertices, the intersected edge in mesh_M on which
it lies.
The face vertices are ordered such that the normal implied by their winding
points in the direction of the plane's normal.
If there is no intersection, then neither polygon_vertices nor cut_edges will
change.
@param[in] tet_index The index of the tetrahedron to attempt to
intersect.
@param[in] mesh_M The volume mesh containing the tetrahedron to
intersect. The vertex positions are all
measured and expressed in Frame M.
@param[in] plane_M The definition of a plane measured and
expressed in Frame M.
@param[in, out] polygon_vertices The list of vertices of the resulting
intersection polygon.
@param[in, out] cut_edges The optional list of volume mesh edges that
contains at index *i* the edge in mesh_M
that was intersected to create vertex *i*
in polygon_vertices. It is ignored if
omitted or `nullptr` is passed in.
@pre `tet_index` lies in the range `[0, mesh_M.mesh().num_elements())`.
*/
template <typename T>
void SliceTetrahedronWithPlane(
int tet_index, const VolumeMesh<double>& mesh_M, const Plane<T>& plane_M,
std::vector<Vector3<T>>* polygon_vertices,
std::vector<SortedPair<int>>* cut_edges = nullptr);
/* Intersects a tetrahedron with a plane; the resulting polygon is passed
into the provided MeshBuilder.
This method constructs a mesh by a sequence of invocations. It guarantees
the output surface mesh has the same topological coherency as the input mesh.
For example, if the plane cuts through a tetrahedron edge e at a point P, then
each of the tetrahedra incident to that edge will be cut into a polygon on
the plane which all share a _single_ vertex at P; no duplicate vertices will be
introduced.
This is accomplished by storing the edges that have already been evaluated.
Call after call, the cached set of intersected edges grows and subsequent
calls look up edges in the cache to see if the plane-edge intersection has
already been accounted for.
The unique vertices, their corresponding pressure values, and the unique
faces are all stored within the MeshBuilder; only their indices are stored
in the cache.
The face vertices are ordered such that the normal implied by their winding
points in the direction of the plane's normal.
If there is no intersection, then neither the mesh, nor the cache data
structures will change.
@param[in] tet_index The index of the tetrahedron to attempt to
intersect.
@param[in] field_M The _linear_ volume mesh field (and mesh) containing
the tetrahedra to intersect. The vertex positions
are all measured and expressed in Frame M. The
linearity of the field is a strict requirement of
the algorithm.
@param[in] plane_M The definition of a plane measured and expressed
in Frame M.
@param[in] X_WM The relative pose between the mesh frame M and the
world frame W.
@param[in,out] builder_W The mesh builder that will add the resulting
intersecting polygon to the mesh (along with sampled
pressure values). It builds the mesh in the world
frame (requiring polygon quantities to be expressed
appropriately).
@param[in,out] cut_edges The cache of volume mesh edges that have already
been cut and the index of the surface mesh vertex
(indexing into vertices_W) that represents the cut
point.
@returns The number of faces added.
@pre `tet_index` lies in the range `[0, field_M.mesh().num_elements())`.
*/
template <typename MeshBuilder>
int SliceTetWithPlane(
int tet_index, const VolumeMeshFieldLinear<double, double>& field_M,
const Plane<typename MeshBuilder::ScalarType>& plane_M,
const math::RigidTransform<typename MeshBuilder::ScalarType>& X_WM,
MeshBuilder* builder_W,
std::unordered_map<SortedPair<int>, int>* cut_edges);
/* Computes a ContactSurface by intersecting a plane with a set of tetrahedra
drawn from the given volume mesh (and its pressure field). The indicated
tetrahedra would typically be the result of broadphase culling.
The mesh representation of the resulting contact surface is defined by the type
of MeshBuilder provided: polygon or triangle.
@param[in] mesh_id The id associated with the volume mesh.
@param[in] mesh_field_M The _linear_ mesh field (and corresponding mesh,
mesh_M) from which we compute the ContactSurface.
The field (and the mesh vertices) are measured and
expressed in Frame M. The linearity of the field is
a strict requirement of the underlying algorithm.
@param[in] plane_id The id associated with the plane.
@param[in] plane_M The plane to intersect against the tetrahedra;
measured and expressed in the same frame M.
@param[in] tet_indices Indices for the tetrahedra in mesh_M to test against
the plane.
@param[in] X_WM The relative pose between the mesh frame M and the
world frame W. Used to guarantee that the contact
surface is measured and expressed in the world
frame.
@returns `nullptr` if there is no intersection, otherwise the appropriate
ContactSurface. The normals of the contact surface mesh will all
be parallel with the plane normal.
@pre `i ∈ [0, N) ∀ i ∈ tet_indices`, where N is the number of tetrahedra in
mesh_M.
*/
template <typename MeshBuilder>
std::unique_ptr<ContactSurface<typename MeshBuilder::ScalarType>>
ComputeContactSurface(
GeometryId mesh_id,
const VolumeMeshFieldLinear<double, double>& mesh_field_M,
GeometryId plane_id, const Plane<typename MeshBuilder::ScalarType>& plane_M,
const std::vector<int>& tet_indices,
const math::RigidTransform<typename MeshBuilder::ScalarType>& X_WM);
// TODO(SeanCurtis-TRI): This is, in some sense, the "public" api. It refers to
// half spaces. Does it belong in this file? Does it belong elsewhere? At the
// end of the day, where does surface mesh-soft half space go?
/* Computes the ContactSurface formed by a rigid half space and a given
soft mesh.
The definition of the half space is implicit in the call -- it is the type
defined by the HalfSpace class, thus, only its id and its pose in a common
frame (the world frame) is necessary.
@param[in] id_S The id of the soft mesh.
@param[in] field_S The _linear_ mesh field (and its corresponding mesh
`mesh_S`) for the soft mesh. The field and mesh
vertices are measured and expressed in Frame S. The
linearity of the field is a strict requirement of
the underlying algorithm.
@param[in] bvh_S The bounding-volume hierarchy of the soft volume
mesh measured and expressed in Frame S.
@param[in] X_WS The relative pose of Frame S and the world frame W.
@param[in] id_R The id of the rigid half space.
@param[in] X_WR The relative pose between Frame R -- the frame the
half space is defined in -- and the world frame W.
@param[in] representation The preferred representation of each contact
polygon.
@returns `nullptr` if there is no collision, otherwise the ContactSurface
between geometries S and R. The normals of the contact surface mesh
will all be parallel with the plane normal.
@tparam_nonsymbolic_scalar
*/
template <typename T>
std::unique_ptr<ContactSurface<T>>
ComputeContactSurfaceFromSoftVolumeRigidHalfSpace(
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 math::RigidTransform<T>& X_WR,
HydroelasticContactRepresentation representation);
} // namespace internal
} // namespace geometry
} // namespace drake