Add bvhactive flag to visual/global. Fixes #1279.
PiperOrigin-RevId: 599778272 Change-Id: If5530629035cc68b198447503388f59a04041b13
This commit is contained in:
committed by
Copybara-Service
parent
2feefbc5d2
commit
fea7c10b95
@@ -715,6 +715,12 @@ is effectively a miscellaneous subsection.
|
|||||||
This attribute specifies how the equivalent inertia is visualized. "false":
|
This attribute specifies how the equivalent inertia is visualized. "false":
|
||||||
use box, "true": use ellipsoid.
|
use box, "true": use ellipsoid.
|
||||||
|
|
||||||
|
.. _visual-global-bvactive:
|
||||||
|
|
||||||
|
:at:`bvactive`: :at-val:`[false, true], "true"`
|
||||||
|
This attribute specifies whether collision and raycasting code should mark elements of Bounding Volume Hierarchies
|
||||||
|
as intersecting, for the purpose of visualization. Setting this attribute to "false" can speed up simulation for
|
||||||
|
models with high-resolution meshes.
|
||||||
|
|
||||||
.. _visual-quality:
|
.. _visual-quality:
|
||||||
|
|
||||||
@@ -1122,6 +1128,15 @@ disables the rendering of the corresponding object.
|
|||||||
:at:`frustum`: :at-val:`real(4), "1 1 0 0.2"`
|
:at:`frustum`: :at-val:`real(4), "1 1 0 0.2"`
|
||||||
Color used to render the camera frustum.
|
Color used to render the camera frustum.
|
||||||
|
|
||||||
|
.. _visual-rgba-bv:
|
||||||
|
|
||||||
|
:at:`bv`: :at-val:`real(4), "0 1 0 0.5"`
|
||||||
|
Color used to render bounding volumes.
|
||||||
|
|
||||||
|
.. _visual-rgba-bvactive:
|
||||||
|
|
||||||
|
:at:`bvactive`: :at-val:`real(4), "1 0 0 0.5"`
|
||||||
|
Color used to render active bounding volumes, if the :ref:`bvactive<visual-global-bvactive>` flag is "true".
|
||||||
|
|
||||||
.. _asset:
|
.. _asset:
|
||||||
|
|
||||||
|
|||||||
+4
-2
@@ -65,7 +65,7 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`linewidth<visual-global-linewidth>` | :ref:`glow<visual-global-glow>` | :ref:`offwidth<visual-global-offwidth>` | :ref:`offheight<visual-global-offheight>` | |
|
| | | | :ref:`linewidth<visual-global-linewidth>` | :ref:`glow<visual-global-glow>` | :ref:`offwidth<visual-global-offwidth>` | :ref:`offheight<visual-global-offheight>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`realtime<visual-global-realtime>` | :ref:`ellipsoidinertia<visual-global-ellipsoidinertia>` | | | |
|
| | | | :ref:`realtime<visual-global-realtime>` | :ref:`ellipsoidinertia<visual-global-ellipsoidinertia>` | :ref:`bvactive<visual-global-bvactive>` | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_| visual |br| |_| |L| | | .. table:: |
|
| |_| visual |br| |_| |L| | | .. table:: |
|
||||||
@@ -126,7 +126,9 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`contacttorque<visual-rgba-contacttorque>` | :ref:`contactgap<visual-rgba-contactgap>` | :ref:`rangefinder<visual-rgba-rangefinder>` | :ref:`constraint<visual-rgba-constraint>` | |
|
| | | | :ref:`contacttorque<visual-rgba-contacttorque>` | :ref:`contactgap<visual-rgba-contactgap>` | :ref:`rangefinder<visual-rgba-rangefinder>` | :ref:`constraint<visual-rgba-constraint>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`slidercrank<visual-rgba-slidercrank>` | :ref:`crankbroken<visual-rgba-crankbroken>` | :ref:`frustum<visual-rgba-frustum>` | | |
|
| | | | :ref:`slidercrank<visual-rgba-slidercrank>` | :ref:`crankbroken<visual-rgba-crankbroken>` | :ref:`frustum<visual-rgba-frustum>` | :ref:`bv<visual-rgba-bv>` | |
|
||||||
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
|
| | | | :ref:`bvactive<visual-rgba-bvactive>` | | | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| mujoco |br| |L| | | *no attributes* |
|
| mujoco |br| |L| | | *no attributes* |
|
||||||
|
|||||||
+15
-6
@@ -12,19 +12,26 @@ General
|
|||||||
2. Removed the :ref:`timer<mjtTimer>` for midphase colllision detection, it is now folded in with the narrowphase
|
2. Removed the :ref:`timer<mjtTimer>` for midphase colllision detection, it is now folded in with the narrowphase
|
||||||
timer. This is because timing the two phases seperately required fine-grained timers inside the collision
|
timer. This is because timing the two phases seperately required fine-grained timers inside the collision
|
||||||
functions; these functions are so small and fast that the timer itself was incurring a measurable cost.
|
functions; these functions are so small and fast that the timer itself was incurring a measurable cost.
|
||||||
|
3. Added the flag :ref:`bvactive<visual-global-bvactive>` to ``visual/global``, allowing users to turn off
|
||||||
|
visualisation of active bounding volumes (the red/green boxes in this :ref:`this changelog item<midphase>`). For
|
||||||
|
models with very high-resolution meshes, the computation required for this visualization can slow down simulation
|
||||||
|
speed. Fixes :github:issue:`1279`.
|
||||||
|
|
||||||
|
- Added color of :ref:`bounding volumes<visual-rgba-bv>` and :ref:`active bounding volumes<visual-rgba-bvactive>`
|
||||||
|
to :ref:`visual/rgba<visual-rgba>`.
|
||||||
|
|
||||||
MJX
|
MJX
|
||||||
^^^
|
^^^
|
||||||
3. Added :ref:`dyntype<actuator-general-dyntype>` ``filterexact``.
|
4. Added :ref:`dyntype<actuator-general-dyntype>` ``filterexact``.
|
||||||
4. Added :at:`site` transmission.
|
5. Added :at:`site` transmission.
|
||||||
5. Updated MJX colab tutorial with more stable quadruped environment.
|
6. Updated MJX colab tutorial with more stable quadruped environment.
|
||||||
6. Added ``mjx.ray`` which mirrors :ref:`mj_ray` for planes, spheres, capsules, boxes, and meshes.
|
7. Added ``mjx.ray`` which mirrors :ref:`mj_ray` for planes, spheres, capsules, boxes, and meshes.
|
||||||
|
|
||||||
Bug fixes
|
Bug fixes
|
||||||
^^^^^^^^^
|
^^^^^^^^^
|
||||||
7. Fixed a bug that prevented the use of pins with plugins if flexes are not in the worldbody. Fixes
|
8. Fixed a bug that prevented the use of pins with plugins if flexes are not in the worldbody. Fixes
|
||||||
:github:issue:`1270`.
|
:github:issue:`1270`.
|
||||||
7. Fixed a bug in the :ref:`muscle model<CMuscle>` that led to non-zero values outside the lower
|
9. Fixed a bug in the :ref:`muscle model<CMuscle>` that led to non-zero values outside the lower
|
||||||
bound of the length range. Fixes :github:issue:`1342`.
|
bound of the length range. Fixes :github:issue:`1342`.
|
||||||
|
|
||||||
|
|
||||||
@@ -624,6 +631,8 @@ General
|
|||||||
:align: right
|
:align: right
|
||||||
:width: 350px
|
:width: 350px
|
||||||
|
|
||||||
|
.. _midphase:
|
||||||
|
|
||||||
2. Added a collision mid-phase for pruning geoms in body pairs, see :ref:`documentation<coSelection>` for more details.
|
2. Added a collision mid-phase for pruning geoms in body pairs, see :ref:`documentation<coSelection>` for more details.
|
||||||
This is based on static AABB bounding volume hierarchy (a BVH binary tree) in the body inertial frame. The GIF on
|
This is based on static AABB bounding volume hierarchy (a BVH binary tree) in the body inertial frame. The GIF on
|
||||||
the right is cut from `this longer video <https://youtu.be/e0babIM8hBo>`__.
|
the right is cut from `this longer video <https://youtu.be/e0babIM8hBo>`__.
|
||||||
|
|||||||
@@ -753,6 +753,7 @@ struct mjVisual_ { // visualization options
|
|||||||
int offwidth; // width of offscreen buffer
|
int offwidth; // width of offscreen buffer
|
||||||
int offheight; // height of offscreen buffer
|
int offheight; // height of offscreen buffer
|
||||||
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
|
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
|
||||||
|
int bvactive; // visualize active bounding volumes (0: no, 1: yes)
|
||||||
} global;
|
} global;
|
||||||
|
|
||||||
struct { // rendering quality
|
struct { // rendering quality
|
||||||
@@ -830,6 +831,8 @@ struct mjVisual_ { // visualization options
|
|||||||
float slidercrank[4]; // slidercrank
|
float slidercrank[4]; // slidercrank
|
||||||
float crankbroken[4]; // used when crank must be stretched/broken
|
float crankbroken[4]; // used when crank must be stretched/broken
|
||||||
float frustum[4]; // camera frustum
|
float frustum[4]; // camera frustum
|
||||||
|
float bv[4]; // bounding volume
|
||||||
|
float bvactive[4]; // active bounding volume
|
||||||
} rgba;
|
} rgba;
|
||||||
};
|
};
|
||||||
typedef struct mjVisual_ mjVisual;
|
typedef struct mjVisual_ mjVisual;
|
||||||
|
|||||||
@@ -459,6 +459,7 @@ struct mjVisual_ { // visualization options
|
|||||||
int offwidth; // width of offscreen buffer
|
int offwidth; // width of offscreen buffer
|
||||||
int offheight; // height of offscreen buffer
|
int offheight; // height of offscreen buffer
|
||||||
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
|
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
|
||||||
|
int bvactive; // visualize active bounding volumes (0: no, 1: yes)
|
||||||
} global;
|
} global;
|
||||||
|
|
||||||
struct { // rendering quality
|
struct { // rendering quality
|
||||||
@@ -536,6 +537,8 @@ struct mjVisual_ { // visualization options
|
|||||||
float slidercrank[4]; // slidercrank
|
float slidercrank[4]; // slidercrank
|
||||||
float crankbroken[4]; // used when crank must be stretched/broken
|
float crankbroken[4]; // used when crank must be stretched/broken
|
||||||
float frustum[4]; // camera frustum
|
float frustum[4]; // camera frustum
|
||||||
|
float bv[4]; // bounding volume
|
||||||
|
float bvactive[4]; // active bounding volume
|
||||||
} rgba;
|
} rgba;
|
||||||
};
|
};
|
||||||
typedef struct mjVisual_ mjVisual;
|
typedef struct mjVisual_ mjVisual;
|
||||||
|
|||||||
@@ -352,6 +352,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
doc='geom for inertia visualization (0: box, 1: ellipsoid)', # pylint: disable=line-too-long
|
doc='geom for inertia visualization (0: box, 1: ellipsoid)', # pylint: disable=line-too-long
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='bvactive',
|
||||||
|
type=ValueType(name='int'),
|
||||||
|
doc='visualize active bounding volumes (0: no, 1: yes)', # pylint: disable=line-too-long
|
||||||
|
),
|
||||||
),
|
),
|
||||||
),
|
),
|
||||||
doc='',
|
doc='',
|
||||||
@@ -780,6 +785,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
),
|
),
|
||||||
doc='camera frustum',
|
doc='camera frustum',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='bv',
|
||||||
|
type=ArrayType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
extents=(4,),
|
||||||
|
),
|
||||||
|
doc='bounding volume',
|
||||||
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='bvactive',
|
||||||
|
type=ArrayType(
|
||||||
|
inner_type=ValueType(name='float'),
|
||||||
|
extents=(4,),
|
||||||
|
),
|
||||||
|
doc='active bounding volume',
|
||||||
|
),
|
||||||
),
|
),
|
||||||
),
|
),
|
||||||
doc='',
|
doc='',
|
||||||
|
|||||||
@@ -1400,6 +1400,7 @@ PYBIND11_MODULE(_structs, m) {
|
|||||||
X(offwidth);
|
X(offwidth);
|
||||||
X(offheight);
|
X(offheight);
|
||||||
X(ellipsoidinertia);
|
X(ellipsoidinertia);
|
||||||
|
X(bvactive);
|
||||||
#undef X
|
#undef X
|
||||||
|
|
||||||
py::class_<raw::MjVisualQuality> mjVisualQuality(mjVisual, "Quality");
|
py::class_<raw::MjVisualQuality> mjVisualQuality(mjVisual, "Quality");
|
||||||
@@ -1529,6 +1530,8 @@ PYBIND11_MODULE(_structs, m) {
|
|||||||
X(slidercrank);
|
X(slidercrank);
|
||||||
X(crankbroken);
|
X(crankbroken);
|
||||||
X(frustum);
|
X(frustum);
|
||||||
|
X(bv);
|
||||||
|
X(bvactive);
|
||||||
#undef X
|
#undef X
|
||||||
|
|
||||||
#define X(var) \
|
#define X(var) \
|
||||||
|
|||||||
@@ -233,6 +233,8 @@ class MjWrapper<raw::MjVisualRgba> : public WrapperBase<raw::MjVisualRgba> {
|
|||||||
X(slidercrank);
|
X(slidercrank);
|
||||||
X(crankbroken);
|
X(crankbroken);
|
||||||
X(frustum);
|
X(frustum);
|
||||||
|
X(bv);
|
||||||
|
X(bvactive);
|
||||||
#undef X
|
#undef X
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -896,6 +896,7 @@ void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m, int oldstate)
|
|||||||
{mjITEM_EDITNUM, "Extent", 2, &(stat->extent), "1"},
|
{mjITEM_EDITNUM, "Extent", 2, &(stat->extent), "1"},
|
||||||
{mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"},
|
{mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"},
|
||||||
{mjITEM_RADIO, "Inertia", 5, &(vis->global.ellipsoidinertia), "Box\nEllipsoid"},
|
{mjITEM_RADIO, "Inertia", 5, &(vis->global.ellipsoidinertia), "Box\nEllipsoid"},
|
||||||
|
{mjITEM_RADIO, "BVH active", 5, &(vis->global.bvactive), "False\nTrue"},
|
||||||
{mjITEM_SEPARATOR, "Map", 1},
|
{mjITEM_SEPARATOR, "Map", 1},
|
||||||
{mjITEM_EDITFLOAT, "Stiffness", 2, &(vis->map.stiffness), "1"},
|
{mjITEM_EDITFLOAT, "Stiffness", 2, &(vis->map.stiffness), "1"},
|
||||||
{mjITEM_EDITFLOAT, "Rot stiffness", 2, &(vis->map.stiffnessrot), "1"},
|
{mjITEM_EDITFLOAT, "Rot stiffness", 2, &(vis->map.stiffnessrot), "1"},
|
||||||
|
|||||||
@@ -273,7 +273,9 @@ void mj_collision(const mjModel* m, mjData* d) {
|
|||||||
mj_clearEfc(d);
|
mj_clearEfc(d);
|
||||||
|
|
||||||
// reset the visualization flags
|
// reset the visualization flags
|
||||||
memset(d->bvh_active, 0, m->nbvh);
|
if (m->vis.global.bvactive) {
|
||||||
|
memset(d->bvh_active, 0, m->nbvh);
|
||||||
|
}
|
||||||
|
|
||||||
// return if disabled
|
// return if disabled
|
||||||
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)
|
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)
|
||||||
@@ -634,6 +636,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
|
|||||||
mjtByte isbody2 = (bf2 < nbody);
|
mjtByte isbody2 = (bf2 < nbody);
|
||||||
int f1 = isbody1 ? -1 : bf1 - nbody;
|
int f1 = isbody1 ? -1 : bf1 - nbody;
|
||||||
int f2 = isbody2 ? -1 : bf2 - nbody;
|
int f2 = isbody2 ? -1 : bf2 - nbody;
|
||||||
|
int mark_active = m->vis.global.bvactive;
|
||||||
const int bvhadr1 = isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[f1];
|
const int bvhadr1 = isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[f1];
|
||||||
const int bvhadr2 = isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[f2];
|
const int bvhadr2 = isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[f2];
|
||||||
const int* child1 = m->bvh_child + 2*bvhadr1;
|
const int* child1 = m->bvh_child + 2*bvhadr1;
|
||||||
@@ -705,8 +708,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
|
|||||||
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
|
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
|
||||||
margin, NULL, NULL, &initialize)) {
|
margin, NULL, NULL, &initialize)) {
|
||||||
mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
|
mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
|
||||||
d->bvh_active[node1 + bvhadr1] = 1;
|
if (mark_active) {
|
||||||
d->bvh_active[node2 + bvhadr2] = 1;
|
d->bvh_active[node1 + bvhadr1] = 1;
|
||||||
|
d->bvh_active[node2 + bvhadr2] = 1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
continue;
|
continue;
|
||||||
@@ -742,8 +747,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
|
|||||||
if (m->geom_type[nodeid1] != mjGEOM_PLANE) {
|
if (m->geom_type[nodeid1] != mjGEOM_PLANE) {
|
||||||
mj_collideGeomElem(m, d, nodeid1, f2, nodeid2);
|
mj_collideGeomElem(m, d, nodeid1, f2, nodeid2);
|
||||||
}
|
}
|
||||||
d->bvh_active[node1 + bvhadr1] = 1;
|
if (mark_active) {
|
||||||
d->bvh_active[node2 + bvhadr2] = 1;
|
d->bvh_active[node1 + bvhadr1] = 1;
|
||||||
|
d->bvh_active[node2 + bvhadr2] = 1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
continue;
|
continue;
|
||||||
@@ -771,8 +778,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
|
|||||||
// box filter applied in mj_collideElems, bitmask filter applied earlier
|
// box filter applied in mj_collideElems, bitmask filter applied earlier
|
||||||
if (isleaf1 && isleaf2) {
|
if (isleaf1 && isleaf2) {
|
||||||
mj_collideElems(m, d, f1, nodeid1, f2, nodeid2);
|
mj_collideElems(m, d, f1, nodeid1, f2, nodeid2);
|
||||||
d->bvh_active[node1 + bvhadr1] = 1;
|
if (mark_active) {
|
||||||
d->bvh_active[node2 + bvhadr2] = 1;
|
d->bvh_active[node1 + bvhadr1] = 1;
|
||||||
|
d->bvh_active[node2 + bvhadr2] = 1;
|
||||||
|
}
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -784,8 +793,10 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
d->bvh_active[node1 + bvhadr1] = 1;
|
if (mark_active) {
|
||||||
d->bvh_active[node2 + bvhadr2] = 1;
|
d->bvh_active[node1 + bvhadr1] = 1;
|
||||||
|
d->bvh_active[node2 + bvhadr2] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
// keep traversing the tree
|
// keep traversing the tree
|
||||||
if (!isleaf1 && isleaf2) {
|
if (!isleaf1 && isleaf2) {
|
||||||
|
|||||||
@@ -496,7 +496,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
|
|||||||
const int* faceid = m->bvh_nodeid + bvhadr;
|
const int* faceid = m->bvh_nodeid + bvhadr;
|
||||||
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
|
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
|
||||||
const int* child = m->bvh_child + 2*bvhadr;
|
const int* child = m->bvh_child + 2*bvhadr;
|
||||||
mjtByte* visited = d->bvh_active + bvhadr;
|
mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL;
|
||||||
|
|
||||||
mj_markStack(d);
|
mj_markStack(d);
|
||||||
// TODO(quaglino): Store bvh max depths to make this bound tighter.
|
// TODO(quaglino): Store bvh max depths to make this bound tighter.
|
||||||
@@ -521,9 +521,6 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
|
|||||||
|
|
||||||
// node1 is a leaf
|
// node1 is a leaf
|
||||||
if (faceid[node] != -1) {
|
if (faceid[node] != -1) {
|
||||||
if (visited[node]) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
||||||
faces[*npoints] = faceid[node];
|
faces[*npoints] = faceid[node];
|
||||||
if (++(*npoints) == MAXSDFFACE) {
|
if (++(*npoints) == MAXSDFFACE) {
|
||||||
@@ -531,7 +528,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
|
|||||||
mj_freeStack(d);
|
mj_freeStack(d);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
visited[node] = 1;
|
if (bvh_active) bvh_active[node] = 1;
|
||||||
}
|
}
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
@@ -541,7 +538,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
visited[node] = 1;
|
if (bvh_active) bvh_active[node] = 1;
|
||||||
|
|
||||||
// recursive call
|
// recursive call
|
||||||
for (int i=0; i < 2; i++) {
|
for (int i=0; i < 2; i++) {
|
||||||
|
|||||||
@@ -194,6 +194,7 @@ void mj_defaultVisual(mjVisual* vis) {
|
|||||||
vis->global.offheight = 480;
|
vis->global.offheight = 480;
|
||||||
vis->global.realtime = 1.0;
|
vis->global.realtime = 1.0;
|
||||||
vis->global.ellipsoidinertia = 0;
|
vis->global.ellipsoidinertia = 0;
|
||||||
|
vis->global.bvactive = 1;
|
||||||
|
|
||||||
// rendering quality
|
// rendering quality
|
||||||
vis->quality.shadowsize = 4096;
|
vis->quality.shadowsize = 4096;
|
||||||
@@ -272,6 +273,8 @@ void mj_defaultVisual(mjVisual* vis) {
|
|||||||
setf4(vis->rgba.slidercrank, .5, .3, .8, 1.);
|
setf4(vis->rgba.slidercrank, .5, .3, .8, 1.);
|
||||||
setf4(vis->rgba.crankbroken, .9, .0, .0, 1.);
|
setf4(vis->rgba.crankbroken, .9, .0, .0, 1.);
|
||||||
setf4(vis->rgba.frustum, 1., 1., .0, .2);
|
setf4(vis->rgba.frustum, 1., 1., .0, .2);
|
||||||
|
setf4(vis->rgba.bv, 0., 1., .0, .5);
|
||||||
|
setf4(vis->rgba.bvactive, 1., 0., .0, .5);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -627,6 +627,7 @@ int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3],
|
|||||||
// ray vs tree intersection
|
// ray vs tree intersection
|
||||||
mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
|
mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
|
||||||
const mjtNum* vec) {
|
const mjtNum* vec) {
|
||||||
|
int mark_active = m->vis.global.bvactive;
|
||||||
const int meshid = m->geom_dataid[id];
|
const int meshid = m->geom_dataid[id];
|
||||||
const int bvhadr = m->mesh_bvhadr[meshid];
|
const int bvhadr = m->mesh_bvhadr[meshid];
|
||||||
const int* faceid = m->bvh_nodeid + bvhadr;
|
const int* faceid = m->bvh_nodeid + bvhadr;
|
||||||
@@ -701,12 +702,17 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
|
|||||||
// update
|
// update
|
||||||
if (sol >= 0 && (x < 0 || sol < x)) {
|
if (sol >= 0 && (x < 0 || sol < x)) {
|
||||||
x = sol;
|
x = sol;
|
||||||
|
if (mark_active) {
|
||||||
|
d->bvh_active[node + bvhadr] = 1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// used for rendering
|
// used for rendering
|
||||||
d->bvh_active[node + bvhadr] = 1;
|
if (mark_active) {
|
||||||
|
d->bvh_active[node + bvhadr] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
// add children to the stack
|
// add children to the stack
|
||||||
for (int i=0; i < 2; i++) {
|
for (int i=0; i < 2; i++) {
|
||||||
|
|||||||
@@ -532,6 +532,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
mjvGeom* thisgeom;
|
mjvGeom* thisgeom;
|
||||||
mjvPerturb localpert;
|
mjvPerturb localpert;
|
||||||
float scl = m->stat.meansize;
|
float scl = m->stat.meansize;
|
||||||
|
int mark_active = m->vis.global.bvactive;
|
||||||
|
|
||||||
// make default pert if missing
|
// make default pert if missing
|
||||||
if (!pert) {
|
if (!pert) {
|
||||||
@@ -636,7 +637,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
category = mjCAT_DECOR;
|
category = mjCAT_DECOR;
|
||||||
objtype = mjOBJ_UNKNOWN;
|
objtype = mjOBJ_UNKNOWN;
|
||||||
if (vopt->flags[mjVIS_BODYBVH]) {
|
if (vopt->flags[mjVIS_BODYBVH]) {
|
||||||
float rgba[] = {1, 0, 0, 1};
|
|
||||||
for (int i = 0; i < m->nbvhstatic; i++) {
|
for (int i = 0; i < m->nbvhstatic; i++) {
|
||||||
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
|
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
|
||||||
if (scn->ngeom >= scn->maxgeom) break;
|
if (scn->ngeom >= scn->maxgeom) break;
|
||||||
@@ -671,8 +671,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
mju_rotVecMat(pos, center, xmat);
|
mju_rotVecMat(pos, center, xmat);
|
||||||
mju_addTo3(pos, xpos);
|
mju_addTo3(pos, xpos);
|
||||||
|
|
||||||
rgba[0] = d->bvh_active[i] ? 1 : 0;
|
// set box color
|
||||||
rgba[1] = d->bvh_active[i] ? 0 : 1;
|
const float* rgba = m->vis.rgba.bv;
|
||||||
|
if (mark_active && d->bvh_active[i]) {
|
||||||
|
rgba = m->vis.rgba.bvactive;
|
||||||
|
}
|
||||||
|
|
||||||
START
|
START
|
||||||
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
|
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
|
||||||
@@ -685,10 +688,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
category = mjCAT_DECOR;
|
category = mjCAT_DECOR;
|
||||||
objtype = mjOBJ_UNKNOWN;
|
objtype = mjOBJ_UNKNOWN;
|
||||||
if (vopt->flags[mjVIS_FLEXBVH]) {
|
if (vopt->flags[mjVIS_FLEXBVH]) {
|
||||||
float rgba[] = {1, 0, 0, 0.1};
|
|
||||||
for (int f=0; f < m->nflex; f++) {
|
for (int f=0; f < m->nflex; f++) {
|
||||||
if (m->flex_bvhnum[f] &&
|
if (m->flex_bvhnum[f] && vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
|
||||||
vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
|
|
||||||
for (int i=m->flex_bvhadr[f]; i < m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
|
for (int i=m->flex_bvhadr[f]; i < m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
|
||||||
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
|
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
|
||||||
if (scn->ngeom >= scn->maxgeom) break;
|
if (scn->ngeom >= scn->maxgeom) break;
|
||||||
@@ -698,10 +699,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// set box data
|
// get box data
|
||||||
mjtNum *aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
|
mjtNum *aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
|
||||||
rgba[0] = d->bvh_active[i] ? 1 : 0;
|
|
||||||
rgba[1] = d->bvh_active[i] ? 0 : 1;
|
// set box color
|
||||||
|
const float* rgba = m->vis.rgba.bv;
|
||||||
|
if (mark_active && d->bvh_active[i]) {
|
||||||
|
rgba = m->vis.rgba.bvactive;
|
||||||
|
}
|
||||||
|
|
||||||
START
|
START
|
||||||
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
|
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
|
||||||
@@ -715,7 +720,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
category = mjCAT_DECOR;
|
category = mjCAT_DECOR;
|
||||||
objtype = mjOBJ_UNKNOWN;
|
objtype = mjOBJ_UNKNOWN;
|
||||||
if (vopt->flags[mjVIS_MESHBVH]) {
|
if (vopt->flags[mjVIS_MESHBVH]) {
|
||||||
float rgba[] = {1, 0, 0, 1};
|
|
||||||
for (int geomid = 0; geomid < m->ngeom; geomid++) {
|
for (int geomid = 0; geomid < m->ngeom; geomid++) {
|
||||||
int meshid = m->geom_dataid[geomid];
|
int meshid = m->geom_dataid[geomid];
|
||||||
if (meshid == -1) {
|
if (meshid == -1) {
|
||||||
@@ -732,13 +736,17 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!d->bvh_active[i]) {
|
// box color
|
||||||
continue;
|
const float* rgba = m->vis.rgba.bv;
|
||||||
|
if (mark_active) {
|
||||||
|
if (d->bvh_active[i]) {
|
||||||
|
rgba = m->vis.rgba.bvactive;
|
||||||
|
} else {
|
||||||
|
// when marking active bvs, skip inactive volumes
|
||||||
|
continue;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
rgba[0] = d->bvh_active[i] ? 1 : 0;
|
|
||||||
rgba[1] = d->bvh_active[i] ? 0 : 1;
|
|
||||||
|
|
||||||
// get xpos, xmat, size
|
// get xpos, xmat, size
|
||||||
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
|
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
|
||||||
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
|
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
|
||||||
|
|||||||
@@ -113,8 +113,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
|
|
||||||
{"visual", "*", "0"},
|
{"visual", "*", "0"},
|
||||||
{"<"},
|
{"<"},
|
||||||
{"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow",
|
{"global", "?", "11", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow",
|
||||||
"offwidth", "offheight", "realtime", "ellipsoidinertia"},
|
"offwidth", "offheight", "realtime", "ellipsoidinertia", "bvactive"},
|
||||||
{"quality", "?", "5", "shadowsize", "offsamples", "numslices", "numstacks",
|
{"quality", "?", "5", "shadowsize", "offsamples", "numslices", "numstacks",
|
||||||
"numquads"},
|
"numquads"},
|
||||||
{"headlight", "?", "4", "ambient", "diffuse", "specular", "active"},
|
{"headlight", "?", "4", "ambient", "diffuse", "specular", "active"},
|
||||||
@@ -124,11 +124,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
{"scale", "?", "17", "forcewidth", "contactwidth", "contactheight", "connect", "com",
|
{"scale", "?", "17", "forcewidth", "contactwidth", "contactheight", "connect", "com",
|
||||||
"camera", "light", "selectpoint", "jointlength", "jointwidth", "actuatorlength",
|
"camera", "light", "selectpoint", "jointlength", "jointwidth", "actuatorlength",
|
||||||
"actuatorwidth", "framelength", "framewidth", "constraint", "slidercrank", "frustum"},
|
"actuatorwidth", "framelength", "framewidth", "constraint", "slidercrank", "frustum"},
|
||||||
{"rgba", "?", "23", "fog", "haze", "force", "inertia", "joint",
|
{"rgba", "?", "25", "fog", "haze", "force", "inertia", "joint",
|
||||||
"actuator", "actuatornegative", "actuatorpositive", "com",
|
"actuator", "actuatornegative", "actuatorpositive", "com",
|
||||||
"camera", "light", "selectpoint", "connect", "contactpoint", "contactforce",
|
"camera", "light", "selectpoint", "connect", "contactpoint", "contactforce",
|
||||||
"contactfriction", "contacttorque", "contactgap", "rangefinder",
|
"contactfriction", "contacttorque", "contactgap", "rangefinder",
|
||||||
"constraint", "slidercrank", "crankbroken", "frustum"},
|
"constraint", "slidercrank", "crankbroken", "frustum", "bv", "bvactive"},
|
||||||
{">"},
|
{">"},
|
||||||
|
|
||||||
{"statistic", "*", "5", "meaninertia", "meanmass", "meansize", "extent", "center"},
|
{"statistic", "*", "5", "meaninertia", "meanmass", "meansize", "extent", "center"},
|
||||||
@@ -2723,6 +2723,10 @@ void mjXReader::Visual(XMLElement* section) {
|
|||||||
if (MapValue(elem, "ellipsoidinertia", &ellipsoidinertia, bool_map, 2)) {
|
if (MapValue(elem, "ellipsoidinertia", &ellipsoidinertia, bool_map, 2)) {
|
||||||
vis->global.ellipsoidinertia = (ellipsoidinertia==1);
|
vis->global.ellipsoidinertia = (ellipsoidinertia==1);
|
||||||
}
|
}
|
||||||
|
int bvactive;
|
||||||
|
if (MapValue(elem, "bvactive", &bvactive, bool_map, 2)) {
|
||||||
|
vis->global.bvactive = (bvactive==1);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// quality sub-element
|
// quality sub-element
|
||||||
@@ -2808,6 +2812,8 @@ void mjXReader::Visual(XMLElement* section) {
|
|||||||
ReadAttr(elem, "slidercrank", 4, vis->rgba.slidercrank, text);
|
ReadAttr(elem, "slidercrank", 4, vis->rgba.slidercrank, text);
|
||||||
ReadAttr(elem, "crankbroken", 4, vis->rgba.crankbroken, text);
|
ReadAttr(elem, "crankbroken", 4, vis->rgba.crankbroken, text);
|
||||||
ReadAttr(elem, "frustum", 4, vis->rgba.frustum, text);
|
ReadAttr(elem, "frustum", 4, vis->rgba.frustum, text);
|
||||||
|
ReadAttr(elem, "bv", 4, vis->rgba.bv, text);
|
||||||
|
ReadAttr(elem, "bvactive", 4, vis->rgba.bvactive, text);
|
||||||
}
|
}
|
||||||
|
|
||||||
// advance to next element
|
// advance to next element
|
||||||
|
|||||||
@@ -1030,6 +1030,7 @@ void mjXWriter::Visual(XMLElement* root) {
|
|||||||
WriteAttrInt(elem, "offwidth", vis->global.offwidth, visdef.global.offwidth);
|
WriteAttrInt(elem, "offwidth", vis->global.offwidth, visdef.global.offwidth);
|
||||||
WriteAttrInt(elem, "offheight", vis->global.offheight, visdef.global.offheight);
|
WriteAttrInt(elem, "offheight", vis->global.offheight, visdef.global.offheight);
|
||||||
WriteAttrKey(elem, "ellipsoidinertia", bool_map, 2, vis->global.ellipsoidinertia, visdef.global.ellipsoidinertia);
|
WriteAttrKey(elem, "ellipsoidinertia", bool_map, 2, vis->global.ellipsoidinertia, visdef.global.ellipsoidinertia);
|
||||||
|
WriteAttrKey(elem, "bvactive", bool_map, 2, vis->global.bvactive, visdef.global.bvactive);
|
||||||
if (!elem->FirstAttribute()) {
|
if (!elem->FirstAttribute()) {
|
||||||
section->DeleteChild(elem);
|
section->DeleteChild(elem);
|
||||||
}
|
}
|
||||||
@@ -1122,6 +1123,8 @@ void mjXWriter::Visual(XMLElement* root) {
|
|||||||
WriteAttr(elem, "slidercrank", 4, vis->rgba.slidercrank, visdef.rgba.slidercrank);
|
WriteAttr(elem, "slidercrank", 4, vis->rgba.slidercrank, visdef.rgba.slidercrank);
|
||||||
WriteAttr(elem, "crankbroken", 4, vis->rgba.crankbroken, visdef.rgba.crankbroken);
|
WriteAttr(elem, "crankbroken", 4, vis->rgba.crankbroken, visdef.rgba.crankbroken);
|
||||||
WriteAttr(elem, "frustum", 4, vis->rgba.frustum, visdef.rgba.frustum);
|
WriteAttr(elem, "frustum", 4, vis->rgba.frustum, visdef.rgba.frustum);
|
||||||
|
WriteAttr(elem, "bv", 4, vis->rgba.bv, visdef.rgba.bv);
|
||||||
|
WriteAttr(elem, "bvactive", 4, vis->rgba.bvactive, visdef.rgba.bvactive);
|
||||||
if (!elem->FirstAttribute()) {
|
if (!elem->FirstAttribute()) {
|
||||||
section->DeleteChild(elem);
|
section->DeleteChild(elem);
|
||||||
}
|
}
|
||||||
|
|||||||
+8
-1
@@ -3,6 +3,13 @@
|
|||||||
<mesh file="stanford_bunny.obj"/>
|
<mesh file="stanford_bunny.obj"/>
|
||||||
</asset>
|
</asset>
|
||||||
<worldbody>
|
<worldbody>
|
||||||
<geom type="mesh" mesh="stanford_bunny" pos="1 0 0" euler="0 90 0"/>
|
<site name="lidar" type="cylinder" pos=".7 .1 0" zaxis="1 0 0" size=".02 .05"/>
|
||||||
|
<body mocap="true" pos="1 0 0" euler="0 90 0">
|
||||||
|
<geom type="mesh" mesh="stanford_bunny"/>
|
||||||
|
</body>
|
||||||
</worldbody>
|
</worldbody>
|
||||||
|
|
||||||
|
<sensor>
|
||||||
|
<rangefinder site="lidar"/>
|
||||||
|
</sensor>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
|
|||||||
@@ -4985,6 +4985,7 @@ public unsafe struct global {
|
|||||||
public int offwidth;
|
public int offwidth;
|
||||||
public int offheight;
|
public int offheight;
|
||||||
public int ellipsoidinertia;
|
public int ellipsoidinertia;
|
||||||
|
public int bvactive;
|
||||||
}
|
}
|
||||||
|
|
||||||
[StructLayout(LayoutKind.Sequential)]
|
[StructLayout(LayoutKind.Sequential)]
|
||||||
@@ -5067,6 +5068,8 @@ public unsafe struct rgba {
|
|||||||
public fixed float slidercrank[4];
|
public fixed float slidercrank[4];
|
||||||
public fixed float crankbroken[4];
|
public fixed float crankbroken[4];
|
||||||
public fixed float frustum[4];
|
public fixed float frustum[4];
|
||||||
|
public fixed float bv[4];
|
||||||
|
public fixed float bvactive[4];
|
||||||
}
|
}
|
||||||
|
|
||||||
[StructLayout(LayoutKind.Sequential)]
|
[StructLayout(LayoutKind.Sequential)]
|
||||||
|
|||||||
Reference in New Issue
Block a user