chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Breaking change: Add surface normal output to MuJoCo raycast functions.

Browse Source 
PiperOrigin-RevId: 855781592
Change-Id: Id96b1ca7eaf722e260cc69d7706c28dc51f52d92
This commit is contained in:
Yuval Tassa
2026-01-13 10:21:31 -08:00
committed by Copybara-Service
parent 37762e3f70
commit 218226fc95
17 changed files with 457 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+30 -20
View File
@@ -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 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. 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 <#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. - 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 ``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. - Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies.
- ``geomgroup`` is an array of length :ref:`mjNGROUP<glNumeric>`, where 1 means the group should be included. Pass - ``geomgroup`` is an array of length :ref:`mjNGROUP<glNumeric>`, where 1 means the group should be included. Pass
NULL to skip geom group exclusion. NULL to skip geom group exclusion.
- If ``flg_static`` is 0, static geoms will be excluded. - 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: .. _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. Intersect ray with hfield; return nearest distance or -1 if no intersection.
*Nullable:* ``normal``
.. _mj_rayMesh: .. _mj_rayMesh:
`mj_rayMesh <#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. Intersect ray with mesh; return nearest distance or -1 if no intersection.
*Nullable:* ``normal``
.. _mju_rayGeom: .. _mju_rayGeom:
`mju_rayGeom <#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. 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, 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: .. _mju_raySkin:
doc/APIreference/functions_override.rst
+3 -1
View File
@@ -332,12 +332,14 @@ Intersect ray ``pnt+x*vec, x >= 0`` with geoms.
- Return distance ``x`` to nearest surface, or -1 if no intersection. - 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 ``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. - Exclude geoms in body with id ``bodyexclude``, use -1 to include all bodies.
- ``geomgroup`` is an array of length :ref:`mjNGROUP<glNumeric>`, where 1 means the group should be included. Pass - ``geomgroup`` is an array of length :ref:`mjNGROUP<glNumeric>`, where 1 means the group should be included. Pass
NULL to skip geom group exclusion. NULL to skip geom group exclusion.
- If ``flg_static`` is 0, static geoms will be excluded. - If ``flg_static`` is 0, static geoms will be excluded.
*Nullable:* ``geomgroup``, ``geomid`` *Nullable:* ``geomgroup``, ``geomid``, ``normal``
.. _Interaction: .. _Interaction:
doc/changelog.rst
+12 -1
View File
@@ -8,6 +8,17 @@ Upcoming version (not yet released)
.. admonition:: Breaking API changes .. admonition:: Breaking API changes
:class: attention :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 - 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. :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 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-usethread>` compiler flag (on by default), was - Multi threaded mesh processing, enabled by the :ref:`usethread<compiler-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 in fact disabled by the flag. Fixing this bug speeds up compilation of mesh-heavy models by (up to) the number of
available cores. 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. nullable.
Version 3.4.0 (December 5, 2025) Version 3.4.0 (December 5, 2025)
doc/includes/references.h
+12 -10
View File
@@ -3255,21 +3255,23 @@ void mj_loadPluginLibrary(const char* path);
void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback); void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback);
int mj_version(void); int mj_version(void);
const char* mj_versionString(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], 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, 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, 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, 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], 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 mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, mjtNum normal[3]);
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); 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, mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]);
void mjv_defaultCamera(mjvCamera* cam); void mjv_defaultCamera(mjvCamera* cam);
include/mujoco/mujoco.h
+24 -18
View File
@@ -626,39 +626,45 @@ MJAPI const char* mj_versionString(void);
//---------------------------------- Ray casting --------------------------------------------------- //---------------------------------- 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. // 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. // 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], 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, 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. // 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, 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. // 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, 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. // 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], 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, // 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
MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, MJAPI mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); 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, // Intersect ray with skin; return nearest distance or -1 if no intersection,
// and also output nearest vertex id. // and also output nearest vertex id.
python/mujoco/bindings_test.py
+31 -2
View File
@@ -1288,8 +1288,9 @@ Euler integrator, semi-implicit in velocity.
geomid = np.zeros(1, np.int32) geomid = np.zeros(1, np.int32)
mujoco.mj_forward(self.model, self.data) mujoco.mj_forward(self.model, self.data)
mujoco.mj_ray( 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( mujoco.mj_ray(
self.model, self.model,
self.data, self.data,
@@ -1301,6 +1302,7 @@ Euler integrator, semi-implicit in velocity.
geomid, geomid,
) )
# Check that named arguments work # Check that named arguments work
normal = np.zeros(3, np.float64)
mujoco.mj_ray( mujoco.mj_ray(
m=self.model, m=self.model,
d=self.data, d=self.data,
@@ -1310,8 +1312,34 @@ Euler integrator, semi-implicit in velocity.
flg_static=0, flg_static=0,
bodyexclude=0, bodyexclude=0,
geomid=geomid, 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): def test_mj_multi_ray(self):
nray = 3 nray = 3
geom1 = np.zeros(1, np.int32) geom1 = np.zeros(1, np.int32)
@@ -1333,6 +1361,7 @@ Euler integrator, semi-implicit in velocity.
bodyexclude=-1, bodyexclude=-1,
geomid=geomid, geomid=geomid,
dist=dist, dist=dist,
normal=None,
nray=nray, nray=nray,
cutoff=mujoco.mjMAXVAL, cutoff=mujoco.mjMAXVAL,
) )
@@ -1340,7 +1369,7 @@ Euler integrator, semi-implicit in velocity.
for i in range(0, 3): for i in range(0, 3):
self.assertEqual( self.assertEqual(
dist[i], 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], geom1)
self.assertEqual(geomid[i], geom_ex[i]) self.assertEqual(geomid[i], geom_ex[i])
python/mujoco/functions.cc
+91 -17
View File
@@ -30,6 +30,7 @@
#include "private.h" #include "private.h"
#include "raw.h" #include "raw.h"
#include "structs.h" #include "structs.h"
#include "util/func_wrap.h"
#include <pybind11/eigen.h> #include <pybind11/eigen.h>
#include <pybind11/pybind11.h> #include <pybind11/pybind11.h>
#include <pybind11/stl.h> #include <pybind11/stl.h>
@@ -677,33 +678,106 @@ PYBIND11_MODULE(_functions, pymodule) {
std::optional<Eigen::Ref<const Eigen::Vector<mjtByte, mjNGROUP>>> std::optional<Eigen::Ref<const Eigen::Vector<mjtByte, mjNGROUP>>>
geomgroup, geomgroup,
mjtByte flg_static, int bodyexclude, Eigen::Ref<EigenVectorI> geomid, mjtByte flg_static, int bodyexclude, Eigen::Ref<EigenVectorI> geomid,
Eigen::Ref<EigenVectorX> dist, int nray, mjtNum cutoff) { Eigen::Ref<EigenVectorX> dist,
std::optional<Eigen::Ref<EigenVectorX>> normal,
int nray, mjtNum cutoff) {
if (dist.size() != nray || geomid.size() != nray) { if (dist.size() != nray || geomid.size() != nray) {
throw py::type_error("dist and geomid should be of size nray"); throw py::type_error("dist and geomid should be of size nray");
} }
if (vec.size() != 3 * nray) { if (vec.size() != 3 * nray) {
throw py::type_error("vec should be of 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)( InterceptMjErrors(::mj_multiRay)(
m, d, &(*pnt)[0], vec.data(), m, d, &(*pnt)[0], vec.data(),
geomgroup.has_value() ? geomgroup->data() : nullptr, flg_static, 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<traits::mj_ray>( pymodule.def(
pymodule, "mj_ray",
[](const raw::MjModel* m, const raw::MjData* d, const mjtNum(*pnt)[3], util::UnwrapArgs(
const mjtNum(*vec)[3], [](const raw::MjModel* m, const raw::MjData* d, const mjtNum(*pnt)[3],
std::optional<Eigen::Ref<const Eigen::Vector<mjtByte, mjNGROUP>>> const mjtNum(*vec)[3],
geomgroup, std::optional<Eigen::Ref<const Eigen::Vector<mjtByte, mjNGROUP>>>
mjtByte flg_static, int bodyexclude, int(*geomid)[1]) { geomgroup,
return mj_ray(m, d, &(*pnt)[0], &(*vec)[0], mjtByte flg_static, int bodyexclude,
geomgroup.has_value() ? geomgroup->data() : nullptr, std::optional<Eigen::Ref<Eigen::Vector<int, 1>>> geomid,
flg_static, bodyexclude, &(*geomid)[0]); std::optional<Eigen::Ref<Eigen::Vector<mjtNum, 3>>> normal) {
}); return mj_ray(m, d, &(*pnt)[0], &(*vec)[0],
Def<traits::mj_rayHfield>(pymodule); geomgroup.has_value() ? geomgroup->data() : nullptr,
Def<traits::mj_rayMesh>(pymodule); flg_static, bodyexclude,
Def<traits::mju_rayGeom>(pymodule); geomid.has_value() ? geomid->data() : nullptr,
Def<traits::mju_rayFlex>(pymodule); 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<py::gil_scoped_release>());
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<Eigen::Ref<Eigen::Vector<mjtNum, 3>>> 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<py::gil_scoped_release>());
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<Eigen::Ref<Eigen::Vector<mjtNum, 3>>> 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<py::gil_scoped_release>());
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<Eigen::Ref<Eigen::Vector<mjtNum, 3>>> 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<py::gil_scoped_release>());
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<Eigen::Ref<Eigen::Vector<int, 1>>> vertid,
std::optional<Eigen::Ref<Eigen::Vector<mjtNum, 3>>> 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<py::gil_scoped_release>());
Def<traits::mju_raySkin>(pymodule); Def<traits::mju_raySkin>(pymodule);
// Interaction // Interaction
python/mujoco/introspect/functions.py
+120 -72
View File
@@ -3729,74 +3729,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
parameters=(), parameters=(),
doc='Return the current version of MuJoCo as a null-terminated string.', # pylint: disable=line-too-long 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', ('mj_ray',
FunctionDecl( FunctionDecl(
name='mj_ray', name='mj_ray',
@@ -3851,8 +3783,92 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
), ),
nullable=True, 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', ('mj_rayHfield',
FunctionDecl( FunctionDecl(
@@ -3889,6 +3905,14 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
extents=(3,), 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 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,), 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 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', name='geomtype',
type=ValueType(name='int'), 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 doc='Intersect ray with pure geom; return nearest distance or -1 if no intersection.', # pylint: disable=line-too-long
)), )),
('mju_rayFlex', ('mj_rayFlex',
FunctionDecl( FunctionDecl(
name='mju_rayFlex', name='mj_rayFlex',
return_type=ValueType(name='mjtNum'), return_type=ValueType(name='mjtNum'),
parameters=( parameters=(
FunctionParameterDecl( FunctionParameterDecl(
@@ -4040,8 +4080,16 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
), ),
nullable=True, 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', ('mju_raySkin',
FunctionDecl( FunctionDecl(
src/engine/engine_collision_sdf.c
+3 -3
View File
@@ -286,10 +286,10 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
mju_addTo3(a, d->geom_xpos + 3 * i); mju_addTo3(a, d->geom_xpos + 3 * i);
mjtNum dir[3] = {-a[0], -a[1], -a[2]}; mjtNum dir[3] = {-a[0], -a[1], -a[2]};
mjtNum r = mju_norm3(dir); 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) { if (dist > r) {
mju_scl3(dir, dir, -1); mju_scl3(dir, dir, -1);
return -mj_rayMesh(m, d, i, a, dir); return -mj_rayMesh(m, d, i, a, dir, NULL);
} }
return dist; 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); mju_addTo3(a, d->geom_xpos+3*i);
mjtNum dir[3] = {-a[0], -a[1], -a[2]}; mjtNum dir[3] = {-a[0], -a[1], -a[2]};
mjtNum r = mju_norm3(dir); 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[0] = dist > r ? 1 : -1;
gradient[1] = dist > r ? 1 : -1; gradient[1] = dist > r ? 1 : -1;
gradient[2] = dist > r ? 1 : -1; gradient[2] = dist > r ? 1 : -1;
src/engine/engine_ray.c
+37 -84
View File
@@ -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 // intersect ray with hfield, compute normal if given
static mjtNum mj_rayHfieldNormal(const mjModel* m, const mjData* d, int geomid, mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) { const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {
// clear normal if given // clear normal if given
if (normal) mju_zero3(normal); 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 // ray vs axis-aligned bounding box using slab method
// see Ericson, Real-time Collision Detection section 5.3.3. // see Ericson, Real-time Collision Detection section 5.3.3.
int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[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 // intersect ray with signed distance field, compute normal if given
static mjtNum mj_raySdfNormal(const mjModel* m, const mjData* d, int g, static mjtNum mj_raySdf(const mjModel* m, const mjData* d, int g,
const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) { const mjtNum pnt[3], const mjtNum vec[3], mjtNum normal[3]) {
if (normal) mju_zero3(normal); if (normal) mju_zero3(normal);
mjtNum distance_total = 0; 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 // intersect ray with mesh, compute normal if given
static mjtNum mj_rayMeshNormal(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3], mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3],
const mjtNum vec[3], mjtNum normal[3]) { const mjtNum vec[3], mjtNum normal[3]) {
// clear normal if given // clear normal if given
if (normal) mju_zero3(normal); 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 // intersect ray with primitive geom, no meshes or hfields, compute normal if given
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum pnt[3], mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
const mjtNum vec[3]) { const mjtNum pnt[3], const mjtNum vec[3], int geomtype,
return mj_rayMeshNormal(m, d, id, pnt, vec, NULL); mjtNum normal[3]) {
}
// 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]) {
switch ((mjtGeom) geomtype) { switch ((mjtGeom) geomtype) {
case mjGEOM_PLANE: case mjGEOM_PLANE:
return ray_plane(pos, mat, size, pnt, vec, normal); 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 // intersect ray with flex, return nearest vertex id, compute normal if given
mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer, mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
mjtByte flg_skin, int flexid, const mjtNum pnt[3], mjtByte flg_skin, int flexid, const mjtNum pnt[3],
const mjtNum vec[3], int vertid[1], mjtNum normal[3]) { const mjtNum vec[3], int vertid[1], mjtNum normal[3]) {
int dim = m->flex_dim[flexid]; int dim = m->flex_dim[flexid];
// clear normal if given // clear normal if given
@@ -1102,8 +1081,8 @@ mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, int flex_layer,
mju_quat2Mat(mat, quat); mju_quat2Mat(mat, quat);
// intersect ray with capsule // intersect ray with capsule
mjtNum sol = mju_rayGeomNormal(pos, mat, size, pnt, vec, mjGEOM_CAPSULE, mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE,
normal ? normal_local : NULL); normal ? normal_local : NULL);
// update // update
if (sol >= 0 && (x < 0 || sol < x)) { 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; size[0] = radius;
// intersect ray with sphere // intersect ray with sphere
mjtNum sol = mju_rayGeomNormal(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE, mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE,
normal ? normal_local : NULL); normal ? normal_local : NULL);
// update // update
if (sol >= 0 && (x < 0 || sol < x)) { 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; 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 // intersect ray with skin, return nearest vertex id
mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]) { 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 // 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 // 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 // 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], 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, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int geomid[1], mjtNum normal[3]) { int geomid[1], mjtNum normal[3]) {
int ngeom = m->ngeom; int ngeom = m->ngeom;
mjtNum dist, newdist; mjtNum dist, newdist;
mjtNum normal_local[3]; 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)) { if (!ray_eliminate(m, d, i, geomgroup, flg_static, bodyexclude)) {
int type = m->geom_type[i]; int type = m->geom_type[i];
if (type == mjGEOM_MESH) { 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) { } 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) { } 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 { } 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); 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 // 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], void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
const mjtNum ray_xmat[9], const mjtByte* geomgroup, mjtByte flg_static, 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 // clear result
dist = -1; dist = -1;
*geomid = -1; if (geomid) *geomid = -1;
if (normal) mju_zero3(normal); if (normal) mju_zero3(normal);
// get ray spherical coordinates // 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 // dispatch to type-specific ray function
int type = m->geom_type[i]; int type = m->geom_type[i];
if (type == mjGEOM_MESH) { 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) { } 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) { } 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 { } 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); m->geom_size+3*i, pnt, vec, type, p_normal);
} }
// update if closer intersection found // update if closer intersection found
if (newdist >= 0 && (newdist < dist || dist < 0)) { if (newdist >= 0 && (newdist < dist || dist < 0)) {
dist = newdist; dist = newdist;
*geomid = i; if (geomid) *geomid = i;
if (normal) mju_copy3(normal, normal_local); 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 // performs multiple ray intersections, compute normals if given
void mj_multiRayNormal(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, 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) {
mj_markStack(d); mj_markStack(d);
// allocate source // 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) { if (mju_dot3(vec+3*i, vec+3*i) < mjMINVAL) {
dist[i] = -1; dist[i] = -1;
} else { } 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); 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); 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);
}
src/engine/engine_ray.h
+15 -31
View File
@@ -29,17 +29,11 @@ MJAPI void mju_multiRayPrepare(const mjModel* m, const mjData* d,
int bodyexclude, mjtNum cutoff, mjtNum* geom_ba, int bodyexclude, mjtNum cutoff, mjtNum* geom_ba,
int* geom_eliminate); int* geom_eliminate);
// intersect multiple rays emanating from a single source // intersect multiple rays emanating from a single source, compute normals if given
// similar semantics to mj_ray, but vec is (nray x 3) and dist is (nray). // 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, MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
int* geomid, mjtNum* dist, int nray, mjtNum cutoff); int* geomid, mjtNum* dist, mjtNum* normal, 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);
// intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude // 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 // 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], 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, 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, 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 // intersect ray with triangle
MJAPI mjtNum ray_triangle(mjtNum v[][3], const mjtNum lpnt[3], const mjtNum lvec[3], 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]); 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, 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], 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 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]);
// intersect ray with flex, return nearest vertex id, compute normal if given // intersect ray with flex, return nearest vertex id, compute normal if given
MJAPI mjtNum mju_rayFlexNormal(const mjModel* m, const mjData* d, MJAPI mjtNum mj_rayFlex(const mjModel* m, const mjData* d, int flex_layer,
int flex_layer, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face,
mjtByte flg_skin, int flexid, mjtByte flg_skin, int flexid, const mjtNum pnt[3],
const mjtNum pnt[3], const mjtNum vec[3], const mjtNum vec[3], int vertid[1], mjtNum normal[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]);
// intersect ray with skin, return nearest vertex id // intersect ray with skin, return nearest vertex id
MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
src/engine/engine_sensor.c
+7 -7
View File
@@ -505,8 +505,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
int geomid; int geomid;
mjtNum normal[3]; mjtNum normal[3];
mjtNum* p_normal = (dataspec & (1 << mjRAYDATA_NORMAL)) ? normal : NULL; mjtNum* p_normal = (dataspec & (1 << mjRAYDATA_NORMAL)) ? normal : NULL;
mjtNum dist = mj_rayNormal(m, d, origin, rvec, NULL, 1, mjtNum dist = mj_ray(m, d, origin, rvec, NULL, 1,
m->site_bodyid[objid], &geomid, p_normal); m->site_bodyid[objid], &geomid, p_normal);
// for site sensor: pass NULL for cam_z so depth = dist // for site sensor: pass NULL for cam_z so depth = dist
fill_raydata(ptr, dataspec, dist, origin, rvec, normal, NULL, NULL); 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 // cast all rays with normals if needed
mj_multiRayNormal(m, d, cam_xpos, vec, NULL, 1, bodyexclude, mj_multiRay(m, d, cam_xpos, vec, NULL, 1, bodyexclude,
geomid, dist, normals, npixel, mjMAXVAL); geomid, dist, normals, npixel, mjMAXVAL);
// fill in output for each pixel // fill in output for each pixel
ptr = d->sensordata + adr; ptr = d->sensordata + adr;
@@ -576,8 +576,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
int geomid; int geomid;
mjtNum normal[3]; mjtNum normal[3];
mjtNum dist = mj_rayNormal(m, d, origin, direction, NULL, 1, mjtNum dist = mj_ray(m, d, origin, direction, NULL, 1,
bodyexclude, &geomid, normal); bodyexclude, &geomid, normal);
ptr = fill_raydata(ptr, dataspec, dist, origin, direction, ptr = fill_raydata(ptr, dataspec, dist, origin, direction,
normal, cam_xpos, cam_z); 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) // 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, if (mju_rayGeom(d->site_xpos+3*objid, d->site_xmat+9*objid,
m->site_size+3*objid, con->pos, conray, m->site_size+3*objid, con->pos, conray,
m->site_type[objid]) >= 0) { m->site_type[objid], NULL) >= 0) {
d->sensordata[adr] += conforce[0]; d->sensordata[adr] += conforce[0];
} }
} }
src/engine/engine_vis_interact.c
+6 -5
View File
@@ -840,7 +840,8 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// find intersection with geoms // find intersection with geoms
*geomid = -1; *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 // find intersection with flexes
int flexbodyid = -1; 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++) { for (int i=0; i < m->nflex; i++) {
// process one flex // process one flex
int vertid; int vertid;
mjtNum newdist = mju_rayFlex(m, d, vopt->flex_layer, mjtNum newdist =
vopt->flags[mjVIS_FLEXVERT], vopt->flags[mjVIS_FLEXEDGE], mj_rayFlex(m, d, vopt->flex_layer, vopt->flags[mjVIS_FLEXVERT],
vopt->flags[mjVIS_FLEXFACE], vopt->flags[mjVIS_FLEXSKIN], vopt->flags[mjVIS_FLEXEDGE], vopt->flags[mjVIS_FLEXFACE],
i, pos, ray, &vertid); vopt->flags[mjVIS_FLEXSKIN], i, pos, ray, &vertid, NULL);
// update if closer intersection found // update if closer intersection found
if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) { if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) {
src/experimental/platform/interaction.cc
+3 -3
View File
@@ -342,7 +342,7 @@ static PickResult PickGeom(const mjModel* m, const mjData* d,
const mjvOption* vis_options) { const mjvOption* vis_options) {
PickResult result; PickResult result;
result.dist = mj_ray(m, d, ray_pos, ray_dir, vis_options->geomgroup, 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); mju_addScl3(result.point, ray_pos, ray_dir, result.dist);
result.body = m->geom_bodyid[result.geom]; result.body = m->geom_bodyid[result.geom];
return result; return result;
@@ -364,8 +364,8 @@ static PickResult PickFlex(const mjModel* m, const mjData* d,
for (int i = 0; i < m->nflex; i++) { for (int i = 0; i < m->nflex; i++) {
int vertid; int vertid;
const mjtNum test_dist = const mjtNum test_dist =
mju_rayFlex(m, d, vis_options->flex_layer, flag_vert, flag_edge, mj_rayFlex(m, d, vis_options->flex_layer, flag_vert, flag_edge,
flag_face, flag_skin, i, ray_pos, ray_dir, &vertid); flag_face, flag_skin, i, ray_pos, ray_dir, &vertid, nullptr);
if (test_dist < 0) { if (test_dist < 0) {
continue; continue;
test/engine/engine_ray_test.cc
+32 -27
View File
@@ -100,7 +100,7 @@ TEST_F(RayTest, NoExclusions) {
mj_kinematics(model, data); mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, 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_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
EXPECT_FLOAT_EQ(distance, 0.9); EXPECT_FLOAT_EQ(distance, 0.9);
mj_deleteData(data); mj_deleteData(data);
@@ -123,26 +123,26 @@ TEST_F(RayTest, Exclusions) {
mj_kinematics(model, data); mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, 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_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
EXPECT_FLOAT_EQ(distance, 0.9); EXPECT_FLOAT_EQ(distance, 0.9);
// Exclude nearest geom // Exclude nearest geom
geomgroup[1] = 0; geomgroup[1] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid); &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0"); EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
EXPECT_FLOAT_EQ(distance, 2.9); EXPECT_FLOAT_EQ(distance, 2.9);
geomgroup[0] = 0; geomgroup[0] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid); &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2"); EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2");
EXPECT_FLOAT_EQ(distance, 4.9); EXPECT_FLOAT_EQ(distance, 4.9);
geomgroup[2] = 0; geomgroup[2] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid); &geomid, nullptr);
EXPECT_EQ(geomid, -1); EXPECT_EQ(geomid, -1);
EXPECT_FLOAT_EQ(distance, -1); EXPECT_FLOAT_EQ(distance, -1);
@@ -166,7 +166,7 @@ TEST_F(RayTest, ExcludeStatic) {
mj_kinematics(model, data); mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, 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_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
EXPECT_FLOAT_EQ(distance, 2.9); EXPECT_FLOAT_EQ(distance, 2.9);
mj_deleteData(data); mj_deleteData(data);
@@ -206,7 +206,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
mjtNum dist_multiray[N*M]; mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M]; int rgeomid_multiray[N*M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray, 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 // compare results with single ray function
mjtNum dist; mjtNum dist;
@@ -215,7 +215,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
for (int i = 0; i < N; ++i) { for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) { for (int j = 0; j < M; ++j) {
int idx = i * 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_FLOAT_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
nhits += dist >= 0; nhits += dist >= 0;
@@ -258,8 +258,8 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
mjtNum dist_multiray[N*M]; mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M]; int rgeomid_multiray[N*M];
mjtNum normal_multiray[3*N*M]; mjtNum normal_multiray[3*N*M];
mj_multiRayNormal(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, normal_multiray, N * M, mjMAXVAL); dist_multiray, normal_multiray, N * M, mjMAXVAL);
// compare results with single ray normal function // compare results with single ray normal function
mjtNum dist; mjtNum dist;
@@ -269,8 +269,8 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
for (int i = 0; i < N; ++i) { for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) { for (int j = 0; j < M; ++j) {
int idx = i * M + j; int idx = i * M + j;
dist = mj_rayNormal(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
normal); normal);
EXPECT_FLOAT_EQ(dist, dist_multiray[idx]); EXPECT_FLOAT_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
EXPECT_FLOAT_EQ(normal[0], normal_multiray[3*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[2], mjPI);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI); EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
mjtNum vec1[] = {1, 0, 0}; 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); EXPECT_FLOAT_EQ(dist, 0.1);
// pnt at phi = Pi, -Pi // 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[0], -mjPI); // atan(y<0, x<0)
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0) EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
mjtNum vec2[] = {-1, 0, 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); EXPECT_FLOAT_EQ(dist, 0.4);
// with cutoff // with cutoff
@@ -326,7 +328,8 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_EQ(flags[0], 0); EXPECT_EQ(flags[0], 0);
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags); mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags);
EXPECT_EQ(flags[0], 1); 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); EXPECT_FLOAT_EQ(dist, -1);
// pnt on the boundary of the box // 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[1], 0);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI); EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
mjtNum vec3[] = {1, 1, 0}; 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); EXPECT_FLOAT_EQ(dist, -1);
// size 0 geom // size 0 geom
@@ -346,14 +350,16 @@ TEST_F(RayTest, EdgeCases) {
// margin = atan(max_half / dist) where max_half = max(aabb[3..5]) // margin = atan(max_half / dist) where max_half = max(aabb[3..5])
// For a zero-size AABB: max_half = 0, so margin = 0 // For a zero-size AABB: max_half = 0, so margin = 0
mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos); 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); mjtNum margin4 = mju_atan2(max_half4, dist4);
EXPECT_NEAR(geom_ba[0], 0 - margin4, 1e-6); EXPECT_NEAR(geom_ba[0], 0 - margin4, 1e-6);
EXPECT_NEAR(geom_ba[1], mjPI/2 - 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[2], 0 + margin4, 1e-6);
EXPECT_NEAR(geom_ba[3], mjPI/2 + margin4, 1e-6); EXPECT_NEAR(geom_ba[3], mjPI/2 + margin4, 1e-6);
mjtNum vec4[] = {1, 0, 0}; 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); EXPECT_FLOAT_EQ(dist, 0.9);
mj_deleteData(d); mj_deleteData(d);
@@ -457,7 +463,7 @@ void _rayMeshTest(const mjModel* m) {
for (int j = 0; j < M; ++j) { for (int j = 0; j < M; ++j) {
int idx = i * M + j; int idx = i * M + j;
dist_old = _rayMesh(m, d, /*geomid=*/0, pnt, vec + 3 * idx); 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); EXPECT_FLOAT_EQ(dist_new, dist_old);
} }
} }
@@ -585,16 +591,16 @@ TEST_F(RayTest, RayNormal) {
mjtNum r, normal[3]; mjtNum r, normal[3];
if (!is_flex) { if (!is_flex) {
int geomid; 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 // compare with sensor, expect geomid to be 0
EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time; EXPECT_EQ(r, d->sensordata[0]) << path << ", time " << d->time;
EXPECT_EQ(geomid, r >= 0 ? 0 : -1); EXPECT_EQ(geomid, r >= 0 ? 0 : -1);
} else { } else {
r = mju_rayFlexNormal(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1, r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
/*flg_edge*/ 1, /*flg_face*/ 1, /*flg_edge*/ 1, /*flg_face*/ 1,
/*flg_skin*/ 1, /*flex_id*/ 0, /*flg_skin*/ 1, /*flex_id*/ 0,
pnt, vec, nullptr, normal); pnt, vec, nullptr, normal);
// no sensor comparison: rangefinders only intersect with geoms // no sensor comparison: rangefinders only intersect with geoms
} }
@@ -619,10 +625,9 @@ TEST_F(RayTest, RayNormal) {
mjtNum dr, dpnt[3]; mjtNum dr, dpnt[3];
mju_addScl3(dpnt, pnt, nudge, eps); mju_addScl3(dpnt, pnt, nudge, eps);
if (!is_flex) { 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 { } else {
dr = mju_rayFlexNormal(m, d, 0, 1, 1, 1, 1, 0, dpnt, vec, nullptr, dr = mj_rayFlex(m, d, 0, 1, 1, 1, 1, 0, dpnt, vec, nullptr, nullptr);
nullptr);
} }
mju_addScl3(ds[i], dpnt, vec, dr); mju_addScl3(ds[i], dpnt, vec, dr);
} }
unity/Runtime/Bindings/MjBindings.cs
+6 -6
View File
@@ -6807,22 +6807,22 @@ public static unsafe extern int mj_version();
public static unsafe extern string mj_versionString(); public static unsafe extern string mj_versionString();
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] [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)] [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)] [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)] [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)] [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)] [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)] [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); public static unsafe extern double mju_raySkin(int nface, int nvert, int* face, float* vert, double* pnt, double* vec, int* vertid);
wasm/codegen/generated/bindings.cc
+25 -19
View File
@@ -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()); 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, pnt);
UNPACK_ARRAY(mjtNum, vec); UNPACK_ARRAY(mjtNum, vec);
UNPACK_NULLABLE_ARRAY(mjtByte, geomgroup); UNPACK_NULLABLE_ARRAY(mjtByte, geomgroup);
UNPACK_VALUE(int, geomid); UNPACK_NULLABLE_VALUE(int, geomid);
UNPACK_VALUE(mjtNum, dist); UNPACK_VALUE(mjtNum, dist);
UNPACK_NULLABLE_VALUE(mjtNum, normal);
CHECK_SIZE(dist, nray); CHECK_SIZE(dist, nray);
CHECK_SIZE(geomid, nray); CHECK_SIZE(geomid, nray);
CHECK_SIZE(vec, 3 * 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) { 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()); 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, pnt);
UNPACK_ARRAY(mjtNum, vec); UNPACK_ARRAY(mjtNum, vec);
UNPACK_NULLABLE_ARRAY(mjtByte, geomgroup); UNPACK_NULLABLE_ARRAY(mjtByte, geomgroup);
UNPACK_NULLABLE_VALUE(int, geomid); 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, pnt);
UNPACK_ARRAY(mjtNum, vec); 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, pnt);
UNPACK_ARRAY(mjtNum, vec); 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) { 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()); 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) { 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, 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) {
UNPACK_ARRAY(mjtNum, pos); UNPACK_ARRAY(mjtNum, pos);
UNPACK_ARRAY(mjtNum, mat); UNPACK_ARRAY(mjtNum, mat);
UNPACK_ARRAY(mjtNum, size); UNPACK_ARRAY(mjtNum, size);
UNPACK_ARRAY(mjtNum, pnt); UNPACK_ARRAY(mjtNum, pnt);
UNPACK_ARRAY(mjtNum, vec); 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) { 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_printScene", &mj_printScene_wrapper);
function("mj_projectConstraint", &mj_projectConstraint_wrapper); function("mj_projectConstraint", &mj_projectConstraint_wrapper);
function("mj_ray", &mj_ray_wrapper); function("mj_ray", &mj_ray_wrapper);
function("mj_rayFlex", &mj_rayFlex_wrapper);
function("mj_rayHfield", &mj_rayHfield_wrapper); function("mj_rayHfield", &mj_rayHfield_wrapper);
function("mj_rayMesh", &mj_rayMesh_wrapper); function("mj_rayMesh", &mj_rayMesh_wrapper);
function("mj_referenceConstraint", &mj_referenceConstraint_wrapper); function("mj_referenceConstraint", &mj_referenceConstraint_wrapper);
@@ -12684,7 +12691,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
function("mju_quat2Vel", &mju_quat2Vel_wrapper); function("mju_quat2Vel", &mju_quat2Vel_wrapper);
function("mju_quatIntegrate", &mju_quatIntegrate_wrapper); function("mju_quatIntegrate", &mju_quatIntegrate_wrapper);
function("mju_quatZ2Vec", &mju_quatZ2Vec_wrapper); function("mju_quatZ2Vec", &mju_quatZ2Vec_wrapper);
function("mju_rayFlex", &mju_rayFlex_wrapper);
function("mju_rayGeom", &mju_rayGeom_wrapper); function("mju_rayGeom", &mju_rayGeom_wrapper);
function("mju_raySkin", &mju_raySkin_wrapper); function("mju_raySkin", &mju_raySkin_wrapper);
function("mju_rotVecQuat", &mju_rotVecQuat_wrapper); function("mju_rotVecQuat", &mju_rotVecQuat_wrapper);