From f1007df013834dac260fbae8243da973cb68551f Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 4 Jan 2023 07:57:11 -0800 Subject: [PATCH] Implement hash map for mj_name2id using djb2 and linear probing. PiperOrigin-RevId: 499485127 Change-Id: I5aed6549db18b92d7c8a3b7590e50a00ce13a439 --- doc/changelog.rst | 4 + doc/includes/references.h | 2 + include/mujoco/mjmodel.h | 2 + include/mujoco/mjxmacro.h | 5 +- src/engine/engine_crossplatform.h | 6 + src/engine/engine_io.c | 3 + src/engine/engine_support.c | 271 +++++++++++++++++++-------- src/engine/engine_support.h | 3 + src/user/user_model.cc | 104 +++++++--- test/engine/engine_support_test.cc | 108 +++++++++++ unity/Runtime/Bindings/MjBindings.cs | 2 + 11 files changed, 404 insertions(+), 106 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 5d545921..fb542d64 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,10 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- +- The function :ref:`mj_name2id` is now implemented using a hash function instead of a linear search for better + performance. Version 2.3.1 (December 6, 2022) ----------------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index 41d7edd0..c996f46d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -785,6 +785,7 @@ struct mjModel_ { int nuser_actuator; // number of mjtNums in actuator_user int nuser_sensor; // number of mjtNums in sensor_user int nnames; // number of chars in all names + int nnames_map; // number of slots in the names hash map // sizes set after mjModel construction (only affect mjData) int nM; // number of non-zeros in sparse inertia matrix @@ -1146,6 +1147,7 @@ struct mjModel_ { int* name_keyadr; // keyframe name pointers (nkey x 1) int* name_pluginadr; // plugin instance name pointers (nplugin x 1) char* names; // names of all objects, 0-terminated (nnames x 1) + int* names_map; // internal hash map of names (nnames_map x 1) }; typedef struct mjModel_ mjModel; typedef enum mjtPluginCapabilityBit_ { diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index db697e76..926498f6 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -581,6 +581,7 @@ struct mjModel_ { int nuser_actuator; // number of mjtNums in actuator_user int nuser_sensor; // number of mjtNums in sensor_user int nnames; // number of chars in all names + int nnames_map; // number of slots in the names hash map // sizes set after mjModel construction (only affect mjData) int nM; // number of non-zeros in sparse inertia matrix @@ -942,6 +943,7 @@ struct mjModel_ { int* name_keyadr; // keyframe name pointers (nkey x 1) int* name_pluginadr; // plugin instance name pointers (nplugin x 1) char* names; // names of all objects, 0-terminated (nnames x 1) + int* names_map; // internal hash map of names (nnames_map x 1) }; typedef struct mjModel_ mjModel; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index a001b102..4812ff0b 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -113,6 +113,7 @@ X( nuser_actuator ) \ X( nuser_sensor ) \ X( nnames ) \ + X( nnames_map ) \ X( nM ) \ X( nD ) \ X( nemax ) \ @@ -432,8 +433,8 @@ X( int, name_tupleadr, ntuple, 1 ) \ X( int, name_keyadr, nkey, 1 ) \ X( int, name_pluginadr, nplugin, 1 ) \ - X( char, names, nnames, 1 ) - + X( char, names, nnames, 1 ) \ + X( int, names_map, nnames_map, 1 ) \ //-------------------------------- mjData ---------------------------------------------------------- diff --git a/src/engine/engine_crossplatform.h b/src/engine/engine_crossplatform.h index 195247b6..3728516a 100644 --- a/src/engine/engine_crossplatform.h +++ b/src/engine/engine_crossplatform.h @@ -46,4 +46,10 @@ #endif #endif +#if defined(__GNUC__) && __GNUC__ >= 7 + #define mjFALLTHROUGH __attribute__ ((fallthrough)) +#else + #define mjFALLTHROUGH ((void) 0) +#endif + #endif // MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_ diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index a5fdf901..26c8c581 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -438,6 +438,9 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt, m->nuser_actuator = nuser_actuator; m->nuser_sensor = nuser_sensor; m->nnames = nnames; + m->nnames_map = nbody + njnt + ngeom + nsite + ncam + nlight + nmesh + nskin + + nhfield + ntex + nmat + npair + nexclude + neq + ntendon + + nu + nsensor + nnumeric + ntext + ntuple + nkey + nplugin; #define X(name) \ if ((m->name) < 0) { \ diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 040440e0..a61438da 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -20,6 +20,7 @@ #include #include "engine/engine_array_safety.h" #include "engine/engine_core_constraint.h" +#include "engine/engine_crossplatform.h" #include "engine/engine_io.h" #include "engine/engine_macro.h" #include "engine/engine_util_blas.h" @@ -433,123 +434,231 @@ int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, //-------------------------- name functions -------------------------------------------------------- // get number of objects and name addresses for given object type -static int _getnumadr(const mjModel* m, mjtObj type, int** padr) { +static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) { + int num = -1; + // map address starts at the end, subtract with explicit switch fallthrough below + *mapadr = m->nnames_map; + // get address list and size for object type switch (type) { - case mjOBJ_BODY: - case mjOBJ_XBODY: - *padr = m->name_bodyadr; - return m->nbody; + case mjOBJ_BODY: + case mjOBJ_XBODY: + *mapadr -= m->nbody; + *padr = m->name_bodyadr; + num = m->nbody; + mjFALLTHROUGH; - case mjOBJ_JOINT: - *padr = m->name_jntadr; - return m->njnt; + case mjOBJ_JOINT: + *mapadr -= m->njnt; + if (num < 0) { + *padr = m->name_jntadr; + num = m->njnt; + } + mjFALLTHROUGH; - case mjOBJ_GEOM: - *padr = m->name_geomadr; - return m->ngeom; + case mjOBJ_GEOM: + *mapadr -= m->ngeom; + if (num < 0) { + *padr = m->name_geomadr; + num = m->ngeom; + } + mjFALLTHROUGH; - case mjOBJ_SITE: - *padr = m->name_siteadr; - return m->nsite; + case mjOBJ_SITE: + *mapadr -= m->nsite; + if (num < 0) { + *padr = m->name_siteadr; + num = m->nsite; + } + mjFALLTHROUGH; - case mjOBJ_CAMERA: - *padr = m->name_camadr; - return m->ncam; + case mjOBJ_CAMERA: + *mapadr -= m->ncam; + if (num < 0) { + *padr = m->name_camadr; + num = m->ncam; + } + mjFALLTHROUGH; - case mjOBJ_LIGHT: - *padr = m->name_lightadr; - return m->nlight; + case mjOBJ_LIGHT: + *mapadr -= m->nlight; + if (num < 0) { + *padr = m->name_lightadr; + num = m->nlight; + } + mjFALLTHROUGH; - case mjOBJ_MESH: - *padr = m->name_meshadr; - return m->nmesh; + case mjOBJ_MESH: + *mapadr -= m->nmesh; + if (num < 0) { + *padr = m->name_meshadr; + num = m->nmesh; + } + mjFALLTHROUGH; - case mjOBJ_SKIN: - *padr = m->name_skinadr; - return m->nskin; + case mjOBJ_SKIN: + *mapadr -= m->nskin; + if (num < 0) { + *padr = m->name_skinadr; + num = m->nskin; + } + mjFALLTHROUGH; - case mjOBJ_HFIELD: - *padr = m->name_hfieldadr; - return m->nhfield; + case mjOBJ_HFIELD: + *mapadr -= m->nhfield; + if (num < 0) { + *padr = m->name_hfieldadr; + num = m->nhfield; + } + mjFALLTHROUGH; - case mjOBJ_TEXTURE: - *padr = m->name_texadr; - return m->ntex; + case mjOBJ_TEXTURE: + *mapadr -= m->ntex; + if (num < 0) { + *padr = m->name_texadr; + num = m->ntex; + } + mjFALLTHROUGH; - case mjOBJ_MATERIAL: - *padr = m->name_matadr; - return m->nmat; + case mjOBJ_MATERIAL: + *mapadr -= m->nmat; + if (num < 0) { + *padr = m->name_matadr; + num = m->nmat; + } + mjFALLTHROUGH; - case mjOBJ_PAIR: - *padr = m->name_pairadr; - return m->npair; + case mjOBJ_PAIR: + *mapadr -= m->npair; + if (num < 0) { + *padr = m->name_pairadr; + num = m->npair; + } + mjFALLTHROUGH; - case mjOBJ_EXCLUDE: - *padr = m->name_excludeadr; - return m->nexclude; + case mjOBJ_EXCLUDE: + *mapadr -= m->nexclude; + if (num < 0) { + *padr = m->name_excludeadr; + num = m->nexclude; + } + mjFALLTHROUGH; - case mjOBJ_EQUALITY: - *padr = m->name_eqadr; - return m->neq; + case mjOBJ_EQUALITY: + *mapadr -= m->neq; + if (num < 0) { + *padr = m->name_eqadr; + num = m->neq; + } + mjFALLTHROUGH; - case mjOBJ_TENDON: - *padr = m->name_tendonadr; - return m->ntendon; + case mjOBJ_TENDON: + *mapadr -= m->ntendon; + if (num < 0) { + *padr = m->name_tendonadr; + num = m->ntendon; + } + mjFALLTHROUGH; - case mjOBJ_ACTUATOR: - *padr = m->name_actuatoradr; - return m->nu; + case mjOBJ_ACTUATOR: + *mapadr -= m->nu; + if (num < 0) { + *padr = m->name_actuatoradr; + num = m->nu; + } + mjFALLTHROUGH; - case mjOBJ_SENSOR: - *padr = m->name_sensoradr; - return m->nsensor; + case mjOBJ_SENSOR: + *mapadr -= m->nsensor; + if (num < 0) { + *padr = m->name_sensoradr; + num = m->nsensor; + } + mjFALLTHROUGH; - case mjOBJ_NUMERIC: - *padr = m->name_numericadr; - return m->nnumeric; + case mjOBJ_NUMERIC: + *mapadr -= m->nnumeric; + if (num < 0) { + *padr = m->name_numericadr; + num = m->nnumeric; + } + mjFALLTHROUGH; - case mjOBJ_TEXT: - *padr = m->name_textadr; - return m->ntext; + case mjOBJ_TEXT: + *mapadr -= m->ntext; + if (num < 0) { + *padr = m->name_textadr; + num = m->ntext; + } + mjFALLTHROUGH; - case mjOBJ_TUPLE: - *padr = m->name_tupleadr; - return m->ntuple; + case mjOBJ_TUPLE: + *mapadr -= m->ntuple; + if (num < 0) { + *padr = m->name_tupleadr; + num = m->ntuple; + } + mjFALLTHROUGH; - case mjOBJ_KEY: - *padr = m->name_keyadr; - return m->nkey; + case mjOBJ_KEY: + *mapadr -= m->nkey; + if (num < 0) { + *padr = m->name_keyadr; + num = m->nkey; + } + mjFALLTHROUGH; - case mjOBJ_PLUGIN: - *padr = m->name_pluginadr; - return m->nplugin; + case mjOBJ_PLUGIN: + *mapadr -= m->nplugin; + if (num < 0) { + *padr = m->name_pluginadr; + num = m->nplugin; + } + mjFALLTHROUGH; - default: - *padr = 0; - return 0; + default: + if (num < 0) { + *padr = 0; + num = 0; + } } + return num; } +// get string hash, see http://www.cse.yorku.ca/~oz/hash.html +uint64_t mj_hashdjb2(const char* s, uint64_t n) { + uint64_t h = 5381; + int c; + while ((c = *s++)) { + h = ((h << 5) + h) + c; + } + return h % n; +} // get id of object with specified name; -1: not found int mj_name2id(const mjModel* m, int type, const char* name) { - int num = 0; + int mapadr; int* adr = 0; // get number of objects and name addresses - num = _getnumadr(m, type, &adr); + int num = _getnumadr(m, type, &adr, &mapadr); // search if (num) { - for (int i=0; inames+adr[i], m->nnames-adr[i])) { - return i; - } - } - } + // look up at hash address + uint64_t hash = mj_hashdjb2(name, num); + uint64_t i = hash; + do { + int j = m->names_map[mapadr + i]; + if (j < 0) return -1; + if (!strncmp(name, m->names+adr[j], m->nnames-adr[j])) { + return j; + } + if (++i == num) i = 0; + } while(i != hash); + } return -1; } @@ -557,11 +666,11 @@ int mj_name2id(const mjModel* m, int type, const char* name) { // get name of object with specified id; 0: invalid type or id, or null name const char* mj_id2name(const mjModel* m, int type, int id) { - int num = 0; + int mapadr; int* adr = 0; // get number of objects and name addresses - num = _getnumadr(m, type, &adr); + int num = _getnumadr(m, type, &adr, &mapadr); if (id>=0 && idnames[adr[id]]) { diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index c1e9e548..b53f5c99 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -78,6 +78,9 @@ MJAPI int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, //-------------------------- name functions -------------------------------------------------------- +// get string hash, see http://www.cse.yorku.ca/~oz/hash.html +uint64_t mj_hashdjb2(const char* s, uint64_t n); + // get id of object with specified name; -1: not found; type is mjtObj MJAPI int mj_name2id(const mjModel* m, int type, const char* name); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index f93f70a5..1c1d622e 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1201,7 +1201,21 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) { // process names from one list: concatenate, compute addresses template -static int namelist(vector& list, int adr, int* name_adr, char* names) { +static int namelist(vector& list, int adr, int* name_adr, char* names, int* map) { + // compute hash map addresses + for (unsigned int i=0; iname.empty()) { + continue; + } + + uint64_t j = mj_hashdjb2(list[i]->name.c_str(), list.size()); + + // find first empty slot using linear probing + for (; map[j]!=-1; j=(j+1) % list.size()) {} + map[j] = i; + } + for (unsigned int i=0; i& list, int adr, int* name_adr, char* names) { void mjCModel::CopyNames(mjModel* m) { // start with model name int adr = (int)modelname.size()+1; + int* map_adr = m->names_map; mju_strncpy(m->names, modelname.c_str(), m->nnames); + memset(m->names_map, -1, sizeof(int) * m->nnames_map); // process all lists - adr = namelist(bodies, adr, m->name_bodyadr, m->names); - adr = namelist(joints, adr, m->name_jntadr, m->names); - adr = namelist(geoms, adr, m->name_geomadr, m->names); - adr = namelist(sites, adr, m->name_siteadr, m->names); - adr = namelist(cameras, adr, m->name_camadr, m->names); - adr = namelist(lights, adr, m->name_lightadr, m->names); - adr = namelist(meshes, adr, m->name_meshadr, m->names); - adr = namelist(skins, adr, m->name_skinadr, m->names); - adr = namelist(hfields, adr, m->name_hfieldadr, m->names); - adr = namelist(textures, adr, m->name_texadr, m->names); - adr = namelist(materials, adr, m->name_matadr, m->names); - adr = namelist(pairs, adr, m->name_pairadr, m->names); - adr = namelist(excludes, adr, m->name_excludeadr, m->names); - adr = namelist(equalities, adr, m->name_eqadr, m->names); - adr = namelist(tendons, adr, m->name_tendonadr, m->names); - adr = namelist(actuators, adr, m->name_actuatoradr, m->names); - adr = namelist(sensors, adr, m->name_sensoradr, m->names); - adr = namelist(numerics, adr, m->name_numericadr, m->names); - adr = namelist(texts, adr, m->name_textadr, m->names); - adr = namelist(tuples, adr, m->name_tupleadr, m->names); - adr = namelist(keys, adr, m->name_keyadr, m->names); - adr = namelist(plugins, adr, m->name_pluginadr, m->names); + adr = namelist(bodies, adr, m->name_bodyadr, m->names, map_adr); + map_adr += bodies.size(); + + adr = namelist(joints, adr, m->name_jntadr, m->names, map_adr); + map_adr += joints.size(); + + adr = namelist(geoms, adr, m->name_geomadr, m->names, map_adr); + map_adr += geoms.size(); + + adr = namelist(sites, adr, m->name_siteadr, m->names, map_adr); + map_adr += sites.size(); + + adr = namelist(cameras, adr, m->name_camadr, m->names, map_adr); + map_adr += cameras.size(); + + adr = namelist(lights, adr, m->name_lightadr, m->names, map_adr); + map_adr += lights.size(); + + adr = namelist(meshes, adr, m->name_meshadr, m->names, map_adr); + map_adr += meshes.size(); + + adr = namelist(skins, adr, m->name_skinadr, m->names, map_adr); + map_adr += skins.size(); + + adr = namelist(hfields, adr, m->name_hfieldadr, m->names, map_adr); + map_adr += hfields.size(); + + adr = namelist(textures, adr, m->name_texadr, m->names, map_adr); + map_adr += textures.size(); + + adr = namelist(materials, adr, m->name_matadr, m->names, map_adr); + map_adr += materials.size(); + + adr = namelist(pairs, adr, m->name_pairadr, m->names, map_adr); + map_adr += pairs.size(); + + adr = namelist(excludes, adr, m->name_excludeadr, m->names, map_adr); + map_adr += excludes.size(); + + adr = namelist(equalities, adr, m->name_eqadr, m->names, map_adr); + map_adr += equalities.size(); + + adr = namelist(tendons, adr, m->name_tendonadr, m->names, map_adr); + map_adr += tendons.size(); + + adr = namelist(actuators, adr, m->name_actuatoradr, m->names, map_adr); + map_adr += actuators.size(); + + adr = namelist(sensors, adr, m->name_sensoradr, m->names, map_adr); + map_adr += sensors.size(); + + adr = namelist(numerics, adr, m->name_numericadr, m->names, map_adr); + map_adr += numerics.size(); + + adr = namelist(texts, adr, m->name_textadr, m->names, map_adr); + map_adr += texts.size(); + + adr = namelist(tuples, adr, m->name_tupleadr, m->names, map_adr); + map_adr += tuples.size(); + + adr = namelist(keys, adr, m->name_keyadr, m->names, map_adr); + map_adr += keys.size(); + + adr = namelist(plugins, adr, m->name_pluginadr, m->names, map_adr); // check size, SHOULD NOT OCCUR if (adr != nnames) { diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 25e5fd36..5b351edd 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -155,6 +155,114 @@ TEST_F(JacobianTest, SubtreeJacNoInternalAcc) { mj_deleteModel(model); } +using Name2idTest = MujocoTest; + +static constexpr char name2idTestingModel[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +TEST_F(Name2idTest, FindIds) { + mjModel* model = LoadModelFromString(name2idTestingModel); + + EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1); + EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2); + EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1); + EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1); + EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1); + EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0); + EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0); + + mj_deleteModel(model); +} + +TEST_F(Name2idTest, MissingIds) { + mjModel* model = LoadModelFromString(name2idTestingModel); + + EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1); + EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1); + + mj_deleteModel(model); +} + +TEST_F(Name2idTest, EmptyIds) { + mjModel* model = LoadModelFromString(name2idTestingModel); + + EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1); + + mj_deleteModel(model); +} + +TEST_F(Name2idTest, Namespaces) { + mjModel* model = LoadModelFromString(name2idTestingModel); + + EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3); + + mj_deleteModel(model); +} + using VersionTest = MujocoTest; TEST_F(VersionTest, MjVersion) { diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 40be7f75..1fec67e2 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1911,6 +1911,7 @@ public unsafe struct mjModel_ { public int nuser_actuator; public int nuser_sensor; public int nnames; + public int nnames_map; public int nM; public int nD; public int nemax; @@ -2210,6 +2211,7 @@ public unsafe struct mjModel_ { public int* name_keyadr; public int* name_pluginadr; public char* names; + public int* names_map; } [StructLayout(LayoutKind.Sequential)]