Skip to content

Commit 59d0676

Browse files
haroonqcopybara-github
haroonq
authored and
copybara-github
committed
Remove redundant checks for exhausted geoms.
PiperOrigin-RevId: 813284297 Change-Id: I565c517bef782ba07ff323e206b75f6f905e5809
1 parent 2dc533b commit 59d0676

File tree

1 file changed

+53
-58
lines changed

1 file changed

+53
-58
lines changed

‎src/engine/engine_vis_visualize.c‎

Lines changed: 53 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -379,7 +379,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
379379
mju_add3(to, from, vec);
380380
mjv_connector(thisgeom,
381381
bf[0] > 0 && bf[1] > 0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW,
382-
m->vis.scale.forcewidth * scl,from, to);
382+
m->vis.scale.forcewidth * scl, from, to);
383383
f2f(thisgeom->rgba, j == 2 ? m->vis.rgba.contactfriction : m->vis.rgba.contactforce, 4);
384384
if (vopt->label == mjLABEL_CONTACTFORCE && j == (split ? 1 : 0)) {
385385
mjSNPRINTF(thisgeom->label, "%-.3g", mju_norm3(frc));
@@ -852,7 +852,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
852852
if (vopt->flags[mjVIS_BODYBVH]) {
853853
for (int i = 0; i < m->nbvhstatic; i++) {
854854
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
855-
if (scn->ngeom >= scn->maxgeom) break;
856855
if (m->bvh_depth[i] != vopt->bvh_depth) {
857856
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
858857
continue;
@@ -876,10 +875,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
876875
// get xpos, xmat, size
877876
const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid;
878877
const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid;
879-
const mjtNum *size = isleaf ? m->geom_aabb + 6*geomid + 3 : m->bvh_aabb + 6*i + 3;
878+
const mjtNum* size = isleaf ? m->geom_aabb + 6*geomid + 3 : m->bvh_aabb + 6*i + 3;
880879

881880
// offset xpos with aabb center (not always at frame origin)
882-
const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
881+
const mjtNum* center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
883882
mjtNum pos[3];
884883
mju_mulMatVec3(pos, xmat, center);
885884
mju_addTo3(pos, xpos);
@@ -909,15 +908,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
909908
if (m->flex_bvhnum[f] && vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
910909
for (int i=m->flex_bvhadr[f]; i < m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
911910
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
912-
if (scn->ngeom >= scn->maxgeom) break;
913911
if (m->bvh_depth[i] != vopt->bvh_depth) {
914912
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
915913
continue;
916914
}
917915
}
918916

919917
// get box data
920-
mjtNum *aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
918+
mjtNum* aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
921919

922920
// set box color
923921
const float* rgba = m->vis.rgba.bv;
@@ -956,7 +954,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
956954
for (int i=0; i < 2; i++) {
957955
for (int j=0; j < 2; j++) {
958956
for (int k=0; k < 2; k++) {
959-
if (scn->ngeom >= scn->maxgeom) break;
960957
if (i == 0) {
961958
if (geomsExhausted(scn)) {
962959
return;
@@ -1004,7 +1001,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
10041001
for (int b = 0; b < m->mesh_bvhnum[meshid]; b++) {
10051002
int i = b + m->mesh_bvhadr[meshid];
10061003
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
1007-
if (scn->ngeom >= scn->maxgeom) break;
10081004
if (m->bvh_depth[i] != vopt->bvh_depth) {
10091005
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
10101006
continue;
@@ -1025,10 +1021,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
10251021
// get xpos, xmat, size
10261022
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
10271023
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
1028-
const mjtNum *size = m->bvh_aabb + 6*i + 3;
1024+
const mjtNum* size = m->bvh_aabb + 6*i + 3;
10291025

10301026
// offset xpos with aabb center (not always at geom origin)
1031-
const mjtNum *center = m->bvh_aabb + 6*i;
1027+
const mjtNum* center = m->bvh_aabb + 6*i;
10321028
mjtNum pos[3];
10331029
mju_mulMatVec3(pos, xmat, center);
10341030
mju_addTo3(pos, xpos);
@@ -1072,10 +1068,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
10721068
// get xpos, xmat, size
10731069
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
10741070
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
1075-
const mjtNum *size = m->oct_aabb + 6*i + 3;
1071+
const mjtNum* size = m->oct_aabb + 6*i + 3;
10761072

10771073
// offset xpos with aabb center (not always at geom origin)
1078-
const mjtNum *center = m->oct_aabb + 6*i;
1074+
const mjtNum* center = m->oct_aabb + 6*i;
10791075
mjtNum pos[3];
10801076
mju_mulMatVec3(pos, xmat, center);
10811077
mju_addTo3(pos, xpos);
@@ -1120,43 +1116,43 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
11201116
for (int i=0; i < m->mesh_facenum[mesh_id]; i++) {
11211117
if (geomsExhausted(scn)) {
11221118
return;
1123-
} else {
1124-
// triangle in global frame
1125-
mjtNum pos[3][3];
1126-
for (int j = 0; j < 3; j++) {
1127-
mjtNum v[3] = {mesh_vert[3 * face[3 * i + j] + 0],
1128-
mesh_vert[3 * face[3 * i + j] + 1],
1129-
mesh_vert[3 * face[3 * i + j] + 2]};
1130-
mju_mulMatVec3(pos[j], geom_mat, v);
1131-
mju_addTo3(pos[j], geom_pos);
1132-
}
1119+
}
11331120

1134-
// color
1135-
float rgba[4] = {0, 0, 0, 1.0};
1136-
mjtNum nval[3] = {0, 0, 0};
1137-
for (int r = 0; r < mjMIN(nchannel, 3); r++) {
1138-
for (int j = 0; j < 3; j++) {
1139-
mjtNum val = sensordata[r*m->mesh_vertnum[mesh_id] + face[3*i+j]];
1140-
rgba[r] += mju_abs(val) / maxval;
1141-
if (val) {
1142-
nval[r] += 1;
1143-
}
1144-
}
1145-
if (nval[r]) {
1146-
rgba[r] /= nval[r];
1121+
// triangle in global frame
1122+
mjtNum pos[3][3];
1123+
for (int j = 0; j < 3; j++) {
1124+
mjtNum v[3] = {mesh_vert[3 * face[3 * i + j] + 0],
1125+
mesh_vert[3 * face[3 * i + j] + 1],
1126+
mesh_vert[3 * face[3 * i + j] + 2]};
1127+
mju_mulMatVec3(pos[j], geom_mat, v);
1128+
mju_addTo3(pos[j], geom_pos);
1129+
}
1130+
1131+
// color
1132+
float rgba[4] = {0, 0, 0, 1.0};
1133+
mjtNum nval[3] = {0, 0, 0};
1134+
for (int r = 0; r < mjMIN(nchannel, 3); r++) {
1135+
for (int j = 0; j < 3; j++) {
1136+
mjtNum val = sensordata[r*m->mesh_vertnum[mesh_id] + face[3*i+j]];
1137+
rgba[r] += mju_abs(val) / maxval;
1138+
if (val) {
1139+
nval[r] += 1;
11471140
}
11481141
}
1149-
1150-
if (rgba[0]==0 && rgba[1]==0 && rgba[2]==0) {
1151-
rgba[3] = .1;
1142+
if (nval[r]) {
1143+
rgba[r] /= nval[r];
11521144
}
1145+
}
11531146

1154-
// draw triangles, one per side
1155-
thisgeom = acquireGeom(scn, i, category, objtype);
1156-
makeTriangle(thisgeom, pos[0], pos[1], pos[2], rgba);
1157-
thisgeom->objid = id;
1158-
releaseGeom(&thisgeom, scn);
1147+
if (rgba[0]==0 && rgba[1]==0 && rgba[2]==0) {
1148+
rgba[3] = .1;
11591149
}
1150+
1151+
// draw triangles, one per side
1152+
thisgeom = acquireGeom(scn, i, category, objtype);
1153+
makeTriangle(thisgeom, pos[0], pos[1], pos[2], rgba);
1154+
thisgeom->objid = id;
1155+
releaseGeom(&thisgeom, scn);
11601156
}
11611157
}
11621158
}
@@ -1260,7 +1256,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
12601256
thisgeom = acquireGeom(scn, i, category, objtype);
12611257

12621258
// construct geom
1263-
sz[0] = 2*sz[0];
1259+
sz[0] = 2*sz[0];
12641260
sz[1] = sz[2] = sz[0];
12651261
mju_quat2Mat(mat, pert->refquat);
12661262
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, pert->refselpos, mat, rgba);
@@ -2007,8 +2003,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
20072003
getFrustum(zver, zhor, znear, m->cam_intrinsic + 4*i, m->cam_sensorsize + 2*i);
20082004

20092005
// frustum frame to convert from planes to vertex representation
2010-
mjtNum *cam_xpos = d->cam_xpos+3*i;
2011-
mjtNum *cam_xmat = d->cam_xmat+9*i;
2006+
mjtNum* cam_xpos = d->cam_xpos+3*i;
2007+
mjtNum* cam_xmat = d->cam_xmat+9*i;
20122008
mjtNum x[] = {cam_xmat[0], cam_xmat[3], cam_xmat[6]};
20132009
mjtNum y[] = {cam_xmat[1], cam_xmat[4], cam_xmat[7]};
20142010
mjtNum z[] = {cam_xmat[2], cam_xmat[5], cam_xmat[8]};
@@ -2044,37 +2040,36 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
20442040
if (geomsExhausted(scn)) {
20452041
return;
20462042
}
2047-
20482043
thisgeom = acquireGeom(scn, i, category, objtype);
20492044
makeTriangle(thisgeom, vnear[e], vfar[e], vnear[(e+1)%4], rgba);
20502045
releaseGeom(&thisgeom, scn);
2046+
20512047
if (geomsExhausted(scn)) {
20522048
return;
20532049
}
2054-
20552050
thisgeom = acquireGeom(scn, i, category, objtype);
20562051
makeTriangle(thisgeom, vfar[e], vfar[(e+1)%4], vnear[(e+1)%4], rgba);
20572052
releaseGeom(&thisgeom, scn);
2053+
20582054
if (geomsExhausted(scn)) {
20592055
return;
20602056
}
2061-
20622057
thisgeom = acquireGeom(scn, i, category, objtype);
20632058
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vnear[(e+1)%4]);
20642059
f2f(thisgeom->rgba, rgba, 4);
20652060
releaseGeom(&thisgeom, scn);
2061+
20662062
if (geomsExhausted(scn)) {
20672063
return;
20682064
}
2069-
20702065
thisgeom = acquireGeom(scn, i, category, objtype);
20712066
mjv_connector(thisgeom, mjGEOM_LINE, 3, vfar[e], vfar[(e+1)%4]);
20722067
f2f(thisgeom->rgba, rgba, 4);
20732068
releaseGeom(&thisgeom, scn);
2069+
20742070
if (geomsExhausted(scn)) {
20752071
return;
20762072
}
2077-
20782073
thisgeom = acquireGeom(scn, i, category, objtype);
20792074
mjv_connector(thisgeom, mjGEOM_LINE, 3, vnear[e], vfar[e]);
20802075
f2f(thisgeom->rgba, rgba, 4);
@@ -2505,14 +2500,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
25052500
}
25062501

25072502
thisgeom = acquireGeom(scn, i, category, objtype);
2508-
nxt = d->xanchor+3*j;
2503+
nxt = d->xanchor+3*j;
25092504

25102505
// construct geom
25112506
mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, nxt);
25122507
f2f(thisgeom->rgba, m->vis.rgba.connect, 4);
25132508

25142509
releaseGeom(&thisgeom, scn);
2515-
cur = nxt;
2510+
cur = nxt;
25162511
}
25172512
}
25182513

@@ -2522,7 +2517,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
25222517
}
25232518

25242519
thisgeom = acquireGeom(scn, i, category, objtype);
2525-
nxt = d->xipos+3*m->body_parentid[i];
2520+
nxt = d->xipos+3*m->body_parentid[i];
25262521
mjv_connector(thisgeom, mjGEOM_CAPSULE, scl * m->vis.scale.connect, cur, nxt);
25272522
f2f(thisgeom->rgba, m->vis.rgba.connect, 4);
25282523
releaseGeom(&thisgeom, scn);
@@ -2585,7 +2580,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
25852580
for (int i=1; i < m->nbody; i++) {
25862581
if (!mju_isZero(d->xfrc_applied+6*i, 6) && (category & catmask)) {
25872582
// point of application and force
2588-
mjtNum *xpos = d->xipos+3*i;
2583+
mjtNum* xpos = d->xipos+3*i;
25892584
xfrc = d->xfrc_applied+6*i;
25902585

25912586
// force perturbation
@@ -3330,7 +3325,7 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt,
33303325
//----------------------------------- catenary functions -------------------------------------------
33313326

33323327
// returns hyperbolic cosine and optionally computes hyperbolic sine
3333-
static inline mjtNum cosh_sinh(mjtNum x, mjtNum *sinh) {
3328+
static inline mjtNum cosh_sinh(mjtNum x, mjtNum* sinh) {
33343329
mjtNum expx = mju_exp(x);
33353330
if (sinh) {
33363331
*sinh = 0.5 * (expx - 1/expx);
@@ -3348,7 +3343,7 @@ static inline mjtNum catenary_intercept(mjtNum v, mjtNum h, mjtNum length) {
33483343

33493344

33503345
// returns residual of catenary equation and optionally computes its gradient w.r.t b
3351-
static inline mjtNum catenary_residual(mjtNum b, mjtNum intercept, mjtNum *grad) {
3346+
static inline mjtNum catenary_residual(mjtNum b, mjtNum intercept, mjtNum* grad) {
33523347
mjtNum a = 0.5 / b;
33533348
mjtNum sinh, cosh = cosh_sinh(a, &sinh);
33543349
if (grad) {
@@ -3364,7 +3359,7 @@ static const mjtNum tolerance = 1e-9;
33643359

33653360

33663361

3367-
// solve trancendental catenary equation using change of variables proposed in
3362+
// solve transcendental catenary equation using change of variables proposed in
33683363
// https://math.stackexchange.com/a/1002996
33693364
static inline mjtNum solve_catenary(mjtNum v, mjtNum h, mjtNum length) {
33703365
mjtNum intercept = catenary_intercept(v, h, length);

0 commit comments

Comments
 (0)