|
28 | 28 | """ |
29 | 29 |
|
30 | 30 | import enum |
| 31 | +from typing import Any |
31 | 32 |
|
32 | 33 | import warp as wp |
33 | 34 |
|
|
37 | 38 | # Near-zero direction components (e.g. from quaternion rotation noise ~1e-14) |
38 | 39 | # are treated as non-negative, biasing toward the +1 vertex. |
39 | 40 | BOX_SUPPORT_DEADBAND = 1.0e-10 |
| 41 | +_CENTERED_BOX_SUPPORT_TIE_EPSILON = 1.0e-6 |
| 42 | +TRIANGLE_PRISM_EXTRUSION = 1.0 |
| 43 | +"""Depth [m] a triangle is extruded along -Z to give a heightfield cell volume.""" |
40 | 44 |
|
41 | 45 |
|
42 | 46 | # Is not allowed to share values with GeoType |
@@ -171,7 +175,7 @@ def support_map(geom: GenericShapeData, direction: wp.vec3, data_provider: Suppo |
171 | 175 | # always the heightfield's down direction. |
172 | 176 | if geom.shape_type == GeoTypeEx.TRIANGLE_PRISM: |
173 | 177 | if direction[2] < 0.0: |
174 | | - result = result + wp.vec3(0.0, 0.0, -1.0) |
| 178 | + result = result + wp.vec3(0.0, 0.0, -TRIANGLE_PRISM_EXTRUSION) |
175 | 179 | elif geom.shape_type == GeoType.BOX: |
176 | 180 | # Use a relative deadband so near-zero direction components |
177 | 181 | # (from solver rotation drift ~1e-7) cannot flip the sign |
@@ -347,6 +351,109 @@ def support_map_lean(geom: GenericShapeData, direction: wp.vec3, data_provider: |
347 | 351 | return result |
348 | 352 |
|
349 | 353 |
|
| 354 | +def create_shape_support_function(support_func: Any, center_ties: bool = False): |
| 355 | + """Create a support function with built-in shape policies.""" |
| 356 | + if center_ties: |
| 357 | + |
| 358 | + @wp.func |
| 359 | + def shape_support(geom: Any, direction: wp.vec3, data_provider: Any) -> wp.vec3: |
| 360 | + result = support_func(geom, direction, data_provider) |
| 361 | + if geom.shape_type == GeoType.BOX: |
| 362 | + contribution = wp.cw_mul(wp.abs(direction), geom.scale) |
| 363 | + threshold = _CENTERED_BOX_SUPPORT_TIE_EPSILON * (contribution[0] + contribution[1] + contribution[2]) |
| 364 | + if contribution[0] <= threshold: |
| 365 | + result[0] = 0.0 |
| 366 | + if contribution[1] <= threshold: |
| 367 | + result[1] = 0.0 |
| 368 | + if contribution[2] <= threshold: |
| 369 | + result[2] = 0.0 |
| 370 | + return result |
| 371 | + |
| 372 | + else: |
| 373 | + |
| 374 | + @wp.func |
| 375 | + def shape_support(geom: Any, direction: wp.vec3, data_provider: Any) -> wp.vec3: |
| 376 | + return support_func(geom, direction, data_provider) |
| 377 | + |
| 378 | + return shape_support |
| 379 | + |
| 380 | + |
| 381 | +def create_triangle_prism_penetration_refiner(support_func: Any): |
| 382 | + """Create physical-surface refinement for triangle-prism collision proxies. |
| 383 | +
|
| 384 | + MPR operates on closed convex proxies, but a proxy may contain artificial |
| 385 | + faces that are needed only to give it volume. The returned function maps a |
| 386 | + triangle-prism result back to its physical face. |
| 387 | +
|
| 388 | + Args: |
| 389 | + support_func: Support function for individual shapes. |
| 390 | +
|
| 391 | + Returns: |
| 392 | + A function that refines MPR witness points, normal, and penetration. |
| 393 | + """ |
| 394 | + |
| 395 | + shape_support = create_shape_support_function(support_func, center_ties=True) |
| 396 | + |
| 397 | + @wp.func |
| 398 | + def refine_penetration( |
| 399 | + geom_a: Any, |
| 400 | + geom_b: Any, |
| 401 | + orientation_b: wp.quat, |
| 402 | + position_b: wp.vec3, |
| 403 | + extend: float, |
| 404 | + data_provider: Any, |
| 405 | + point_a: wp.vec3, |
| 406 | + point_b: wp.vec3, |
| 407 | + normal: wp.vec3, |
| 408 | + penetration: float, |
| 409 | + ) -> tuple[wp.vec3, wp.vec3, wp.vec3, float]: |
| 410 | + if geom_a.shape_type == int(GeoTypeEx.TRIANGLE_PRISM): |
| 411 | + surface_normal = wp.cross(geom_a.scale, geom_a.auxiliary) |
| 412 | + normal_length_sq = wp.length_sq(surface_normal) |
| 413 | + if normal_length_sq >= 1.0e-24: |
| 414 | + surface_normal /= wp.sqrt(normal_length_sq) |
| 415 | + if surface_normal[2] < 0.0: |
| 416 | + surface_normal = -surface_normal |
| 417 | + |
| 418 | + surface_point_a = shape_support(geom_a, surface_normal, data_provider) |
| 419 | + direction_b = wp.quat_rotate_inv(orientation_b, -surface_normal) |
| 420 | + surface_point_b = shape_support(geom_b, direction_b, data_provider) |
| 421 | + surface_point_b = wp.quat_rotate(orientation_b, surface_point_b) + position_b |
| 422 | + if extend != 0.0: |
| 423 | + offset = surface_normal * extend * 0.5 |
| 424 | + surface_point_a += offset |
| 425 | + surface_point_b -= offset |
| 426 | + surface_penetration = wp.dot(surface_point_a - surface_point_b, surface_normal) |
| 427 | + |
| 428 | + # A finite triangle must not use a support point beyond its |
| 429 | + # footprint. A neighboring heightfield triangle may own that |
| 430 | + # point, while at the outer boundary there may be no surface. |
| 431 | + projected_b = surface_point_b - wp.dot(surface_point_b, surface_normal) * surface_normal |
| 432 | + closest_b = closest_point_on_triangle( |
| 433 | + projected_b, |
| 434 | + wp.vec3(0.0), |
| 435 | + geom_a.scale, |
| 436 | + geom_a.auxiliary, |
| 437 | + ) |
| 438 | + support_on_face = wp.length_sq(projected_b - closest_b) < 1.0e-10 |
| 439 | + |
| 440 | + if not support_on_face: |
| 441 | + # A neighboring cell owns the deepest point, so measure this cell's overlap |
| 442 | + # at MPR's own witness instead. The face normal still applies -- the |
| 443 | + # surface is what shape B is resting on -- but the depth is only what this |
| 444 | + # triangle actually carries. |
| 445 | + surface_point_b = point_b |
| 446 | + surface_penetration = wp.dot(surface_point_a - point_b, surface_normal) |
| 447 | + normal = surface_normal |
| 448 | + penetration = surface_penetration |
| 449 | + point_b = surface_point_b |
| 450 | + point_a = point_b + penetration * normal |
| 451 | + |
| 452 | + return point_a, point_b, normal, penetration |
| 453 | + |
| 454 | + return refine_penetration |
| 455 | + |
| 456 | + |
350 | 457 | @wp.func |
351 | 458 | def extract_shape_data( |
352 | 459 | shape_idx: int, |
|
0 commit comments