Check all edge pairs in convex-convex.
PiperOrigin-RevId: 620004680 Change-Id: I5d75c0fd21ed3d3e420eed762cfc5a7e04896032
This commit is contained in:
Baruch Tabanpour
committed by
Copybara-Service
Copybara-Service
parent
a8db22f0d0
commit
d258d5e152
@@ -336,8 +336,8 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
|
||||
cap_closest_pt,
|
||||
)
|
||||
|
||||
edge = jp.take(convex.vert, convex.face_edge, axis=0).reshape(-1, 2, 3)
|
||||
face_edge_normal = convex.face_edge_normal.reshape((-1, 2, 3)) # pytype: disable=attribute-error
|
||||
edge = jp.take(convex.vert, convex.edge, axis=0)
|
||||
edge_face_normal = convex.edge_face_normal # pytype: disable=attribute-error
|
||||
|
||||
res = jax.vmap(get_edge_axis)(edge.reshape(-1, 2, 3))
|
||||
e_idx = jp.abs(res[0]).argmin()
|
||||
@@ -349,8 +349,8 @@ def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact:
|
||||
cap_closest_pt,
|
||||
) = jax.tree_map(lambda x, i=e_idx: jp.take(x, i, axis=0), res)
|
||||
|
||||
face_edge_normals = face_edge_normal[e_idx]
|
||||
edge_voronoi_front = ((face_edge_normals @ edge_axis) < 0).all()
|
||||
edge_face_normals = edge_face_normal[e_idx]
|
||||
edge_voronoi_front = ((edge_face_normals @ edge_axis) < 0).all()
|
||||
shallow = ~degenerate_edge_dir & edge_voronoi_front
|
||||
edge_penetration = jp.where(shallow, cap.size[0] - edge_dist, -1)
|
||||
|
||||
@@ -630,8 +630,9 @@ def _sat_bruteforce(
|
||||
We return both the edge and face contacts. Valid contacts can be checked with
|
||||
dist < 0. Resulting edge contacts should be preferred over face contacts.
|
||||
|
||||
This method checks all unique edge-pairs and is thus costly to run over large
|
||||
meshes, but is more performant for smaller meshes (boxes, tetrahedra, etc.).
|
||||
This method checks all separating axes via a brute force support function, and
|
||||
is thus costly to run over large meshes, but is more performant for smaller
|
||||
meshes (boxes, tetrahedra, etc.).
|
||||
|
||||
Args:
|
||||
faces_a: Faces for hull A.
|
||||
@@ -731,7 +732,7 @@ def _arcs_intersect(
|
||||
return (cba * dba < 0) & (adc * bdc < 0) & (cba * bdc > 0)
|
||||
|
||||
|
||||
def _sat_approx(
|
||||
def _sat_gaussmap(
|
||||
centroid_a: jax.Array,
|
||||
faces_a: jax.Array,
|
||||
faces_b: jax.Array,
|
||||
@@ -739,23 +740,16 @@ def _sat_approx(
|
||||
vertices_b: jax.Array,
|
||||
normals_a: jax.Array,
|
||||
normals_b: jax.Array,
|
||||
face_edges_a: jax.Array,
|
||||
face_edges_b: jax.Array,
|
||||
face_edge_normals_a: jax.Array,
|
||||
face_edge_normals_b: jax.Array,
|
||||
edges_a: jax.Array,
|
||||
edges_b: jax.Array,
|
||||
edge_face_normals_a: jax.Array,
|
||||
edge_face_normals_b: jax.Array,
|
||||
) -> Tuple[jax.Array, jax.Array, jax.Array]:
|
||||
"""Runs the Separating Axis Test for a pair of hulls.
|
||||
|
||||
Runs the separating axis test for all faces. After obtaining a reference
|
||||
and incident face, tests edge separating axes via edge intersections
|
||||
on gauss maps.
|
||||
|
||||
Certain meshes can have nearly parallel coplanar faces that are not merged.
|
||||
Thus reference/incident faces can be non-overlapping, and face contacts will
|
||||
not get generated. Since we only check edge separating axes on
|
||||
reference/incident faces, the correct edge separating axes may also be
|
||||
missing. We mitigate this issue by checking nearly coplanar anti-parallel
|
||||
reference/incident faces for support, hence why this method is "approximate".
|
||||
Runs the separating axis test for all faces. Tests edge separating axes via
|
||||
edge intersections on gauss maps for all edge pairs. h/t to Dirk Gregorius
|
||||
for the implementation details and gauss map trick.
|
||||
|
||||
Args:
|
||||
centroid_a: Centroid of hull A.
|
||||
@@ -765,10 +759,10 @@ def _sat_approx(
|
||||
vertices_b: Vertices for hull B.
|
||||
normals_a: Normal vectors for hull A faces.
|
||||
normals_b: Normal vectors for hull B faces.
|
||||
face_edges_a: Edges for faces in hull A.
|
||||
face_edges_b: Edges for faces in hull B.
|
||||
face_edge_normals_a: Edge normals for faces in hull A.
|
||||
face_edge_normals_b: Edge normals for faces in hull B.
|
||||
edges_a: Edges for hull A.
|
||||
edges_b: Edges for hull B.
|
||||
edge_face_normals_a: Face normals for edges in hull A.
|
||||
edge_face_normals_b: Face normals for edges in hull B.
|
||||
|
||||
Returns:
|
||||
tuple of dist, pos, and normal
|
||||
@@ -816,75 +810,61 @@ def _sat_approx(
|
||||
-best_axis,
|
||||
)
|
||||
|
||||
# Handle edge separating axes by checking edge pairs on the reference and
|
||||
# incident faces. In principle, the correct edge-edge separating axis can be
|
||||
# created from edge pairs on the reference and incident faces.
|
||||
# first get the edge directions and face edge normals
|
||||
face_edges_a = face_edges_a[face_a_idx]
|
||||
v_norm = jax.vmap(math.normalize)
|
||||
face_edges_dir_a = v_norm(face_edges_a[:, 0] - face_edges_a[:, 1])
|
||||
face_edges_b = face_edges_b[face_b_idx]
|
||||
face_edges_dir_b = v_norm(face_edges_b[:, 0] - face_edges_b[:, 1])
|
||||
face_edge_normals_a = face_edge_normals_a[face_a_idx]
|
||||
face_edge_normals_b = face_edge_normals_b[face_b_idx]
|
||||
|
||||
# scatter to all edge pairs
|
||||
edge_idx_a = jp.repeat(
|
||||
jp.arange(face_edges_a.shape[0]), face_edges_b.shape[0]
|
||||
# Handle edge separating axes by checking all edge pairs.
|
||||
a_idx = jp.tile(jp.arange(edges_a.shape[0]), reps=edges_b.shape[0])
|
||||
b_idx = jp.repeat(
|
||||
jp.arange(edges_b.shape[0]), repeats=edges_a.shape[0], axis=0
|
||||
)
|
||||
normal_a_1 = edge_face_normals_a[a_idx, 0]
|
||||
normal_a_2 = edge_face_normals_a[a_idx, 1]
|
||||
normal_b_1 = edge_face_normals_b[b_idx, 0]
|
||||
normal_b_2 = edge_face_normals_b[b_idx, 1]
|
||||
is_minkowski_face = jax.vmap(_arcs_intersect)(
|
||||
normal_a_1, normal_a_2, -normal_b_1, -normal_b_2
|
||||
)
|
||||
edge_idx_b = jp.tile(jp.arange(face_edges_b.shape[0]), face_edges_a.shape[0])
|
||||
face_edges_dir_a = face_edges_dir_a[edge_idx_a]
|
||||
face_edges_dir_b = face_edges_dir_b[edge_idx_b]
|
||||
face_edges_pt_a = face_edges_a[:, 0][edge_idx_a]
|
||||
face_edges_pt_b = face_edges_b[:, 0][edge_idx_b]
|
||||
face_edge_normals_a = face_edge_normals_a[edge_idx_a]
|
||||
face_edge_normals_b = face_edge_normals_b[edge_idx_b]
|
||||
|
||||
@jax.vmap
|
||||
# get distances
|
||||
edge_a_dir = jax.vmap(math.normalize)(edges_a[:, 0] - edges_a[:, 1])[a_idx]
|
||||
edge_b_dir = jax.vmap(math.normalize)(edges_b[:, 0] - edges_b[:, 1])[b_idx]
|
||||
edges_a, edges_b = edges_a[a_idx], edges_b[b_idx]
|
||||
edge_a_pt, edge_a_pt_2 = edges_a[:, 0], edges_a[:, 1]
|
||||
edge_b_pt, edge_b_pt_2 = edges_b[:, 0], edges_b[:, 1]
|
||||
|
||||
def get_normals(a_dir, a_pt, b_dir):
|
||||
edge_axis = math.normalize(jp.cross(a_dir, b_dir))
|
||||
edge_axis = jp.cross(a_dir, b_dir)
|
||||
degenerate_edge_axis = jp.sum(edge_axis**2) < 1e-6
|
||||
edge_axis = math.normalize(edge_axis)
|
||||
# correct normal to point from a to b, object b is at the origin
|
||||
sign = jp.where(jp.dot(edge_axis, a_pt - centroid_a) > 0.0, 1.0, -1.0)
|
||||
return edge_axis * sign
|
||||
return edge_axis * sign, degenerate_edge_axis
|
||||
|
||||
edge_axes = get_normals(face_edges_dir_a, face_edges_pt_a, face_edges_dir_b)
|
||||
edge_dist = jax.vmap(jp.dot)(edge_axes, face_edges_pt_b - face_edges_pt_a)
|
||||
# handle degenerate axes
|
||||
edge_dist = jp.where((edge_axes**2).sum(axis=1) < 1e-6, 1e6, edge_dist)
|
||||
# ensure edges create a minkowski face by testing intersection on gauss maps
|
||||
is_minkowski_face = jax.vmap(_arcs_intersect)(
|
||||
face_edge_normals_a[:, 0],
|
||||
face_edge_normals_a[:, 1],
|
||||
-face_edge_normals_b[:, 0],
|
||||
-face_edge_normals_b[:, 1],
|
||||
)
|
||||
edge_dist = jp.where(is_minkowski_face, edge_dist, 1e6)
|
||||
edge_dist = jp.where(edge_dist > 0, -1e6, edge_dist)
|
||||
edge_axes, degenerate_edge_axes = jax.vmap(get_normals)(
|
||||
edge_a_dir, edge_a_pt, edge_b_dir)
|
||||
edge_dist = jax.vmap(jp.dot)(edge_axes, edge_b_pt - edge_a_pt)
|
||||
# handle degenerate axis
|
||||
edge_dist = jp.where(degenerate_edge_axes, -jp.inf, edge_dist)
|
||||
# ensure edges create minkowski face
|
||||
edge_dist = jp.where(is_minkowski_face, edge_dist, -jp.inf)
|
||||
|
||||
best_edge_idx = edge_dist.argmax()
|
||||
best_edge_dist = edge_dist[best_edge_idx]
|
||||
# prefer edge over face contacts as long as we have a valid edge contact
|
||||
is_edge_contact = (best_edge_dist > dist.min() + 1e-6) & (best_edge_dist < 0)
|
||||
normal = jp.where(is_edge_contact, edge_axes[best_edge_idx], normal)
|
||||
dist = jp.where(is_edge_contact, jp.array([best_edge_dist, 1, 1, 1]), dist)
|
||||
|
||||
# A failure mode occurs if faces are very narrow and nearly parallel
|
||||
# (i.e. faces did not get merged properly as coplanar faces). The face
|
||||
# contacts will be empty since the reference/incident faces may not overlap.
|
||||
# An edge-edge separating axis will not be found, since the reference and
|
||||
# incident faces will also not overlap or necessarily create a minkowski face.
|
||||
# Thus, we approximate the contact for anti-parallel faces with support.
|
||||
anti_parallel = incident_face_norm.dot(ref_face_norm) < -0.97
|
||||
dist = dist.at[0].set(
|
||||
jp.where(
|
||||
anti_parallel
|
||||
& ~is_face_separating
|
||||
& (dist > 0).all()
|
||||
& ~is_edge_contact,
|
||||
-support[best_idx],
|
||||
dist[0],
|
||||
)
|
||||
is_edge_contact = jp.where(
|
||||
dist.max() < 0, best_edge_dist > dist.max() - 1e-6,
|
||||
(best_edge_dist < 0) & ~jp.isinf(best_edge_dist)
|
||||
)
|
||||
is_edge_contact = is_edge_contact & ~is_face_separating
|
||||
normal = jp.where(is_edge_contact, edge_axes[best_edge_idx], normal)
|
||||
dist = jp.where(
|
||||
is_edge_contact,
|
||||
jp.array([best_edge_dist, 1, 1, 1]),
|
||||
dist,
|
||||
)
|
||||
a_closest, b_closest = math.closest_segment_to_segment_points(
|
||||
edge_a_pt[best_edge_idx], edge_a_pt_2[best_edge_idx],
|
||||
edge_b_pt[best_edge_idx], edge_b_pt_2[best_edge_idx])
|
||||
pos = jp.where(
|
||||
is_edge_contact,
|
||||
jp.tile(0.5 * (a_closest + b_closest), (4, 1)), pos)
|
||||
|
||||
return dist, pos, normal
|
||||
|
||||
@@ -924,15 +904,15 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
|
||||
unique_edges1 = jp.take(vertices1, c1.edge, axis=0)
|
||||
unique_edges2 = jp.take(vertices2, c2.edge, axis=0)
|
||||
|
||||
face_edges1 = jp.take(vertices1, c1.face_edge, axis=0)
|
||||
face_edges2 = jp.take(vertices2, c2.face_edge, axis=0)
|
||||
edges1 = jp.take(vertices1, c1.edge, axis=0)
|
||||
edges2 = jp.take(vertices2, c2.edge, axis=0)
|
||||
|
||||
face_edge_normals1 = c1.face_edge_normal @ to_local_mat.T
|
||||
face_edge_normals2 = c2.face_edge_normal
|
||||
edge_face_normals1 = c1.edge_face_normal @ to_local_mat.T
|
||||
edge_face_normals2 = c2.edge_face_normal
|
||||
|
||||
enable_bruteforce = (
|
||||
unique_edges1.shape[0] * unique_edges2.shape[0]
|
||||
< face_edges1[0].shape[0] * face_edges2[0].shape[0]
|
||||
< edges1[0].shape[0] * edges2[0].shape[0]
|
||||
)
|
||||
if enable_bruteforce:
|
||||
dist, pos, normal = _sat_bruteforce(
|
||||
@@ -946,7 +926,7 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
|
||||
unique_edges2,
|
||||
)
|
||||
else:
|
||||
dist, pos, normal = _sat_approx(
|
||||
dist, pos, normal = _sat_gaussmap(
|
||||
to_local_pos,
|
||||
faces1,
|
||||
faces2,
|
||||
@@ -954,10 +934,10 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact:
|
||||
vertices2,
|
||||
normals1,
|
||||
normals2,
|
||||
face_edges1,
|
||||
face_edges2,
|
||||
face_edge_normals1,
|
||||
face_edge_normals2,
|
||||
edges1,
|
||||
edges2,
|
||||
edge_face_normals1,
|
||||
edge_face_normals2,
|
||||
)
|
||||
|
||||
# Go back to world frame.
|
||||
|
||||
Reference in New Issue
Block a user