diff --git a/mujoco_warp/_src/collision_convex.py b/mujoco_warp/_src/collision_convex.py index ec41b8a53..1b399b8ab 100644 --- a/mujoco_warp/_src/collision_convex.py +++ b/mujoco_warp/_src/collision_convex.py @@ -236,7 +236,6 @@ def ccd_hfield_kernel( epa_vert_index1_in: wp.array2d(dtype=int), epa_vert_index2_in: wp.array2d(dtype=int), epa_face_in: wp.array2d(dtype=int), - epa_pr_in: wp.array2d(dtype=wp.vec3), epa_norm2_in: wp.array2d(dtype=float), epa_horizon_in: wp.array2d(dtype=int), # Data out: @@ -380,7 +379,6 @@ def ccd_hfield_kernel( epa_vert_index1 = epa_vert_index1_in[tid] epa_vert_index2 = epa_vert_index2_in[tid] epa_face = epa_face_in[tid] - epa_pr = epa_pr_in[tid] epa_norm2 = epa_norm2_in[tid] epa_horizon = epa_horizon_in[tid] @@ -461,7 +459,6 @@ def ccd_hfield_kernel( epa_vert_index1, epa_vert_index2, epa_face, - epa_pr, epa_norm2, epa_horizon, ) @@ -720,7 +717,6 @@ def eval_ccd_write_contact( epa_vert_index1_in: wp.array2d(dtype=int), epa_vert_index2_in: wp.array2d(dtype=int), epa_face_in: wp.array2d(dtype=int), - epa_pr_in: wp.array2d(dtype=wp.vec3), epa_norm2_in: wp.array2d(dtype=float), epa_horizon_in: wp.array2d(dtype=int), multiccd_polygon_in: wp.array2d(dtype=wp.vec3), @@ -792,7 +788,6 @@ def eval_ccd_write_contact( epa_vert_index1_in[tid], epa_vert_index2_in[tid], epa_face_in[tid], - epa_pr_in[tid], epa_norm2_in[tid], epa_horizon_in[tid], ) @@ -933,7 +928,6 @@ def ccd_kernel( epa_vert_index1_in: wp.array2d(dtype=int), epa_vert_index2_in: wp.array2d(dtype=int), epa_face_in: wp.array2d(dtype=int), - epa_pr_in: wp.array2d(dtype=wp.vec3), epa_norm2_in: wp.array2d(dtype=float), epa_horizon_in: wp.array2d(dtype=int), multiccd_polygon_in: wp.array2d(dtype=wp.vec3), @@ -1031,7 +1025,6 @@ def ccd_kernel( epa_vert_index1_in, epa_vert_index2_in, epa_face_in, - epa_pr_in, epa_norm2_in, epa_horizon_in, multiccd_polygon_in, @@ -1150,8 +1143,6 @@ def _pair_count(p1: int, p2: int) -> int: epa_vert_index2 = wp.empty(shape=(d.naconmax, 5 + epa_iterations), dtype=int) # epa_face: faces of polytope represented by three indices epa_face = wp.empty(shape=(d.naconmax, 6 + MJ_MAX_EPAFACES * epa_iterations), dtype=int) - # epa_pr: projection of origin on polytope faces - epa_pr = wp.empty(shape=(d.naconmax, 6 + MJ_MAX_EPAFACES * epa_iterations), dtype=wp.vec3) # epa_norm2: epa_pr * epa_pr epa_norm2 = wp.empty(shape=(d.naconmax, 6 + MJ_MAX_EPAFACES * epa_iterations), dtype=float) # epa_horizon: index pair (i j) of edges on horizon @@ -1235,7 +1226,6 @@ def _pair_count(p1: int, p2: int) -> int: epa_vert_index1, epa_vert_index2, epa_face, - epa_pr, epa_norm2, epa_horizon, ], @@ -1320,7 +1310,6 @@ def _pair_count(p1: int, p2: int) -> int: epa_vert_index1, epa_vert_index2, epa_face, - epa_pr, epa_norm2, epa_horizon, multiccd_polygon, diff --git a/mujoco_warp/_src/collision_gjk.py b/mujoco_warp/_src/collision_gjk.py index 6a6a02b90..84fbacc9d 100644 --- a/mujoco_warp/_src/collision_gjk.py +++ b/mujoco_warp/_src/collision_gjk.py @@ -63,7 +63,6 @@ class Polytope: # 10 bits per each vertex index, while the last significant bits are for # invalid and deleted face face: wp.array(dtype=int) - face_pr: wp.array(dtype=wp.vec3) face_norm2: wp.array(dtype=float) nface: int @@ -191,12 +190,17 @@ def _attach_face(pt: Polytope, idx: int, v1: int, v2: int, v3: int) -> float: # compute witness point v r, ret = _project_origin_plane(pt.vert1[v3] - pt.vert2[v3], pt.vert1[v2] - pt.vert2[v2], pt.vert1[v1] - pt.vert2[v1]) - if ret: + if ret == 0: return 0.0 - face = v1 + (v2 << 10) + (v3 << 20) + if ret == 1: + face = v3 + (v2 << 10) + (v1 << 20) + elif ret == 2: + face = v3 + (v1 << 10) + (v2 << 20) + else: + face = v2 + (v3 << 10) + (v1 << 20) + pt.face[idx] = face - pt.face_pr[idx] = r pt.face_norm2[idx] = wp.dot(r, r) return pt.face_norm2[idx] @@ -287,24 +291,24 @@ def _project_origin_plane(v1: wp.vec3, v2: wp.vec3, v3: wp.vec3) -> Tuple[wp.vec return z, 1 if nv != 0 and nn > MINVAL: v = (nv / nn) * n - return v, 0 + return v, 1 # n = (v2 - v1) x (v3 - v1) n = wp.cross(diff21, diff31) nv = wp.dot(n, v1) nn = wp.dot(n, n) if nn == 0: - return z, 1 + return z, 0 if nv != 0 and nn > MINVAL: v = (nv / nn) * n - return v, 0 + return v, 2 # n = (v1 - v3) x (v2 - v3) n = wp.cross(diff31, diff32) nv = wp.dot(n, v3) nn = wp.dot(n, n) v = (nv / nn) * n - return v, 0 + return v, 3 @wp.func @@ -387,7 +391,7 @@ def _S3D(s1: wp.vec3, s2: wp.vec3, s3: wp.vec3, s4: wp.vec3) -> wp.vec4: def _S2D(s1: wp.vec3, s2: wp.vec3, s3: wp.vec3) -> wp.vec3: # project origin onto affine hull of the simplex p_o, ret = _project_origin_plane(s1, s2, s3) - if ret: + if ret == 0: v = _S1D(s1, s2) return wp.vec3(v[0], v[1], 0.0) @@ -861,7 +865,7 @@ def _epa_witness( v2 = pt.vert1[face[1]] - pt.vert2[face[1]] v3 = pt.vert1[face[2]] - pt.vert2[face[2]] - coordinates = _tri_affine_coord(v1, v2, v3, pt.face_pr[face_idx]) + coordinates = _tri_affine_coord(v1, v2, v3, _face_pr(pt, face_idx)) l1 = coordinates[0] l2 = coordinates[1] l3 = coordinates[2] @@ -1196,6 +1200,16 @@ def _is_invalid_face(face: int) -> bool: return bool(face & 0xC0000000) +@wp.func +def _face_pr(pt: Polytope, idx: int) -> wp.vec3: + face = _get_face_verts(pt.face[idx]) + v1 = pt.vert1[face[0]] - pt.vert2[face[0]] + v2 = pt.vert1[face[1]] - pt.vert2[face[1]] + v3 = pt.vert1[face[2]] - pt.vert2[face[2]] + n = wp.cross(v3 - v2, v2 - v1) + return (wp.dot(n, v2) / wp.dot(n, n)) * n + + @wp.func def _epa( # In: @@ -1245,7 +1259,7 @@ def _epa( # compute support point w from the closest face's normal lower = wp.sqrt(lower2) wi = pt.nvert - face_pr_normalized = pt.face_pr[idx] / lower + face_pr_normalized = _face_pr(pt, idx) / lower i1, i2 = _epa_support(pt, wi, geom1, geom2, geomtype1, geomtype2, face_pr_normalized) w = pt.vert1[wi] - pt.vert2[wi] geom1.index = i1 @@ -1286,7 +1300,7 @@ def _epa( if _is_face_deleted(pt.face[i]): continue - if wp.dot(pt.face_pr[i], w) - pt.face_norm2[i] > 1e-10: + if wp.dot(_face_pr(pt, i), w) - pt.face_norm2[i] > 1e-10: nvalid = wp.where(_is_invalid_face(pt.face[i]), nvalid, nvalid - 1) pt.face[i] = _delete_face(pt.face[i]) face = _get_face_verts(pt.face[i]) @@ -2222,7 +2236,6 @@ def ccd( vert_index1: wp.array(dtype=int), vert_index2: wp.array(dtype=int), face: wp.array(dtype=int), - face_pr: wp.array(dtype=wp.vec3), face_norm2: wp.array(dtype=float), horizon: wp.array(dtype=int), ) -> Tuple[float, int, wp.vec3, wp.vec3, int]: @@ -2281,7 +2294,6 @@ def ccd( pt.vert_index1 = vert_index1 pt.vert_index2 = vert_index2 pt.face = face - pt.face_pr = face_pr pt.face_norm2 = face_norm2 pt.horizon = horizon diff --git a/mujoco_warp/_src/collision_gjk_test.py b/mujoco_warp/_src/collision_gjk_test.py index 92999a111..9e721d680 100644 --- a/mujoco_warp/_src/collision_gjk_test.py +++ b/mujoco_warp/_src/collision_gjk_test.py @@ -50,7 +50,6 @@ def _geom_dist( epa_vert_index1 = wp.empty(5 + m.opt.ccd_iterations, dtype=int) epa_vert_index2 = wp.empty(5 + m.opt.ccd_iterations, dtype=int) epa_face = wp.empty(6 + MJ_MAX_EPAFACES * m.opt.ccd_iterations, dtype=int) - epa_pr = wp.empty(6 + MJ_MAX_EPAFACES * m.opt.ccd_iterations, dtype=wp.vec3) epa_norm2 = wp.empty(6 + MJ_MAX_EPAFACES * m.opt.ccd_iterations, dtype=float) epa_horizon = wp.empty(2 * MJ_MAX_EPAHORIZON, dtype=int) multiccd_polygon = wp.empty(2 * nmaxpolygon, dtype=wp.vec3) @@ -96,7 +95,6 @@ def _ccd_kernel( vert_index1: wp.array(dtype=int), vert_index2: wp.array(dtype=int), face: wp.array(dtype=int), - face_pr: wp.array(dtype=wp.vec3), face_norm2: wp.array(dtype=float), horizon: wp.array(dtype=int), polygon: wp.array(dtype=wp.vec3), @@ -199,7 +197,6 @@ def _ccd_kernel( vert_index1, vert_index2, face, - face_pr, face_norm2, horizon, ) @@ -268,7 +265,6 @@ def _ccd_kernel( epa_vert_index1, epa_vert_index2, epa_face, - epa_pr, epa_norm2, epa_horizon, multiccd_polygon,