Skip to content

Remove epa_pr #1014

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
kbayes wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Remove epa_pr #1014

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ba273e2
remove epa_pr
kbayes Jan 13, 2026
a348470
fix params
kbayes Jan 13, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 0 additions & 11 deletions mujoco_warp/_src/collision_convex.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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]

Expand Down Expand Up @@ -461,7 +459,6 @@ def ccd_hfield_kernel(
epa_vert_index1,
epa_vert_index2,
epa_face,
epa_pr,
epa_norm2,
epa_horizon,
)
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -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],
)
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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,
],
Expand Down Expand Up @@ -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,
Expand Down
40 changes: 26 additions & 14 deletions mujoco_warp/_src/collision_gjk.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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])
Expand Down Expand Up @@ -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]:
Expand Down Expand Up @@ -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

Expand Down
4 changes: 0 additions & 4 deletions mujoco_warp/_src/collision_gjk_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -199,7 +197,6 @@ def _ccd_kernel(
vert_index1,
vert_index2,
face,
face_pr,
face_norm2,
horizon,
)
Expand Down Expand Up @@ -268,7 +265,6 @@ def _ccd_kernel(
epa_vert_index1,
epa_vert_index2,
epa_face,
epa_pr,
epa_norm2,
epa_horizon,
multiccd_polygon,
Expand Down
Loading