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

Add cutoff argument to mj_multiRay.

Browse Source 
PiperOrigin-RevId: 532158635
Change-Id: I70f98be9f781c8281e21aa5c17bd85f3654e9383
This commit is contained in:
Alessio Quaglino
2023-05-15 10:43:12 -07:00
committed by Copybara-Service
parent ca9c55d22b
commit fcc825c5e6
7 changed files with 53 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_ray.c
+18 -8
View File
@@ -861,16 +861,14 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
// 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, const mjtByte* geomgroup, mjtByte flg_static,
int bodyexclude, mjtNum* geom_ba, int* geom_eliminate) {
int bodyexclude, mjtNum cutoff, mjtNum* geom_ba, int* geom_eliminate) {
if (ray_xmat) {
mju_error("ray_xmat is currently unused, should be NULL");
}
if (geom_eliminate) {
// compute eliminate flag for all geoms
for (int geomid=0; geomid<m->ngeom; geomid++)
geom_eliminate[geomid] = ray_eliminate(m, d, geomid, geomgroup, flg_static, bodyexclude);
}
// compute eliminate flag for all geoms
for (int geomid=0; geomid<m->ngeom; geomid++)
geom_eliminate[geomid] = ray_eliminate(m, d, geomid, geomgroup, flg_static, bodyexclude);
for (int b=0; b<m->nbody; b++) {
// skip precomputation if no bounding volume is available
@@ -886,6 +884,17 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
mjtNum* xpos = d->geom_xpos + 3*g;
mjtNum* xmat = d->geom_xmat + 9*g;
// skip if eliminated by flags
if (geom_eliminate[g]) {
continue;
}
// add to geom_eliminate if distance of bounding sphere is above cutoff
if (mju_dist3(d->geom_xpos+3*g, pnt)>cutoff+m->geom_rbound[g]) {
geom_eliminate[g] = 1;
continue;
}
if (point_in_box(aabb, xpos, xmat, pnt)) {
(geom_ba+4*g)[0] = -mjPI;
(geom_ba+4*g)[1] = 0;
@@ -1009,7 +1018,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
// Performs multiple ray intersections with the precomputes 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) {
int* geomid, mjtNum* dist, int nray, mjtNum cutoff) {
mjMARKSTACK;
// allocate source
@@ -1017,7 +1026,8 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum*
int* geom_eliminate = mj_stackAllocInt(d, m->ngeom);
// initialize source
mju_multiRayPrepare(m, d, pnt, NULL, geomgroup, flg_static, bodyexclude, geom_ba, geom_eliminate);
mju_multiRayPrepare(m, d, pnt, NULL, geomgroup, flg_static, bodyexclude,
cutoff, geom_ba, geom_eliminate);
// loop over rays
for (int i=0; i<nray; i++) {
src/engine/engine_ray.h
+6 -4
View File
@@ -23,15 +23,17 @@
extern "C" {
#endif
MJAPI void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
const mjtNum* ray_xmat, const mjtByte* geomgroup, mjtByte flg_static,
int bodyexclude, mjtNum* geom_ba, int* geom_eliminate);
MJAPI void mju_multiRayPrepare(const mjModel* m, const mjData* d,
const mjtNum pnt[3], const mjtNum* ray_xmat,
const mjtByte* geomgroup, mjtByte flg_static,
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 an array of (nray x 3) directions.
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);
int* geomid, mjtNum* dist, int nray, mjtNum cutoff);
// 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