From 794ef0b771635d663863d95a21f5747ef878e6e4 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Sun, 9 Oct 2022 08:06:00 -0700 Subject: [PATCH] Reset arena pointer if memory cannot be allocated for all `efc_` arrays. Startblock: diffbase is submitted PiperOrigin-RevId: 479922015 Change-Id: If60d89628e033ff253509f109b2c4d28912a3903 --- src/engine/engine_core_constraint.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 7ccf6ae5..e60ab3f4 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -34,6 +34,15 @@ //-------------------------- utility functions ----------------------------------------------------- +// internal function for clearing arena pointers for efc_ arrays in mjData +static inline void clearEfc(mjData* d) { +#define X(type, name, nr, nc) d->name = NULL; + MJDATA_ARENA_POINTERS +#undef X + d->nefc = 0; + d->contact = d->arena; +} + // determine type of friction cone int mj_isPyramidal(const mjModel* m) { if (m->opt.cone==mjCONE_PYRAMIDAL) { @@ -111,11 +120,7 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) { // move arena pointer back to the end of the existing contact array and invalidate efc_ arrays d->parena = d->ncon * sizeof(mjContact); - d->nefc = 0; -#define X(type, name, nr, nc) d->name = NULL; - MJDATA_ARENA_POINTERS -#undef X - d->contact = d->arena; + clearEfc(d); // copy contact mjContact* dst = mj_arenaAlloc(d, sizeof(mjContact), _Alignof(mjContact)); @@ -1426,7 +1431,8 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \ if (!d->name) { \ mj_warning(d, mjWARN_CNSTRFULL, d->nstack * sizeof(mjtNum)); \ - d->nefc = 0; \ + clearEfc(d); \ + d->parena = d->ncon * sizeof(mjContact); \ return; \ }