From 218226fc956dbeec267894161ae753f6375be570 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 13 Jan 2026 10:21:31 -0800 Subject: [PATCH] Breaking change: Add surface normal output to MuJoCo raycast functions. PiperOrigin-RevId: 855781592 Change-Id: Id96b1ca7eaf722e260cc69d7706c28dc51f52d92 --- doc/APIreference/functions.rst | 50 +++--- doc/APIreference/functions_override.rst | 4 +- doc/changelog.rst | 13 +- doc/includes/references.h | 22 +-- include/mujoco/mujoco.h | 42 ++--- python/mujoco/bindings_test.py | 33 +++- python/mujoco/functions.cc | 108 +++++++++++-- python/mujoco/introspect/functions.py | 192 ++++++++++++++--------- src/engine/engine_collision_sdf.c | 6 +- src/engine/engine_ray.c | 121 +++++--------- src/engine/engine_ray.h | 46 ++---- src/engine/engine_sensor.c | 14 +- src/engine/engine_vis_interact.c | 11 +- src/experimental/platform/interaction.cc | 6 +- test/engine/engine_ray_test.cc | 59 +++---- unity/Runtime/Bindings/MjBindings.cs | 12 +- wasm/codegen/generated/bindings.cc | 44 +++--- 17 files changed, 457 insertions(+), 326 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 8d643c21..8ac117b3 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1150,19 +1150,6 @@ after :ref:`mj_kinematics`, or functions that call it (e.g. :ref:`mj_fwdPositio intersect with all geoms types, are :ref:`mj_ray` which casts a single ray, and :ref:`mj_multiRay` which casts multiple rays from a single point. -.. _mj_multiRay: - -`mj_multiRay <#mj_multiRay>`__ -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -.. mujoco-include:: mj_multiRay - -Intersect multiple rays emanating from a single point. - -Similar semantics to mj_ray, but vec is an array of (nray x 3) directions. - -*Nullable:* ``geomgroup`` - .. _mj_ray: `mj_ray <#mj_ray>`__ @@ -1174,12 +1161,29 @@ Intersect ray ``pnt+x*vec, x >= 0`` with geoms. - Return distance ``x`` to nearest surface, or -1 if no intersection. - If ``geomid`` is not NULL, write the id of the intersected geom or -1 if not intersection. +- If ``normal`` is not NULL, write the surface normal at the intersection point. The normal always points **out of the + geometry**, regardless of the ray's direction (i.e., including rays hitting the surface from the inside). - Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies. - ``geomgroup`` is an array of length :ref:`mjNGROUP`, where 1 means the group should be included. Pass NULL to skip geom group exclusion. - If ``flg_static`` is 0, static geoms will be excluded. -*Nullable:* ``geomgroup``, ``geomid`` +*Nullable:* ``geomgroup``, ``geomid``, ``normal`` + +.. _mj_multiRay: + +`mj_multiRay <#mj_multiRay>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_multiRay + +Intersect multiple rays emanating from a single point, compute normals if given. + +Similar semantics to mj_ray, but vec, normal and dist are arrays. + +Geoms further than cutoff are ignored. + +*Nullable:* ``geomgroup``, ``geomid``, ``normal`` .. _mj_rayHfield: @@ -1190,6 +1194,8 @@ Intersect ray ``pnt+x*vec, x >= 0`` with geoms. Intersect ray with hfield; return nearest distance or -1 if no intersection. +*Nullable:* ``normal`` + .. _mj_rayMesh: `mj_rayMesh <#mj_rayMesh>`__ @@ -1199,6 +1205,8 @@ Intersect ray with hfield; return nearest distance or -1 if no intersection. Intersect ray with mesh; return nearest distance or -1 if no intersection. +*Nullable:* ``normal`` + .. _mju_rayGeom: `mju_rayGeom <#mju_rayGeom>`__ @@ -1208,17 +1216,19 @@ Intersect ray with mesh; return nearest distance or -1 if no intersection. Intersect ray with pure geom; return nearest distance or -1 if no intersection. -.. _mju_rayFlex: +*Nullable:* ``normal`` -`mju_rayFlex <#mju_rayFlex>`__ -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +.. _mj_rayFlex: -.. mujoco-include:: mju_rayFlex +`mj_rayFlex <#mj_rayFlex>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_rayFlex Intersect ray with flex; return nearest distance or -1 if no intersection, -and also output nearest vertex id. +and also output nearest vertex id and surface normal. -*Nullable:* ``vertid`` +*Nullable:* ``vertid``, ``normal`` .. _mju_raySkin: diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index a464c585..38629433 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -332,12 +332,14 @@ Intersect ray ``pnt+x*vec, x >= 0`` with geoms. - Return distance ``x`` to nearest surface, or -1 if no intersection. - If ``geomid`` is not NULL, write the id of the intersected geom or -1 if not intersection. +- If ``normal`` is not NULL, write the surface normal at the intersection point. The normal always points **out of the + geometry**, regardless of the ray's direction (i.e., including rays hitting the surface from the inside). - Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies. - ``geomgroup`` is an array of length :ref:`mjNGROUP`, where 1 means the group should be included. Pass NULL to skip geom group exclusion. - If ``flg_static`` is 0, static geoms will be excluded. -*Nullable:* ``geomgroup``, ``geomid`` +*Nullable:* ``geomgroup``, ``geomid``, ``normal`` .. _Interaction: diff --git a/doc/changelog.rst b/doc/changelog.rst index 1e666d13..1e90385d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,6 +8,17 @@ Upcoming version (not yet released) .. admonition:: Breaking API changes :class: attention + - Ray-cast functions now optionally compute the surface normal at the ray intersection. This is a breaking change due + to the addition of the ``mjtNum normal[3]`` argument. The modified functions are :ref:`mj_ray`, :ref:`mj_multiRay`, + :ref:`mju_rayGeom`, :ref:`mj_rayFlex`, :ref:`mj_rayHfield` and :ref:`mj_rayMesh`. + + **Migration:** In C/C++, pass ``NULL`` to the ``normal`` argument (last argument for all functions except + :ref:`mj_multiRay`). Note that in the Python bindings, if the new ``normal`` argument is last, it defaults to + ``None``, so no changes are required. + + - ``mju_rayFlex`` has been renamed to :ref:`mj_rayFlex` for consistency with other functions that take + ``mjModel*`` and ``mjData*`` arguments. + - The ``mjModel.cam_orthographic`` field has been renamed to ``cam_projection``, with the semantic of a new enum type :ref:`mjtProjection`. This will allow for more projection types in the future like fisheye cameras. Relatedly, the ``camera/orthographic`` MJCF attribute for cameras has been renamed to @@ -68,7 +79,7 @@ Bug fixes - Multi threaded mesh processing, enabled by the :ref:`usethread` compiler flag (on by default), was in fact disabled by the flag. Fixing this bug speeds up compilation of mesh-heavy models by (up to) the number of available cores. -- The ``vertid`` argument of :ref:`mju_rayFlex` and :ref:`mju_raySkin` was marked as nullable but was not; it is now +- The ``vertid`` argument of :ref:`mj_rayFlex` and :ref:`mju_raySkin` was marked as nullable but was not; it is now nullable. Version 3.4.0 (December 5, 2025) diff --git a/doc/includes/references.h b/doc/includes/references.h index 04db929f..98bed114 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3255,21 +3255,23 @@ void mj_loadPluginLibrary(const char* path); void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback); int mj_version(void); const char* mj_versionString(void); -void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int* geomid, mjtNum* dist, int nray, mjtNum cutoff); mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int geomid[1]); + int geomid[1], mjtNum normal[3]); +void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, + int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff); mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3]); + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]); mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3]); + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]); mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], - const mjtNum pnt[3], const mjtNum vec[3], int geomtype); -mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, - mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, - const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); + const mjtNum pnt[3], const mjtNum vec[3], int geomtype, + mjtNum normal[3]); +mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer, + mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, + mjtByte flg_skin, int flexid, const mjtNum pnt[3], + const mjtNum vec[3], int vertid[1], mjtNum normal[3]); mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); void mjv_defaultCamera(mjvCamera* cam); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 4e75ef2f..b42d7891 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -626,39 +626,45 @@ MJAPI const char* mj_versionString(void); //---------------------------------- Ray casting --------------------------------------------------- -// Intersect multiple rays emanating from a single point. -// Similar semantics to mj_ray, but vec is an array of (nray x 3) directions. -// Nullable: geomgroup -MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int* geomid, mjtNum* dist, int nray, mjtNum cutoff); - // Intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms in bodyexclude. -// Return distance (x) to nearest surface, or -1 if no intersection and output geomid. +// Return distance (x) to nearest surface, or -1 if no intersection. // geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion. -// Nullable: geomgroup, geomid +// Nullable: geomgroup, geomid, normal MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int geomid[1]); + int geomid[1], mjtNum normal[3]); + +// Intersect multiple rays emanating from a single point, compute normals if given. +// Similar semantics to mj_ray, but vec, normal and dist are arrays. +// Geoms further than cutoff are ignored. +// Nullable: geomgroup, geomid, normal +MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, + int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff); // Intersect ray with hfield; return nearest distance or -1 if no intersection. +// Nullable: normal MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3]); + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]); // Intersect ray with mesh; return nearest distance or -1 if no intersection. +// Nullable: normal MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3]); + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]); // Intersect ray with pure geom; return nearest distance or -1 if no intersection. +// Nullable: normal MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], - const mjtNum pnt[3], const mjtNum vec[3], int geomtype); + const mjtNum pnt[3], const mjtNum vec[3], int geomtype, + mjtNum normal[3]); // Intersect ray with flex; return nearest distance or -1 if no intersection, -// and also output nearest vertex id. -// Nullable: vertid -MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, - mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, - const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); +// and also output nearest vertex id and surface normal. +// Nullable: vertid, normal +MJAPI mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer, + mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, + mjtByte flg_skin, int flexid, const mjtNum pnt[3], + const mjtNum vec[3], int vertid[1], mjtNum normal[3]); // Intersect ray with skin; return nearest distance or -1 if no intersection, // and also output nearest vertex id. diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 42e758cf..321188a6 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1288,8 +1288,9 @@ Euler integrator, semi-implicit in velocity. geomid = np.zeros(1, np.int32) mujoco.mj_forward(self.model, self.data) mujoco.mj_ray( - self.model, self.data, [0, 0, 0], [0, 0, 1], None, 0, 0, geomid + self.model, self.data, [0, 0, 0], [0, 0, 1], None, 0, 0, geomid, None ) + # Check the normal argument is optional mujoco.mj_ray( self.model, self.data, @@ -1301,6 +1302,7 @@ Euler integrator, semi-implicit in velocity. geomid, ) # Check that named arguments work + normal = np.zeros(3, np.float64) mujoco.mj_ray( m=self.model, d=self.data, @@ -1310,8 +1312,34 @@ Euler integrator, semi-implicit in velocity. flg_static=0, bodyexclude=0, geomid=geomid, + normal=normal, ) + def test_mju_ray_geom(self): + # Test mju_rayGeom with a plane at origin + pos = np.zeros(3) + mat = np.eye(3).flatten() + size = np.array([10.0, 10.0, 1.0]) + pnt = np.array([5.0, 5.0, 5.0]) + # Normalize direction for Euclidean distance + vec = np.array([-1.0, -1.0, -1.0]) + vec = vec / np.linalg.norm(vec) + normal = np.zeros(3) + + # Call with normal argument + dist = mujoco.mju_rayGeom( + pos, mat, size, pnt, vec, mujoco.mjtGeom.mjGEOM_PLANE, normal + ) + expected_dist = np.sqrt(3 * 5 * 5) + np.testing.assert_allclose(dist, expected_dist) + np.testing.assert_allclose(normal, [0, 0, 1]) + + # Call without normal argument (should still work) + dist2 = mujoco.mju_rayGeom( + pos, mat, size, pnt, vec, mujoco.mjtGeom.mjGEOM_PLANE + ) + np.testing.assert_allclose(dist2, expected_dist) + def test_mj_multi_ray(self): nray = 3 geom1 = np.zeros(1, np.int32) @@ -1333,6 +1361,7 @@ Euler integrator, semi-implicit in velocity. bodyexclude=-1, geomid=geomid, dist=dist, + normal=None, nray=nray, cutoff=mujoco.mjMAXVAL, ) @@ -1340,7 +1369,7 @@ Euler integrator, semi-implicit in velocity. for i in range(0, 3): self.assertEqual( dist[i], - mujoco.mj_ray(self.model, self.data, pnt, vec[i], None, 1, -1, geom1), + mujoco.mj_ray(self.model, self.data, pnt, vec[i], None, 1, -1, geom1, None), ) self.assertEqual(geomid[i], geom1) self.assertEqual(geomid[i], geom_ex[i]) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 3ddb86fd..fd83692a 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -30,6 +30,7 @@ #include "private.h" #include "raw.h" #include "structs.h" +#include "util/func_wrap.h" #include #include #include @@ -677,33 +678,106 @@ PYBIND11_MODULE(_functions, pymodule) { std::optional>> geomgroup, mjtByte flg_static, int bodyexclude, Eigen::Ref geomid, - Eigen::Ref dist, int nray, mjtNum cutoff) { + Eigen::Ref dist, + std::optional> normal, + int nray, mjtNum cutoff) { if (dist.size() != nray || geomid.size() != nray) { throw py::type_error("dist and geomid should be of size nray"); } if (vec.size() != 3 * nray) { throw py::type_error("vec should be of size 3*nray"); } + if (normal.has_value() && normal->size() != 3 * nray) { + throw py::type_error("normal should be of size 3*nray"); + } InterceptMjErrors(::mj_multiRay)( m, d, &(*pnt)[0], vec.data(), geomgroup.has_value() ? geomgroup->data() : nullptr, flg_static, - bodyexclude, geomid.data(), dist.data(), nray, cutoff); + bodyexclude, geomid.data(), dist.data(), + normal.has_value() ? normal->data() : nullptr, nray, cutoff); }); - Def( - pymodule, - [](const raw::MjModel* m, const raw::MjData* d, const mjtNum(*pnt)[3], - const mjtNum(*vec)[3], - std::optional>> - geomgroup, - mjtByte flg_static, int bodyexclude, int(*geomid)[1]) { - return mj_ray(m, d, &(*pnt)[0], &(*vec)[0], - geomgroup.has_value() ? geomgroup->data() : nullptr, - flg_static, bodyexclude, &(*geomid)[0]); - }); - Def(pymodule); - Def(pymodule); - Def(pymodule); - Def(pymodule); + pymodule.def( + "mj_ray", + util::UnwrapArgs( + [](const raw::MjModel* m, const raw::MjData* d, const mjtNum(*pnt)[3], + const mjtNum(*vec)[3], + std::optional>> + geomgroup, + mjtByte flg_static, int bodyexclude, + std::optional>> geomid, + std::optional>> normal) { + return mj_ray(m, d, &(*pnt)[0], &(*vec)[0], + geomgroup.has_value() ? geomgroup->data() : nullptr, + flg_static, bodyexclude, + geomid.has_value() ? geomid->data() : nullptr, + normal.has_value() ? normal->data() : nullptr); + }), + py::arg("m"), py::arg("d"), py::arg("pnt"), py::arg("vec"), + py::arg("geomgroup"), py::arg("flg_static"), py::arg("bodyexclude"), + py::arg("geomid"), py::arg("normal") = std::nullopt, + py::doc(traits::mj_ray::doc), + py::call_guard()); + pymodule.def( + "mj_rayHfield", + util::UnwrapArgs( + [](const raw::MjModel* m, const raw::MjData* d, int geomid, + const mjtNum(*pnt)[3], const mjtNum(*vec)[3], + std::optional>> normal) { + return mj_rayHfield(m, d, geomid, &(*pnt)[0], &(*vec)[0], + normal.has_value() ? normal->data() : nullptr); + }), + py::arg("m"), py::arg("d"), py::arg("geomid"), py::arg("pnt"), + py::arg("vec"), py::arg("normal") = std::nullopt, + py::doc(traits::mj_rayHfield::doc), + py::call_guard()); + pymodule.def( + "mj_rayMesh", + util::UnwrapArgs( + [](const raw::MjModel* m, const raw::MjData* d, int geomid, + const mjtNum(*pnt)[3], const mjtNum(*vec)[3], + std::optional>> normal) { + return mj_rayMesh(m, d, geomid, &(*pnt)[0], &(*vec)[0], + normal.has_value() ? normal->data() : nullptr); + }), + py::arg("m"), py::arg("d"), py::arg("geomid"), py::arg("pnt"), + py::arg("vec"), py::arg("normal") = std::nullopt, + py::doc(traits::mj_rayMesh::doc), + py::call_guard()); + pymodule.def( + "mju_rayGeom", + util::UnwrapArgs( + [](const mjtNum(*pos)[3], const mjtNum(*mat)[9], + const mjtNum(*size)[3], const mjtNum(*pnt)[3], + const mjtNum(*vec)[3], int geomtype, + std::optional>> normal) { + return mju_rayGeom(&(*pos)[0], &(*mat)[0], &(*size)[0], &(*pnt)[0], + &(*vec)[0], geomtype, + normal.has_value() ? normal->data() : nullptr); + }), + py::arg("pos"), py::arg("mat"), py::arg("size"), py::arg("pnt"), + py::arg("vec"), py::arg("geomtype"), py::arg("normal") = std::nullopt, + py::doc(traits::mju_rayGeom::doc), + py::call_guard()); + pymodule.def( + "mj_rayFlex", + util::UnwrapArgs( + [](const raw::MjModel* m, const raw::MjData* d, int flex_layer, + mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, + mjtByte flg_skin, int flexid, const mjtNum(*pnt)[3], + const mjtNum(*vec)[3], + std::optional>> vertid, + std::optional>> normal) { + return mj_rayFlex(m, d, flex_layer, flg_vert, flg_edge, flg_face, + flg_skin, flexid, &(*pnt)[0], &(*vec)[0], + vertid.has_value() ? vertid->data() : nullptr, + normal.has_value() ? normal->data() : nullptr); + }), + py::arg("m"), py::arg("d"), py::arg("flex_layer"), py::arg("flg_vert"), + py::arg("flg_edge"), py::arg("flg_face"), py::arg("flg_skin"), + py::arg("flexid"), py::arg("pnt"), py::arg("vec"), + py::arg("vertid") = std::nullopt, py::arg("normal") = std::nullopt, + py::doc(traits::mj_rayFlex::doc), + py::call_guard()); Def(pymodule); // Interaction diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 6554eb9e..072a63e3 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -3729,74 +3729,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ parameters=(), doc='Return the current version of MuJoCo as a null-terminated string.', # pylint: disable=line-too-long )), - ('mj_multiRay', - FunctionDecl( - name='mj_multiRay', - return_type=ValueType(name='void'), - parameters=( - FunctionParameterDecl( - name='m', - type=PointerType( - inner_type=ValueType(name='mjModel', is_const=True), - ), - ), - FunctionParameterDecl( - name='d', - type=PointerType( - inner_type=ValueType(name='mjData'), - ), - ), - FunctionParameterDecl( - name='pnt', - type=ArrayType( - inner_type=ValueType(name='mjtNum', is_const=True), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='vec', - type=PointerType( - inner_type=ValueType(name='mjtNum', is_const=True), - ), - ), - FunctionParameterDecl( - name='geomgroup', - type=PointerType( - inner_type=ValueType(name='mjtByte', is_const=True), - ), - nullable=True, - ), - FunctionParameterDecl( - name='flg_static', - type=ValueType(name='mjtByte'), - ), - FunctionParameterDecl( - name='bodyexclude', - type=ValueType(name='int'), - ), - FunctionParameterDecl( - name='geomid', - type=PointerType( - inner_type=ValueType(name='int'), - ), - ), - FunctionParameterDecl( - name='dist', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - ), - FunctionParameterDecl( - name='nray', - type=ValueType(name='int'), - ), - FunctionParameterDecl( - name='cutoff', - type=ValueType(name='mjtNum'), - ), - ), - doc='Intersect multiple rays emanating from a single point. Similar semantics to mj_ray, but vec is an array of (nray x 3) directions.', # pylint: disable=line-too-long - )), ('mj_ray', FunctionDecl( name='mj_ray', @@ -3851,8 +3783,92 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), nullable=True, ), + FunctionParameterDecl( + name='normal', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + nullable=True, + ), ), - doc='Intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms in bodyexclude. Return distance (x) to nearest surface, or -1 if no intersection and output geomid. geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion.', # pylint: disable=line-too-long + doc='Intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms in bodyexclude. Return distance (x) to nearest surface, or -1 if no intersection. geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion.', # pylint: disable=line-too-long + )), + ('mj_multiRay', + FunctionDecl( + name='mj_multiRay', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData'), + ), + ), + FunctionParameterDecl( + name='pnt', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='vec', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + FunctionParameterDecl( + name='geomgroup', + type=PointerType( + inner_type=ValueType(name='mjtByte', is_const=True), + ), + nullable=True, + ), + FunctionParameterDecl( + name='flg_static', + type=ValueType(name='mjtByte'), + ), + FunctionParameterDecl( + name='bodyexclude', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='geomid', + type=PointerType( + inner_type=ValueType(name='int'), + ), + nullable=True, + ), + FunctionParameterDecl( + name='dist', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + FunctionParameterDecl( + name='normal', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + nullable=True, + ), + FunctionParameterDecl( + name='nray', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='cutoff', + type=ValueType(name='mjtNum'), + ), + ), + doc='Intersect multiple rays emanating from a single point, compute normals if given. Similar semantics to mj_ray, but vec, normal and dist are arrays. Geoms further than cutoff are ignored.', # pylint: disable=line-too-long )), ('mj_rayHfield', FunctionDecl( @@ -3889,6 +3905,14 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ extents=(3,), ), ), + FunctionParameterDecl( + name='normal', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + nullable=True, + ), ), doc='Intersect ray with hfield; return nearest distance or -1 if no intersection.', # pylint: disable=line-too-long )), @@ -3927,6 +3951,14 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ extents=(3,), ), ), + FunctionParameterDecl( + name='normal', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + nullable=True, + ), ), doc='Intersect ray with mesh; return nearest distance or -1 if no intersection.', # pylint: disable=line-too-long )), @@ -3974,12 +4006,20 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ name='geomtype', type=ValueType(name='int'), ), + FunctionParameterDecl( + name='normal', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + nullable=True, + ), ), doc='Intersect ray with pure geom; return nearest distance or -1 if no intersection.', # pylint: disable=line-too-long )), - ('mju_rayFlex', + ('mj_rayFlex', FunctionDecl( - name='mju_rayFlex', + name='mj_rayFlex', return_type=ValueType(name='mjtNum'), parameters=( FunctionParameterDecl( @@ -4040,8 +4080,16 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), nullable=True, ), + FunctionParameterDecl( + name='normal', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + nullable=True, + ), ), - doc='Intersect ray with flex; return nearest distance or -1 if no intersection, and also output nearest vertex id.', # pylint: disable=line-too-long + doc='Intersect ray with flex; return nearest distance or -1 if no intersection, and also output nearest vertex id and surface normal.', # pylint: disable=line-too-long )), ('mju_raySkin', FunctionDecl( diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 89278f53..e0f01143 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -286,10 +286,10 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p mju_addTo3(a, d->geom_xpos + 3 * i); mjtNum dir[3] = {-a[0], -a[1], -a[2]}; mjtNum r = mju_norm3(dir); - mjtNum dist = mj_rayMesh(m, d, i, a, dir); + mjtNum dist = mj_rayMesh(m, d, i, a, dir, NULL); if (dist > r) { mju_scl3(dir, dir, -1); - return -mj_rayMesh(m, d, i, a, dir); + return -mj_rayMesh(m, d, i, a, dir, NULL); } return dist; } @@ -414,7 +414,7 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, mju_addTo3(a, d->geom_xpos+3*i); mjtNum dir[3] = {-a[0], -a[1], -a[2]}; mjtNum r = mju_norm3(dir); - mjtNum dist = mj_rayMesh(m, d, i, a, dir); + mjtNum dist = mj_rayMesh(m, d, i, a, dir, NULL); gradient[0] = dist > r ? 1 : -1; gradient[1] = dist > r ? 1 : -1; gradient[2] = dist > r ? 1 : -1; diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 66e7a1d7..ef4bb686 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -559,8 +559,8 @@ static mjtNum ray_box(const mjtNum pos[3], const mjtNum mat[9], const mjtNum siz // intersect ray with hfield, compute normal if given -static mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) { +mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid, + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) { // clear normal if given if (normal) mju_zero3(normal); @@ -738,13 +738,6 @@ static mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid, } -// intersect ray with hfield -mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3]) { - return mj_rayHfieldNormal(m, d, geomid, pnt, vec, NULL); -} - - // ray vs axis-aligned bounding box using slab method // see Ericson, Real-time Collision Detection section 5.3.3. int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3], @@ -889,8 +882,8 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3 // intersect ray with signed distance field, compute normal if given -static mjtNum mj_raySdfNormal(const mjModel* m, const mjData* d, int g, - const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) { +static mjtNum mj_raySdf(const mjModel* m, const mjData* d, int g, + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) { if (normal) mju_zero3(normal); mjtNum distance_total = 0; @@ -956,8 +949,8 @@ static mjtNum mj_raySdfNormal(const mjModel* m, const mjData* d, int g, } // intersect ray with mesh, compute normal if given -static mjtNum mj_rayMeshNormal(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3], - const mjtNum vec[3], mjtNum normal[3]) { +mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3], + const mjtNum vec[3], mjtNum normal[3]) { // clear normal if given if (normal) mju_zero3(normal); @@ -975,17 +968,10 @@ static mjtNum mj_rayMeshNormal(const mjModel* m, const mjData* d, int id, const } -// intersect ray with mesh -mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3], - const mjtNum vec[3]) { - return mj_rayMeshNormal(m, d, id, pnt, vec, NULL); -} - - -// intersect ray and find normal with primitive geom, no meshes or hfields, compute normal if given -mjtNum mju_rayGeomNormal(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], - const mjtNum pnt[3], const mjtNum vec[3], int geomtype, - mjtNum normal[3]) { +// intersect ray with primitive geom, no meshes or hfields, compute normal if given +mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], + const mjtNum pnt[3], const mjtNum vec[3], int geomtype, + mjtNum normal[3]) { switch ((mjtGeom) geomtype) { case mjGEOM_PLANE: return ray_plane(pos, mat, size, pnt, vec, normal); @@ -1012,18 +998,11 @@ mjtNum mju_rayGeomNormal(const mjtNum pos[3], const mjtNum mat[9], const mjtNum } -// intersect ray with primitive geom, no meshes or hfields -mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], - const mjtNum pnt[3], const mjtNum vec[3], int geomtype) { - return mju_rayGeomNormal(pos, mat, size, pnt, vec, geomtype, NULL); -} - - // intersect ray with flex, return nearest vertex id, compute normal if given -mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer, - mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, - mjtByte flg_skin, int flexid, const mjtNum pnt[3], - const mjtNum vec[3], int vertid[1], mjtNum normal[3]) { +mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer, + mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, + mjtByte flg_skin, int flexid, const mjtNum pnt[3], + const mjtNum vec[3], int vertid[1], mjtNum normal[3]) { int dim = m->flex_dim[flexid]; // clear normal if given @@ -1102,8 +1081,8 @@ mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer, mju_quat2Mat(mat, quat); // intersect ray with capsule - mjtNum sol = mju_rayGeomNormal(pos, mat, size, pnt, vec, mjGEOM_CAPSULE, - normal ? normal_local : NULL); + mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE, + normal ? normal_local : NULL); // update if (sol >= 0 && (x < 0 || sol < x)) { @@ -1136,8 +1115,8 @@ mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer, size[0] = radius; // intersect ray with sphere - mjtNum sol = mju_rayGeomNormal(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE, - normal ? normal_local : NULL); + mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE, + normal ? normal_local : NULL); // update if (sol >= 0 && (x < 0 || sol < x)) { @@ -1208,17 +1187,6 @@ mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer, return x; } - -// intersect ray with flex, return nearest vertex id -mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, - mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, - mjtByte flg_skin, int flexid, const mjtNum pnt[3], - const mjtNum vec[3], int vertid[1]) { - return mju_rayFlexNormal(m, d, flex_layer, flg_vert, flg_edge, flg_face, - flg_skin, flexid, pnt, vec, vertid, NULL); -} - - // intersect ray with skin, return nearest vertex id mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]) { @@ -1337,9 +1305,9 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3], // intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude // return geomid and distance (x) to nearest surface, or -1 if no intersection // geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion -mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int geomid[1], mjtNum normal[3]) { +mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, + int geomid[1], mjtNum normal[3]) { int ngeom = m->ngeom; mjtNum dist, newdist; mjtNum normal_local[3]; @@ -1360,13 +1328,13 @@ mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], cons if (!ray_eliminate(m, d, i, geomgroup, flg_static, bodyexclude)) { int type = m->geom_type[i]; if (type == mjGEOM_MESH) { - newdist = mj_rayMeshNormal(m, d, i, pnt, vec, p_normal); + newdist = mj_rayMesh(m, d, i, pnt, vec, p_normal); } else if (type == mjGEOM_HFIELD) { - newdist = mj_rayHfieldNormal(m, d, i, pnt, vec, p_normal); + newdist = mj_rayHfield(m, d, i, pnt, vec, p_normal); } else if (type == mjGEOM_SDF) { - newdist = mj_raySdfNormal(m, d, i, pnt, vec, p_normal); + newdist = mj_raySdf(m, d, i, pnt, vec, p_normal); } else { - newdist = mju_rayGeomNormal(d->geom_xpos+3*i, d->geom_xmat+9*i, + newdist = mju_rayGeom(d->geom_xpos+3*i, d->geom_xmat+9*i, m->geom_size+3*i, pnt, vec, type, p_normal); } @@ -1383,13 +1351,6 @@ mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], cons } -// intersect ray -mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int geomid[1]) { - return mj_rayNormal(m, d, pnt, vec, geomgroup, flg_static, bodyexclude, geomid, NULL); -} - - // Initializes spherical bounding angles (geom_ba) and flag vector for a given source void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum ray_xmat[9], const mjtByte* geomgroup, mjtByte flg_static, @@ -1502,7 +1463,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co // clear result dist = -1; - *geomid = -1; + if (geomid) *geomid = -1; if (normal) mju_zero3(normal); // get ray spherical coordinates @@ -1556,20 +1517,20 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co // dispatch to type-specific ray function int type = m->geom_type[i]; if (type == mjGEOM_MESH) { - newdist = mj_rayMeshNormal(m, d, i, pnt, vec, p_normal); + newdist = mj_rayMesh(m, d, i, pnt, vec, p_normal); } else if (type == mjGEOM_HFIELD) { - newdist = mj_rayHfieldNormal(m, d, i, pnt, vec, p_normal); + newdist = mj_rayHfield(m, d, i, pnt, vec, p_normal); } else if (type == mjGEOM_SDF) { - newdist = mj_raySdfNormal(m, d, i, pnt, vec, p_normal); + newdist = mj_raySdf(m, d, i, pnt, vec, p_normal); } else { - newdist = mju_rayGeomNormal(d->geom_xpos+3*i, d->geom_xmat+9*i, - m->geom_size+3*i, pnt, vec, type, p_normal); + newdist = mju_rayGeom(d->geom_xpos+3*i, d->geom_xmat+9*i, + m->geom_size+3*i, pnt, vec, type, p_normal); } // update if closer intersection found if (newdist >= 0 && (newdist < dist || dist < 0)) { dist = newdist; - *geomid = i; + if (geomid) *geomid = i; if (normal) mju_copy3(normal, normal_local); } } @@ -1580,9 +1541,9 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co // performs multiple ray intersections, compute normals if given -void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff) { +void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, + int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff) { mj_markStack(d); // allocate source @@ -1598,7 +1559,8 @@ void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const m if (mju_dot3(vec+3*i, vec+3*i) < mjMINVAL) { dist[i] = -1; } else { - dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i, + int* p_geomid = geomid ? geomid + i : NULL; + dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, p_geomid, normal ? normal+3*i : NULL); } } @@ -1606,12 +1568,3 @@ void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const m mj_freeStack(d); } - -// performs multiple ray intersections with the precomputed bv and flags -void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int* geomid, mjtNum* dist, int nray, mjtNum cutoff) { - mj_multiRayNormal(m, d, pnt, vec, geomgroup, flg_static, bodyexclude, - geomid, dist, NULL, nray, cutoff); -} - diff --git a/src/engine/engine_ray.h b/src/engine/engine_ray.h index 7923054f..b4fae179 100644 --- a/src/engine/engine_ray.h +++ b/src/engine/engine_ray.h @@ -29,17 +29,11 @@ MJAPI void mju_multiRayPrepare(const mjModel* m, const mjData* d, int bodyexclude, mjtNum cutoff, mjtNum* geom_ba, int* geom_eliminate); -// intersect multiple rays emanating from a single source -// similar semantics to mj_ray, but vec is (nray x 3) and dist is (nray). +// intersect multiple rays emanating from a single source, compute normals if given +// similar semantics to mj_ray, but vec, normal and dist are arrays MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int* geomid, mjtNum* dist, int nray, mjtNum cutoff); - -// intersect multiple rays, compute normals if given -// similar semantics to mj_rayNormal, but vec, normal and dist are arrays. -MJAPI void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff); + int* geomid, mjtNum* dist, mjtNum* normal, int nray, mjtNum cutoff); // intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude @@ -47,40 +41,30 @@ MJAPI void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], c // geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int geomid[1]); + int geomid[1], mjtNum normal[3]); -// intersect ray with hfield +// intersect ray with hfield, compute normal if given MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3]); + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]); // intersect ray with triangle MJAPI mjtNum ray_triangle(mjtNum v[][3], const mjtNum lpnt[3], const mjtNum lvec[3], const mjtNum b0[3], const mjtNum b1[3], mjtNum normal[3]); -// intersect ray with mesh +// intersect ray with mesh, compute normal if given MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid, - const mjtNum pnt[3], const mjtNum vec[3]); + const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]); -// intersect ray with primitive geom, no meshes or hfields +// intersect ray with primitive geom, no meshes or hfields, compute normal if given MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], - const mjtNum pnt[3], const mjtNum vec[3], int geomtype); - -// intersect ray with geom, compute normal if given -MJAPI mjtNum mj_rayNormal(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], - const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, - int geomid[1], mjtNum normal[3]); + const mjtNum pnt[3], const mjtNum vec[3], int geomtype, + mjtNum normal[3]); // intersect ray with flex, return nearest vertex id, compute normal if given -MJAPI mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, - int flex_layer, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, - mjtByte flg_skin, int flexid, - const mjtNum pnt[3], const mjtNum vec[3], - int vertid[1], mjtNum normal[3]); - -// intersect ray with flex, return nearest vertex id -MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, - mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, - const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); +MJAPI mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer, + mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, + mjtByte flg_skin, int flexid, const mjtNum pnt[3], + const mjtNum vec[3], int vertid[1], mjtNum normal[3]); // intersect ray with skin, return nearest vertex id MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 2a337d25..99d5d006 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -505,8 +505,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) { int geomid; mjtNum normal[3]; mjtNum* p_normal = (dataspec & (1 << mjRAYDATA_NORMAL)) ? normal : NULL; - mjtNum dist = mj_rayNormal(m, d, origin, rvec, NULL, 1, - m->site_bodyid[objid], &geomid, p_normal); + mjtNum dist = mj_ray(m, d, origin, rvec, NULL, 1, + m->site_bodyid[objid], &geomid, p_normal); // for site sensor: pass NULL for cam_z so depth = dist fill_raydata(ptr, dataspec, dist, origin, rvec, normal, NULL, NULL); @@ -550,8 +550,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) { } // cast all rays with normals if needed - mj_multiRayNormal(m, d, cam_xpos, vec, NULL, 1, bodyexclude, - geomid, dist, normals, npixel, mjMAXVAL); + mj_multiRay(m, d, cam_xpos, vec, NULL, 1, bodyexclude, + geomid, dist, normals, npixel, mjMAXVAL); // fill in output for each pixel ptr = d->sensordata + adr; @@ -576,8 +576,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) { int geomid; mjtNum normal[3]; - mjtNum dist = mj_rayNormal(m, d, origin, direction, NULL, 1, - bodyexclude, &geomid, normal); + mjtNum dist = mj_ray(m, d, origin, direction, NULL, 1, + bodyexclude, &geomid, normal); ptr = fill_raydata(ptr, dataspec, dist, origin, direction, normal, cam_xpos, cam_z); @@ -1123,7 +1123,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { // add if ray-zone intersection (always true when con->pos inside zone) if (mju_rayGeom(d->site_xpos+3*objid, d->site_xmat+9*objid, m->site_size+3*objid, con->pos, conray, - m->site_type[objid]) >= 0) { + m->site_type[objid], NULL) >= 0) { d->sensordata[adr] += conforce[0]; } } diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index b89cb45a..1c7b602c 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -840,7 +840,8 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, // find intersection with geoms *geomid = -1; - mjtNum geomdist = mj_ray(m, d, pos, ray, vopt->geomgroup, vopt->flags[mjVIS_STATIC], -1, geomid); + mjtNum geomdist = mj_ray(m, d, pos, ray, + vopt->geomgroup, vopt->flags[mjVIS_STATIC], -1, geomid, NULL); // find intersection with flexes int flexbodyid = -1; @@ -852,10 +853,10 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, for (int i=0; i < m->nflex; i++) { // process one flex int vertid; - mjtNum newdist = mju_rayFlex(m, d, vopt->flex_layer, - vopt->flags[mjVIS_FLEXVERT], vopt->flags[mjVIS_FLEXEDGE], - vopt->flags[mjVIS_FLEXFACE], vopt->flags[mjVIS_FLEXSKIN], - i, pos, ray, &vertid); + mjtNum newdist = + mj_rayFlex(m, d, vopt->flex_layer, vopt->flags[mjVIS_FLEXVERT], + vopt->flags[mjVIS_FLEXEDGE], vopt->flags[mjVIS_FLEXFACE], + vopt->flags[mjVIS_FLEXSKIN], i, pos, ray, &vertid, NULL); // update if closer intersection found if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) { diff --git a/src/experimental/platform/interaction.cc b/src/experimental/platform/interaction.cc index 5a14be1f..149e0c1a 100644 --- a/src/experimental/platform/interaction.cc +++ b/src/experimental/platform/interaction.cc @@ -342,7 +342,7 @@ static PickResult PickGeom(const mjModel* m, const mjData* d, const mjvOption* vis_options) { PickResult result; result.dist = mj_ray(m, d, ray_pos, ray_dir, vis_options->geomgroup, - vis_options->flags[mjVIS_STATIC], -1, &result.geom); + vis_options->flags[mjVIS_STATIC], -1, &result.geom, nullptr); mju_addScl3(result.point, ray_pos, ray_dir, result.dist); result.body = m->geom_bodyid[result.geom]; return result; @@ -364,8 +364,8 @@ static PickResult PickFlex(const mjModel* m, const mjData* d, for (int i = 0; i < m->nflex; i++) { int vertid; const mjtNum test_dist = - mju_rayFlex(m, d, vis_options->flex_layer, flag_vert, flag_edge, - flag_face, flag_skin, i, ray_pos, ray_dir, &vertid); + mj_rayFlex(m, d, vis_options->flex_layer, flag_vert, flag_edge, + flag_face, flag_skin, i, ray_pos, ray_dir, &vertid, nullptr); if (test_dist < 0) { continue; diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index cacbdde4..c55583f2 100644 --- a/test/engine/engine_ray_test.cc +++ b/test/engine/engine_ray_test.cc @@ -100,7 +100,7 @@ TEST_F(RayTest, NoExclusions) { mj_kinematics(model, data); mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, - bodyexclude, &geomid); + bodyexclude, &geomid, nullptr); EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1"); EXPECT_FLOAT_EQ(distance, 0.9); mj_deleteData(data); @@ -123,26 +123,26 @@ TEST_F(RayTest, Exclusions) { mj_kinematics(model, data); mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, - bodyexclude, &geomid); + bodyexclude, &geomid, nullptr); EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1"); EXPECT_FLOAT_EQ(distance, 0.9); // Exclude nearest geom geomgroup[1] = 0; distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, - &geomid); + &geomid, nullptr); EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0"); EXPECT_FLOAT_EQ(distance, 2.9); geomgroup[0] = 0; distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, - &geomid); + &geomid, nullptr); EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2"); EXPECT_FLOAT_EQ(distance, 4.9); geomgroup[2] = 0; distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, - &geomid); + &geomid, nullptr); EXPECT_EQ(geomid, -1); EXPECT_FLOAT_EQ(distance, -1); @@ -166,7 +166,7 @@ TEST_F(RayTest, ExcludeStatic) { mj_kinematics(model, data); mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, - bodyexclude, &geomid); + bodyexclude, &geomid, nullptr); EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0"); EXPECT_FLOAT_EQ(distance, 2.9); mj_deleteData(data); @@ -206,7 +206,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) { mjtNum dist_multiray[N*M]; int rgeomid_multiray[N*M]; mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray, - N * M, mjMAXVAL); + nullptr, N * M, mjMAXVAL); // compare results with single ray function mjtNum dist; @@ -215,7 +215,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) { for (int i = 0; i < N; ++i) { for (int j = 0; j < M; ++j) { int idx = i * M + j; - dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid); + dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, nullptr); EXPECT_FLOAT_EQ(dist, dist_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); nhits += dist >= 0; @@ -258,8 +258,8 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) { mjtNum dist_multiray[N*M]; int rgeomid_multiray[N*M]; mjtNum normal_multiray[3*N*M]; - mj_multiRayNormal(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, - dist_multiray, normal_multiray, N * M, mjMAXVAL); + mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, + dist_multiray, normal_multiray, N * M, mjMAXVAL); // compare results with single ray normal function mjtNum dist; @@ -269,8 +269,8 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) { for (int i = 0; i < N; ++i) { for (int j = 0; j < M; ++j) { int idx = i * M + j; - dist = mj_rayNormal(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, - normal); + dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, + normal); EXPECT_FLOAT_EQ(dist, dist_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); EXPECT_FLOAT_EQ(normal[0], normal_multiray[3*idx]); @@ -308,7 +308,8 @@ TEST_F(RayTest, EdgeCases) { EXPECT_FLOAT_EQ(geom_ba[2], mjPI); EXPECT_FLOAT_EQ(geom_ba[3], mjPI); mjtNum vec1[] = {1, 0, 0}; - mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL); + mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, + mjMAXVAL); EXPECT_FLOAT_EQ(dist, 0.1); // pnt at phi = Pi, -Pi @@ -317,7 +318,8 @@ TEST_F(RayTest, EdgeCases) { EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0) EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0) mjtNum vec2[] = {-1, 0, 0}; - mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL); + mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, + mjMAXVAL); EXPECT_FLOAT_EQ(dist, 0.4); // with cutoff @@ -326,7 +328,8 @@ TEST_F(RayTest, EdgeCases) { EXPECT_EQ(flags[0], 0); mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags); EXPECT_EQ(flags[0], 1); - mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, 1, cutoff2); + mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, + cutoff2); EXPECT_FLOAT_EQ(dist, -1); // pnt on the boundary of the box @@ -335,7 +338,8 @@ TEST_F(RayTest, EdgeCases) { EXPECT_FLOAT_EQ(geom_ba[1], 0); EXPECT_FLOAT_EQ(geom_ba[3], mjPI); mjtNum vec3[] = {1, 1, 0}; - mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL); + mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, + mjMAXVAL); EXPECT_FLOAT_EQ(dist, -1); // size 0 geom @@ -346,14 +350,16 @@ TEST_F(RayTest, EdgeCases) { // margin = atan(max_half / dist) where max_half = max(aabb[3..5]) // For a zero-size AABB: max_half = 0, so margin = 0 mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos); - mjtNum max_half4 = mju_max(m->geom_aabb[3], mju_max(m->geom_aabb[4], m->geom_aabb[5])); + mjtNum max_half4 = + mju_max(m->geom_aabb[3], mju_max(m->geom_aabb[4], m->geom_aabb[5])); mjtNum margin4 = mju_atan2(max_half4, dist4); EXPECT_NEAR(geom_ba[0], 0 - margin4, 1e-6); EXPECT_NEAR(geom_ba[1], mjPI/2 - margin4, 1e-6); EXPECT_NEAR(geom_ba[2], 0 + margin4, 1e-6); EXPECT_NEAR(geom_ba[3], mjPI/2 + margin4, 1e-6); mjtNum vec4[] = {1, 0, 0}; - mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, 1, mjMAXVAL); + mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, + mjMAXVAL); EXPECT_FLOAT_EQ(dist, 0.9); mj_deleteData(d); @@ -457,7 +463,7 @@ void _rayMeshTest(const mjModel* m) { for (int j = 0; j < M; ++j) { int idx = i * M + j; dist_old = _rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx); - dist_new = mj_rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx); + dist_new = mj_rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx, nullptr); EXPECT_FLOAT_EQ(dist_new, dist_old); } } @@ -585,16 +591,16 @@ TEST_F(RayTest, RayNormal) { mjtNum r, normal[3]; if (!is_flex) { int geomid; - r = mj_rayNormal(m, d, pnt, vec, nullptr, 1, -1, &geomid, normal); + r = mj_ray(m, d, pnt, vec, nullptr, 1, -1, &geomid, normal); // compare with sensor, expect geomid to be 0 EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time; EXPECT_EQ(geomid, r >= 0 ? 0 : -1); } else { - r = mju_rayFlexNormal(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1, - /*flg_edge*/ 1, /*flg_face*/ 1, - /*flg_skin*/ 1, /*flex_id*/ 0, - pnt, vec, nullptr, normal); + r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1, + /*flg_edge*/ 1, /*flg_face*/ 1, + /*flg_skin*/ 1, /*flex_id*/ 0, + pnt, vec, nullptr, normal); // no sensor comparison: rangefinders only intersect with geoms } @@ -619,10 +625,9 @@ TEST_F(RayTest, RayNormal) { mjtNum dr, dpnt[3]; mju_addScl3(dpnt, pnt, nudge, eps); if (!is_flex) { - dr = mj_rayNormal(m, d, dpnt, vec, NULL, 1, -1, nullptr, nullptr); + dr = mj_ray(m, d, dpnt, vec, NULL, 1, -1, nullptr, nullptr); } else { - dr = mju_rayFlexNormal(m, d, 0, 1, 1, 1, 1, 0, dpnt, vec, nullptr, - nullptr); + dr = mj_rayFlex(m, d, 0, 1, 1, 1, 1, 0, dpnt, vec, nullptr, nullptr); } mju_addScl3(ds[i], dpnt, vec, dr); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 0705d069..fe14ba1e 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6807,22 +6807,22 @@ public static unsafe extern int mj_version(); public static unsafe extern string mj_versionString(); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void mj_multiRay(mjModel_* m, mjData_* d, double* pnt, double* vec, byte* geomgroup, byte flg_static, int bodyexclude, int* geomid, double* dist, int nray, double cutoff); +public static unsafe extern double mj_ray(mjModel_* m, mjData_* d, double* pnt, double* vec, byte* geomgroup, byte flg_static, int bodyexclude, int* geomid, double* normal); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mj_ray(mjModel_* m, mjData_* d, double* pnt, double* vec, byte* geomgroup, byte flg_static, int bodyexclude, int* geomid); +public static unsafe extern void mj_multiRay(mjModel_* m, mjData_* d, double* pnt, double* vec, byte* geomgroup, byte flg_static, int bodyexclude, int* geomid, double* dist, double* normal, int nray, double cutoff); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mj_rayHfield(mjModel_* m, mjData_* d, int geomid, double* pnt, double* vec); +public static unsafe extern double mj_rayHfield(mjModel_* m, mjData_* d, int geomid, double* pnt, double* vec, double* normal); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mj_rayMesh(mjModel_* m, mjData_* d, int geomid, double* pnt, double* vec); +public static unsafe extern double mj_rayMesh(mjModel_* m, mjData_* d, int geomid, double* pnt, double* vec, double* normal); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mju_rayGeom(double* pos, double* mat, double* size, double* pnt, double* vec, int geomtype); +public static unsafe extern double mju_rayGeom(double* pos, double* mat, double* size, double* pnt, double* vec, int geomtype, double* normal); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mju_rayFlex(mjModel_* m, mjData_* d, int flex_layer, byte flg_vert, byte flg_edge, byte flg_face, byte flg_skin, int flexid, double* pnt, double* vec, int* vertid); +public static unsafe extern double mj_rayFlex(mjModel_* m, mjData_* d, int flex_layer, byte flg_vert, byte flg_edge, byte flg_face, byte flg_skin, int flexid, double* pnt, double* vec, int* vertid, double* normal); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern double mju_raySkin(int nface, int nvert, int* face, float* vert, double* pnt, double* vec, int* vertid); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 6a31d3e8..164437c5 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -8273,16 +8273,17 @@ void mj_mulM2_wrapper(const MjModel& m, const MjData& d, const val& res, const N mj_mulM2(m.get(), d.get(), res_.data(), vec_.data()); } -void mj_multiRay_wrapper(const MjModel& m, MjData& d, const NumberArray& pnt, const NumberArray& vec, const NumberArray& geomgroup, mjtByte flg_static, int bodyexclude, const val& geomid, const val& dist, int nray, mjtNum cutoff) { +void mj_multiRay_wrapper(const MjModel& m, MjData& d, const NumberArray& pnt, const NumberArray& vec, const NumberArray& geomgroup, mjtByte flg_static, int bodyexclude, const val& geomid, const val& dist, const val& normal, int nray, mjtNum cutoff) { UNPACK_ARRAY(mjtNum, pnt); UNPACK_ARRAY(mjtNum, vec); UNPACK_NULLABLE_ARRAY(mjtByte, geomgroup); - UNPACK_VALUE(int, geomid); + UNPACK_NULLABLE_VALUE(int, geomid); UNPACK_VALUE(mjtNum, dist); + UNPACK_NULLABLE_VALUE(mjtNum, normal); CHECK_SIZE(dist, nray); CHECK_SIZE(geomid, nray); CHECK_SIZE(vec, 3 * nray); - mj_multiRay(m.get(), d.get(), pnt_.data(), vec_.data(), geomgroup_.data(), flg_static, bodyexclude, geomid_.data(), dist_.data(), nray, cutoff); + mj_multiRay(m.get(), d.get(), pnt_.data(), vec_.data(), geomgroup_.data(), flg_static, bodyexclude, geomid_.data(), dist_.data(), normal_.data(), nray, cutoff); } int mj_name2id_wrapper(const MjModel& m, int type, const String& name) { @@ -8347,24 +8348,35 @@ void mj_projectConstraint_wrapper(const MjModel& m, MjData& d) { mj_projectConstraint(m.get(), d.get()); } -mjtNum mj_ray_wrapper(const MjModel& m, const MjData& d, const NumberArray& pnt, const NumberArray& vec, const NumberArray& geomgroup, mjtByte flg_static, int bodyexclude, const val& geomid) { +mjtNum mj_ray_wrapper(const MjModel& m, const MjData& d, const NumberArray& pnt, const NumberArray& vec, const NumberArray& geomgroup, mjtByte flg_static, int bodyexclude, const val& geomid, const val& normal) { UNPACK_ARRAY(mjtNum, pnt); UNPACK_ARRAY(mjtNum, vec); UNPACK_NULLABLE_ARRAY(mjtByte, geomgroup); UNPACK_NULLABLE_VALUE(int, geomid); - return mj_ray(m.get(), d.get(), pnt_.data(), vec_.data(), geomgroup_.data(), flg_static, bodyexclude, geomid_.data()); + UNPACK_NULLABLE_VALUE(mjtNum, normal); + return mj_ray(m.get(), d.get(), pnt_.data(), vec_.data(), geomgroup_.data(), flg_static, bodyexclude, geomid_.data(), normal_.data()); } -mjtNum mj_rayHfield_wrapper(const MjModel& m, const MjData& d, int geomid, const NumberArray& pnt, const NumberArray& vec) { +mjtNum mj_rayFlex_wrapper(const MjModel& m, const MjData& d, int flex_layer, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, const NumberArray& pnt, const NumberArray& vec, const val& vertid, const val& normal) { UNPACK_ARRAY(mjtNum, pnt); UNPACK_ARRAY(mjtNum, vec); - return mj_rayHfield(m.get(), d.get(), geomid, pnt_.data(), vec_.data()); + UNPACK_NULLABLE_VALUE(int, vertid); + UNPACK_NULLABLE_VALUE(mjtNum, normal); + return mj_rayFlex(m.get(), d.get(), flex_layer, flg_vert, flg_edge, flg_face, flg_skin, flexid, pnt_.data(), vec_.data(), vertid_.data(), normal_.data()); } -mjtNum mj_rayMesh_wrapper(const MjModel& m, const MjData& d, int geomid, const NumberArray& pnt, const NumberArray& vec) { +mjtNum mj_rayHfield_wrapper(const MjModel& m, const MjData& d, int geomid, const NumberArray& pnt, const NumberArray& vec, const val& normal) { UNPACK_ARRAY(mjtNum, pnt); UNPACK_ARRAY(mjtNum, vec); - return mj_rayMesh(m.get(), d.get(), geomid, pnt_.data(), vec_.data()); + UNPACK_NULLABLE_VALUE(mjtNum, normal); + return mj_rayHfield(m.get(), d.get(), geomid, pnt_.data(), vec_.data(), normal_.data()); +} + +mjtNum mj_rayMesh_wrapper(const MjModel& m, const MjData& d, int geomid, const NumberArray& pnt, const NumberArray& vec, const val& normal) { + UNPACK_ARRAY(mjtNum, pnt); + UNPACK_ARRAY(mjtNum, vec); + UNPACK_NULLABLE_VALUE(mjtNum, normal); + return mj_rayMesh(m.get(), d.get(), geomid, pnt_.data(), vec_.data(), normal_.data()); } void mj_referenceConstraint_wrapper(const MjModel& m, MjData& d) { @@ -9870,20 +9882,14 @@ void mju_quatZ2Vec_wrapper(const val& quat, const NumberArray& vec) { mju_quatZ2Vec(quat_.data(), vec_.data()); } -mjtNum mju_rayFlex_wrapper(const MjModel& m, const MjData& d, int flex_layer, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, const NumberArray& pnt, const NumberArray& vec, const val& vertid) { - UNPACK_ARRAY(mjtNum, pnt); - UNPACK_ARRAY(mjtNum, vec); - UNPACK_NULLABLE_VALUE(int, vertid); - return mju_rayFlex(m.get(), d.get(), flex_layer, flg_vert, flg_edge, flg_face, flg_skin, flexid, pnt_.data(), vec_.data(), vertid_.data()); -} - -mjtNum mju_rayGeom_wrapper(const NumberArray& pos, const NumberArray& mat, const NumberArray& size, const NumberArray& pnt, const NumberArray& vec, int geomtype) { +mjtNum mju_rayGeom_wrapper(const NumberArray& pos, const NumberArray& mat, const NumberArray& size, const NumberArray& pnt, const NumberArray& vec, int geomtype, const val& normal) { UNPACK_ARRAY(mjtNum, pos); UNPACK_ARRAY(mjtNum, mat); UNPACK_ARRAY(mjtNum, size); UNPACK_ARRAY(mjtNum, pnt); UNPACK_ARRAY(mjtNum, vec); - return mju_rayGeom(pos_.data(), mat_.data(), size_.data(), pnt_.data(), vec_.data(), geomtype); + UNPACK_NULLABLE_VALUE(mjtNum, normal); + return mju_rayGeom(pos_.data(), mat_.data(), size_.data(), pnt_.data(), vec_.data(), geomtype, normal_.data()); } mjtNum mju_raySkin_wrapper(int nface, int nvert, const NumberArray& face, const NumberArray& vert, const NumberArray& pnt, const NumberArray& vec, const val& vertid) { @@ -12453,6 +12459,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mj_printScene", &mj_printScene_wrapper); function("mj_projectConstraint", &mj_projectConstraint_wrapper); function("mj_ray", &mj_ray_wrapper); + function("mj_rayFlex", &mj_rayFlex_wrapper); function("mj_rayHfield", &mj_rayHfield_wrapper); function("mj_rayMesh", &mj_rayMesh_wrapper); function("mj_referenceConstraint", &mj_referenceConstraint_wrapper); @@ -12684,7 +12691,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mju_quat2Vel", &mju_quat2Vel_wrapper); function("mju_quatIntegrate", &mju_quatIntegrate_wrapper); function("mju_quatZ2Vec", &mju_quatZ2Vec_wrapper); - function("mju_rayFlex", &mju_rayFlex_wrapper); function("mju_rayGeom", &mju_rayGeom_wrapper); function("mju_raySkin", &mju_raySkin_wrapper); function("mju_rotVecQuat", &mju_rotVecQuat_wrapper);