From 0d4bc1de00182e87e81717a07543490e56e8c64f Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 5 Feb 2025 04:08:38 -0800 Subject: [PATCH] Do not use flexedge_length if trilinear interpolation is used. PiperOrigin-RevId: 723452451 Change-Id: I5a67fb74c13e80ae6d390694dd0c9f8c5e4d5213 --- src/engine/engine_setconst.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 211effe8..73f8919e 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -502,22 +502,23 @@ static void setStat(mjModel* m, mjData* d) { // adjust body size for flex edges involving body for (int f=0; f < m->nflex; f++) { - mjtNum meanedge = 0; - for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) { - meanedge += m->flexedge_length0[e] / m->flex_edgenum[f]; - if (m->flex_interp[f]) { - continue; + if (m->flex_interp[f]) { + for (int v1=m->flex_nodeadr[f]; v1 < m->flex_nodeadr[f]+m->flex_nodenum[f]; v1++) { + for (int v2=m->flex_nodeadr[f]; v2 < m->flex_nodeadr[f]+m->flex_nodenum[f]; v2++) { + mjtNum edge = mju_dist3(d->xpos+3*m->flex_nodebodyid[v1], + d->xpos+3*m->flex_nodebodyid[v2]); + body[m->flex_nodebodyid[v1]] = mju_max(body[m->flex_nodebodyid[v1]], edge); + } } - + continue; + } + for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) { int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]]; int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]]; body[b1] = mju_max(body[b1], m->flexedge_length0[e]); body[b2] = mju_max(body[b2], m->flexedge_length0[e]); } - for (int v=m->flex_nodeadr[f]; v < m->flex_nodeadr[f]+m->flex_node[f]; v++) { - body[m->flex_nodebodyid[v]] = mju_max(body[m->flex_nodebodyid[v]], meanedge); - } } // compute meansize, make sure all sizes are above min