Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions asv/benchmarks/collision/benchmark_contact_reduction.py
Original file line number Diff line number Diff line change
Expand Up @@ -59,9 +59,8 @@ def benchmark_insert_kernel(
normal = wp.vec3(nx, ny, nz)

depth = -0.01 + float(tid % 100) * 0.0001
feature = tid % 10

export_and_reduce_contact(shape_a, shape_b, position, normal, depth, feature, reducer_data, beta0, beta1)
export_and_reduce_contact(shape_a, shape_b, position, normal, depth, reducer_data, beta0, beta1)


class FastGlobalContactReducerInsert:
Expand Down
14 changes: 4 additions & 10 deletions newton/_src/geometry/collision_convex.py
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ def create_solve_convex_multi_contact(support_func: Any, writer_func: Any, post_

Args:
support_func: Support mapping function for shapes that takes
(geometry, direction, data_provider) and returns (point, feature_id)
(geometry, direction, data_provider) and returns a support point
writer_func: Function to write contact data (signature: (ContactData, writer_data) -> None)
post_process_contact: Function to post-process contact data

Expand Down Expand Up @@ -147,7 +147,6 @@ def solve_convex_multi_contact(
contact_data.contact_point_center = point
contact_data.contact_normal_a_to_b = normal
contact_data.contact_distance = signed_distance
contact_data.feature = wp.uint32(0)

contact_data = post_process_contact(
contact_data, geom_a, position_a, orientation_a, geom_b, position_b, orientation_b
Expand Down Expand Up @@ -188,7 +187,7 @@ def create_solve_convex_single_contact(support_func: Any, writer_func: Any, post

Args:
support_func: Support mapping function for shapes that takes
(geometry, direction, data_provider) and returns (point, feature_id)
(geometry, direction, data_provider) and returns a support point
writer_func: Function to write contact data (signature: (ContactData, writer_data) -> None)
post_process_contact: Function to post-process contact data

Expand Down Expand Up @@ -237,9 +236,7 @@ def solve_convex_single_contact(
# Enlarge a little bit to avoid contact flickering when the signed distance is close to 0
enlarge = 1e-4
# Try MPR first (optimized for overlapping shapes, which is the common case)
collision, signed_distance, point, normal, _feature_a_id, _feature_b_id = wp.static(
create_solve_mpr(support_func)
)(
collision, signed_distance, point, normal = wp.static(create_solve_mpr(support_func))(
geom_a,
geom_b,
orientation_a,
Expand All @@ -253,9 +250,7 @@ def solve_convex_single_contact(

if not collision:
# MPR reported no collision, fall back to GJK for separated shapes
collision, signed_distance, point, normal, _feature_a_id, _feature_b_id = wp.static(
create_solve_closest_distance(support_func)
)(
collision, signed_distance, point, normal = wp.static(create_solve_closest_distance(support_func))(
geom_a,
geom_b,
orientation_a,
Expand All @@ -271,7 +266,6 @@ def solve_convex_single_contact(
contact_data.contact_point_center = point
contact_data.contact_normal_a_to_b = normal
contact_data.contact_distance = signed_distance
contact_data.feature = wp.uint32(0)

contact_data = post_process_contact(
contact_data, geom_a, position_a, orientation_a, geom_b, position_b, orientation_b
Expand Down
55 changes: 6 additions & 49 deletions newton/_src/geometry/collision_core.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,42 +39,6 @@
_vec1 = wp.types.vector(1, wp.float32)


@wp.func
def build_pair_key2(shape_a: wp.uint32, shape_b: wp.uint32) -> wp.uint64:
"""
Build a 64-bit key from two shape indices.
Upper 32 bits: shape_a
Lower 32 bits: shape_b
"""
key = wp.uint64(shape_a)
key = key << wp.uint64(32)
key = key | wp.uint64(shape_b)
return key


@wp.func
def build_pair_key3(shape_a: wp.uint32, shape_b: wp.uint32, triangle_idx: wp.uint32) -> wp.uint64:
"""
Build a 63-bit key from two shape indices and a triangle index (MSB is 0 for signed int64 compatibility).
Bit 63: 0 (reserved for sign bit)
Bits 62-43: shape_a (20 bits)
Bits 42-23: shape_b (20 bits)
Bits 22-0: triangle_idx (23 bits)

Max values: shape_a < 2^20 (1,048,576), shape_b < 2^20 (1,048,576), triangle_idx < 2^23 (8,388,608)
"""
assert shape_a < wp.uint32(1048576), "shape_a must be < 2^20 (1,048,576)"
assert shape_b < wp.uint32(1048576), "shape_b must be < 2^20 (1,048,576)"
assert triangle_idx < wp.uint32(8388608), "triangle_idx must be < 2^23 (8,388,608)"

key = wp.uint64(shape_a & wp.uint32(0xFFFFF)) # Mask to 20 bits
key = key << wp.uint64(20)
key = key | wp.uint64(shape_b & wp.uint32(0xFFFFF)) # Mask to 20 bits
key = key << wp.uint64(23)
key = key | wp.uint64(triangle_idx & wp.uint32(0x7FFFFF)) # Mask to 23 bits
return key


@wp.func
def is_discrete_shape(shape_type: int) -> bool:
"""A discrete shape can be represented with a finite amount of flat polygon faces."""
Expand Down Expand Up @@ -313,7 +277,6 @@ def compute_gjk_mpr_contacts(
thickness_a: float,
thickness_b: float,
writer_data: Any,
pair_key: wp.uint64,
):
"""
Compute contacts between two shapes using GJK/MPR algorithm and write them.
Expand Down Expand Up @@ -361,7 +324,6 @@ def compute_gjk_mpr_contacts(
contact_template.shape_a = shape_a
contact_template.shape_b = shape_b
contact_template.margin = rigid_contact_margin
contact_template.feature_pair_key = pair_key

if wp.static(ENABLE_MULTI_CONTACT):
wp.static(create_solve_convex_multi_contact(support_map, writer_func, post_process_contact))(
Expand Down Expand Up @@ -430,30 +392,29 @@ def compute_tight_aabb_from_support(

# Compute AABB extents by evaluating support function in local space
# Dot products are done in local space to avoid expensive rotations
support_point = wp.vec3()

# Max X: support along +local_x, dot in local space
support_point, _feature_id = support_map(shape_data, local_x, data_provider)
support_point = support_map(shape_data, local_x, data_provider)
max_x = wp.dot(local_x, support_point)

# Max Y: support along +local_y, dot in local space
support_point, _feature_id = support_map(shape_data, local_y, data_provider)
support_point = support_map(shape_data, local_y, data_provider)
max_y = wp.dot(local_y, support_point)

# Max Z: support along +local_z, dot in local space
support_point, _feature_id = support_map(shape_data, local_z, data_provider)
support_point = support_map(shape_data, local_z, data_provider)
max_z = wp.dot(local_z, support_point)

# Min X: support along -local_x, dot in local space
support_point, _feature_id = support_map(shape_data, -local_x, data_provider)
support_point = support_map(shape_data, -local_x, data_provider)
min_x = wp.dot(local_x, support_point)

# Min Y: support along -local_y, dot in local space
support_point, _feature_id = support_map(shape_data, -local_y, data_provider)
support_point = support_map(shape_data, -local_y, data_provider)
min_y = wp.dot(local_y, support_point)

# Min Z: support along -local_z, dot in local space
support_point, _feature_id = support_map(shape_data, -local_z, data_provider)
support_point = support_map(shape_data, -local_z, data_provider)
min_z = wp.dot(local_z, support_point)

# AABB in world space (add world position to extents)
Expand Down Expand Up @@ -665,9 +626,6 @@ def find_contacts(
shape_data_b, quat_b, pos_b, pos_a, bsphere_radius_a + rigid_contact_margin
)

# Build pair key for contact matching
pair_key = build_pair_key2(wp.uint32(shape_a), wp.uint32(shape_b))

# Compute and write contacts using GJK/MPR
wp.static(create_compute_gjk_mpr_contacts(writer_func))(
shape_data_a,
Expand All @@ -682,7 +640,6 @@ def find_contacts(
thickness_a,
thickness_b,
writer_data,
pair_key,
)

return find_contacts
Expand Down
4 changes: 0 additions & 4 deletions newton/_src/geometry/contact_data.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,6 @@ class ContactData:
shape_a: Index of the first shape in the collision pair
shape_b: Index of the second shape in the collision pair
margin: Contact detection margin/threshold
feature: Shape-specific feature identifier (e.g., vertex, edge, or face ID)
feature_pair_key: Unique key for contact pair matching across timesteps
contact_stiffness: Contact stiffness. 0.0 means no stiffness was set.
contact_damping: Contact damping scale. 0.0 means no damping was set.
contact_friction_scale: Friction scaling factor. 0.0 means no friction was set.
Expand All @@ -59,8 +57,6 @@ class ContactData:
shape_a: int
shape_b: int
margin: float
feature: wp.uint32
feature_pair_key: wp.uint64
contact_stiffness: float
contact_damping: float
contact_friction_scale: float
Loading
Loading