diff --git a/asv/benchmarks/collision/benchmark_contact_reduction.py b/asv/benchmarks/collision/benchmark_contact_reduction.py index 047d8a2f5..cb67fd229 100644 --- a/asv/benchmarks/collision/benchmark_contact_reduction.py +++ b/asv/benchmarks/collision/benchmark_contact_reduction.py @@ -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: diff --git a/newton/_src/geometry/collision_convex.py b/newton/_src/geometry/collision_convex.py index cbb1e8426..7b6d23117 100644 --- a/newton/_src/geometry/collision_convex.py +++ b/newton/_src/geometry/collision_convex.py @@ -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 @@ -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 @@ -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 @@ -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, @@ -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, @@ -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 diff --git a/newton/_src/geometry/collision_core.py b/newton/_src/geometry/collision_core.py index 447364be1..1aa094444 100644 --- a/newton/_src/geometry/collision_core.py +++ b/newton/_src/geometry/collision_core.py @@ -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.""" @@ -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. @@ -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))( @@ -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) @@ -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, @@ -682,7 +640,6 @@ def find_contacts( thickness_a, thickness_b, writer_data, - pair_key, ) return find_contacts diff --git a/newton/_src/geometry/contact_data.py b/newton/_src/geometry/contact_data.py index 63d55b2ef..abd5e76ca 100644 --- a/newton/_src/geometry/contact_data.py +++ b/newton/_src/geometry/contact_data.py @@ -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. @@ -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 diff --git a/newton/_src/geometry/contact_matcher.py b/newton/_src/geometry/contact_matcher.py deleted file mode 100644 index 85a95c12e..000000000 --- a/newton/_src/geometry/contact_matcher.py +++ /dev/null @@ -1,234 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers -# SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Contact matching across simulation timesteps for warm-starting physics solvers. - -Tracks contacts between timesteps by matching (key, payload) pairs, enabling: -- Contact persistence for stable simulation -- Warm-starting constraint solvers -- Smooth contact forces over time - -The matcher uses sorted keys with binary search for O(log n) lookups per contact. -""" - -import warp as wp - - -@wp.func -def binary_search_find_range_start( - keys: wp.array(dtype=wp.uint64, ndim=1), - key_to_find: wp.uint64, - lower: wp.int32, - upper: wp.int32, -) -> wp.int32: - """ - Binary search to find the start index of the first occurrence of key_to_find. - Returns -1 if key is not found. - """ - # Find lower bound: first position where keys[i] >= key_to_find - left = lower - right = upper - - while left < right: - mid = left + (right - left) // 2 - if keys[mid] < key_to_find: - left = mid + 1 - else: - right = mid - - # Validate that the key was actually found - if left >= upper or keys[left] != key_to_find: - return wp.int32(-1) - - return left - - -@wp.kernel(enable_backward=False) -def find_keys_in_buffer_and_update_map( - # Prev step data - sorted_keys_prev_step: wp.array(dtype=wp.uint64, ndim=1), - num_keys_prev_step: wp.array(dtype=wp.int32, ndim=1), - payloads_prev_step: wp.array(dtype=wp.uint32, ndim=1), - sorted_to_unsorted_map_prev_step: wp.array(dtype=wp.int32, ndim=1), - # Current step data - raw_keys_current_step: wp.array(dtype=wp.uint64, ndim=1), - num_keys_current_step: wp.array(dtype=wp.int32, ndim=1), - payloads_current_step: wp.array(dtype=wp.uint32, ndim=1), - # Output - result_index_map_new_to_old: wp.array(dtype=wp.int32, ndim=1), -): - """ - Match current contacts to previous timestep contacts using (key, payload) pairs. - - For each current contact, finds matching contact from previous step by: - 1. Binary search on sorted keys (O(log n)) - 2. Linear scan through matching keys to find matching payload - - Outputs mapping: current_index -> previous_original_index (or -1 if new contact) - """ - tid = wp.tid() - if tid >= num_keys_current_step[0]: - return - - key_to_find = raw_keys_current_step[tid] - payload_to_find = payloads_current_step[tid] - count = num_keys_prev_step[0] - - start = binary_search_find_range_start(sorted_keys_prev_step, key_to_find, 0, count) - if start == -1: - result_index_map_new_to_old[tid] = -1 - return - - result = wp.int32(-1) - - while start < count: - if sorted_keys_prev_step[start] != key_to_find: - break - - if payloads_prev_step[start] == payload_to_find: - result = sorted_to_unsorted_map_prev_step[start] - break - - start += 1 - - result_index_map_new_to_old[tid] = result - - -@wp.func_native(""" - return 0x7FFFFFFFFFFFFFFFull; -""") -def uint64_sentinel_value() -> wp.uint64: ... - - -@wp.kernel(enable_backward=False) -def prepare_sort( - key_source: wp.array(dtype=wp.uint64, ndim=1), - keys: wp.array(dtype=wp.uint64, ndim=1), - sorted_to_unsorted_map: wp.array(dtype=wp.int32, ndim=1), - count: wp.array(dtype=wp.int32, ndim=1), -): - """Prepare data for radix sort by copying keys and initializing index map.""" - tid = wp.tid() - if tid < count[0]: - keys[tid] = key_source[tid] - sorted_to_unsorted_map[tid] = tid - else: - keys[tid] = ( - uint64_sentinel_value() - ) # Fill unused slots with sentinel value (sorts to end when treated as signed int64) - - -@wp.kernel(enable_backward=False) -def reorder_payloads( - payload_source: wp.array(dtype=wp.uint32, ndim=1), - payloads: wp.array(dtype=wp.uint32, ndim=1), - sorted_to_unsorted_map: wp.array(dtype=wp.int32, ndim=1), - count: wp.array(dtype=wp.int32, ndim=1), - count_copy: wp.array(dtype=wp.int32, ndim=1), -): - """Reorder payloads to match sorted key order and save count for next timestep.""" - tid = wp.tid() - if tid == 0: - count_copy[0] = count[0] # Save current count for next timestep - - if tid < count[0]: - payloads[tid] = payload_source[sorted_to_unsorted_map[tid]] - - -class ContactMatcher: - """ - Matches contacts across simulation timesteps for warm-starting physics solvers. - - Maintains sorted contact data from previous timestep and matches current contacts - to find persistent contacts. Uses binary search for efficient O(log n) lookups. - - Args: - max_num_contacts: Maximum number of contacts to track - device: Device to allocate buffers on (None for default) - """ - - def __init__(self, max_num_contacts: int, device=None): - self.max_num_contacts = max_num_contacts - # Factor of 2 because that is a requirement of the sort algorithm - self.sorted_keys_prev_step = wp.zeros(2 * max_num_contacts, dtype=wp.uint64, device=device) - self.sorted_to_unsorted_map_prev_step = wp.zeros(2 * max_num_contacts, dtype=wp.int32, device=device) - self.payloads_prev_step = wp.zeros(max_num_contacts, dtype=wp.uint32, device=device) - self.num_keys_prev_step = wp.zeros(1, dtype=wp.int32, device=device) - - def launch( - self, - keys: wp.array(dtype=wp.uint64, ndim=1), - num_keys: wp.array(dtype=wp.int32, ndim=1), - payloads: wp.array(dtype=wp.uint32, ndim=1), - result_index_map_new_to_old: wp.array(dtype=wp.int32, ndim=1), - device=None, - ): - """ - Match current contacts to previous timestep and prepare for next timestep. - - Args: - keys: Contact pair keys (shape_a, shape_b, optional triangle_idx) - num_keys: Single-element array with number of active contacts - payloads: Contact feature IDs for disambiguation - result_index_map_new_to_old: Output mapping from current to previous indices (-1 for new contacts) - device: Device to launch on (None for default) - """ - wp.launch( - kernel=find_keys_in_buffer_and_update_map, - dim=self.max_num_contacts, - inputs=[ - self.sorted_keys_prev_step, - self.num_keys_prev_step, - self.payloads_prev_step, - self.sorted_to_unsorted_map_prev_step, - keys, - num_keys, - payloads, - result_index_map_new_to_old, - ], - device=device, - ) - - wp.launch( - kernel=prepare_sort, - dim=self.max_num_contacts, - inputs=[keys, self.sorted_keys_prev_step, self.sorted_to_unsorted_map_prev_step, num_keys], - device=device, - ) - - # Cast uint64 keys to int64 for radix_sort_pairs (which supports int64 + int32) - keys_as_int64 = wp.array( - ptr=self.sorted_keys_prev_step.ptr, - dtype=wp.int64, - shape=self.sorted_keys_prev_step.shape, - device=self.sorted_keys_prev_step.device, - copy=False, - ) - - wp.utils.radix_sort_pairs(keys_as_int64, self.sorted_to_unsorted_map_prev_step, self.max_num_contacts) - - wp.launch( - kernel=reorder_payloads, - dim=self.max_num_contacts, - inputs=[ - payloads, - self.payloads_prev_step, - self.sorted_to_unsorted_map_prev_step, - num_keys, - self.num_keys_prev_step, - ], - device=device, - ) diff --git a/newton/_src/geometry/contact_reduction.py b/newton/_src/geometry/contact_reduction.py index 1ac784265..de2f694fe 100644 --- a/newton/_src/geometry/contact_reduction.py +++ b/newton/_src/geometry/contact_reduction.py @@ -60,7 +60,7 @@ class ContactStruct: position: wp.vec3 normal: wp.vec3 depth: wp.float32 - feature: wp.int32 + mode: wp.int32 # Used to track collision mode (e.g., which mesh the triangle came from) projection: wp.float32 @@ -361,7 +361,6 @@ class ContactReductionFunctions: 1. Contacts are binned by normal direction (20 bins based on icosahedron faces) 2. Within each bin, contacts compete for slots using a scoring function 3. Winners are determined via atomic operations in shared memory - 4. Duplicates (same geometric feature) are filtered out **Scoring Function:** @@ -402,7 +401,7 @@ def __init__(self, betas: tuple = (1000000.0, 0.0001)): # Warp functions self.store_reduced_contact = self._create_store_reduced_contact() - self.filter_unique_contacts = self._create_filter_unique_contacts() + self.collect_active_contacts = self._create_collect_active_contacts() def create_betas_array(self, device=None) -> wp.array: """Create a warp array with the beta values.""" @@ -435,7 +434,7 @@ def store_reduced_contact( Args: thread_id: Thread index within the block active: Whether this thread has a valid contact to store - c: Contact data (position, normal, depth, feature) + c: Contact data (position, normal, depth, mode) buffer: Shared memory buffer for winning contacts active_ids: Tracks which slots contain valid contacts betas_arr: Array of depth thresholds (contact participates if depth < beta) @@ -513,71 +512,42 @@ def store_reduced_contact( return store_reduced_contact - def _create_filter_unique_contacts(self): - """Create the filter_unique_contacts warp function. + def _create_collect_active_contacts(self): + """Create the collect_active_contacts warp function. - The returned function removes duplicate contacts that won multiple slots - but originate from the same geometric feature (e.g., same triangle). - Only the first occurrence per feature is kept. + The returned function collects all valid contacts from the reduction buffer + into a compact list of slot indices. """ - num_reduction_slots = self.num_reduction_slots num_betas = self.num_betas - get_smem = self.get_smem_reduction @wp.func - def filter_unique_contacts( + def collect_active_contacts( thread_id: int, buffer: wp.array(dtype=ContactStruct), active_ids: wp.array(dtype=int), empty_marker: float, ): - """Remove duplicate contacts, keeping first occurrence per feature. + """Collect all valid contacts into a compact list. Args: thread_id: Thread index within the block buffer: Shared memory buffer containing reduced contacts - active_ids: Output array of unique contact slot indices + active_ids: Output array of valid contact slot indices empty_marker: Sentinel value indicating empty slots """ slots_per_bin = wp.static(NUM_SPATIAL_DIRECTIONS * num_betas + 1) num_slots = wp.static(NUM_NORMAL_BINS * slots_per_bin) - keep_flags = wp.array( - ptr=wp.static(get_smem)(), - shape=(wp.static(num_reduction_slots),), - dtype=wp.int32, - ) - - for i in range(thread_id, num_slots, wp.block_dim()): - keep_flags[i] = 0 - synchronize() - - if thread_id < wp.static(NUM_NORMAL_BINS): - bin_id = thread_id - base_key = bin_id * slots_per_bin - for slot_i in range(slots_per_bin): - key_i = base_key + slot_i - if buffer[key_i].projection > empty_marker: - feature_i = buffer[key_i].feature - is_dup = int(0) - for slot_j in range(slot_i): - key_j = base_key + slot_j - if buffer[key_j].projection > empty_marker and buffer[key_j].feature == feature_i: - is_dup = 1 - if is_dup == 0: - keep_flags[key_i] = 1 - synchronize() - if thread_id == 0: write_idx = int(0) for key in range(num_slots): - if keep_flags[key] == 1: + if buffer[key].projection > empty_marker: active_ids[write_idx] = key write_idx += 1 active_ids[num_slots] = write_idx synchronize() - return filter_unique_contacts + return collect_active_contacts get_shared_memory_pointer_block_dim_plus_2_ints = create_shared_memory_pointer_block_dim_func(2) diff --git a/newton/_src/geometry/contact_reduction_global.py b/newton/_src/geometry/contact_reduction_global.py index 6a538cedc..00837134c 100644 --- a/newton/_src/geometry/contact_reduction_global.py +++ b/newton/_src/geometry/contact_reduction_global.py @@ -40,7 +40,6 @@ ) from .collision_core import ( - build_pair_key3, create_compute_gjk_mpr_contacts, get_triangle_shape_from_mesh, ) @@ -209,7 +208,7 @@ class GlobalContactReducerData: # Contact buffer arrays position_depth: wp.array(dtype=wp.vec4) - normal_feature: wp.array(dtype=wp.vec4) + normal: wp.array(dtype=wp.vec3) shape_pairs: wp.array(dtype=wp.vec2i) contact_count: wp.array(dtype=wp.int32) capacity: int @@ -288,15 +287,15 @@ class GlobalContactReducer: values (one per slot = direction x beta + deepest). This allows one thread to process all slots for a bin and deduplicate locally. - Contact data is packed into vec4 for efficient memory access: + Contact data is packed for efficient memory access: - position_depth: vec4(position.x, position.y, position.z, depth) - - normal_feature: vec4(normal.x, normal.y, normal.z, float_bits(feature)) + - normal: vec3(normal.x, normal.y, normal.z) Attributes: capacity: Maximum number of contacts that can be stored values_per_key: Number of value slots per hashtable entry (13 for 2 betas) position_depth: vec4 array storing position.xyz and depth - normal_feature: vec4 array storing normal.xyz and feature + normal: vec3 array storing contact normal shape_pairs: vec2i array storing (shape_a, shape_b) per contact contact_count: Atomic counter for allocated contacts hashtable: HashTable for tracking best contacts (keys only) @@ -325,9 +324,9 @@ def __init__( # Values per key: 6 directions x num_betas + 1 deepest self.values_per_key = NUM_SPATIAL_DIRECTIONS * num_betas + 1 - # Contact buffer (struct of arrays with vec4 packing) + # Contact buffer (struct of arrays) self.position_depth = wp.zeros(capacity, dtype=wp.vec4, device=device) - self.normal_feature = wp.zeros(capacity, dtype=wp.vec4, device=device) + self.normal = wp.zeros(capacity, dtype=wp.vec3, device=device) self.shape_pairs = wp.zeros(capacity, dtype=wp.vec2i, device=device) # Atomic counter for contact allocation @@ -393,7 +392,7 @@ def get_data_struct(self) -> GlobalContactReducerData: """ data = GlobalContactReducerData() data.position_depth = self.position_depth - data.normal_feature = self.normal_feature + data.normal = self.normal data.shape_pairs = self.shape_pairs data.contact_count = self.contact_count data.capacity = self.capacity @@ -412,7 +411,6 @@ def export_contact_to_buffer( position: wp.vec3, normal: wp.vec3, depth: float, - feature: int, reducer_data: GlobalContactReducerData, ) -> int: """Store a contact in the buffer without reduction. @@ -423,7 +421,6 @@ def export_contact_to_buffer( position: Contact position in world space normal: Contact normal depth: Penetration depth (negative = penetrating) - feature: Feature identifier for deduplication reducer_data: GlobalContactReducerData with all arrays Returns: @@ -436,7 +433,7 @@ def export_contact_to_buffer( # Store contact data (packed into vec4) reducer_data.position_depth[contact_id] = wp.vec4(position[0], position[1], position[2], depth) - reducer_data.normal_feature[contact_id] = wp.vec4(normal[0], normal[1], normal[2], int_as_float(wp.int32(feature))) + reducer_data.normal[contact_id] = normal reducer_data.shape_pairs[contact_id] = wp.vec2i(shape_a, shape_b) return contact_id @@ -459,12 +456,11 @@ def reduce_contact_in_hashtable( """ # Read contact data from buffer pd = reducer_data.position_depth[contact_id] - nf = reducer_data.normal_feature[contact_id] + normal = reducer_data.normal[contact_id] pair = reducer_data.shape_pairs[contact_id] position = wp.vec3(pd[0], pd[1], pd[2]) depth = pd[3] - normal = wp.vec3(nf[0], nf[1], nf[2]) shape_a = pair[0] shape_b = pair[1] @@ -525,13 +521,12 @@ def export_and_reduce_contact( position: wp.vec3, normal: wp.vec3, depth: float, - feature: int, reducer_data: GlobalContactReducerData, beta0: float, beta1: float, ) -> int: """Legacy wrapper for backward compatibility.""" - contact_id = export_contact_to_buffer(shape_a, shape_b, position, normal, depth, feature, reducer_data) + contact_id = export_contact_to_buffer(shape_a, shape_b, position, normal, depth, reducer_data) if contact_id >= 0: reduce_contact_in_hashtable(contact_id, reducer_data, beta0, beta1) @@ -574,27 +569,25 @@ def reduce_buffered_contacts_kernel( def unpack_contact( contact_id: int, position_depth: wp.array(dtype=wp.vec4), - normal_feature: wp.array(dtype=wp.vec4), + normal: wp.array(dtype=wp.vec3), ): """Unpack contact data from the buffer. Args: contact_id: Index into the contact buffer position_depth: Contact buffer for position.xyz + depth - normal_feature: Contact buffer for normal.xyz + feature + normal: Contact buffer for normal Returns: - Tuple of (position, normal, depth, feature) + Tuple of (position, normal, depth) """ pd = position_depth[contact_id] - nf = normal_feature[contact_id] + n = normal[contact_id] position = wp.vec3(pd[0], pd[1], pd[2]) depth = pd[3] - normal = wp.vec3(nf[0], nf[1], nf[2]) - feature = float_as_int(nf[3]) - return position, normal, depth, feature + return position, n, depth @wp.func @@ -623,7 +616,6 @@ def write_contact_to_reducer( depth = contact_data.contact_distance shape_a = contact_data.shape_a shape_b = contact_data.shape_b - feature = int(contact_data.feature) # Store contact ONLY (registration to hashtable happens in a separate kernel) # This reduces register pressure on the contact generation kernel @@ -633,7 +625,6 @@ def write_contact_to_reducer( position=position, normal=normal, depth=depth, - feature=feature, reducer_data=reducer_data, ) @@ -667,7 +658,7 @@ def export_reduced_contacts_kernel( ht_active_slots: wp.array(dtype=wp.int32), # Contact buffer arrays position_depth: wp.array(dtype=wp.vec4), - normal_feature: wp.array(dtype=wp.vec4), + normal: wp.array(dtype=wp.vec3), shape_pairs: wp.array(dtype=wp.vec2i), # Shape data for extracting thickness shape_data: wp.array(dtype=wp.vec4), @@ -723,7 +714,7 @@ def export_reduced_contacts_kernel( num_exported = num_exported + 1 # Unpack contact data - position, normal, depth, feature = unpack_contact(contact_id, position_depth, normal_feature) + position, contact_normal, depth = unpack_contact(contact_id, position_depth, normal) # Get shape pair pair = shape_pairs[contact_id] @@ -742,7 +733,7 @@ def export_reduced_contacts_kernel( # Create ContactData struct contact_data = ContactData() contact_data.contact_point_center = position - contact_data.contact_normal_a_to_b = normal + contact_data.contact_normal_a_to_b = contact_normal contact_data.contact_distance = depth contact_data.radius_eff_a = 0.0 contact_data.radius_eff_b = 0.0 @@ -751,8 +742,6 @@ def export_reduced_contacts_kernel( contact_data.shape_a = shape_a contact_data.shape_b = shape_b contact_data.margin = margin - contact_data.feature = wp.uint32(feature) - contact_data.feature_pair_key = wp.uint64(0) # Call the writer function writer_func(contact_data, writer_data, -1) @@ -848,9 +837,6 @@ def mesh_triangle_contacts_to_reducer_kernel( margin_b = shape_contact_margin[shape_b] margin = wp.max(margin_a, margin_b) - # Build pair key including triangle index - pair_key = build_pair_key3(wp.uint32(shape_a), wp.uint32(shape_b), wp.uint32(tri_idx)) - # Compute and write contacts using GJK/MPR wp.static(create_compute_gjk_mpr_contacts(write_to_reducer_with_betas))( shape_data_a, @@ -865,7 +851,6 @@ def mesh_triangle_contacts_to_reducer_kernel( thickness_a, thickness_b, reducer_data, - pair_key, ) return mesh_triangle_contacts_to_reducer_kernel diff --git a/newton/_src/geometry/mpr.py b/newton/_src/geometry/mpr.py index 07833687a..32539552c 100644 --- a/newton/_src/geometry/mpr.py +++ b/newton/_src/geometry/mpr.py @@ -38,7 +38,6 @@ - Works directly with penetrating contacts (no need for EPA as a separate step) - More numerically stable than EPA for deep penetrations - Returns collision normal, signed distance, and witness points -- Supports feature ID tracking for contact persistence The implementation uses support mapping to query shape geometry, making it applicable to any convex shape that provides a support function. @@ -72,7 +71,7 @@ def create_support_map_function(support_func: Any): Args: support_func: Support mapping function for individual shapes that takes - (geometry, direction, data_provider) and returns (point, feature_id) + (geometry, direction, data_provider) and returns a support point Returns: Tuple of three functions: @@ -89,7 +88,7 @@ def support_map_b( orientation_b: wp.quat, position_b: wp.vec3, data_provider: Any, - ) -> tuple[wp.vec3, int]: + ) -> wp.vec3: """ Support mapping for shape B with transformation. @@ -101,19 +100,19 @@ def support_map_b( data_provider: Support mapping data provider Returns: - Tuple of (support point in world space, feature ID) + Support point in world space """ # Transform direction to local space of shape B tmp = wp.quat_rotate_inv(orientation_b, direction) # Get support point in local space - result, feature_id = support_func(geom_b, tmp, data_provider) + result = support_func(geom_b, tmp, data_provider) # Transform result to world space result = wp.quat_rotate(orientation_b, result) result = result + position_b - return result, feature_id + return result @wp.func def minkowski_support( @@ -124,7 +123,7 @@ def minkowski_support( position_b: wp.vec3, extend: float, data_provider: Any, - ) -> tuple[Vert, int, int]: + ) -> Vert: """ Compute support point on Minkowski difference A - B. @@ -138,20 +137,16 @@ def minkowski_support( data_provider: Support mapping data provider Returns: - Tuple of (Vert containing support points, feature ID A, feature ID B) + Vert containing support points """ v = Vert() # Support point on A in positive direction - tmp_result_a = support_func(geom_a, direction, data_provider) - point_a = tmp_result_a[0] - feature_a_id = tmp_result_a[1] + point_a = support_func(geom_a, direction, data_provider) # Support point on B in negative direction tmp_direction = -direction - tmp_result_b = support_map_b(geom_b, tmp_direction, orientation_b, position_b, data_provider) - v.B = tmp_result_b[0] - feature_b_id = tmp_result_b[1] + v.B = support_map_b(geom_b, tmp_direction, orientation_b, position_b, data_provider) # Apply contact offset extension d = wp.normalize(direction) * extend * 0.5 @@ -161,7 +156,7 @@ def minkowski_support( # Store BtoA vector v.BtoA = point_a - v.B - return v, feature_a_id, feature_b_id + return v @wp.func def geometric_center( @@ -275,9 +270,7 @@ def solve_mpr_core( normal = -v0.BtoA # First support point - v1, _feature_a_id, _feature_b_id = minkowski_support( - geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider - ) + v1 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider) point_a = vert_a(v1) point_b = v1.B @@ -297,9 +290,7 @@ def solve_mpr_core( return True, point_a, point_b, normal, penetration # Second support point - v2, _feature_a_id, _feature_b_id = minkowski_support( - geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider - ) + v2 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider) if wp.dot(v2.BtoA, normal) <= 0.0: return False, point_a, point_b, normal, penetration @@ -334,9 +325,7 @@ def solve_mpr_core( phase1 += 1 - v3, _feature_a_id, _feature_b_id = minkowski_support( - geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider - ) + v3 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider) if wp.dot(v3.BtoA, normal) <= 0.0: return False, point_a, point_b, normal, penetration @@ -383,9 +372,7 @@ def solve_mpr_core( # If the origin is inside the wedge, we have a hit hit = d >= 0.0 - v4, _feature_a_id, _feature_b_id = minkowski_support( - geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider - ) + v4 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider) temp3 = v4.BtoA - v3.BtoA delta = wp.dot(temp3, normal) diff --git a/newton/_src/geometry/multicontact.py b/newton/_src/geometry/multicontact.py index 29da9ad44..073595698 100644 --- a/newton/_src/geometry/multicontact.py +++ b/newton/_src/geometry/multicontact.py @@ -22,8 +22,8 @@ Multi-contact manifold generation for collision detection. This module implements contact manifold generation algorithms for computing -multiple contact points between colliding shapes. It includes polygon clipping, -feature tracking, and contact point selection algorithms. +multiple contact points between colliding shapes. It includes polygon clipping +and contact point selection algorithms. """ from typing import Any @@ -719,109 +719,6 @@ def remove_zero_length_edges( return new_loop_count -@wp.func -def edge_key(per_vertex_features: wp.types.vector(6, wp.uint8), count: int, edge_id: int) -> wp.uint32: - """ - Creates a packed edge key from two consecutive feature IDs. - Used to create compact identifiers for edges defined by vertex pairs. - - Args: - per_vertex_features: Array of feature IDs. - count: Number of features in the array. - edge_id: Index of the first vertex of the edge. - - Returns: - 16-bit packed edge key: (first_feature << 8) | second_feature. - """ - # An edge always goes from one vertex to the next, wrapping around at the end. - first = per_vertex_features[edge_id] - second = per_vertex_features[(edge_id + 1) % count] - return wp.uint32(wp.uint32(first) << wp.uint32(8)) | wp.uint32(second) - - -@wp.func -def feature_id( - loop_seg_ids: wp.array(dtype=wp.uint8), - loop_id: int, - loop_count: int, - features_a: wp.types.vector(6, wp.uint8), - features_b: wp.types.vector(6, wp.uint8), - m_a_count: int, - m_b_count: int, -) -> wp.uint32: - """ - Determines the feature identifier for a vertex in the clipped contact polygon. - This function assigns feature IDs that encode which geometric features from the original - collision shapes (vertices, edges, or edge-edge intersections) each contact point represents. - - ENCODING SCHEME: - - Original trim poly vertex: 8-bit feature ID from features_a - - Original loop poly vertex: 16-bit (features_b[vertex] << 8) - - Edge intersections: 32-bit ((edge1_key << 16) | edge2_key) - - Shape intersections: 32-bit ((shapeA_edge << 16) | shapeB_edge) - - SEGMENT ID CONVENTION: - - IDs 0-5: segments from trim polygon (shape A) - - IDs 6+: segments from loop polygon (shape B, with offset) - - Args: - loop_seg_ids: Array of segment IDs for the current clipped polygon. - loop_id: Index of the vertex to compute feature ID for. - loop_count: Total number of vertices in the polygon. - features_a: Original feature IDs from trim polygon (shape A). - features_b: Original feature IDs from loop polygon (shape B). - m_a_count: Number of vertices in original trim polygon. - m_b_count: Number of vertices in original loop polygon. - - Returns: - A feature ID encoding the geometric origin of this contact point. - """ - feature = wp.uint32(0) - - incoming = loop_seg_ids[(loop_id - 1 + loop_count) % loop_count] - outgoing = loop_seg_ids[loop_id] - incoming_belongs_to_trim_poly = incoming < 6 - outgoing_belongs_to_trim_poly = outgoing < 6 - - if incoming_belongs_to_trim_poly != outgoing_belongs_to_trim_poly: - # This must be an intersection point - if incoming_belongs_to_trim_poly: - x = edge_key(features_a, m_a_count, int(incoming)) - else: - x = edge_key(features_b, m_b_count, int(incoming) - 6) - - if outgoing_belongs_to_trim_poly: - y = edge_key(features_a, m_a_count, int(outgoing)) - else: - y = edge_key(features_b, m_b_count, int(outgoing) - 6) - - feature = (x << wp.uint32(16)) | y - else: - if incoming_belongs_to_trim_poly: - next_seg = (int(incoming) + 1) % m_a_count - is_original_poly_point = next_seg == int(outgoing) - if is_original_poly_point: - feature = wp.uint32(features_a[int(outgoing)]) - else: - # Should not happen because input poly A would have self intersections - x = edge_key(features_a, m_a_count, int(incoming)) - y = edge_key(features_a, m_a_count, int(outgoing)) - feature = (x << wp.uint32(16)) | y - else: - next_seg = (int(incoming) - 6 + 1) % m_b_count + 6 - is_original_poly_point = next_seg == int(outgoing) - if is_original_poly_point: - # Shifted such that not the same id can get generated as produced by features_a - feature = wp.uint32(features_b[int(outgoing) - 6]) << wp.uint32(8) - else: - # Should not happen because input poly B would have self intersections - x = edge_key(features_b, m_b_count, int(incoming) - 6) - y = edge_key(features_b, m_b_count, int(outgoing) - 6) - feature = (x << wp.uint32(16)) | y - - return feature - - @wp.func def add_avoid_duplicates_vec2( arr: wp.array(dtype=wp.vec2), arr_count: int, vec: wp.vec2, eps: float @@ -871,7 +768,7 @@ def create_build_manifold(support_func: Any, writer_func: Any, post_process_cont 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 @@ -883,10 +780,8 @@ def create_build_manifold(support_func: Any, writer_func: Any, post_process_cont @wp.func def extract_4_point_contact_manifolds( m_a: wp.array(dtype=wp.vec2), - features_a: wp.types.vector(6, wp.uint8), m_a_count: int, m_b: wp.array(dtype=wp.vec2), - features_b: wp.types.vector(6, wp.uint8), m_b_count: int, normal: wp.vec3, cross_vector_1: wp.vec3, @@ -918,10 +813,8 @@ def extract_4_point_contact_manifolds( Args: m_a: Contact polygon vertices for shape A (2D contact plane space, up to 6 points). - features_a: Feature IDs for each vertex of polygon A. m_a_count: Number of vertices in polygon A. m_b: Contact polygon vertices for shape B (2D contact plane space, up to 6 points, space for 12). - features_b: Feature IDs for each vertex of polygon B. m_b_count: Number of vertices in polygon B. normal: Collision normal vector pointing from A to B. cross_vector_1: First tangent vector (forms contact plane basis). @@ -956,7 +849,6 @@ def extract_4_point_contact_manifolds( loop_count = remove_zero_length_edges(m_b, loop_seg_ids, loop_count, EPS) if loop_count > 1: - original_loop_count = loop_count result = wp.vec4i() if loop_count > 4: result = approx_max_quadrilateral_area_with_calipers(m_b, loop_count) @@ -967,10 +859,6 @@ def extract_4_point_contact_manifolds( for i in range(loop_count): ia = int(result[i]) - feature = feature_id( - loop_seg_ids, ia, original_loop_count, features_a, features_b, m_a_count, m_b_count - ) - # Transform back to world space using projectors p_world = m_b[ia].x * cross_vector_1 + m_b[ia].y * cross_vector_2 + center @@ -985,7 +873,6 @@ def extract_4_point_contact_manifolds( contact_data.contact_point_center = contact_point contact_data.contact_normal_a_to_b = normal contact_data.contact_distance = signed_distance - contact_data.feature = feature contact_data = post_process_contact( contact_data, geom_a, position_a, quaternion_a, geom_b, position_b, quaternion_b @@ -1019,11 +906,10 @@ def build_manifold( 1. Finding contact polygons using perturbed support mapping in 6 directions 2. Clipping the polygons against each other in contact plane space 3. Selecting the best 4 points using rotating calipers algorithm if more than 4 exist - 4. Transforming results back to world space with feature tracking + 4. Transforming results back to world space 5. Post-processing each contact and writing it via the writer function - The contact normal is the same for all contact points in the manifold. The two shapes - must always be queried in the same order to get stable feature IDs for contact tracking. + The contact normal is the same for all contact points in the manifold. Args: geom_a: Geometry data for the first shape. @@ -1041,11 +927,6 @@ def build_manifold( Returns: Number of valid contact points written (0-5). - - Note: - The feature IDs encode geometric information about which features (vertices, edges, - or edge-edge intersections) each contact point represents, allowing the physics - solver to maintain contact consistency over time. """ ROT_DELTA_ANGLE = wp.static(2.0 * wp.pi / float(6)) @@ -1057,9 +938,6 @@ def build_manifold( # Create an orthonormal basis from the collision normal. tangent_a, tangent_b = build_orthonormal_basis(normal) - features_a = vec6_uint8(wp.uint8(0)) - features_b = vec6_uint8(wp.uint8(0)) - plane_tracker_a = IncrementalPlaneTracker() plane_tracker_b = IncrementalPlaneTracker() @@ -1087,7 +965,7 @@ def build_manifold( # 1. Transform the world-space direction into shape A's local space. tmp = wp.quat_rotate_inv(quaternion_a, offset_normal) # 2. Find the furthest point on shape A in that local direction. - (pt_a, feature_a) = support_func(geom_a, tmp, data_provider) + pt_a = support_func(geom_a, tmp, data_provider) # 3. Transform the local-space support point back to world space. pt_a_3d = wp.quat_rotate(quaternion_a, pt_a) + position_a # 4. Project to 2D contact plane space @@ -1095,9 +973,7 @@ def build_manifold( pt_a_2d = wp.vec2(wp.dot(tangent_a, projected_a), wp.dot(tangent_b, projected_a)) # 5. Add the 2D projected point, checking for duplicates. a_count, was_added_a = add_avoid_duplicates_vec2(a_buffer, a_count, pt_a_2d, EPS) - # Only store feature ID if the point was actually added (not a duplicate) if was_added_a: - features_a[a_count - 1] = wp.uint8(int(feature_a) + 1) plane_tracker_a = update_incremental_plane_tracker(plane_tracker_a, pt_a_3d, a_count - 1) # Invert the direction for the other shape. @@ -1106,15 +982,13 @@ def build_manifold( # Find the support point on shape B in the opposite perturbed direction. # (Process is identical to the one for shape A). tmp = wp.quat_rotate_inv(quaternion_b, offset_normal) - (pt_b, feature_b) = support_func(geom_b, tmp, data_provider) + pt_b = support_func(geom_b, tmp, data_provider) pt_b_3d = wp.quat_rotate(quaternion_b, pt_b) + position_b # Project to 2D contact plane space projected_b = pt_b_3d - center pt_b_2d = wp.vec2(wp.dot(tangent_a, projected_b), wp.dot(tangent_b, projected_b)) b_count, was_added_b = add_avoid_duplicates_vec2(b_buffer, b_count, pt_b_2d, EPS) - # Only store feature ID if the point was actually added (not a duplicate) if was_added_b: - features_b[b_count - 1] = wp.uint8(int(feature_b) + 1) plane_tracker_b = update_incremental_plane_tracker(plane_tracker_b, pt_b_3d, b_count - 1) # Early-out for simple cases: if both have <=2 or either is empty @@ -1134,10 +1008,8 @@ def build_manifold( # Extract and write up to 4 contact points num_manifold_points, normal_dot = extract_4_point_contact_manifolds( a_buffer, - features_a, a_count, b_buffer, - features_b, b_count, normal, tangent_a, @@ -1167,7 +1039,6 @@ def build_manifold( contact_data.contact_point_center = deepest_contact_center contact_data.contact_normal_a_to_b = normal contact_data.contact_distance = deepest_signed_distance - contact_data.feature = wp.uint32(0) # Use 0 for the deepest contact feature ID contact_data = post_process_contact( contact_data, geom_a, position_a, quaternion_a, geom_b, position_b, quaternion_b diff --git a/newton/_src/geometry/narrow_phase.py b/newton/_src/geometry/narrow_phase.py index d4d41cb96..54e199cf2 100644 --- a/newton/_src/geometry/narrow_phase.py +++ b/newton/_src/geometry/narrow_phase.py @@ -22,8 +22,6 @@ from ..geometry.collision_core import ( ENABLE_TILE_BVH_QUERY, - build_pair_key2, - build_pair_key3, compute_tight_aabb_from_support, create_compute_gjk_mpr_contacts, create_find_contacts, @@ -65,9 +63,6 @@ class ContactWriterData: contact_normal: wp.array(dtype=wp.vec3) contact_penetration: wp.array(dtype=float) contact_tangent: wp.array(dtype=wp.vec3) - # Contact matching arrays (optional) - contact_pair_key: wp.array(dtype=wp.uint64) - contact_key: wp.array(dtype=wp.uint32) @wp.func @@ -123,10 +118,6 @@ def write_contact_simple( world_x = wp.vec3(0.0, 1.0, 0.0) writer_data.contact_tangent[index] = wp.normalize(world_x - wp.dot(world_x, normal) * normal) - if writer_data.contact_key.shape[0] > 0 and writer_data.contact_pair_key.shape[0] > 0: - writer_data.contact_key[index] = contact_data.feature - writer_data.contact_pair_key[index] = contact_data.feature_pair_key - def create_narrow_phase_kernel_gjk_mpr(external_aabb: bool, writer_func: Any): @wp.kernel(enable_backward=False) @@ -474,9 +465,6 @@ def narrow_phase_process_mesh_triangle_contacts_kernel( margin_b = shape_contact_margin[shape_b] margin = wp.max(margin_a, margin_b) - # Build pair key including triangle index for unique contact tracking - pair_key = build_pair_key3(wp.uint32(shape_a), wp.uint32(shape_b), wp.uint32(tri_idx)) - # Compute and write contacts using GJK/MPR with standard post-processing wp.static(create_compute_gjk_mpr_contacts(writer_func))( shape_data_a, @@ -491,7 +479,6 @@ def narrow_phase_process_mesh_triangle_contacts_kernel( thickness_a, thickness_b, writer_data, - pair_key, ) return narrow_phase_process_mesh_triangle_contacts_kernel @@ -571,9 +558,6 @@ def narrow_phase_process_mesh_plane_contacts_kernel( margin_plane = shape_contact_margin[plane_shape] margin = wp.max(margin_mesh, margin_plane) - # Build pair key for this mesh-plane pair - pair_key = build_pair_key2(wp.uint32(mesh_shape), wp.uint32(plane_shape)) - # Strided loop over vertices across all threads in the launch total_num_threads = total_num_blocks * wp.block_dim() for vertex_idx in range(tid, num_vertices, total_num_threads): @@ -610,8 +594,6 @@ def narrow_phase_process_mesh_plane_contacts_kernel( contact_data.shape_a = mesh_shape contact_data.shape_b = plane_shape contact_data.margin = margin - contact_data.feature = wp.uint32(vertex_idx + 1) - contact_data.feature_pair_key = pair_key writer_func(contact_data, writer_data, -1) @@ -702,9 +684,6 @@ def narrow_phase_process_mesh_plane_contacts_reduce_kernel( margin_plane = shape_contact_margin[plane_shape] margin = wp.max(margin_mesh, margin_plane) - # Build pair key for this mesh-plane pair - pair_key = build_pair_key2(wp.uint32(mesh_shape), wp.uint32(plane_shape)) - # Reset contact buffer for this pair for i in range(t, wp.static(num_reduction_slots), wp.block_dim()): contacts_shared_mem[i].projection = empty_marker @@ -749,7 +728,7 @@ def narrow_phase_process_mesh_plane_contacts_reduce_kernel( c.position = contact_pos c.normal = contact_normal c.depth = distance - c.feature = vertex_idx + c.mode = 0 c.projection = empty_marker # Apply contact reduction @@ -778,8 +757,6 @@ def narrow_phase_process_mesh_plane_contacts_reduce_kernel( contact_data.shape_a = mesh_shape contact_data.shape_b = plane_shape contact_data.margin = margin - contact_data.feature = wp.uint32(contact.feature + 1) - contact_data.feature_pair_key = pair_key writer_func(contact_data, writer_data, -1) @@ -936,8 +913,6 @@ def __init__( # None values for when optional features are disabled self.empty_tangent = None - self.empty_contact_pair_key = None - self.empty_contact_key = None # Betas array for contact reduction (using the configured contact_reduction_betas tuple) self.betas = create_betas_array(betas=self.betas_tuple, device=device) @@ -1140,7 +1115,7 @@ def launch_custom_write( self.global_contact_reducer.ht_values, self.global_contact_reducer.hashtable.active_slots, self.global_contact_reducer.position_depth, - self.global_contact_reducer.normal_feature, + self.global_contact_reducer.normal, self.global_contact_reducer.shape_pairs, shape_data, shape_contact_margin, @@ -1224,8 +1199,6 @@ def launch( contact_count: wp.array(dtype=int), # Number of active contacts after narrow contact_tangent: wp.array(dtype=wp.vec3) | None = None, # Represents x axis of local contact frame (None to disable) - contact_pair_key: wp.array(dtype=wp.uint64) | None = None, # Contact pair keys (None to disable) - contact_key: wp.array(dtype=wp.uint32) | None = None, # Contact feature keys (None to disable) device=None, # Device to launch on ): """ @@ -1246,7 +1219,6 @@ def launch( contact_normal: Output array for contact normals contact_penetration: Output array for penetration depths contact_tangent: Output array for contact tangents, or None to disable tangent computation - contact_key: Output array for contact feature keys, or None to disable key collection contact_count: Output array (single element) for contact count device: Device to launch on """ @@ -1259,14 +1231,6 @@ def launch( if contact_tangent is None: contact_tangent = self.empty_tangent - # Handle optional contact_pair_key array - use empty array if None - if contact_pair_key is None: - contact_pair_key = self.empty_contact_pair_key - - # Handle optional contact_key array - use empty array if None - if contact_key is None: - contact_key = self.empty_contact_key - # Clear all counters and contact count contact_count.zero_() self.shape_pairs_mesh_count.zero_() @@ -1284,8 +1248,6 @@ def launch( writer_data.contact_normal = contact_normal writer_data.contact_penetration = contact_penetration writer_data.contact_tangent = contact_tangent - writer_data.contact_pair_key = contact_pair_key - writer_data.contact_key = contact_key # Delegate to launch_custom_write self.launch_custom_write( diff --git a/newton/_src/geometry/sdf_contact.py b/newton/_src/geometry/sdf_contact.py index 4910c50d9..472c0839f 100644 --- a/newton/_src/geometry/sdf_contact.py +++ b/newton/_src/geometry/sdf_contact.py @@ -17,9 +17,6 @@ import warp as wp -from ..geometry.collision_core import ( - build_pair_key2, -) from ..geometry.contact_data import ContactData from ..geometry.sdf_utils import SDFData @@ -762,9 +759,6 @@ def mesh_sdf_collision_kernel( cutoff_b = shape_contact_margin[mesh_shape_b] margin = wp.max(cutoff_a, cutoff_b) - # Build pair key for this mesh-mesh pair - pair_key = build_pair_key2(wp.uint32(mesh_shape_a), wp.uint32(mesh_shape_b)) - # Test both directions: mesh A against SDF B, and mesh B against SDF A for mode in range(2): # Initialize with dummy values (will be set in mode branches) @@ -899,11 +893,6 @@ def mesh_sdf_collision_kernel( contact_data.shape_a = mesh_shape_a contact_data.shape_b = mesh_shape_b contact_data.margin = margin - if mode == 0: - contact_data.feature = wp.uint32(tri_idx + 1) - else: - contact_data.feature = wp.uint32(tri_idx + 1) | (wp.uint32(1) << wp.uint32(31)) - contact_data.feature_pair_key = pair_key writer_func(contact_data, writer_data, -1) @@ -914,7 +903,7 @@ def mesh_sdf_collision_kernel( # Extract functions and constants from the contact reduction configuration num_reduction_slots = contact_reduction_funcs.num_reduction_slots store_reduced_contact_func = contact_reduction_funcs.store_reduced_contact - filter_unique_contacts_func = contact_reduction_funcs.filter_unique_contacts + collect_active_contacts_func = contact_reduction_funcs.collect_active_contacts get_smem_slots_plus_1 = contact_reduction_funcs.get_smem_slots_plus_1 get_smem_slots_contacts = contact_reduction_funcs.get_smem_slots_contacts @@ -1120,10 +1109,7 @@ def mesh_sdf_collision_reduce_kernel( c.position = point_centered # Centered world-space position c.normal = normal_world # Normalized world-space normal pointing pair[0]->pair[1] c.depth = dist - # Encode mode into feature to distinguish triangles from mesh0 vs mesh1 - # Mode 0: positive triangle index, Mode 1: negative (-(index+1)) - tri_idx = selected_triangles[t] - c.feature = tri_idx if mode == 0 else -(tri_idx + 1) + c.mode = mode # Track which mesh the triangle came from c.projection = empty_marker store_reduced_contact_func( @@ -1142,8 +1128,8 @@ def mesh_sdf_collision_reduce_kernel( # normal points from pair[0] to pair[1] synchronize() - # Filter out duplicate contacts (same contact may have won multiple directions) - filter_unique_contacts_func(t, contacts_shared_mem, active_contacts_shared_mem, empty_marker) + # Collect all valid contacts from the reduction buffer + collect_active_contacts_func(t, contacts_shared_mem, active_contacts_shared_mem, empty_marker) num_contacts_to_keep = wp.min( active_contacts_shared_mem[wp.static(num_reduction_slots)], wp.static(num_reduction_slots) @@ -1166,9 +1152,9 @@ def mesh_sdf_collision_reduce_kernel( contact_data.radius_eff_a = 0.0 contact_data.radius_eff_b = 0.0 # SDF mesh's thickness is already baked into the SDF, so set it to 0 - # contact.feature >= 0 means mode 0: mesh0 triangles vs mesh1's SDF -> thickness1 already in SDF - # contact.feature < 0 means mode 1: mesh1 triangles vs mesh0's SDF -> thickness0 already in SDF - if contact.feature >= 0: + # mode == 0: mesh0 triangles vs mesh1's SDF -> thickness1 already in SDF + # mode == 1: mesh1 triangles vs mesh0's SDF -> thickness0 already in SDF + if contact.mode == 0: contact_data.thickness_a = thickness0 contact_data.thickness_b = 0.0 else: @@ -1177,13 +1163,6 @@ def mesh_sdf_collision_reduce_kernel( contact_data.shape_a = pair[0] contact_data.shape_b = pair[1] contact_data.margin = margin - # The high bit distinguishes contacts from mesh B (mode 1) vs mesh A (mode 0) - if contact.feature >= 0: - feature_id = wp.uint32(contact.feature + 1) - else: - feature_id = wp.uint32(-contact.feature) | (wp.uint32(1) << wp.uint32(31)) - contact_data.feature = feature_id - contact_data.feature_pair_key = build_pair_key2(wp.uint32(pair[0]), wp.uint32(pair[1])) writer_func(contact_data, writer_data, -1) diff --git a/newton/_src/geometry/sdf_hydroelastic.py b/newton/_src/geometry/sdf_hydroelastic.py index cc18e0d6e..dd633fc21 100644 --- a/newton/_src/geometry/sdf_hydroelastic.py +++ b/newton/_src/geometry/sdf_hydroelastic.py @@ -21,7 +21,7 @@ import warp as wp from ..sim.model import Model -from .collision_core import build_pair_key2, sat_box_intersection +from .collision_core import sat_box_intersection from .contact_data import ContactData from .contact_reduction import ( NUM_NORMAL_BINS, @@ -1412,8 +1412,6 @@ def decode_contacts_kernel( contact_data.shape_a = shape_a contact_data.shape_b = shape_b contact_data.margin = margin - contact_data.feature = wp.uint32(tid + 1) - contact_data.feature_pair_key = build_pair_key2(wp.uint32(shape_a), wp.uint32(shape_b)) contact_data.contact_stiffness = c_stiffness writer_func(contact_data, writer_data, output_index) @@ -1797,8 +1795,6 @@ def generate_contacts_from_bins( if contact_idx + total_contacts > writer_data.contact_max: return - pair_key = build_pair_key2(wp.uint32(shape_a), wp.uint32(shape_b)) - # Store contacts for i in range(num_unique_contacts): dir_idx = unique_indices[i] @@ -1832,8 +1828,6 @@ def generate_contacts_from_bins( contact_data.shape_a = shape_a contact_data.shape_b = shape_b contact_data.margin = margin - contact_data.feature = wp.uint32(binned_id[tid, normal_bin_idx, dir_idx] + 1) - contact_data.feature_pair_key = pair_key contact_data.contact_stiffness = c_stiffness contact_data.contact_friction_scale = unique_friction * wp.float32(depth > 0.0) @@ -1858,8 +1852,6 @@ def generate_contacts_from_bins( contact_data.shape_a = shape_a contact_data.shape_b = shape_b contact_data.margin = margin - contact_data.feature = wp.uint32(0) - contact_data.feature_pair_key = pair_key contact_data.contact_stiffness = c_stiffness contact_data.contact_friction_scale = anchor_friction diff --git a/newton/_src/geometry/simplex_solver.py b/newton/_src/geometry/simplex_solver.py index 1d4353aea..f0d96058d 100644 --- a/newton/_src/geometry/simplex_solver.py +++ b/newton/_src/geometry/simplex_solver.py @@ -35,7 +35,6 @@ Key features: - Distance computation between separated shapes - Collision detection when shapes overlap (returns signed_distance = 0) -- Feature ID tracking for contact persistence - Barycentric coordinates (stored as vec4) for witness points - Numerically stable simplex reduction using Johnson's distance subalgorithm @@ -394,9 +393,7 @@ def solve_closest_distance_core( last_search_dir = search_dir # Get support point in search direction - w, _feature_a_id, _feature_b_id = minkowski_support( - geom_a, geom_b, search_dir, orientation_b, position_b, extend, data_provider - ) + w = minkowski_support(geom_a, geom_b, search_dir, orientation_b, position_b, extend, data_provider) # Check for convergence using Frank-Wolfe duality gap # Skip check when using fallback direction to avoid premature exit diff --git a/newton/_src/geometry/support_function.py b/newton/_src/geometry/support_function.py index bad64d44c..f50fdb865 100644 --- a/newton/_src/geometry/support_function.py +++ b/newton/_src/geometry/support_function.py @@ -21,9 +21,8 @@ given direction, which is a fundamental operation for collision detection algorithms like GJK, MPR, and EPA. -The support mapping operates in the shape's local coordinate frame and returns: -- The support point (furthest point in the given direction) -- A feature ID that identifies which geometric feature (vertex, edge, face) the point lies on +The support mapping operates in the shape's local coordinate frame and returns the +support point (furthest point in the given direction). Supported primitives: - Box (axis-aligned rectangular prism) @@ -110,11 +109,9 @@ class GenericShapeData: @wp.func -def support_map( - geom: GenericShapeData, direction: wp.vec3, data_provider: SupportMapDataProvider -) -> tuple[wp.vec3, int]: +def support_map(geom: GenericShapeData, direction: wp.vec3, data_provider: SupportMapDataProvider) -> wp.vec3: """ - Return the support point of a primitive in its local frame, with a feature id. + Return the support point of a primitive in its local frame. Conventions for `geom.scale` and `geom.auxiliary`: - BOX: half-extents in x/y/z @@ -136,7 +133,6 @@ def support_map( dir_safe = direction result = wp.vec3(0.0, 0.0, 0.0) - feature_id = int(0) if geom.shape_type == int(GeoType.CONVEX_MESH): # Convex hull support: find the furthest point in the direction @@ -149,7 +145,6 @@ def support_map( # Find the vertex with the maximum dot product with the direction max_dot = float(-1.0e10) best_vertex = wp.vec3(0.0, 0.0, 0.0) - best_idx = int(0) num_verts = mesh.points.shape[0] @@ -164,9 +159,7 @@ def support_map( if dot_val > max_dot: max_dot = dot_val best_vertex = vertex - best_idx = i result = best_vertex - feature_id = best_idx elif geom.shape_type == int(GeoTypeEx.TRIANGLE): # Triangle vertices: a at origin, b at scale, c at auxiliary @@ -182,13 +175,10 @@ def support_map( # Find the vertex with maximum dot product (furthest in the direction) if dot_a >= dot_b and dot_a >= dot_c: result = tri_a - feature_id = 0 # vertex A elif dot_b >= dot_c: result = tri_b - feature_id = 1 # vertex B else: result = tri_c - feature_id = 2 # vertex C elif geom.shape_type == int(GeoType.BOX): sx = 1.0 if dir_safe[0] >= 0.0 else -1.0 sy = 1.0 if dir_safe[1] >= 0.0 else -1.0 @@ -196,15 +186,6 @@ def support_map( result = wp.vec3(sx * geom.scale[0], sy * geom.scale[1], sz * geom.scale[2]) - # Bit mask consistent with reference: x->4, y->2, z->1 - feature_id = 0 - if sx >= 0.0: - feature_id |= 4 - if sy >= 0.0: - feature_id |= 2 - if sz >= 0.0: - feature_id |= 1 - elif geom.shape_type == int(GeoType.SPHERE): radius = geom.scale[0] if dir_len_sq > eps: @@ -212,7 +193,6 @@ def support_map( else: n = wp.vec3(1.0, 0.0, 0.0) result = n * radius - feature_id = 0 elif geom.shape_type == int(GeoType.CAPSULE): radius = geom.scale[0] @@ -230,10 +210,8 @@ def support_map( # Use sign of Z-component to pick the correct endpoint if dir_safe[2] >= 0.0: result = result + wp.vec3(0.0, 0.0, half_height) - feature_id = 1 # top cap else: result = result + wp.vec3(0.0, 0.0, -half_height) - feature_id = 2 # bottom cap elif geom.shape_type == int(GeoType.ELLIPSOID): # Ellipsoid support for semi-axes a, b, c in direction d: @@ -255,7 +233,6 @@ def support_map( result = wp.vec3(a, 0.0, 0.0) else: result = wp.vec3(a, 0.0, 0.0) - feature_id = 0 elif geom.shape_type == int(GeoType.CYLINDER): radius = geom.scale[0] @@ -274,13 +251,10 @@ def support_map( # Choose between top cap, bottom cap, or lateral surface if dir_safe[2] > 0.0: result = wp.vec3(lateral_point[0], lateral_point[1], half_height) - feature_id = 1 # top cap elif dir_safe[2] < 0.0: result = wp.vec3(lateral_point[0], lateral_point[1], -half_height) - feature_id = 2 # bottom cap else: result = lateral_point - feature_id = 0 # lateral surface elif geom.shape_type == int(GeoType.CONE): radius = geom.scale[0] @@ -298,18 +272,14 @@ def support_map( # Purely vertical direction if dir_safe[2] >= 0.0: result = apex - feature_id = 1 # apex else: result = wp.vec3(radius, 0.0, -half_height) - feature_id = 2 # base edge else: if dir_safe[2] >= k * dir_xy_len: result = apex - feature_id = 1 # apex else: n_xy = dir_xy / dir_xy_len result = wp.vec3(n_xy[0] * radius, n_xy[1] * radius, -half_height) - feature_id = 2 # base edge elif geom.shape_type == int(GeoType.PLANE): # Finite plane support: rectangular plane in XY, extents in scale[0] (half-width X) and scale[1] (half-length Y) @@ -324,19 +294,11 @@ def support_map( # The support point is at the corner in the XY plane (z=0) result = wp.vec3(sx * half_width, sy * half_length, 0.0) - # Feature ID based on which corner - feature_id = 0 - if sx >= 0.0: - feature_id |= 1 - if sy >= 0.0: - feature_id |= 2 - else: - # Unhandled type: return origin and feature 0 + # Unhandled type: return origin result = wp.vec3(0.0, 0.0, 0.0) - feature_id = 0 - return result, feature_id + return result @wp.func diff --git a/newton/_src/sim/collide_unified.py b/newton/_src/sim/collide_unified.py index 6d748ed94..3d91af1f7 100644 --- a/newton/_src/sim/collide_unified.py +++ b/newton/_src/sim/collide_unified.py @@ -59,9 +59,6 @@ class UnifiedContactWriterData: out_thickness0: wp.array(dtype=float) out_thickness1: wp.array(dtype=float) out_tids: wp.array(dtype=int) - # Contact matching arrays (optional) - contact_pair_key: wp.array(dtype=wp.uint64) - contact_key: wp.array(dtype=wp.uint32) # Per-contact shape properties, empty arrays if not enabled. # Zero-values indicate that no per-contact shape properties are set for this contact out_stiffness: wp.array(dtype=float) @@ -92,8 +89,8 @@ def write_contact( Write a contact to the output arrays using ContactData and UnifiedContactWriterData. Args: - contact_data: ContactData struct containing contact information (includes feature and feature_pair_key) - writer_data: UnifiedContactWriterData struct containing body info and output arrays (includes contact_pair_key and contact_key) + contact_data: ContactData struct containing contact information + writer_data: UnifiedContactWriterData struct containing body info and output arrays output_index: If -1, use atomic_add to get the next available index if contact distance is less than margin. If >= 0, use this index directly and skip margin check. """ total_separation_needed = ( @@ -164,11 +161,6 @@ def write_contact( writer_data.out_thickness1[index] = offset_mag_b writer_data.out_tids[index] = 0 # tid not available in this context - # Write contact key only if contact_key array is non-empty - if writer_data.contact_key.shape[0] > 0 and writer_data.contact_pair_key.shape[0] > 0: - writer_data.contact_key[index] = contact_data.feature - writer_data.contact_pair_key[index] = contact_data.feature_pair_key - # Write stiffness/damping/friction only if per-contact shape properties are enabled if writer_data.out_stiffness.shape[0] > 0: writer_data.out_stiffness[index] = contact_data.contact_stiffness @@ -304,7 +296,6 @@ def __init__( shape_world: wp.array(dtype=int) | None = None, shape_flags: wp.array(dtype=int) | None = None, sap_sort_type=None, - enable_contact_matching: bool = False, sdf_hydroelastic: SDFHydroelastic | None = None, ): """ @@ -341,9 +332,6 @@ def __init__( sap_sort_type (SAPSortType | None, optional): Sorting algorithm for SAP broad phase. Only used when broad_phase_mode is BroadPhaseMode.SAP. Options: SEGMENTED or TILE. If None, uses default (SEGMENTED). - enable_contact_matching (bool, optional): Whether to enable contact matching data generation. - If True, allocates buffers for contact_pair_key and contact_key arrays that can be used - with ContactMatcher for warm-starting physics solvers. Defaults to False. sdf_hydroelastic (SDFHydroelastic | None, optional): Pre-configured SDF hydroelastic collision handler. If provided, enables hydroelastic contact computation for SDF-based shape pairs. Defaults to None. """ @@ -351,7 +339,6 @@ def __init__( self.shape_count = shape_count self.broad_phase_mode = broad_phase_mode self.device = device - self.enable_contact_matching = enable_contact_matching self.reduce_contacts = reduce_contacts self.shape_pairs_max = (shape_count * (shape_count - 1)) // 2 @@ -420,14 +407,6 @@ def __init__( self.geom_data = wp.zeros(shape_count, dtype=wp.vec4, device=device) self.geom_transform = wp.zeros(shape_count, dtype=wp.transform, device=device) - # Contact matching arrays (optional) - if enable_contact_matching: - self.narrow_contact_pair_key = wp.zeros(self.rigid_contact_max, dtype=wp.uint64, device=device) - self.narrow_contact_key = wp.zeros(self.rigid_contact_max, dtype=wp.uint32, device=device) - else: - self.narrow_contact_pair_key = None - self.narrow_contact_key = None - if soft_contact_max is None: soft_contact_max = shape_count * particle_count self.soft_contact_margin = soft_contact_margin @@ -449,7 +428,6 @@ def from_model( broad_phase_mode: BroadPhaseMode = BroadPhaseMode.NXN, shape_pairs_filtered: wp.array(dtype=wp.vec2i) | None = None, sap_sort_type=None, - enable_contact_matching: bool = False, sdf_hydroelastic_config: SDFHydroelasticConfig | None = None, ) -> CollisionPipelineUnified: """ @@ -470,8 +448,6 @@ def from_model( Required when broad_phase_mode is BroadPhaseMode.EXPLICIT. For NXN/SAP modes, can use model.shape_contact_pairs if available. sap_sort_type (SAPSortType | None, optional): Sorting algorithm for SAP broad phase. Only used when broad_phase_mode is BroadPhaseMode.SAP. If None, uses default (SEGMENTED). - enable_contact_matching (bool, optional): Whether to enable contact matching data generation. - If True, allocates and populates contact_pair_key and contact_key arrays. Defaults to False. sdf_hydroelastic_config (SDFHydroelasticConfig | None, optional): Configuration for SDF hydroelastic collision handling. Defaults to None. Returns: @@ -516,7 +492,6 @@ def from_model( shape_world=model.shape_world if hasattr(model, "shape_world") else None, shape_flags=model.shape_flags if hasattr(model, "shape_flags") else None, sap_sort_type=sap_sort_type, - enable_contact_matching=enable_contact_matching, sdf_hydroelastic=sdf_hydroelastic, ) @@ -645,13 +620,6 @@ def collide(self, model: Model, state: State) -> Contacts: writer_data.out_thickness0 = contacts.rigid_contact_thickness0 writer_data.out_thickness1 = contacts.rigid_contact_thickness1 writer_data.out_tids = contacts.rigid_contact_tids - # Contact matching arrays (use empty arrays if not enabled) - if self.narrow_contact_pair_key is not None: - writer_data.contact_pair_key = self.narrow_contact_pair_key - writer_data.contact_key = self.narrow_contact_key - else: - writer_data.contact_pair_key = self.narrow_phase.empty_contact_pair_key - writer_data.contact_key = self.narrow_phase.empty_contact_key writer_data.out_stiffness = contacts.rigid_contact_stiffness writer_data.out_damping = contacts.rigid_contact_damping diff --git a/newton/tests/test_contact_matcher.py b/newton/tests/test_contact_matcher.py deleted file mode 100644 index c91c019d4..000000000 --- a/newton/tests/test_contact_matcher.py +++ /dev/null @@ -1,340 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers -# SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import unittest - -import numpy as np -import warp as wp - -import newton -from newton._src.geometry.contact_matcher import ContactMatcher -from newton._src.sim.collide_unified import BroadPhaseMode, CollisionPipelineUnified -from newton.tests.unittest_utils import add_function_test, get_test_devices - - -class TestContactMatcher(unittest.TestCase): - pass - - -def test_contact_matcher_basic(test: TestContactMatcher, device): - """Test basic contact matching with simple key/payload pairs.""" - max_contacts = 10 - matcher = ContactMatcher(max_contacts, device=device) - - # Create test data for timestep 0 - keys_0 = wp.array([1, 2, 3, 4, 5], dtype=wp.uint64, device=device) - payloads_0 = wp.array([10, 20, 30, 40, 50], dtype=wp.uint32, device=device) - num_keys_0 = wp.array([5], dtype=wp.int32, device=device) - result_map_0 = wp.zeros(max_contacts, dtype=wp.int32, device=device) - - # First timestep - no previous contacts, all should be -1 - matcher.launch(keys_0, num_keys_0, payloads_0, result_map_0, device=device) - wp.synchronize_device(device) - result_0 = result_map_0.numpy() - test.assertTrue(np.all(result_0[:5] == -1), "First timestep should have all new contacts") - - # Create test data for timestep 1 - some contacts persist, some are new - keys_1 = wp.array([2, 3, 4, 6, 7], dtype=wp.uint64, device=device) - payloads_1 = wp.array([20, 30, 40, 60, 70], dtype=wp.uint32, device=device) - num_keys_1 = wp.array([5], dtype=wp.int32, device=device) - result_map_1 = wp.zeros(max_contacts, dtype=wp.int32, device=device) - - # Second timestep - should match contacts 2, 3, 4 - matcher.launch(keys_1, num_keys_1, payloads_1, result_map_1, device=device) - wp.synchronize_device(device) - result_1 = result_map_1.numpy() - - # Contacts at indices 0, 1, 2 should match (keys 2, 3, 4 from previous step at indices 1, 2, 3) - test.assertEqual(result_1[0], 1, "Key 2 should match previous index 1") - test.assertEqual(result_1[1], 2, "Key 3 should match previous index 2") - test.assertEqual(result_1[2], 3, "Key 4 should match previous index 3") - # Contacts at indices 3, 4 are new (keys 6, 7) - test.assertEqual(result_1[3], -1, "Key 6 should be new") - test.assertEqual(result_1[4], -1, "Key 7 should be new") - - -def test_contact_matcher_duplicate_keys(test: TestContactMatcher, device): - """Test contact matching with duplicate keys (differentiated by payload).""" - max_contacts = 10 - matcher = ContactMatcher(max_contacts, device=device) - - # Create test data with duplicate keys but different payloads - keys_0 = wp.array([1, 1, 1, 2, 2], dtype=wp.uint64, device=device) - payloads_0 = wp.array([10, 20, 30, 40, 50], dtype=wp.uint32, device=device) - num_keys_0 = wp.array([5], dtype=wp.int32, device=device) - result_map_0 = wp.zeros(max_contacts, dtype=wp.int32, device=device) - - # First timestep - matcher.launch(keys_0, num_keys_0, payloads_0, result_map_0, device=device) - wp.synchronize_device(device) - - # Second timestep - same keys, some payloads match - keys_1 = wp.array([1, 1, 2, 2, 3], dtype=wp.uint64, device=device) - payloads_1 = wp.array([20, 30, 40, 60, 70], dtype=wp.uint32, device=device) - num_keys_1 = wp.array([5], dtype=wp.int32, device=device) - result_map_1 = wp.zeros(max_contacts, dtype=wp.int32, device=device) - - matcher.launch(keys_1, num_keys_1, payloads_1, result_map_1, device=device) - wp.synchronize_device(device) - result_1 = result_map_1.numpy() - - # Key 1 with payload 20 should match previous index 1 - test.assertEqual(result_1[0], 1, "Key 1, payload 20 should match") - # Key 1 with payload 30 should match previous index 2 - test.assertEqual(result_1[1], 2, "Key 1, payload 30 should match") - # Key 2 with payload 40 should match previous index 3 - test.assertEqual(result_1[2], 3, "Key 2, payload 40 should match") - # Key 2 with payload 60 is new (different payload) - test.assertEqual(result_1[3], -1, "Key 2, payload 60 should be new") - # Key 3 is completely new - test.assertEqual(result_1[4], -1, "Key 3 should be new") - - -def test_contact_matcher_stacked_cubes(test: TestContactMatcher, device): - """Test contact matcher with realistic scenario: 2D grid of rotated stacked cubes in static equilibrium. - - This test verifies that: - 1. Frame 0: Initial contacts are detected, but none can be matched (no previous frame) - 2. Frame 1+: ALL contacts should be matched (100%) in a static configuration - using (shape_pair_key, feature_id) pairs. - - The cubes are at rest, so geometry and contact features should be identical - frame-to-frame. If contacts are not matched, the test prints detailed debug info - showing positions, pair keys, and feature keys for analysis. - - Scene: 5x5 grid where each position has 5 stacked cubes, each rotated 30 degrees - more than the one below it (125 total cubes). - """ - # Build model with ground plane and 2D grid of stacked cubes - # Use very high stiffness and damping to minimize motion - builder = newton.ModelBuilder() - builder.default_shape_cfg.ke = 1e6 # Very stiff contacts - builder.default_shape_cfg.kd = 1e4 # High damping to prevent oscillation - - # Ground plane - builder.add_ground_plane() - - # Parameters for 2D grid of stacked cubes - cube_size = 0.5 - n_cubes_per_stack = 4 # Number of cubes in each stack - s = 3 # Grid dimension in x - t = 3 # Grid dimension in y - spacing = 2.0 # Spacing between stacks - rotation_increment = np.pi / 6.0 # 30 degrees in radians - - # Create 2D grid of stacks - for i in range(s): - for j in range(t): - # Calculate base position for this stack - x_base = (i - (s - 1) / 2.0) * spacing - y_base = (j - (t - 1) / 2.0) * spacing - - # Create stack of n cubes at this grid position - for k in range(n_cubes_per_stack): - # Z position (stacked vertically) - z_pos = cube_size / 2 + k * cube_size - - # Rotation increases by 30 degrees for each level - rotation_angle = k * rotation_increment - rotation = wp.quat_from_axis_angle(wp.vec3(0.0, 0.0, 1.0), rotation_angle) - - # Create cube - cube = builder.add_link(xform=wp.transform([x_base, y_base, z_pos], rotation)) - builder.add_shape_box( - body=cube, - hx=cube_size / 2, - hy=cube_size / 2, - hz=cube_size / 2, - ) - - model = builder.finalize(device=device) - - # Create states - state_0 = model.state() - state_1 = model.state() - - # Initialize contact matcher - max_contacts = 1000 - matcher = ContactMatcher(max_contacts, device=device) - - # Create collision pipeline with contact matching enabled - collision_pipeline = CollisionPipelineUnified.from_model( - model, - broad_phase_mode=BroadPhaseMode.NXN, - enable_contact_matching=True, - ) - - # Simulate for a few timesteps and track contacts - solver = newton.solvers.SolverXPBD(model, iterations=2) - sim_dt = 1.0 / 60.0 - - contact_counts = [] - persistent_counts = [] - contact_history = [] # Store detailed contact info for debugging - - for frame in range(5): - # Get contacts using unified pipeline - contacts = collision_pipeline.collide(model, state_0) - num_contacts = contacts.rigid_contact_count.numpy()[0] - contact_counts.append(num_contacts) - - frame_data = { - "frame": frame, - "num_contacts": num_contacts, - "positions": [], - "pair_keys": [], - "feature_keys": [], - "match_results": [], - } - - if num_contacts > 0: - # Use contact_pair_key and contact_key directly from narrow phase - keys_wp = collision_pipeline.narrow_contact_pair_key - payloads_wp = collision_pipeline.narrow_contact_key - num_keys_wp = contacts.rigid_contact_count - result_map = wp.zeros(max_contacts, dtype=wp.int32, device=device) - - # Collect data for debugging - positions_np = contacts.rigid_contact_point0.numpy()[:num_contacts] - pair_keys_np = keys_wp.numpy()[:num_contacts] - feature_keys_np = payloads_wp.numpy()[:num_contacts] - - # Match contacts - matcher.launch(keys_wp, num_keys_wp, payloads_wp, result_map, device=device) - wp.synchronize_device(device) - result = result_map.numpy()[:num_contacts] - - # Store frame data - frame_data["positions"] = positions_np.copy() - frame_data["pair_keys"] = pair_keys_np.copy() - frame_data["feature_keys"] = feature_keys_np.copy() - frame_data["match_results"] = result.copy() - - # Count persistent contacts (matched to previous timestep) - persistent = np.sum(result >= 0) - persistent_counts.append(persistent) - else: - persistent_counts.append(0) - - contact_history.append(frame_data) - - # Step simulation - state_0.clear_forces() - solver.step(state_0, state_1, model.control(), contacts, sim_dt) - state_0, state_1 = state_1, state_0 - - # Verify results - test.assertGreater(contact_counts[0], 0, "Frame 0: Should have contacts in first timestep") - test.assertEqual(persistent_counts[0], 0, "Frame 0: First timestep should have no previous contacts to match") - - # Starting from frame 1, ALL contacts should be matched in static configuration - # The cubes are at rest, so contact features must persist frame-to-frame - if len(persistent_counts) > 1: - for i in range(1, len(persistent_counts)): - if contact_counts[i] > 0: - persistence_ratio = persistent_counts[i] / contact_counts[i] - if persistent_counts[i] != contact_counts[i]: - # Print detailed contact history for debugging - print(f"\n=== CONTACT MATCHING FAILURE at Frame {i} ===") - print(f"Expected: {contact_counts[i]} matched, Got: {persistent_counts[i]}") - print(f"Persistence ratio: {persistence_ratio:.2%}\n") - - # Print previous frame and current frame contact details - for frame_idx in [i - 1, i]: - frame_data = contact_history[frame_idx] - print(f"--- Frame {frame_idx} ({frame_data['num_contacts']} contacts) ---") - for c in range(frame_data["num_contacts"]): - pos = frame_data["positions"][c] - pair_key = frame_data["pair_keys"][c] - feature_key = frame_data["feature_keys"][c] - match_result = frame_data["match_results"][c] if frame_idx == i else "N/A" - print( - f" Contact {c}: pos=({pos[0]:.6f}, {pos[1]:.6f}, {pos[2]:.6f}) " - f"pair_key=0x{pair_key:016x} feature_key={feature_key} match={match_result}" - ) - print() - - test.assertEqual( - persistent_counts[i], - contact_counts[i], - f"Frame {i}: ALL {contact_counts[i]} contacts should be matched in static configuration " - f"(found {persistent_counts[i]}, ratio: {persistence_ratio:.2%})", - ) - - -def test_contact_matcher_empty_contacts(test: TestContactMatcher, device): - """Test contact matcher with zero contacts.""" - max_contacts = 10 - matcher = ContactMatcher(max_contacts, device=device) - - # Create empty contact data - keys = wp.zeros(max_contacts, dtype=wp.uint64, device=device) - payloads = wp.zeros(max_contacts, dtype=wp.uint32, device=device) - num_keys = wp.array([0], dtype=wp.int32, device=device) - result_map = wp.zeros(max_contacts, dtype=wp.int32, device=device) - - # Should handle empty contacts gracefully - matcher.launch(keys, num_keys, payloads, result_map, device=device) - - # No assertion needed - just verify it doesn't crash - test.assertTrue(True, "Should handle empty contacts without crashing") - - -def test_contact_matcher_max_capacity(test: TestContactMatcher, device): - """Test contact matcher at maximum capacity.""" - max_contacts = 20 - matcher = ContactMatcher(max_contacts, device=device) - - # Fill to capacity - keys_0 = wp.array(list(range(max_contacts)), dtype=wp.uint64, device=device) - payloads_0 = wp.array(list(range(100, 100 + max_contacts)), dtype=wp.uint32, device=device) - num_keys_0 = wp.array([max_contacts], dtype=wp.int32, device=device) - result_map_0 = wp.zeros(max_contacts, dtype=wp.int32, device=device) - - # First timestep - matcher.launch(keys_0, num_keys_0, payloads_0, result_map_0, device=device) - wp.synchronize_device(device) - - # Second timestep - all contacts persist - matcher.launch(keys_0, num_keys_0, payloads_0, result_map_0, device=device) - wp.synchronize_device(device) - result = result_map_0.numpy() - - # All contacts should match their previous indices - expected = np.arange(max_contacts, dtype=np.int32) - test.assertTrue( - np.array_equal(result, expected), "All contacts at max capacity should match their previous indices" - ) - - -# Register tests for all devices -devices = get_test_devices() -add_function_test(TestContactMatcher, "test_contact_matcher_basic", test_contact_matcher_basic, devices=devices) -add_function_test( - TestContactMatcher, "test_contact_matcher_duplicate_keys", test_contact_matcher_duplicate_keys, devices=devices -) -add_function_test( - TestContactMatcher, "test_contact_matcher_stacked_cubes", test_contact_matcher_stacked_cubes, devices=devices -) -add_function_test( - TestContactMatcher, "test_contact_matcher_empty_contacts", test_contact_matcher_empty_contacts, devices=devices -) -add_function_test( - TestContactMatcher, "test_contact_matcher_max_capacity", test_contact_matcher_max_capacity, devices=devices -) - - -if __name__ == "__main__": - unittest.main(verbosity=2) diff --git a/newton/tests/test_contact_reduction.py b/newton/tests/test_contact_reduction.py index ecf00e7fd..1a646344e 100644 --- a/newton/tests/test_contact_reduction.py +++ b/newton/tests/test_contact_reduction.py @@ -129,19 +129,6 @@ def test_create_betas_array(test, device): test.assertAlmostEqual(arr_np[1], 1000000.0, places=1) -def test_contact_struct_fields(test, device): - """Test ContactStruct has expected fields.""" - arr = wp.zeros(1, dtype=ContactStruct, device=device) - arr_np = arr.numpy() - - # Check that expected fields exist - test.assertIn("position", arr_np.dtype.names) - test.assertIn("normal", arr_np.dtype.names) - test.assertIn("depth", arr_np.dtype.names) - test.assertIn("feature", arr_np.dtype.names) - test.assertIn("projection", arr_np.dtype.names) - - # ============================================================================= # Tests for get_slot function (works on CPU and GPU) # ============================================================================= @@ -217,7 +204,7 @@ def _generate_test_contact(t: int) -> ContactStruct: c.position = wp.vec3(wp.sin(ft * 0.1) * ft * 0.01, 0.0, wp.cos(ft * 0.1) * ft * 0.01) c.normal = wp.vec3(0.0, 1.0, 0.0) c.depth = 0.1 - c.feature = t % 10 # Assign features 0-9 cyclically + c.mode = t % 10 # Assign features 0-9 cyclically c.projection = 0.0 return c @@ -226,7 +213,7 @@ def _create_reduction_test_kernel(reduction_funcs: ContactReductionFunctions): """Create a test kernel for contact reduction with shared memory.""" num_slots = reduction_funcs.num_reduction_slots store_reduced_contact = reduction_funcs.store_reduced_contact - filter_unique_contacts = reduction_funcs.filter_unique_contacts + collect_active_contacts = reduction_funcs.collect_active_contacts @wp.kernel(enable_backward=False) def reduction_test_kernel( @@ -262,8 +249,8 @@ def reduction_test_kernel( store_reduced_contact(t, has_contact, c, buffer, active_ids, betas_arr, empty_marker) - # Filter duplicates - filter_unique_contacts(t, buffer, active_ids, empty_marker) + # Collect active contacts + collect_active_contacts(t, buffer, active_ids, empty_marker) # Write output num_contacts = active_ids[wp.static(num_slots)] @@ -358,9 +345,7 @@ def test_contact_reduction_reduces_count(test, device): count = out_count.numpy()[0] # With 64 active contacts (128 threads, every other one active), - # reduction should produce fewer contacts due to: - # 1. Keeping only best contact per (bin, direction) slot - # 2. Filtering duplicate features within each bin + # reduction should produce fewer contacts due to keeping only best contact per (bin, direction) slot test.assertGreater(count, 0, "Should have at least one contact") test.assertLess(count, 64, "Reduction should reduce contact count") @@ -386,7 +371,6 @@ def test_contact_reduction_reduces_count(test, device): TestContactReduction, "test_compute_num_reduction_slots", test_compute_num_reduction_slots, devices=[device] ) add_function_test(TestContactReduction, "test_create_betas_array", test_create_betas_array, devices=[device]) - add_function_test(TestContactReduction, "test_contact_struct_fields", test_contact_struct_fields, devices=[device]) # get_slot tests add_function_test( diff --git a/newton/tests/test_contact_reduction_global.py b/newton/tests/test_contact_reduction_global.py index 66708e4b2..e465905fe 100644 --- a/newton/tests/test_contact_reduction_global.py +++ b/newton/tests/test_contact_reduction_global.py @@ -105,7 +105,6 @@ def store_contact_kernel( position=wp.vec3(1.0, 2.0, 3.0), normal=wp.vec3(0.0, 1.0, 0.0), depth=-0.01, - feature=42, reducer_data=reducer_data, beta0=1000.0, beta1=0.001, @@ -147,7 +146,6 @@ def store_multiple_contacts_kernel( position=wp.vec3(x, 0.0, 0.0), normal=wp.vec3(0.0, 1.0, 0.0), depth=-0.01, - feature=tid, reducer_data=reducer_data, beta0=1000.0, beta1=0.001, @@ -187,7 +185,6 @@ def store_different_pairs_kernel( position=wp.vec3(0.0, 0.0, 0.0), normal=wp.vec3(0.0, 1.0, 0.0), depth=-0.01, - feature=tid, reducer_data=reducer_data, beta0=1000.0, beta1=0.001, @@ -224,7 +221,6 @@ def store_one_contact_kernel( position=wp.vec3(0.0, 0.0, 0.0), normal=wp.vec3(0.0, 1.0, 0.0), depth=-0.01, - feature=0, reducer_data=reducer_data, beta0=1000.0, beta1=0.001, @@ -279,7 +275,6 @@ def stress_kernel( position=wp.vec3(x, y, 0.0), normal=wp.vec3(nx / n_len, ny / n_len, nz / n_len), depth=-0.01, - feature=tid, reducer_data=reducer_data, beta0=1000.0, beta1=0.001, @@ -316,7 +311,6 @@ def store_contact_kernel( position=wp.vec3(1.0, 2.0, 3.0), normal=wp.vec3(0.0, 1.0, 0.0), depth=-0.01, - feature=42, reducer_data=reducer_data, beta0=1000.0, beta1=0.001, @@ -380,7 +374,6 @@ def store_contacts_kernel( position=wp.vec3(float(tid), 0.0, 0.0), normal=wp.vec3(0.0, 1.0, 0.0), depth=-0.01, - feature=tid, reducer_data=reducer_data, beta0=1000.0, beta1=0.001, @@ -402,8 +395,6 @@ def store_contacts_kernel( contact_penetration_out = wp.zeros(max_output, dtype=float, device=device) contact_count_out = wp.zeros(1, dtype=int, device=device) contact_tangent_out = wp.zeros(0, dtype=wp.vec3, device=device) - contact_pair_key_out = wp.zeros(0, dtype=wp.uint64, device=device) - contact_key_out = wp.zeros(0, dtype=wp.uint32, device=device) # Create dummy shape_data for thickness lookup num_shapes = 200 @@ -424,8 +415,6 @@ def store_contacts_kernel( writer_data.contact_normal = contact_normal_out writer_data.contact_penetration = contact_penetration_out writer_data.contact_tangent = contact_tangent_out - writer_data.contact_pair_key = contact_pair_key_out - writer_data.contact_key = contact_key_out # Launch export kernel total_threads = 128 # Grid stride threads @@ -437,7 +426,7 @@ def store_contacts_kernel( reducer.ht_values, # Values are now managed by GlobalContactReducer reducer.hashtable.active_slots, reducer.position_depth, - reducer.normal_feature, + reducer.normal, reducer.shape_pairs, shape_data, shape_contact_margin,