Use mesh BVH for speeding up ray mesh intersection.
PiperOrigin-RevId: 532516008 Change-Id: Ia9cac060279d030221c1a6b4fad457a857701370
This commit is contained in:
committed by
Copybara-Service
parent
9db444cd46
commit
dab49d487c
@@ -32,9 +32,7 @@ General
|
|||||||
trajectory optimization. See :ref:`mju_cholFactorBand` documentation for details.
|
trajectory optimization. See :ref:`mju_cholFactorBand` documentation for details.
|
||||||
- Added :ref:`mj_multiRay` function for intersecting multiple rays emanating from a single point.
|
- Added :ref:`mj_multiRay` function for intersecting multiple rays emanating from a single point.
|
||||||
This is significantly faster than calling :ref:`mj_ray` multiple times.
|
This is significantly faster than calling :ref:`mj_ray` multiple times.
|
||||||
- Ray-mesh collisions are now up to 10x faster, using a bounding volume hierarchy of mesh faces.
|
|
||||||
- Increased ``mjMAXUIITEM`` (maximum number of UI elements per section in Simulate) to 100.
|
- Increased ``mjMAXUIITEM`` (maximum number of UI elements per section in Simulate) to 100.
|
||||||
- Removed ``const`` qualifier from :ref:`mjData` in :ref:`mj_ray`, :ref:`mj_rayMesh`, and :ref:`mjv_select`.
|
|
||||||
- Added :ref:`documentation<exProvider>` for resource providers.
|
- Added :ref:`documentation<exProvider>` for resource providers.
|
||||||
- Changed the formula for :ref:`mju_sigmoid`, a finite-support sigmoid :math:`s \colon \mathbf R \rightarrow [0, 1]`.
|
- Changed the formula for :ref:`mju_sigmoid`, a finite-support sigmoid :math:`s \colon \mathbf R \rightarrow [0, 1]`.
|
||||||
Previously, the smooth part consisted of two stitched quadratics, once continuously differentiable.
|
Previously, the smooth part consisted of two stitched quadratics, once continuously differentiable.
|
||||||
|
|||||||
@@ -2217,12 +2217,12 @@ const char* mj_versionString();
|
|||||||
void mj_multiRay(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, int nray, mjtNum cutoff);
|
int* geomid, mjtNum* dist, int nray, mjtNum cutoff);
|
||||||
mjtNum mj_ray(const mjModel* m, 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 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 mj_rayMesh(const mjModel* m, 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 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);
|
||||||
@@ -2258,7 +2258,7 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert,
|
|||||||
int flg_paused);
|
int flg_paused);
|
||||||
void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert);
|
void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert);
|
||||||
mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
|
mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
|
||||||
int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
|
int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
|
||||||
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
||||||
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
|
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
|
||||||
void mjv_defaultOption(mjvOption* opt);
|
void mjv_defaultOption(mjvOption* opt);
|
||||||
|
|||||||
@@ -493,7 +493,7 @@ MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const m
|
|||||||
// 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 and output geomid.
|
||||||
// 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, 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]);
|
||||||
|
|
||||||
@@ -502,7 +502,7 @@ 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]);
|
||||||
|
|
||||||
// Intersect ray with mesh, return nearest distance or -1 if no intersection.
|
// Intersect ray with mesh, return nearest distance or -1 if no intersection.
|
||||||
MJAPI mjtNum mj_rayMesh(const mjModel* m, 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]);
|
||||||
|
|
||||||
// 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.
|
||||||
@@ -585,7 +585,7 @@ MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb*
|
|||||||
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
|
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
|
||||||
|
|
||||||
// Select geom or skin with mouse, return bodyid; -1: none selected.
|
// Select geom or skin with mouse, return bodyid; -1: none selected.
|
||||||
MJAPI int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
|
MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
|
||||||
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
||||||
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
|
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
|
||||||
|
|
||||||
|
|||||||
@@ -2847,7 +2847,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
name='d',
|
name='d',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
inner_type=ValueType(name='mjData'),
|
inner_type=ValueType(name='mjData', is_const=True),
|
||||||
),
|
),
|
||||||
),
|
),
|
||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
@@ -2940,7 +2940,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
name='d',
|
name='d',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
inner_type=ValueType(name='mjData'),
|
inner_type=ValueType(name='mjData', is_const=True),
|
||||||
),
|
),
|
||||||
),
|
),
|
||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
@@ -3624,7 +3624,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
|||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
name='d',
|
name='d',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
inner_type=ValueType(name='mjData'),
|
inner_type=ValueType(name='mjData', is_const=True),
|
||||||
),
|
),
|
||||||
),
|
),
|
||||||
FunctionParameterDecl(
|
FunctionParameterDecl(
|
||||||
|
|||||||
@@ -549,7 +549,7 @@ PYBIND11_MODULE(_functions, pymodule) {
|
|||||||
Def<traits::mj_multiRay>(pymodule);
|
Def<traits::mj_multiRay>(pymodule);
|
||||||
Def<traits::mj_ray>(
|
Def<traits::mj_ray>(
|
||||||
pymodule,
|
pymodule,
|
||||||
[](const raw::MjModel* m, raw::MjData* d, const mjtNum(*pnt)[3],
|
[](const raw::MjModel* m, const raw::MjData* d, const mjtNum(*pnt)[3],
|
||||||
const mjtNum(*vec)[3],
|
const mjtNum(*vec)[3],
|
||||||
std::optional<Eigen::Ref<const Eigen::Vector<mjtByte, mjNGROUP>>>
|
std::optional<Eigen::Ref<const Eigen::Vector<mjtByte, mjNGROUP>>>
|
||||||
geomgroup,
|
geomgroup,
|
||||||
|
|||||||
+35
-112
@@ -130,8 +130,8 @@ static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum* x) {
|
|||||||
|
|
||||||
|
|
||||||
// intersect ray with triangle
|
// intersect ray with triangle
|
||||||
mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
|
static mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
|
||||||
const mjtNum* b0, const mjtNum* b1) {
|
const mjtNum* b0, const mjtNum* b1) {
|
||||||
// dif = v[i] - lpnt
|
// dif = v[i] - lpnt
|
||||||
mjtNum dif[3][3];
|
mjtNum dif[3][3];
|
||||||
for (int i=0; i<3; i++) {
|
for (int i=0; i<3; i++) {
|
||||||
@@ -186,6 +186,8 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
|
|||||||
return (-mju_dot3(dif[2], nrm) / denom);
|
return (-mju_dot3(dif[2], nrm) / denom);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//---------------------------- geom-specific intersection functions --------------------------------
|
//---------------------------- geom-specific intersection functions --------------------------------
|
||||||
|
|
||||||
// plane
|
// plane
|
||||||
@@ -597,55 +599,19 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
// ray vs axis-aligned bounding box using slab method
|
// intersect ray with mesh
|
||||||
// see Ericson, Real-time Collision Detection
|
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
|
||||||
int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3],
|
const mjtNum* pnt, const mjtNum* vec) {
|
||||||
const mjtNum xmat[9], const mjtNum* pnt, const mjtNum* vec) {
|
// check geom type
|
||||||
mjtNum tmin = 0.0, tmax = INFINITY;
|
if (m->geom_type[id]!=mjGEOM_MESH) {
|
||||||
|
mju_error("mj_rayMesh: geom with mesh type expected");
|
||||||
// compute min and max
|
|
||||||
mjtNum min[3] = {aabb[0]-aabb[3], aabb[1]-aabb[4], aabb[2]-aabb[5]};
|
|
||||||
mjtNum max[3] = {aabb[0]+aabb[3], aabb[1]+aabb[4], aabb[2]+aabb[5]};
|
|
||||||
|
|
||||||
// compute ray in local coordinates
|
|
||||||
mjtNum src[3], dir[3];
|
|
||||||
ray_map(xpos, xmat, pnt, vec, src, dir);
|
|
||||||
|
|
||||||
// check intersections
|
|
||||||
for (int d = 0; d < 3; ++d) {
|
|
||||||
mjtNum t1 = (min[d] - src[d]) / dir[d];
|
|
||||||
mjtNum t2 = (max[d] - src[d]) / dir[d];
|
|
||||||
mjtNum minval = t1 < t2 ? t1 : t2;
|
|
||||||
mjtNum maxval = t1 < t2 ? t2 : t1;
|
|
||||||
tmin = tmin > minval ? tmin : minval;
|
|
||||||
tmax = tmax < maxval ? tmax : maxval;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return tmin < tmax;
|
// bounding box test
|
||||||
}
|
if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL)<0) {
|
||||||
|
return -1;
|
||||||
// ray vs tree intersection
|
|
||||||
mjtNum mju_rayTree(const mjModel* m, mjData* d, int id, const mjtNum* pnt,
|
|
||||||
const mjtNum* vec) {
|
|
||||||
const int meshid = m->geom_dataid[id];
|
|
||||||
const int bvhadr = m->mesh_bvhadr[meshid];
|
|
||||||
const int* faceid = m->bvh_geomid + bvhadr;
|
|
||||||
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
|
|
||||||
const int* child = m->bvh_child + 2*bvhadr;
|
|
||||||
|
|
||||||
if (meshid==-1) {
|
|
||||||
mju_error("mju_rayTree: this is not a mesh.");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
mjMARKSTACK;
|
|
||||||
// TODO(quaglino): Store bvh max depths to make this bound tighter.
|
|
||||||
int max_stack = m->mesh_bvhnum[meshid];
|
|
||||||
int* stack = mj_stackAllocInt(d, max_stack);
|
|
||||||
|
|
||||||
int nstack = 0;
|
|
||||||
stack[nstack] = 0;
|
|
||||||
nstack++;
|
|
||||||
|
|
||||||
// map to local frame
|
// map to local frame
|
||||||
mjtNum lpnt[3], lvec[3];
|
mjtNum lpnt[3], lvec[3];
|
||||||
ray_map(d->geom_xpos+3*id, d->geom_xmat+9*id, pnt, vec, lpnt, lvec);
|
ray_map(d->geom_xpos+3*id, d->geom_xmat+9*id, pnt, vec, lpnt, lvec);
|
||||||
@@ -667,80 +633,37 @@ mjtNum mju_rayTree(const mjModel* m, mjData* d, int id, const mjtNum* pnt,
|
|||||||
// init solution
|
// init solution
|
||||||
mjtNum x = -1, sol;
|
mjtNum x = -1, sol;
|
||||||
|
|
||||||
while (nstack) {
|
// process all triangles
|
||||||
// pop from stack
|
int face, meshid = m->geom_dataid[id];
|
||||||
nstack--;
|
for (face = m->mesh_faceadr[meshid];
|
||||||
int node = stack[nstack];
|
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid];
|
||||||
|
face++) {
|
||||||
|
// get float vertices
|
||||||
|
float* vf[3];
|
||||||
|
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]);
|
||||||
|
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
|
||||||
|
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
|
||||||
|
|
||||||
// intersection test
|
// convert to mjtNum
|
||||||
int intersect = mju_raySlab(bvh+6*node, d->geom_xpos+3*id, d->geom_xmat+9*id, pnt, vec);
|
mjtNum v[3][3];
|
||||||
|
for (int i=0; i<3; i++) {
|
||||||
// if no intersection, skip
|
for (int j=0; j<3; j++) {
|
||||||
if (!intersect) {
|
v[i][j] = (mjtNum)vf[i][j];
|
||||||
continue;
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// node1 is a leaf
|
// solve
|
||||||
if (faceid[node] != -1) {
|
sol = ray_triangle(v, lpnt, lvec, b0, b1);
|
||||||
int face = faceid[node] + m->mesh_faceadr[meshid];
|
|
||||||
|
|
||||||
// get float vertices
|
// update
|
||||||
float* vf[3];
|
if (sol>=0 && (x<0 || sol<x)) {
|
||||||
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face+0] + m->mesh_vertadr[meshid]);
|
x = sol;
|
||||||
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
|
|
||||||
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
|
|
||||||
|
|
||||||
// convert to mjtNum
|
|
||||||
mjtNum v[3][3];
|
|
||||||
for (int i=0; i<3; i++) {
|
|
||||||
for (int j=0; j<3; j++) {
|
|
||||||
v[i][j] = (mjtNum)vf[i][j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// solve
|
|
||||||
sol = ray_triangle(v, lpnt, lvec, b0, b1);
|
|
||||||
|
|
||||||
// update
|
|
||||||
if (sol>=0 && (x<0 || sol<x)) {
|
|
||||||
x = sol;
|
|
||||||
}
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// used for rendering
|
|
||||||
d->bvh_active[node + bvhadr] = 1;
|
|
||||||
|
|
||||||
// recursive call
|
|
||||||
for (int i=0; i<2; i++) {
|
|
||||||
if (child[2*node+i] != -1) {
|
|
||||||
if (nstack >= max_stack) mju_error("BVH stack depth exceeded.");
|
|
||||||
stack[nstack] = child[2*node+i];
|
|
||||||
nstack++;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mjFREESTACK;
|
|
||||||
return x;
|
return x;
|
||||||
}
|
}
|
||||||
|
|
||||||
// intersect ray with mesh
|
|
||||||
mjtNum mj_rayMesh(const mjModel* m, mjData* d, int id,
|
|
||||||
const mjtNum* pnt, const mjtNum* vec) {
|
|
||||||
// check geom type
|
|
||||||
if (m->geom_type[id]!=mjGEOM_MESH) {
|
|
||||||
mju_error("mj_rayMesh: geom with mesh type expected");
|
|
||||||
}
|
|
||||||
|
|
||||||
// bounding box test
|
|
||||||
if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL)<0) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
return mju_rayTree(m, d, id, pnt, vec);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// intersect ray with pure geom, no meshes or hfields
|
// intersect ray with pure geom, no meshes or hfields
|
||||||
@@ -893,7 +816,7 @@ 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_ray(const mjModel* m, mjData* d, const mjtNum* pnt, const mjtNum* vec,
|
mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec,
|
||||||
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
|
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
|
||||||
int geomid[1]) {
|
int geomid[1]) {
|
||||||
mjtNum dist, newdist;
|
mjtNum dist, newdist;
|
||||||
|
|||||||
@@ -38,7 +38,7 @@ MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const m
|
|||||||
// 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
|
||||||
MJAPI mjtNum mj_ray(const mjModel* m, mjData* d, const mjtNum* pnt, const mjtNum* vec,
|
MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum* vec,
|
||||||
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
|
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
|
||||||
int geomid[1]);
|
int geomid[1]);
|
||||||
|
|
||||||
@@ -46,12 +46,8 @@ MJAPI mjtNum mj_ray(const mjModel* m, mjData* d, const mjtNum* pnt, const mjtNum
|
|||||||
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, const mjtNum* vec);
|
const mjtNum* pnt, const mjtNum* vec);
|
||||||
|
|
||||||
// intersect ray with triangle
|
|
||||||
MJAPI mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
|
|
||||||
const mjtNum* b0, const mjtNum* b1);
|
|
||||||
|
|
||||||
// intersect ray with mesh
|
// intersect ray with mesh
|
||||||
MJAPI mjtNum mj_rayMesh(const mjModel* m, mjData* d, int geomid,
|
MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
|
||||||
const mjtNum* pnt, const mjtNum* vec);
|
const mjtNum* pnt, const mjtNum* vec);
|
||||||
|
|
||||||
// intersect ray with pure geom, no meshes or hfields
|
// intersect ray with pure geom, no meshes or hfields
|
||||||
|
|||||||
@@ -731,7 +731,7 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2)
|
|||||||
|
|
||||||
|
|
||||||
// Select geom or skin with mouse, return bodyid; -1: none selected.
|
// Select geom or skin with mouse, return bodyid; -1: none selected.
|
||||||
int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
|
int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
|
||||||
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
||||||
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]) {
|
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]) {
|
||||||
// get average camera
|
// get average camera
|
||||||
|
|||||||
@@ -73,7 +73,7 @@ MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb*
|
|||||||
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
|
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
|
||||||
|
|
||||||
// Select geom or skin with mouse, return bodyid; -1: none selected.
|
// Select geom or skin with mouse, return bodyid; -1: none selected.
|
||||||
MJAPI int mjv_select(const mjModel* m, mjData* d, const mjvOption* vopt,
|
MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
|
||||||
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
mjtNum aspectratio, mjtNum relx, mjtNum rely,
|
||||||
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
|
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
|
||||||
|
|
||||||
|
|||||||
@@ -140,8 +140,6 @@ TEST_F(RayTest, ExcludeStatic) {
|
|||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------------------------- mj_multiRay --------------------------------
|
|
||||||
|
|
||||||
TEST_F(RayTest, MultiRayEqualsSingleRay) {
|
TEST_F(RayTest, MultiRayEqualsSingleRay) {
|
||||||
mjModel* m = LoadModelFromString(kRayCastingModel);
|
mjModel* m = LoadModelFromString(kRayCastingModel);
|
||||||
ASSERT_THAT(m, NotNull());
|
ASSERT_THAT(m, NotNull());
|
||||||
@@ -260,128 +258,5 @@ TEST_F(RayTest, EdgeCases) {
|
|||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------------------------- mj_rayMesh ---------------------------------
|
|
||||||
|
|
||||||
// map ray to local geom frame
|
|
||||||
static void ray_map(const mjtNum* pos, const mjtNum* mat, const mjtNum* pnt,
|
|
||||||
const mjtNum* vec, mjtNum* lpnt, mjtNum* lvec) {
|
|
||||||
const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]};
|
|
||||||
|
|
||||||
// lpnt = mat' * dif
|
|
||||||
lpnt[0] = mat[0]*dif[0] + mat[3]*dif[1] + mat[6]*dif[2];
|
|
||||||
lpnt[1] = mat[1]*dif[0] + mat[4]*dif[1] + mat[7]*dif[2];
|
|
||||||
lpnt[2] = mat[2]*dif[0] + mat[5]*dif[1] + mat[8]*dif[2];
|
|
||||||
|
|
||||||
// lvec = mat' * vec
|
|
||||||
lvec[0] = mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2];
|
|
||||||
lvec[1] = mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2];
|
|
||||||
lvec[2] = mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2];
|
|
||||||
}
|
|
||||||
|
|
||||||
// old ray mesh intersection
|
|
||||||
mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
|
|
||||||
const mjtNum* pnt, const mjtNum* vec) {
|
|
||||||
// check geom type
|
|
||||||
if (m->geom_type[geomid] != mjGEOM_MESH) {
|
|
||||||
mju_error("mj_rayMesh: geom with mesh type expected");
|
|
||||||
}
|
|
||||||
|
|
||||||
// map to local frame
|
|
||||||
mjtNum lpnt[3], lvec[3];
|
|
||||||
ray_map(d->geom_xpos+3*geomid, d->geom_xmat+9*geomid, pnt, vec, lpnt, lvec);
|
|
||||||
|
|
||||||
// construct basis vectors of normal plane
|
|
||||||
mjtNum b0[3] = {1, 1, 1}, b1[3];
|
|
||||||
if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) &&
|
|
||||||
mju_abs(lvec[0]) >= mju_abs(lvec[2])) {
|
|
||||||
b0[0] = 0;
|
|
||||||
} else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) {
|
|
||||||
b0[1] = 0;
|
|
||||||
} else {
|
|
||||||
b0[2] = 0;
|
|
||||||
}
|
|
||||||
mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0)/mju_dot3(lvec, lvec));
|
|
||||||
mju_normalize3(b1);
|
|
||||||
mju_cross(b0, b1, lvec);
|
|
||||||
mju_normalize3(b0);
|
|
||||||
|
|
||||||
// init solution
|
|
||||||
mjtNum x = -1, sol;
|
|
||||||
|
|
||||||
// process all triangles
|
|
||||||
int face, meshid = m->geom_dataid[geomid];
|
|
||||||
for (face = m->mesh_faceadr[meshid];
|
|
||||||
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid];
|
|
||||||
face++) {
|
|
||||||
// get float vertices
|
|
||||||
float* vf[3];
|
|
||||||
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]);
|
|
||||||
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
|
|
||||||
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
|
|
||||||
|
|
||||||
// convert to mjtNum
|
|
||||||
mjtNum v[3][3];
|
|
||||||
for (int i=0; i < 3; i++) {
|
|
||||||
for (int j=0; j < 3; j++) {
|
|
||||||
v[i][j] = (mjtNum)vf[i][j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// solve
|
|
||||||
sol = ray_triangle(v, lpnt, lvec, b0, b1);
|
|
||||||
|
|
||||||
// update
|
|
||||||
if (sol >= 0 && (x < 0 || sol < x)) {
|
|
||||||
x = sol;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return x;
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_F(RayTest, RayMeshPruning) {
|
|
||||||
char error[1024] = {0};
|
|
||||||
const std::string xml_path =
|
|
||||||
GetTestDataFilePath("testdata/stanford_bunny.xml");
|
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error));
|
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
|
||||||
mjData* d = mj_makeData(m);
|
|
||||||
ASSERT_THAT(d, NotNull());
|
|
||||||
mj_forward(m, d);
|
|
||||||
|
|
||||||
// create ray array
|
|
||||||
constexpr int N = 80;
|
|
||||||
constexpr int M = 60;
|
|
||||||
mjtNum vec[3*N*M];
|
|
||||||
mjtNum pnt[3] = {1, .2, 0};
|
|
||||||
mjtNum cone[4][3] = {{-1, -1, -1}, {-1, -1, 1}, {1, -1, 1}, {1, -1, -1}};
|
|
||||||
memset(vec, 0, 3*N*M*sizeof(mjtNum));
|
|
||||||
|
|
||||||
for (int i = 0; i < N; ++i) {
|
|
||||||
for (int j = 0; j < M; ++j) {
|
|
||||||
for (int k = 0; k < 3; ++k) {
|
|
||||||
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
|
|
||||||
j * cone[1][1] / (M - 1) +
|
|
||||||
(N - i - 1) * cone[2][k] / (N - 1) +
|
|
||||||
(M - j - 1) * cone[3][k] / (M - 1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// compare results with single ray function
|
|
||||||
mjtNum dist_new, dist_old;
|
|
||||||
|
|
||||||
for (int i = 0; i < N; ++i) {
|
|
||||||
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);
|
|
||||||
EXPECT_FLOAT_EQ(dist_new, dist_old);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|||||||
Vendored
-8
@@ -1,8 +0,0 @@
|
|||||||
<mujoco>
|
|
||||||
<asset>
|
|
||||||
<mesh file="stanford_bunny.obj"/>
|
|
||||||
</asset>
|
|
||||||
<worldbody>
|
|
||||||
<geom type="mesh" mesh="stanford_bunny" pos="1 0 0" euler="0 90 0"/>
|
|
||||||
</worldbody>
|
|
||||||
</mujoco>
|
|
||||||
Vendored
+1
-1
@@ -2,7 +2,7 @@
|
|||||||
<compiler fitaabb="true"/>
|
<compiler fitaabb="true"/>
|
||||||
|
|
||||||
<asset>
|
<asset>
|
||||||
<mesh file="../../testdata/stanford_bunny.obj"/>
|
<mesh file="stanford_bunny.obj"/>
|
||||||
</asset>
|
</asset>
|
||||||
|
|
||||||
<worldbody>
|
<worldbody>
|
||||||
|
|||||||
+1
-1
@@ -2,7 +2,7 @@
|
|||||||
<compiler fitaabb="false" exactmeshinertia="true"/>
|
<compiler fitaabb="false" exactmeshinertia="true"/>
|
||||||
|
|
||||||
<asset>
|
<asset>
|
||||||
<mesh file="../../testdata/stanford_bunny.obj"/>
|
<mesh file="stanford_bunny.obj"/>
|
||||||
</asset>
|
</asset>
|
||||||
|
|
||||||
<worldbody>
|
<worldbody>
|
||||||
|
|||||||
Reference in New Issue
Block a user