From edbdb5195cf9ef174ac0f78ca42540f726dda05a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 1 Oct 2025 07:57:17 -0700 Subject: [PATCH] Change MuJoCo engine source code function-spacing convention from 3 blank lines to 2 PiperOrigin-RevId: 813754244 Change-Id: I6836e41c3b021cb727e922c25c60f629b9814c93 --- STYLEGUIDE.md | 10 +++-- src/engine/engine_collision_box.c | 3 -- src/engine/engine_collision_convex.c | 34 ----------------- src/engine/engine_collision_driver.c | 31 --------------- src/engine/engine_collision_gjk.c | 51 ------------------------- src/engine/engine_collision_primitive.c | 14 ------- src/engine/engine_collision_sdf.c | 1 - src/engine/engine_core_constraint.c | 34 ----------------- src/engine/engine_core_smooth.c | 21 ---------- src/engine/engine_core_util.c | 21 ---------- src/engine/engine_derivative.c | 29 -------------- src/engine/engine_derivative_fd.c | 11 ------ src/engine/engine_forward.c | 23 ----------- src/engine/engine_init.c | 6 --- src/engine/engine_inverse.c | 6 --- src/engine/engine_io.c | 31 --------------- src/engine/engine_island.c | 7 ---- src/engine/engine_memory.c | 11 ------ src/engine/engine_name.c | 1 - src/engine/engine_passive.c | 9 ----- src/engine/engine_print.c | 9 ----- src/engine/engine_ray.c | 19 --------- src/engine/engine_sensor.c | 11 ------ src/engine/engine_setconst.c | 6 --- src/engine/engine_solver.c | 25 ------------ src/engine/engine_support.c | 22 ----------- src/engine/engine_util_blas.c | 43 --------------------- src/engine/engine_util_container.c | 3 -- src/engine/engine_util_errmem.c | 5 --- src/engine/engine_util_misc.c | 50 ------------------------ src/engine/engine_util_solve.c | 19 --------- src/engine/engine_util_sparse.c | 16 -------- src/engine/engine_util_spatial.c | 25 ------------ src/engine/engine_vis_init.c | 8 ---- src/engine/engine_vis_interact.c | 14 ------- src/engine/engine_vis_visualize.c | 30 +-------------- 36 files changed, 8 insertions(+), 651 deletions(-) diff --git a/STYLEGUIDE.md b/STYLEGUIDE.md index dfecb4ab..85d598e1 100644 --- a/STYLEGUIDE.md +++ b/STYLEGUIDE.md @@ -6,7 +6,8 @@ possible in your code contributions. ### Scope of this guide -Most of this guide involves C/C++ code. For Python, jump to the section [below](#python-code). For MuJoCo C/C++, code has three main categories: +Most of this guide involves C/C++ code. For Python, jump to the section +[below](#python-code). For MuJoCo C/C++, code has three main categories: 1. **C code:** MuJoCo's core codebase. It consists of public headers under `include/` and C source files and internal headers under `src/`. This style @@ -144,7 +145,7 @@ assignments: variable initialisation in `for` loops. For example, inspect the `mju_transpose` implementation above. -- Three blank lines are required between function implementations in source files. +- Two blank lines are required between function implementations in source files. #### Variable declarations @@ -161,4 +162,7 @@ helping us to complete the migration are very welcome. ### [Python code](#python-code) -For Python code, run `pyink foo.py` to adhere to Google's [Python style guide](https://google.github.io/styleguide/pyguide.html). For sorting and cleaning imports, run `isort foo.py`. Both `pyink` and `isort` can be pip installed via `pip install pyink isort`. +For Python code, run `pyink foo.py` to adhere to Google's +[Python style guide](https://google.github.io/styleguide/pyguide.html). For +sorting and cleaning imports, run `isort foo.py`. Both `pyink` and `isort` can +be pip installed via `pip install pyink isort`. diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index b6d954fc..1a784dd6 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -91,7 +91,6 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin, } - // sphere : box int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) @@ -102,7 +101,6 @@ int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con, } - /* GENERAL THEORY OF OPERATION the following code is mostly for finding (line segment)/(box) collision after which box-sphere is called @@ -601,7 +599,6 @@ int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con, } - // box : box int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, mjtNum margin) { diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index cb0bbee6..60e68dba 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -84,7 +84,6 @@ static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, } - // ccd center function void mjccd_center(const void *obj, ccd_vec3_t *center) { mjc_center(center->v, (const mjCCDObj*) obj); @@ -114,7 +113,6 @@ void mjc_center(mjtNum res[3], const mjCCDObj *obj) { } - // prism center function static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) { // compute mean @@ -126,7 +124,6 @@ static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) { } - // ccd prism center function static void mjccd_prism_center(const void *obj, ccd_vec3_t *center) { mjc_prism_center(center->v, (const mjCCDObj*) obj); @@ -143,7 +140,6 @@ static inline void mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum } - // transform a vector from local to global frame static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNum dir[3], const mjtNum pos[3]) { @@ -157,7 +153,6 @@ static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNu } - // point support function void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjtNum* pos = obj->data->geom_xpos + 3*obj->geom; @@ -167,7 +162,6 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { } - // sphere support function static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -183,7 +177,6 @@ static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) } - // line support function (capsule) void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -208,7 +201,6 @@ void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { } - // capsule support function static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -238,7 +230,6 @@ static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3] } - // ellipsoid support function static void mjc_ellipsoidSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -278,7 +269,6 @@ static void mjc_ellipsoidSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[ } - // cylinder support function static void mjc_cylinderSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -311,7 +301,6 @@ static void mjc_cylinderSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3 } - // box support function static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -343,14 +332,12 @@ static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { } - // dot product between mjtNum and float static inline mjtNum dot3f(const mjtNum a[3], const float b[3]) { return a[0]*(mjtNum)b[0] + a[1]*(mjtNum)b[1] + a[2]*(mjtNum)b[2]; } - // mesh support function via exhaustive search static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -398,7 +385,6 @@ static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { } - // mesh support function via hill climbing static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -449,7 +435,6 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[ } - // prism support function static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { int istart, ibest; @@ -471,7 +456,6 @@ static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) } - // flex support function static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; @@ -512,7 +496,6 @@ static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { } - // libccd support function void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjCCDObj *obj = (mjCCDObj *)_obj; @@ -720,7 +703,6 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } - // libccd prism support function static void mjccd_prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) { mjc_prism_support(vec->v, (mjCCDObj*) obj, dir->v); @@ -784,7 +766,6 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt } - // set flex data for CCD object static void mjc_setCCDObjFlex(mjCCDObj* obj, int flex, int elem, int vert) { obj->flex = flex; @@ -793,7 +774,6 @@ static void mjc_setCCDObjFlex(mjCCDObj* obj, int flex, int elem, int vert) { } - // initialize CCD structure static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { CCD_INIT(ccd); @@ -803,7 +783,6 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { } - // find convex-convex collision static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con, int max_contacts, mjtNum margin) { @@ -873,7 +852,6 @@ static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, m } - // compare new contact to previous contacts, return 1 if it is far from all of them static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) { for (int i=0; i < ncon-1; i++) { @@ -885,7 +863,6 @@ static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) { } - // in-place rotation of spatial frame around given point of origin static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], mjtNum xmat[9], mjtNum xpos[3]) { @@ -907,7 +884,6 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], } - // return number of contacts supported by a single pass of narrowphase static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) { const mjModel* m = obj1->model; @@ -936,7 +912,6 @@ static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) { } - // multi-point convex-convex collision, using libccd int mjc_Convex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -1024,7 +999,6 @@ int mjc_Convex(const mjModel* m, const mjData* d, } - // parameters for plane-mesh extra contacts const int maxplanemesh = 3; const mjtNum tolplanemesh = 0.3; @@ -1063,7 +1037,6 @@ static int addplanemesh(mjContact* con, const float vertex[3], } - // plane-convex collision, using libccd int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -1164,7 +1137,6 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, } - //---------------------------- heightfield collisions --------------------------------------------- // ccd prism first dir @@ -1191,7 +1163,6 @@ static void addVert(int* nvert, mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) { } - // entry point for heightfield collisions int mjc_ConvexHField(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -1379,7 +1350,6 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, } - //--------------------------- fix contact frame normal --------------------------------------------- // compute normal for point outside ellipsoid, using ray-projection SQP @@ -1437,7 +1407,6 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum } - // compute normal for point inside ellipsoid, using diagonal QCQP static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum size[3]) { // algorithm constants @@ -1487,7 +1456,6 @@ static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum } - // entry point void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2) { mjtNum dst1, dst2; @@ -1637,7 +1605,6 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in } - //---------------------------- flex collisions --------------------------------------------- // geom-elem or elem-elem or vert-elem convex collision using ccd @@ -1654,7 +1621,6 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, } - // test a height field and a flex element for collision int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, int g, int f, int e, mjtNum margin) { diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index bb5976eb..f474bce5 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -67,7 +67,6 @@ static inline void resetArena(mjData* d) { } - // plane to geom_center squared distance, g1 is a plane static mjtNum planeGeomDist(const mjModel* m, mjData* d, int g1, int g2) { mjtNum* mat1 = d->geom_xmat + 9*g1; @@ -79,7 +78,6 @@ static mjtNum planeGeomDist(const mjModel* m, mjData* d, int g1, int g2) { } - // return 1 if body has plane geom, 0 otherwise static int hasPlane(const mjModel* m, int body) { int start = m->body_geomadr[body]; @@ -97,7 +95,6 @@ static int hasPlane(const mjModel* m, int body) { } - // filter contact based on type and affinity static int filterBitmask(int contype1, int conaffinity1, int contype2, int conaffinity2) { @@ -105,7 +102,6 @@ static int filterBitmask(int contype1, int conaffinity1, } - // filter contact based on global AABBs static int filterBox(const mjtNum aabb1[6], const mjtNum aabb2[6], mjtNum margin) { if (aabb1[0]+aabb1[3]+margin < aabb2[0]-aabb2[3]) return 1; @@ -118,7 +114,6 @@ static int filterBox(const mjtNum aabb1[6], const mjtNum aabb2[6], mjtNum margin } - // filter contact based sphere-box test, treating sphere as box static int filterSphereBox(const mjtNum s[3], mjtNum bound, const mjtNum aabb[6]) { if (s[0]+bound < aabb[0]-aabb[3]) return 1; @@ -131,7 +126,6 @@ static int filterSphereBox(const mjtNum s[3], mjtNum bound, const mjtNum aabb[6] } - // filter contact based on bounding sphere test (raw) static int filterSphere(const mjtNum pos1[3], const mjtNum pos2[3], mjtNum bound) { mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]}; @@ -141,7 +135,6 @@ static int filterSphere(const mjtNum pos1[3], const mjtNum pos2[3], mjtNum bound } - // filter contact based on bounding sphere test static int mj_filterSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) { // neither geom is a plane @@ -163,7 +156,6 @@ static int mj_filterSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum m } - // filter body pair: 1- discard, 0- proceed static int filterBodyPair(int weldbody1, int weldparent1, int weldbody2, int weldparent2, int dsbl_filterparent) { @@ -183,7 +175,6 @@ static int filterBodyPair(int weldbody1, int weldparent1, int weldbody2, } - // return 1 if bodyflex can collide, 0 otherwise static int canCollide(const mjModel* m, int bf) { if (bf < m->nbody) { @@ -195,7 +186,6 @@ static int canCollide(const mjModel* m, int bf) { } - // return 1 if two bodyflexes can collide, 0 otherwise static int canCollide2(const mjModel* m, int bf1, int bf2) { int nbody = m->nbody; @@ -209,7 +199,6 @@ static int canCollide2(const mjModel* m, int bf1, int bf2) { } - // return 1 if element is active, 0 otherwise int mj_isElemActive(const mjModel* m, int f, int e) { if (m->flex_dim[f] < 3) { @@ -220,7 +209,6 @@ int mj_isElemActive(const mjModel* m, int f, int e) { } - //----------------------------- collision detection entry point ------------------------------------ // compare contact pairs by their geom/elem/vert IDs @@ -483,7 +471,6 @@ void mj_collision(const mjModel* m, mjData* d) { } - //------------------------------------ binary tree search ------------------------------------------ // collision tree node @@ -522,7 +509,6 @@ void mj_collideGeomPair(const mjModel* m, mjData* d, int g1, int g2, int merged, } - // oriented bounding boxes collision (see Gottschalk et al.) int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], const mjtNum xpos1[3], const mjtNum xmat1[9], @@ -621,7 +607,6 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], } - // binary search between two bodyflex trees void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, int merged, int startadr, int pairadr) { @@ -856,7 +841,6 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, } - //----------------------------- broad-phase collision detection ------------------------------------ // make AAMM (xmin[3], xmax[3]) for one bodyflex @@ -929,7 +913,6 @@ static void makeAAMM(const mjModel* m, mjData* d, mjtNum* aamm, int bf, const mj } - // add bodyflex pair in buffer; do not filter if m is NULL static void add_pair(const mjModel* m, int bf1, int bf2, int* npair, int* pair, int maxpair) { @@ -987,7 +970,6 @@ static void add_pair(const mjModel* m, int bf1, int bf2, } - //----------------------------- general Sweep and Prune algorithm ---------------------------------- // helper structure for SAP sorting @@ -1103,7 +1085,6 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int } - // add vector to covariance static void updateCov(mjtNum cov[9], const mjtNum vec[3], const mjtNum cen[3]) { mjtNum dif[3] = {vec[0]-cen[0], vec[1]-cen[1], vec[2]-cen[2]}; @@ -1125,7 +1106,6 @@ static void updateCov(mjtNum cov[9], const mjtNum vec[3], const mjtNum cen[3]) { } - // comparison function for unsigned ints static inline int uintcmp(int* i, int* j, void* context) { if ((unsigned) *i < (unsigned) *j) { @@ -1279,7 +1259,6 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { } - //----------------------------- narrow-phase collision detection ----------------------------------- // compute contact condim, gap, solref, solimp, friction @@ -1379,7 +1358,6 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, } - // set contact parameters static void mj_setContact(const mjModel* m, mjContact* con, int condim, mjtNum includemargin, @@ -1410,7 +1388,6 @@ static void mj_setContact(const mjModel* m, mjContact* con, } - // make capsule from two flex vertices static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2], mjtNum pos[3], mjtNum mat[9], mjtNum size[2]) { @@ -1432,7 +1409,6 @@ static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2], } - // test two geoms for collision, apply filters, add to contact list void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { int num, type1, type2, condim; @@ -1629,7 +1605,6 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { } - // test a plane geom and a flex for collision, add to contact list void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { mjContact con; @@ -1686,7 +1661,6 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { } - // test single triangle plane : vertex static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad, int t0, int t1, int t2, int v) { @@ -1717,7 +1691,6 @@ static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad, } - // test for internal flex collisions, add to contact list // ignore margin to avoid permament self-collision void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { @@ -1782,7 +1755,6 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { } - // test active element self-collisions with SAP // ignore margin to avoid permanent self-collision void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { @@ -1835,7 +1807,6 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { } - // test a geom and an elem for collision, add to contact list void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f])); @@ -1954,7 +1925,6 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { } - // test two elems for collision, add to contact list void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2) { mjtNum margin = mj_assignMargin(m, mju_max(m->flex_margin[f1], m->flex_margin[f2])); @@ -2054,7 +2024,6 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 } - // test element and vertex for collision, add to contact list void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { mjtNum margin = mj_assignMargin(m, m->flex_margin[f]); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 6e86e5df..51aefed1 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -166,7 +166,6 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { } - // GJK algorithm static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact @@ -296,7 +295,6 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { } - // compute the support point in obj1 and obj2 for Minkowski difference static inline void support(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3], const mjtNum dir_neg[3]) { @@ -327,7 +325,6 @@ static inline void support(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2, } - // compute the support points in obj1 and obj2 for the kth approximation point static inline void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum x_k[3], mjtNum x_norm) { @@ -340,7 +337,6 @@ static inline void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2, } - // compute support points in Minkowski difference, return index of new vertex in polytope static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3], mjtNum dnorm) { @@ -361,7 +357,6 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, } - // compute the support point in the Minkowski difference for gjkIntersect (without normalization) static void gjkIntersectSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3]) { @@ -370,7 +365,6 @@ static void gjkIntersectSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2, } - // compute the signed distance of a face along with the normal static inline mjtNum signedDistance(mjtNum normal[3], const Vertex* v1, const Vertex* v2, const Vertex* v3) { @@ -387,7 +381,6 @@ static inline mjtNum signedDistance(mjtNum normal[3], const Vertex* v1, const Ve } - // return 1 if objects are in contact; 0 if not; -1 if inconclusive static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { Vertex simplex[4] = {status->simplex[0], status->simplex[1], @@ -447,7 +440,6 @@ static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { } - // linear combination of n 3D vectors static inline void lincomb(mjtNum res[3], const mjtNum* coef, int n, const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum v4[3]) { @@ -476,7 +468,6 @@ static inline void lincomb(mjtNum res[3], const mjtNum* coef, int n, const mjtNu } - // res = origin projected onto plane defined by v1, v2, v3 static int projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { @@ -514,7 +505,6 @@ static int projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2 } - // res = origin projected onto line defined by v1, v2 static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { // res = v2 - / * (v2 - v1) @@ -527,7 +517,6 @@ static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mj } - // return 1 if both numbers are positive, -1 if both negative and 0 otherwise static inline int sameSign2(mjtNum a, mjtNum b) { if (a > 0 && b > 0) return 1; @@ -536,7 +525,6 @@ static inline int sameSign2(mjtNum a, mjtNum b) { } - // subdistance algorithm for GJK that computes the barycentric coordinates of the point in a // simplex closest to the origin // implementation adapted from Montanari et al, ToG 2017 @@ -564,7 +552,6 @@ static inline void subdistance(mjtNum lambda[4], int n, const Vertex simplex[4]) } - static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3], const mjtNum s4[3]) { // the matrix M is given by @@ -656,7 +643,6 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], } - static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]) { // project origin onto affine hull of the simplex mjtNum p_o[3]; @@ -790,7 +776,6 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const } - static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) { // find projection of origin onto the 1-simplex: mjtNum p_o[3]; @@ -840,7 +825,6 @@ static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, in } - // return 1 if the origin and p3 are on the same side of the plane defined by p0, p1, p2 static int sameSide(const mjtNum p0[3], const mjtNum p1[3], const mjtNum p2[3], const mjtNum p3[3]) { @@ -860,7 +844,6 @@ static int sameSide(const mjtNum p0[3], const mjtNum p1[3], } - // return 1 if the origin is contained in the tetrahedron, 0 otherwise static int testTetra(const mjtNum p0[3], const mjtNum p1[3], const mjtNum p2[3], const mjtNum p3[3]) { @@ -871,7 +854,6 @@ static int testTetra(const mjtNum p0[3], const mjtNum p1[3], } - // matrix for 120 degrees rotation around given axis static void rotmat(mjtNum R[9], const mjtNum axis[3]) { mjtNum n = norm3(axis); @@ -890,7 +872,6 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) { } - // return nonzero if the ray v1v2 intersects the triangle v3v4v5 static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum v4[3], const mjtNum v5[3]) { @@ -910,7 +891,6 @@ static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtN } - // create a polytope from a 1-simplex (returns 0 on success) static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert; @@ -994,7 +974,6 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj } - // compute the affine coordinates of p on the triangle v1v2v3 static void triAffineCoord(mjtNum lambda[3], const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum p[3]) { @@ -1040,7 +1019,6 @@ static void triAffineCoord(mjtNum lambda[3], const mjtNum v1[3], const mjtNum v2 } - // return true if point p and triangle v1v2v3 intersect static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum p[3]) { @@ -1058,7 +1036,6 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu } - // create a polytope from a 2-simplex (returns 0 on success) static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // get vertices of simplex from GJK @@ -1138,7 +1115,6 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj } - // create a polytope from a 3-simplex (returns 0 on success) static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int v1 = insertVertex(pt, status->simplex + 0); @@ -1196,14 +1172,12 @@ static void deleteFace(Polytope* pt, Face* face) { } - // return max number of faces that can be stored in polytope static inline int maxFaces(Polytope* pt) { return pt->maxfaces - pt->nfaces; } - // attach a face to the polytope with the given vertex indices; return squared distance to origin static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3) { @@ -1229,7 +1203,6 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, } - // add an edge to the horizon static inline void addEdge(Polytope* pt, int index, int edge) { pt->horizon.edges[pt->horizon.nedges] = edge; @@ -1237,7 +1210,6 @@ static inline void addEdge(Polytope* pt, int index, int edge) { } - // get edge index where vertex lies static inline int getEdge(Face* face, int vertex) { if (face->verts[0] == vertex) return 0; @@ -1246,7 +1218,6 @@ static inline int getEdge(Face* face, int vertex) { } - // recursive call to build horizon; return 1 if face is visible from w otherwise 0 static int horizonRec(Polytope* pt, Face* face, int e) { // v is visible from w so it is deleted and adjacent faces are checked @@ -1270,7 +1241,6 @@ static int horizonRec(Polytope* pt, Face* face, int e) { } - // create horizon given the face as starting point static void horizon(Polytope* pt, Face* face) { deleteFace(pt, face); @@ -1298,7 +1268,6 @@ static void horizon(Polytope* pt, Face* face) { } - // recover witness points from EPA polytope static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNum x2[3]) { // compute affine coordinates for witness points on plane defined by face @@ -1326,7 +1295,6 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu } - // return a face of the expanded polytope that best approximates the pentration depth // witness points are in status->{x1, x2} static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { @@ -1493,7 +1461,6 @@ static inline mjtNum area4(const mjtNum a[3], const mjtNum b[3], } - // return pointer to next vertex in a polygon static inline mjtNum* next(mjtNum* polygon, int nvert, mjtNum* curr) { if (curr == polygon + 3*(nvert - 1)) { @@ -1503,7 +1470,6 @@ static inline mjtNum* next(mjtNum* polygon, int nvert, mjtNum* curr) { } - // prune a polygon to a maximum area convex quadrilateral static inline void polygonQuad(mjtNum* res[4], mjtNum* polygon, int nvert) { mjtNum* a = polygon, *b = polygon + 3, *c = polygon + 6, *d = polygon + 9; @@ -1551,7 +1517,6 @@ static inline void polygonQuad(mjtNum* res[4], mjtNum* polygon, int nvert) { } - // find the normal of a plane perpendicular to the face (given by its normal n) and intersecting the // face edge (v1, v2) static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], @@ -1565,7 +1530,6 @@ static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], } - // find what side of a plane a point p lies static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) { mjtNum diff[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]}; @@ -1573,7 +1537,6 @@ static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) { } - // compute the intersection of a plane with a line segment (a, b) static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd, const mjtNum a[3], const mjtNum b[3]) { @@ -1591,7 +1554,6 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd, } - // clip a polygon against another polygon static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, const mjtNum* face2, int nface2, const mjtNum n[3], @@ -1707,7 +1669,6 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, } - // compute global coordinates of a local point (l1, l2, l3) static inline void globalcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum pos[3], mjtNum l1, mjtNum l2, mjtNum l3) { @@ -1723,7 +1684,6 @@ static inline void globalcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum } - // find up to n <= 2 common integers of two arrays, return n static int intersect(int res[2], const int* arr1, const int* arr2, int n, int m) { int count = 0; @@ -1739,7 +1699,6 @@ static int intersect(int res[2], const int* arr1, const int* arr2, int n, int m) } - // compute possible polygon normals of a mesh given up to 3 vertices static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, int v1, int v2, int v3) { @@ -1810,7 +1769,6 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, } - // compute normal directional vectors along possible edges given by up to two vertices static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj, const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) { @@ -1865,7 +1823,6 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj } - // try recovering box normal from collision normal static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const mjtNum n[3]) { // list of box face normals @@ -1892,7 +1849,6 @@ static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const } - // compute possible face normals of a box given up to 3 vertices static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, int v1, int v2, int v3, const mjtNum dir[3]) { @@ -1947,7 +1903,6 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, } - // compute possible edge normals for box for edge collisions static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj* obj, const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) { @@ -2038,7 +1993,6 @@ static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) { } - // recover mesh polygon from its index, return number of edges static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) { const mjModel* m = obj->model; @@ -2063,7 +2017,6 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) { } - // find two normals that are facing each other within a tolerance, return 1 if found static inline int alignedFaces(int res[2], const mjtNum* v, int nv, const mjtNum* w, int nw) { @@ -2080,7 +2033,6 @@ static inline int alignedFaces(int res[2], const mjtNum* v, int nv, } - // find two normals that are perpendicular to each other within a tolerance, return 1 if found static inline int alignedFaceEdge(int res[2], const mjtNum* edge, int nedge, const mjtNum* face, int nface) { @@ -2115,7 +2067,6 @@ static inline int simplexDim(int* v1i, int* v2i, int* v3i, mjtNum** v1, mjtNum** } - // recover multiple contacts from EPA polytope static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { @@ -2242,7 +2193,6 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, - // inflate a contact by margin static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) { mjtNum n[3]; @@ -2262,7 +2212,6 @@ static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) } - // general convex collision detection mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // pre-allocate static memory for low iterations diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index 99c6567e..2530df77 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -49,7 +49,6 @@ static int mjraw_PlaneSphere(mjContact* con, mjtNum margin, } - // plane : sphere int mjc_PlaneSphere(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -58,7 +57,6 @@ int mjc_PlaneSphere(const mjModel* m, const mjData* d, } - // plane : capsule int mjc_PlaneCapsule(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -89,7 +87,6 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d, } - // plane : cylinder int mjc_PlaneCylinder(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -194,7 +191,6 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d, } - // plane : box int mjc_PlaneBox(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -242,7 +238,6 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d, } - //--------------------------- sphere and capsule collisions ---------------------------------------- // sphere : sphere (actual implementation, can be called with modified parameters) @@ -280,7 +275,6 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin, } - // sphere : sphere int mjc_SphereSphere(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -289,7 +283,6 @@ int mjc_SphereSphere(const mjModel* m, const mjData* d, } - // raw sphere : capsule int mjraw_SphereCapsule(mjContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, @@ -309,7 +302,6 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin, } - // sphere : capsule int mjc_SphereCapsule(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -318,7 +310,6 @@ int mjc_SphereCapsule(const mjModel* m, const mjData* d, } - // sphere : cylinder int mjc_SphereCylinder(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -392,7 +383,6 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d, } - // raw capsule : capsule int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, @@ -489,7 +479,6 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, } - // capsule : capsule int mjc_CapsuleCapsule(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { @@ -498,14 +487,12 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d, } - // sign of (signed) area of planar triangle static mjtNum areaSign(const mjtNum p1[2], const mjtNum p2[2], const mjtNum p3[2]) { return mju_sign((p1[0]-p3[0])*(p2[1]-p3[1]) - (p2[0]-p3[0])*(p1[1]-p3[1])); } - // find nearest point to p within line segment (u,v); return distance to p static mjtNum pointSegment(mjtNum res[2], const mjtNum p[2], const mjtNum u[2], const mjtNum v[2]) { @@ -532,7 +519,6 @@ static mjtNum pointSegment(mjtNum res[2], const mjtNum p[2], } - // sphere : triangle with radius int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* s, mjtNum rs, diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 830d407c..80b1536c 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -601,7 +601,6 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s, } - //------------------------------ collision functions ----------------------------------------------- // collision between a height field and a signed distance field diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 32683bfb..23fe51ad 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -45,7 +45,6 @@ //-------------------------- utility functions ----------------------------------------------------- - // allocate efc arrays on arena, return 1 on success, 0 on failure static int arenaAllocEfc(const mjModel* m, mjData* d) { #undef MJ_M @@ -83,7 +82,6 @@ static int arenaAllocEfc(const mjModel* m, mjData* d) { } - // determine type of solver int mj_isDual(const mjModel* m) { if (m->opt.solver == mjSOL_PGS || m->opt.noslip_iterations > 0) { @@ -94,7 +92,6 @@ int mj_isDual(const mjModel* m) { } - // assign/clamp contact friction parameters void mj_assignFriction(const mjModel* m, mjtNum* target, const mjtNum* source) { if (mjENABLED(mjENBL_OVERRIDE)) { @@ -110,7 +107,6 @@ void mj_assignFriction(const mjModel* m, mjtNum* target, const mjtNum* source) { - // assign/override contact reference parameters void mj_assignRef(const mjModel* m, mjtNum* target, const mjtNum* source) { if (mjENABLED(mjENBL_OVERRIDE)) { @@ -121,7 +117,6 @@ void mj_assignRef(const mjModel* m, mjtNum* target, const mjtNum* source) { } - // assign/override contact impedance parameters void mj_assignImp(const mjModel* m, mjtNum* target, const mjtNum* source) { if (mjENABLED(mjENBL_OVERRIDE)) { @@ -132,7 +127,6 @@ void mj_assignImp(const mjModel* m, mjtNum* target, const mjtNum* source) { } - // assign/override contact margin mjtNum mj_assignMargin(const mjModel* m, mjtNum source) { if (mjENABLED(mjENBL_OVERRIDE)) { @@ -143,7 +137,6 @@ mjtNum mj_assignMargin(const mjModel* m, mjtNum source) { } - // compute element bodies and weights for given contact point, return #bodies // if v is one of the element vertices, reduce element to fragment static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, int v, @@ -183,7 +176,6 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in } - // compute body weights for a given contact vertex, return #bodies static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v, const mjtNum point[3], int* body, mjtNum* weight, mjtNum bw) { @@ -207,7 +199,6 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v, } - // add contact to d->contact list; return 0 if success; 1 if buffer full 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 @@ -232,7 +223,6 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) { } - // add #size rows to constraint Jacobian; set pos, margin, frictionloss, type, id static void mj_addConstraint(const mjModel* m, mjData* d, const mjtNum* jac, const mjtNum* pos, @@ -328,7 +318,6 @@ static void mj_addConstraint(const mjModel* m, mjData* d, } - // multiply Jacobian by vector void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) { // exit if no constraints @@ -349,7 +338,6 @@ void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* } - // multiply JacobianT by vector void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) { // exit if no constraints @@ -370,7 +358,6 @@ void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* } - //--------------------- instantiate constraints by type -------------------------------------------- // equality constraints @@ -649,7 +636,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } - // frictional dofs and tendons void mj_instantiateFriction(const mjModel* m, mjData* d) { int nv = m->nv, issparse = mj_isSparse(m); @@ -705,7 +691,6 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { } - // joint and tendon limits void mj_instantiateLimit(const mjModel* m, mjData* d) { int side, nv = m->nv, issparse = mj_isSparse(m); @@ -845,7 +830,6 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { } - // compute Jacobian for contact, return number of DOFs affected int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim, mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp, @@ -1027,7 +1011,6 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { } - //------------------------ compute constraint parameters ------------------------------------------- // compute diagApprox @@ -1207,7 +1190,6 @@ void mj_diagApprox(const mjModel* m, mjData* d) { } - // get solref, solimp for specified constraint static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref, mjtNum* solreffriction, mjtNum* solimp) { @@ -1283,7 +1265,6 @@ static void getsolparam(const mjModel* m, const mjData* d, int i, } - // get pos and dim for specified constraint static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int* dim) { // get id of constraint-related object @@ -1319,7 +1300,6 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int } - // return a to the power of b, quick return for powers 1 and 2 // solimp[4] == 2 is the default, so these branches are common static mjtNum power(mjtNum a, mjtNum b) { @@ -1332,7 +1312,6 @@ static mjtNum power(mjtNum a, mjtNum b) { } - // compute impedance and derivative for one constraint static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, mjtNum* imp, mjtNum* impP) { @@ -1385,7 +1364,6 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, } - // compute efc_R, efc_D, efc_KBIP, adjust efc_diagApprox void mj_makeImpedance(const mjModel* m, mjData* d) { int dim, nefc = d->nefc; @@ -1504,7 +1482,6 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { } - //------------------------------------- constraint counting ---------------------------------------- // count the non-zero columns in the Jacobian difference of two bodies @@ -1525,7 +1502,6 @@ static int mj_jacDifPairCount(const mjModel* m, int* chain, } - // count the non-zero columns of the Jacobian returned by mj_jacSum static int mj_jacSumCount(const mjModel* m, mjData* d, int* chain, int n, const int* body) { @@ -1558,7 +1534,6 @@ static int mj_jacSumCount(const mjModel* m, mjData* d, int* chain, } - // return number of constraint non-zeros, handle dense and dof-less cases static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) { // over count for dense allocation @@ -1569,7 +1544,6 @@ static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) { } - // count equality constraints, count Jacobian nonzeros if nnz is not NULL static int mj_ne(const mjModel* m, mjData* d, int* nnz) { int ne = 0, nnze = 0; @@ -1707,7 +1681,6 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { } - // count frictional constraints, count Jacobian nonzeros if nnz is not NULL static int mj_nf(const mjModel* m, const mjData* d, int *nnz) { int nf = 0; @@ -1735,7 +1708,6 @@ static int mj_nf(const mjModel* m, const mjData* d, int *nnz) { } - // count limit constraints, count Jacobian nonzeros if nnz is not NULL static int mj_nl(const mjModel* m, const mjData* d, int *nnz) { int nl = 0; @@ -1802,7 +1774,6 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) { } - // count contact constraints, count Jacobian nonzeros if nnz is not NULL static int mj_nc(const mjModel* m, mjData* d, int* nnz) { int nnzc = 0, nc = 0; @@ -1901,7 +1872,6 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) { } - //---------------------------- top-level API for constraint construction --------------------------- // driver: call all functions above @@ -2005,7 +1975,6 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { } - // compute efc_AR void mj_projectConstraint(const mjModel* m, mjData* d) { int nefc = d->nefc, nv = m->nv; @@ -2243,7 +2212,6 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } - // compute efc_vel, efc_aref void mj_referenceConstraint(const mjModel* m, mjData* d) { int nefc = d->nefc; @@ -2260,7 +2228,6 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) { } - //---------------------------- update constraint state --------------------------------------------- // compute efc_state, efc_force @@ -2461,7 +2428,6 @@ void mj_constraintUpdate_impl(int ne, int nf, int nefc, } - // compute efc_state, efc_force, qfrc_constraint // optional: cost(qacc) = s_hat(jar) where jar = Jac*qacc-aref; cone Hessians void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 1e51fefe..65fec4e0 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -177,7 +177,6 @@ void mj_kinematics(const mjModel* m, mjData* d) { } - // map inertias and motion dofs to global frame centered at subtree-CoM void mj_comPos(const mjModel* m, mjData* d) { int nbody = m->nbody, njnt = m->njnt; @@ -260,7 +259,6 @@ void mj_comPos(const mjModel* m, mjData* d) { } - // compute camera and light positions and orientations void mj_camlight(const mjModel* m, mjData* d) { mjtNum pos[3], matT[9]; @@ -379,7 +377,6 @@ void mj_camlight(const mjModel* m, mjData* d) { } - // update dynamic BVH; leaf aabbs must be updated before call void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) { mj_markStack(d); @@ -427,7 +424,6 @@ void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) { } - // compute flex-related quantities void mj_flex(const mjModel* m, mjData* d) { int nv = m->nv, issparse = mj_isSparse(m); @@ -636,7 +632,6 @@ void mj_flex(const mjModel* m, mjData* d) { } - // compute tendon lengths and moments void mj_tendon(const mjModel* m, mjData* d) { int issparse = mj_isSparse(m), nv = m->nv, nten = m->ntendon; @@ -850,7 +845,6 @@ void mj_tendon(const mjModel* m, mjData* d) { } - // compute time derivative of dense tendon Jacobian for one tendon void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { int nv = m->nv; @@ -970,7 +964,6 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { } - // compute actuator/transmission lengths and moments void mj_transmission(const mjModel* m, mjData* d) { int nv = m->nv, nu = m->nu; @@ -1456,7 +1449,6 @@ void mj_transmission(const mjModel* m, mjData* d) { } - //-------------------------- inertia --------------------------------------------------------------- // add tendon armature to M @@ -1519,7 +1511,6 @@ void mj_tendonArmature(const mjModel* m, mjData* d) { } - // composite rigid body inertia algorithm void mj_crb(const mjModel* m, mjData* d) { int nv = m->nv; @@ -1570,7 +1561,6 @@ void mj_crb(const mjModel* m, mjData* d) { } - void mj_makeM(const mjModel* m, mjData* d) { TM_START; mj_crb(m, d); @@ -1580,7 +1570,6 @@ void mj_makeM(const mjModel* m, mjData* d) { } - // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd // (legacy implementation) void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, @@ -1644,7 +1633,6 @@ void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD } - // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd void mj_factorM(const mjModel* m, mjData* d) { TM_START; @@ -1654,7 +1642,6 @@ void mj_factorM(const mjModel* m, mjData* d) { } - // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv, const int* rownnz, const int* rowadr, const int* colind) { @@ -1680,7 +1667,6 @@ void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv, } - // in-place sparse backsubstitution: x = inv(L'*D*L)*x // (legacy implementation) void mj_solveLD_legacy(const mjModel* m, mjtNum* restrict x, int n, @@ -1793,7 +1779,6 @@ void mj_solveLD_legacy(const mjModel* m, mjtNum* restrict x, int n, } - // in-place sparse backsubstitution: x = inv(L'*D*L)*x void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n, const int* rownnz, const int* rowadr, const int* colind) { @@ -1875,7 +1860,6 @@ void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv, } - // sparse backsubstitution: x = inv(L'*D*L)*y // use factorization in d void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { @@ -1887,7 +1871,6 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) { } - // half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, const mjtNum* sqrtInvD, int n) { @@ -1935,7 +1918,6 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, } - //---------------------------------- velocity ------------------------------------------------------ // compute cvel, cdof_dot @@ -2007,7 +1989,6 @@ void mj_comVel(const mjModel* m, mjData* d) { } - // subtree linear velocity and angular momentum void mj_subtreeVel(const mjModel* m, mjData* d) { int nbody = m->nbody; @@ -2130,7 +2111,6 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { } - // RNE with complete data: compute cacc, cfrc_ext, cfrc_int void mj_rnePostConstraint(const mjModel* m, mjData* d) { int nbody = m->nbody; @@ -2318,7 +2298,6 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { } - // add bias force due to tendon armature void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { int ntendon = m->ntendon, nv = m->nv, issparse = mj_isSparse(m); diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index 8aeedba7..b846e609 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -48,7 +48,6 @@ int mj_isPyramidal(const mjModel* m) { } - //-------------------------- sparse chains --------------------------------------------------------- // merge dof chains for two bodies @@ -95,7 +94,6 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) { } - // merge dof chains for two simple bodies int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) { // swap bodies if wrong order @@ -127,7 +125,6 @@ int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) { } - // get body chain int mj_bodyChain(const mjModel* m, int body, int* chain) { // simple body @@ -173,7 +170,6 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) { } - //-------------------------- Jacobians ------------------------------------------------------------- // compute 3/6-by-nv Jacobian of global point attached to given body @@ -230,21 +226,18 @@ void mj_jac(const mjModel* m, const mjData* d, } - // compute body Jacobian void mj_jacBody(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body) { mj_jac(m, d, jacp, jacr, d->xpos+3*body, body); } - // compute body-com Jacobian void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body) { mj_jac(m, d, jacp, jacr, d->xipos+3*body, body); } - // compute subtree-com Jacobian void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { int nv = m->nv; @@ -273,21 +266,18 @@ void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { } - // compute geom Jacobian void mj_jacGeom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int geom) { mj_jac(m, d, jacp, jacr, d->geom_xpos + 3*geom, m->geom_bodyid[geom]); } - // compute site Jacobian void mj_jacSite(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int site) { mj_jac(m, d, jacp, jacr, d->site_xpos + 3*site, m->site_bodyid[site]); } - // compute translation Jacobian of point, and rotation Jacobian of axis void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacAxis, const mjtNum point[3], const mjtNum axis[3], int body) { @@ -312,7 +302,6 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA } - // compute 3/6-by-nv sparse Jacobian of global point attached to given body void mj_jacSparse(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body, @@ -382,7 +371,6 @@ void mj_jacSparse(const mjModel* m, const mjData* d, } - // sparse Jacobian difference for simple body contacts void mj_jacSparseSimple(const mjModel* m, const mjData* d, mjtNum* jacdifp, mjtNum* jacdifr, const mjtNum* point, @@ -445,7 +433,6 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d, } - // dense or sparse Jacobian difference for two body points: pos2 - pos1, global int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, int b1, int b2, const mjtNum pos1[3], const mjtNum pos2[3], @@ -522,7 +509,6 @@ int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, } - // dense or sparse weighted sum of multiple body Jacobians at same point int mj_jacSum(const mjModel* m, mjData* d, int* chain, int n, const int* body, const mjtNum* weight, @@ -596,7 +582,6 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain, } - // compute 3/6-by-nv Jacobian time derivative of global point attached to given body void mj_jacDot(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, const mjtNum point[3], int body) { @@ -672,7 +657,6 @@ void mj_jacDot(const mjModel* m, const mjData* d, } - // compute subtree angular momentum matrix void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { int nv = m->nv; @@ -742,7 +726,6 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { } - // count warnings, print only the first time void mj_warning(mjData* d, int warning, int info) { // check type @@ -764,8 +747,6 @@ void mj_warning(mjData* d, int warning, int info) { - - // compute object 6D velocity in object-centered frame, world/local orientation void mj_objectVelocity(const mjModel* m, const mjData* d, int objtype, int objid, mjtNum res[6], int flg_local) { @@ -817,7 +798,6 @@ void mj_objectVelocity(const mjModel* m, const mjData* d, } - // compute object 6D acceleration in object-centered frame, world/local orientation void mj_objectAcceleration(const mjModel* m, const mjData* d, int objtype, int objid, mjtNum res[6], int flg_local) { @@ -943,4 +923,3 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], } - diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index cb4175dc..242bf497 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -33,7 +33,6 @@ //------------------------- derivatives of spatial algebra ----------------------------------------- - // derivatives of cross product, Da and Db are 3x3 static void mjd_cross(const mjtNum a[3], const mjtNum b[3], mjtNum* restrict Da, mjtNum* restrict Db) { @@ -61,7 +60,6 @@ static void mjd_cross(const mjtNum a[3], const mjtNum b[3], } - // derivative of mju_crossMotion w.r.t velocity static void mjd_crossMotion_vel(mjtNum D[36], const mjtNum v[6]) { mju_zero(D, 36); @@ -98,7 +96,6 @@ static void mjd_crossMotion_vel(mjtNum D[36], const mjtNum v[6]) { } - // derivative of mju_crossForce w.r.t. velocity static void mjd_crossForce_vel(mjtNum D[36], const mjtNum f[6]) { mju_zero(D, 36); @@ -135,7 +132,6 @@ static void mjd_crossForce_vel(mjtNum D[36], const mjtNum f[6]) { } - // derivative of mju_crossForce w.r.t. force static void mjd_crossForce_frc(mjtNum D[36], const mjtNum vel[6]) { mju_zero(D, 36); @@ -172,7 +168,6 @@ static void mjd_crossForce_frc(mjtNum D[36], const mjtNum vel[6]) { } - // derivative of mju_mulInertVec w.r.t vel static void mjd_mulInertVec_vel(mjtNum D[36], const mjtNum i[10]) { mju_zero(D, 36); @@ -215,7 +210,6 @@ static void mjd_mulInertVec_vel(mjtNum D[36], const mjtNum i[10]) { } - // derivative of mju_subQuat w.r.t inputs void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]) { // no outputs, quick return @@ -262,7 +256,6 @@ void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db } - // derivative of mju_quatIntegrate w.r.t scaled velocity // reference: https://arxiv.org/abs/1711.02508, Eq. 183 void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale, @@ -320,7 +313,6 @@ void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale, } - //------------------------- dense derivatives of component functions ------------------------------- // no longer used, except in tests @@ -388,7 +380,6 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt } - // subtract (d qfrc_bias / d qvel) from qDeriv (dense version) void mjd_rne_vel_dense(const mjModel* m, mjData* d) { int nv = m->nv, nbody = m->nbody; @@ -469,7 +460,6 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { } - //------------------------- sparse derivatives of component functions ------------------------------ // internal sparse format: dense body/dof x sparse dof x 6 (inner size is 6) @@ -496,7 +486,6 @@ static void copyFromParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { } - // add sparse B-row to parent, all overlapping nonzeros static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { // return if this is world or parent is world @@ -530,7 +519,6 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) { } - // derivative of cvel, cdof_dot w.r.t qvel static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot) { int nv = m->nv, nbody = m->nbody; @@ -599,7 +587,6 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D } - // subtract d qfrc_bias / d qvel from qDeriv static void mjd_rne_vel(const mjModel* m, mjData* d) { int nv = m->nv, nbody = m->nbody; @@ -686,7 +673,6 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { } - //--------------------- utility functions for (d force / d vel) Jacobians -------------------------- // add J'*B*J to qDeriv @@ -724,7 +710,6 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* } - // add J'*B*J to qDeriv, sparse version static void addJTBJSparse( const mjModel* m, mjData* d, const mjtNum* J, @@ -758,7 +743,6 @@ static void addJTBJSparse( } - //----------------------------- derivatives of actuator forces ------------------------------------- // derivative of mju_muscleGain w.r.t velocity @@ -810,7 +794,6 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang } - // add (d qfrc_actuator / d qvel) to qDeriv void mjd_actuator_vel(const mjModel* m, mjData* d) { int nu = m->nu; @@ -870,7 +853,6 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { } - //----------------- utilities for ellipsoid-based fluid force derivatives -------------------------- static inline mjtNum pow2(const mjtNum val) { @@ -878,7 +860,6 @@ static inline mjtNum pow2(const mjtNum val) { } - static inline mjtNum ellipsoid_max_moment(const mjtNum size[3], const int dir) { const mjtNum d0 = size[dir]; const mjtNum d1 = size[(dir+1) % 3]; @@ -887,7 +868,6 @@ static inline mjtNum ellipsoid_max_moment(const mjtNum size[3], const int dir) { } - // add 3x3 matrix D to one of the four quadrants of the 6x6 matrix B // row_quad and col_quad should be either 0 or 1 (not checked) static void addToQuadrant(mjtNum* restrict B, const mjtNum D[9], int col_quad, int row_quad) { @@ -904,7 +884,6 @@ static void addToQuadrant(mjtNum* restrict B, const mjtNum D[9], int col_quad, i } - //----------------- components of ellipsoid-based fluid force derivatives -------------------------- // forces due to fluid mass moving with the body, B is 6x6 @@ -952,7 +931,6 @@ static void mjd_addedMassForces( } - // torque due to motion in the fluid, D is 3x3 static inline void mjd_viscous_torque( mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, @@ -1006,7 +984,6 @@ static inline void mjd_viscous_torque( } - // drag due to motion in the fluid, D is 3x3 static inline void mjd_viscous_drag( mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, @@ -1074,7 +1051,6 @@ static inline void mjd_viscous_drag( } - // Kutta lift due to motion in the fluid, D is 3x3 static inline void mjd_kutta_lift( mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density, @@ -1128,7 +1104,6 @@ static inline void mjd_kutta_lift( } - // Magnus force due to motion in the fluid, B is 6x6 static inline void mjd_magnus_force( mjtNum* restrict B, const mjtNum lvel[6], const mjtNum fluid_density, @@ -1156,7 +1131,6 @@ static inline void mjd_magnus_force( } - //----------------- fluid force derivatives, ellipsoid and inertia-box models ---------------------- // fluid forces based on ellipsoid approximation @@ -1264,7 +1238,6 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { } - // fluid forces based on inertia-box approximation void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) { mj_markStack(d); @@ -1419,7 +1392,6 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) { } - //------------------------- derivatives of passive forces ------------------------------------------ // add (d qfrc_passive / d qvel) to qDeriv @@ -1508,7 +1480,6 @@ void mjd_passive_vel(const mjModel* m, mjData* d) { } - //------------------------- main entry points ------------------------------------------------------ // analytical derivative of smooth forces w.r.t velocities: diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 5287af46..95905b91 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -42,7 +42,6 @@ static void getState(const mjModel* m, const mjData* d, mjtNum* state, mjtNum* s } - // dx = (x2 - x1) / h static void diff(mjtNum* restrict dx, const mjtNum* x1, const mjtNum* x2, mjtNum h, int n) { mjtNum inv_h = 1/h; @@ -52,7 +51,6 @@ static void diff(mjtNum* restrict dx, const mjtNum* x1, const mjtNum* x2, mjtNum } - // finite-difference two state vectors ds = (s2 - s1) / h static void stateDiff(const mjModel* m, mjtNum* ds, const mjtNum* s1, const mjtNum* s2, mjtNum h) { int nq = m->nq, nv = m->nv, na = m->na; @@ -66,7 +64,6 @@ static void stateDiff(const mjModel* m, mjtNum* ds, const mjtNum* s1, const mjtN } - // finite-difference two vectors, forward, backward or centered static void clampedDiff(mjtNum* dx, const mjtNum* x, const mjtNum* x_plus, const mjtNum* x_minus, mjtNum h, int nx) { @@ -86,7 +83,6 @@ static void clampedDiff(mjtNum* dx, const mjtNum* x, const mjtNum* x_plus, const } - // finite-difference two state vectors, forward, backward or centered static void clampedStateDiff(const mjModel* m, mjtNum* ds, const mjtNum* s, const mjtNum* s_plus, const mjtNum* s_minus, mjtNum h) { @@ -106,7 +102,6 @@ static void clampedStateDiff(const mjModel* m, mjtNum* ds, const mjtNum* s, cons } - // check if two numbers are inside a given range static int inRange(const mjtNum x1, const mjtNum x2, const mjtNum* range) { return x1 >= range[0] && x1 <= range[1] && @@ -114,7 +109,6 @@ static int inRange(const mjtNum x1, const mjtNum x2, const mjtNum* range) { } - // advance simulation using control callback, skipstage is mjtStage void mj_stepSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) { TM_START; @@ -154,7 +148,6 @@ void mj_stepSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) { } - // compute qfrc_inverse, optionally subtracting qfrc_actuator static void inverseSkip(const mjModel* m, mjData* d, mjtStage stage, int skipsensor, int flg_actuation, mjtNum* force) { @@ -167,7 +160,6 @@ static void inverseSkip(const mjModel* m, mjData* d, mjtStage stage, int skipsen } - //------------------------- derivatives of passive forces ------------------------------------------ // add forward fin-diff approximation of (d qfrc_passive / d qvel) to qDeriv @@ -218,7 +210,6 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { } - //-------------------- derivatives of all smooth (unconstrained) forces ---------------------------- // centered finite difference approximation to mjd_smooth_vel @@ -284,7 +275,6 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { } - //------------------------- main entry points ------------------------------------------------------ @@ -537,7 +527,6 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, } - // finite differenced transition matrices (control theory notation) // d(x_next) = A*dx + B*du // d(sensor) = C*dx + D*du diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 2a9e0bc5..a68521e6 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -65,7 +65,6 @@ void mj_checkPos(const mjModel* m, mjData* d) { } - // check velocities, reset if bad void mj_checkVel(const mjModel* m, mjData* d) { for (int i=0; i < m->nv; i++) { @@ -82,7 +81,6 @@ void mj_checkVel(const mjModel* m, mjData* d) { } - // check accelerations, reset if bad void mj_checkAcc(const mjModel* m, mjData* d) { for (int i=0; i < m->nv; i++) { @@ -102,7 +100,6 @@ void mj_checkAcc(const mjModel* m, mjData* d) { } - //-------------------------- solver components ----------------------------------------------------- // args for internal functions in mj_fwdPosition @@ -128,7 +125,6 @@ void* mj_collisionThreaded(void* args) { } - // position-dependent computations void mj_fwdPosition(const mjModel* m, mjData* d) { TM_START1; @@ -189,7 +185,6 @@ void mj_fwdPosition(const mjModel* m, mjData* d) { } - // velocity-dependent computations void mj_fwdVelocity(const mjModel* m, mjData* d) { TM_START; @@ -233,7 +228,6 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { } - // returns the next act given the current act_dot, after clamping static mjtNum nextActivation(const mjModel* m, const mjData* d, int actuator_id, int act_adr, mjtNum act_dot) { @@ -261,7 +255,6 @@ static mjtNum nextActivation(const mjModel* m, const mjData* d, } - // clamp vector to range static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n, const int* index) { @@ -274,7 +267,6 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, i } - // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot) void mj_fwdActuation(const mjModel* m, mjData* d) { TM_START; @@ -554,7 +546,6 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } - // add up all non-constraint forces, compute qacc_smooth void mj_fwdAcceleration(const mjModel* m, mjData* d) { int nv = m->nv; @@ -570,7 +561,6 @@ void mj_fwdAcceleration(const mjModel* m, mjData* d) { } - // warmstart/init solver static void warmstart(const mjModel* m, mjData* d) { int nv = m->nv, nefc = d->nefc; @@ -651,7 +641,6 @@ static void warmstart(const mjModel* m, mjData* d) { } - // struct encapsulating arguments to thread task struct mjSolIslandArgs_ { const mjModel* m; @@ -700,7 +689,6 @@ static void solve_threaded(const mjModel* m, mjData* d, int flg_Newton) { } - // compute efc_b, efc_force, qfrc_constraint; update qacc void mj_fwdConstraint(const mjModel* m, mjData* d) { TM_START; @@ -794,7 +782,6 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { } - //-------------------------- integrators ---------------------------------------------------------- // advance state and time given activation derivatives, acceleration, and optional velocity @@ -894,14 +881,12 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { } - // Euler integrator, semi-implicit in velocity void mj_Euler(const mjModel* m, mjData* d) { mj_EulerSkip(m, d, 0); } - // RK4 tableau const mjtNum RK4_A[9] = { 0.5, 0, 0, @@ -1007,7 +992,6 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } - // fully implicit in velocity, possibly skipping factorization void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { TM_START; @@ -1075,14 +1059,12 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } - // fully implicit in velocity void mj_implicit(const mjModel* m, mjData* d) { mj_implicitSkip(m, d, 0); } - // return 1 if potential energy was computed by sensor, 0 otherwise static int energyPosSensor(const mjModel* m) { if (mjDISABLED(mjDSBL_SENSOR)) { @@ -1098,7 +1080,6 @@ static int energyPosSensor(const mjModel* m) { } - // return 1 if kinetic energy was computed by sensor, 0 otherwise static int energyVelSensor(const mjModel* m) { if (mjDISABLED(mjDSBL_SENSOR)) { @@ -1114,7 +1095,6 @@ static int energyVelSensor(const mjModel* m) { } - //-------------------------- top-level API --------------------------------------------------------- // forward dynamics with skip; skipstage is mjtStage @@ -1171,14 +1151,12 @@ void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) } - // forward dynamics void mj_forward(const mjModel* m, mjData* d) { mj_forwardSkip(m, d, mjSTAGE_NONE, 0); } - // advance simulation using control callback void mj_step(const mjModel* m, mjData* d) { TM_START; @@ -1217,7 +1195,6 @@ void mj_step(const mjModel* m, mjData* d) { } - // advance simulation in two phases: before input is set by user void mj_step1(const mjModel* m, mjData* d) { TM_START; diff --git a/src/engine/engine_init.c b/src/engine/engine_init.c index 2b22621d..80c3de69 100644 --- a/src/engine/engine_init.c +++ b/src/engine/engine_init.c @@ -45,7 +45,6 @@ void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp) { } - //------------------------------- mjOption --------------------------------------------------------- // set model options to default values @@ -105,8 +104,6 @@ void mj_defaultOption(mjOption* opt) { } - - //------------------------------- mjStatistic ------------------------------------------------------ // set statistics to default values; compute later in compiler @@ -119,7 +116,6 @@ void mj_defaultStatistic(mjStatistic* stat) { } - //------------------------------- mjVisual --------------------------------------------------------- // set 4 floats @@ -230,8 +226,6 @@ void mj_defaultVisual(mjVisual* vis) { } - - //------------------------------ mjLROpt ----------------------------------------------------------- // set default options for length range computation diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index e7e7353a..ae4e1191 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -63,14 +63,12 @@ void mj_invPosition(const mjModel* m, mjData* d) { } - // velocity-dependent computations void mj_invVelocity(const mjModel* m, mjData* d) { mj_fwdVelocity(m, d); } - // convert discrete-time qacc to continuous-time qacc static void mj_discreteAcc(const mjModel* m, mjData* d) { int nv = m->nv, nC = m->nC, nD = m->nD, dof_damping; @@ -152,7 +150,6 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { } - // inverse constraint solver void mj_invConstraint(const mjModel* m, mjData* d) { TM_START; @@ -180,7 +177,6 @@ void mj_invConstraint(const mjModel* m, mjData* d) { } - // inverse dynamics with skip; skipstage is mjtStage void mj_inverseSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) { @@ -250,14 +246,12 @@ void mj_inverseSkip(const mjModel* m, mjData* d, } - // inverse dynamics void mj_inverse(const mjModel* m, mjData* d) { mj_inverseSkip(m, d, mjSTAGE_NONE, 0); } - // compare forward and inverse dynamics, without changing results of forward // fwdinv[0] = norm(qfrc_constraint(forward) - qfrc_constraint(inverse)) // fwdinv[1] = norm(qfrc_applied(forward) - qfrc_inverse) diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 159939b2..e64d836f 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -53,7 +53,6 @@ static const int MAX_ARRAY_SIZE = INT_MAX / 4; //----------------------------------- static utility functions ------------------------------------- - //----------------------------------- static utility functions ------------------------------------- // id used to identify binary mjModel file/buffer @@ -80,7 +79,6 @@ static int getnint(void) { } - // count buffer members in mjModel (mjtSize) static int getnbuffer(void) { int cnt = 0; @@ -93,7 +91,6 @@ static int getnbuffer(void) { } - // count pointers in mjModel static int getnptr(void) { int cnt = 0; @@ -106,7 +103,6 @@ static int getnptr(void) { } - // write to memory buffer static void bufwrite(const void* src, int num, int szbuf, void* buf, int* ptrbuf) { // check pointers @@ -125,7 +121,6 @@ static void bufwrite(const void* src, int num, int szbuf, void* buf, int* ptrbuf } - // read from memory buffer static void bufread(void* dest, int num, int szbuf, const void* buf, int* ptrbuf) { // check pointers @@ -144,7 +139,6 @@ static void bufread(void* dest, int num, int szbuf, const void* buf, int* ptrbuf } - // number of bytes to be skipped to achieve 64-byte alignment static inline unsigned int SKIP(intptr_t offset) { const unsigned int align = 64; @@ -153,7 +147,6 @@ static inline unsigned int SKIP(intptr_t offset) { } - //----------------------------------- mjModel construction ----------------------------------------- // set pointers in mjModel buffer @@ -183,7 +176,6 @@ static void mj_setPtrModel(mjModel* m) { } - // increases buffer size without causing integer overflow, returns 0 if // operation would cause overflow // performs the following operations: @@ -213,14 +205,12 @@ static int safeAddToBufferSize(intptr_t* offset, mjtSize* nbuffer, } - // free model memory without destroying the struct static void freeModelBuffers(mjModel* m) { mju_free(m->buffer); } - // allocate and initialize mjModel structure void mj_makeModel(mjModel** dest, int nq, int nv, int nu, int na, int nbody, int nbvh, @@ -403,7 +393,6 @@ void mj_makeModel(mjModel** dest, } - // copy mjModel, if dest==NULL create new model mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { // allocate new model if needed @@ -456,7 +445,6 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { } - // copy mjModel, skip large arrays not required for abstract visualization void mjv_copyModel(mjModel* dest, const mjModel* src) { // check sizes @@ -488,7 +476,6 @@ void mjv_copyModel(mjModel* dest, const mjModel* src) { } - // save model to binary file, or memory buffer of szbuf>0 void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz) { FILE* fp = 0; @@ -680,7 +667,6 @@ void mj_deleteModel(mjModel* m) { } - // size of buffer needed to hold model int mj_sizeModel(const mjModel* m) { int size = ( @@ -700,8 +686,6 @@ int mj_sizeModel(const mjModel* m) { } - - //-------------------------- sparse system matrix construction ------------------------------------- // construct sparse representation of dof-dof matrix @@ -878,7 +862,6 @@ void mj_makeBSparse(int nv, int nbody, int nB, } - // check D and B sparsity for consistency static void checkDBSparse(const mjModel* m) { // process all dofs @@ -899,7 +882,6 @@ static void checkDBSparse(const mjModel* m) { } - // integer valued dst[D or C or M] = src[M (legacy)], handle different sparsity representations static void copyM2Sparse(int nv, const int* dof_Madr, const int* dof_simplenum, const int* dof_parentid, @@ -1008,7 +990,6 @@ static void mj_setPtrData(const mjModel* m, mjData* d) { } - // initialize plugins, copy into d (required for deletion) void mj_initPlugin(const mjModel* m, mjData* d) { d->nplugin = m->nplugin; @@ -1025,7 +1006,6 @@ void mj_initPlugin(const mjModel* m, mjData* d) { } - // free mjData memory without destroying the struct static void freeDataBuffers(mjData* d) { #ifdef ADDRESS_SANITIZER @@ -1045,7 +1025,6 @@ static void freeDataBuffers(mjData* d) { } - // allocate and initialize raw mjData structure void mj_makeRawData(mjData** dest, const mjModel* m) { intptr_t offset = 0; @@ -1114,7 +1093,6 @@ void mj_makeRawData(mjData** dest, const mjModel* m) { } - // allocate and initialize mjData structure mjData* mj_makeData(const mjModel* m) { mjData* d = NULL; @@ -1127,7 +1105,6 @@ mjData* mj_makeData(const mjModel* m) { } - // copy mjData, if dest==NULL create new data; // flg_all 1: copy all fields, 0: skip fields not required for visualization mjData* mj_copyDataVisual(mjData* dest, const mjModel* m, const mjData* src, int flg_all) { @@ -1275,7 +1252,6 @@ mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src) { } - // clear data, set defaults static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { //------------------------------ save plugin state and data @@ -1428,21 +1404,18 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { } - // clear data, set data->qpos = model->qpos0 void mj_resetData(const mjModel* m, mjData* d) { _resetData(m, d, 0); } - // clear data, set data->qpos = model->qpos0, fill with debug_value void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value) { _resetData(m, d, debug_value); } - // Reset data. If 0 <= key < nkey, set fields from specified keyframe. void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) { _resetData(m, d, 0); @@ -1460,7 +1433,6 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) { } - // de-allocate mjData void mj_deleteData(mjData* d) { if (d) { @@ -1470,7 +1442,6 @@ void mj_deleteData(mjData* d) { } - // number of position and velocity coordinates for each joint type const int nPOS[4] = {7, 4, 1, 1}; const int nVEL[4] = {6, 3, 1, 1}; @@ -1546,7 +1517,6 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { } - // returns the number of objects of the given type // -1: mjOBJ_UNKNOWN // -2: invalid objtype @@ -1613,7 +1583,6 @@ static int numObjects(const mjModel* m, mjtObj objtype) { } - // validate reference fields in a model; return null if valid, error message otherwise const char* mj_validateReferences(const mjModel* m) { // for each field in mjModel that refers to another field, call X with: diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 775544f8..1dff33b1 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -53,7 +53,6 @@ static void clearIsland(mjData* d, size_t parena) { } - // allocate island arrays on arena, return 1 on success, 0 on failure static int arenaAllocIsland(const mjModel* m, mjData* d) { #undef MJ_M @@ -83,7 +82,6 @@ static int arenaAllocIsland(const mjModel* m, mjData* d) { } - //-------------------------- flood-fill and graph construction ------------------------------------ // find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix @@ -139,7 +137,6 @@ int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, cons } - // return id of next tree in Jacobian row i that is different from tree, -1 if not found // start search from *index // write the index of the found tree to *index @@ -188,7 +185,6 @@ static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *ind } - // find first and possibly second nonegative tree ids in Jacobian row i // if row i is special-cased (no more trees), return -1 // otherwise call treeNext, starting scan at index 0, return index @@ -284,7 +280,6 @@ static int treeFirst(const mjModel* m, const mjData* d, int tree[2], int i) { } - // add 0 edges, 1 self-edge or 2 flipped edges to array, increment treenedge // return current number of edges static int addEdge(int* treenedge, int* edge, int nedge, int tree1, int tree2, int nedge_max) { @@ -343,7 +338,6 @@ static int addEdge(int* treenedge, int* edge, int nedge, int tree1, int tree2, i } - // find tree-tree edges, increment treenedge counters, return total number of edges static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edge, int nedge_max) { int nefc = d->nefc; @@ -401,7 +395,6 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg } - //-------------------------- main entry-point ----------------------------------------------------- // discover islands: diff --git a/src/engine/engine_memory.c b/src/engine/engine_memory.c index 8b017850..a1ae7c3b 100644 --- a/src/engine/engine_memory.c +++ b/src/engine/engine_memory.c @@ -76,7 +76,6 @@ static void maybe_unlock_alloc_mutex(mjData* d) { } - static inline mjStackInfo get_stack_info_from_data(const mjData* d) { mjStackInfo stack_info; stack_info.bottom = (uintptr_t)d->arena + (uintptr_t)d->narena; @@ -225,7 +224,6 @@ static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_ } - // internal: allocate size bytes in mjData // declared inline so that modular arithmetic with specific alignments can be optimized out static inline void* stackalloc(mjData* d, size_t size, size_t alignment, @@ -245,7 +243,6 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment, } - // mjStackInfo mark stack frame, inline so ASAN errors point to correct code unit #ifdef ADDRESS_SANITIZER __attribute__((always_inline)) @@ -264,7 +261,6 @@ static inline void markstackinternal(mjData* d, mjStackInfo* stack_info) { } - // mjData mark stack frame #ifndef ADDRESS_SANITIZER void mj_markStack(mjData* d) @@ -286,7 +282,6 @@ void mj__markStack(mjData* d) } - #ifdef ADDRESS_SANITIZER __attribute__((always_inline)) #endif @@ -316,7 +311,6 @@ static inline void freestackinternal(mjStackInfo* stack_info) { } - // mjData free stack frame #ifndef ADDRESS_SANITIZER void mj_freeStack(mjData* d) @@ -338,7 +332,6 @@ void mj__freeStack(mjData* d) } - // returns the number of bytes available on the stack size_t mj_stackBytesAvailable(mjData* d) { if (!d->threadpool) { @@ -352,14 +345,12 @@ size_t mj_stackBytesAvailable(mjData* d) { } - // allocate bytes on the stack void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment) { return stackalloc(d, bytes, alignment, NULL, 0); } - // allocate bytes on the stack, with caller information void* mj_stackAllocInfo(mjData* d, size_t bytes, size_t alignment, const char* caller, int line) { @@ -367,7 +358,6 @@ void* mj_stackAllocInfo(mjData* d, size_t bytes, size_t alignment, } - // allocate mjtNums on the stack mjtNum* mj_stackAllocNum(mjData* d, size_t size) { if (mjUNLIKELY(size >= SIZE_MAX / sizeof(mjtNum))) { @@ -377,7 +367,6 @@ mjtNum* mj_stackAllocNum(mjData* d, size_t size) { } - // allocate ints on the stack int* mj_stackAllocInt(mjData* d, size_t size) { if (mjUNLIKELY(size >= SIZE_MAX / sizeof(int))) { diff --git a/src/engine/engine_name.c b/src/engine/engine_name.c index 013ae8ad..71b48dd1 100644 --- a/src/engine/engine_name.c +++ b/src/engine/engine_name.c @@ -265,7 +265,6 @@ int mj_name2id(const mjModel* m, int type, const char* name) { } - // get name of object with the specified mjtObj type and id, // returns NULL if name not found const char* mj_id2name(const mjModel* m, int type, int id) { diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index e0cfd447..a8396f4b 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -460,7 +460,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) { } - // body-level gravity compensation, return 1 if any, 0 otherwise static int mj_gravcomp(const mjModel* m, mjData* d) { if (!m->ngravcomp || mjDISABLED(mjDSBL_GRAVITY) || mju_norm3(m->opt.gravity) == 0) { @@ -483,7 +482,6 @@ static int mj_gravcomp(const mjModel* m, mjData* d) { } - // fluid forces static int mj_fluid(const mjModel* m, mjData* d) { int has_fluid = m->opt.viscosity > 0 || m->opt.density > 0; @@ -515,7 +513,6 @@ static int mj_fluid(const mjModel* m, mjData* d) { } - // passive contact forces int mj_contactPassive(const mjModel* m, mjData* d) { int ncon = d->ncon, issparse = mj_isSparse(m); @@ -598,7 +595,6 @@ int mj_contactPassive(const mjModel* m, mjData* d) { } - // all passive forces void mj_passive(const mjModel* m, mjData* d) { int nv = m->nv; @@ -692,7 +688,6 @@ void mj_passive(const mjModel* m, mjData* d) { } - //---------------------------------- fluid models -------------------------------------------------- // fluid forces based on inertia-box approximation @@ -755,7 +750,6 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) { } - // fluid forces based on ellipsoid approximation void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) { mjtNum lvel[6], wind[6], lwind[6], lfrc[6], bfrc[6]; @@ -872,7 +866,6 @@ static inline mjtNum mji_ellipsoid_max_moment(const mjtNum size[3], const int di } - // lift and drag forces due to motion in the fluid void mj_viscousForces( const mjtNum local_vels[6], const mjtNum fluid_density, @@ -960,7 +953,6 @@ void mj_viscousForces( } - // read the geom_fluid_coefs array into its constituent parts void readFluidGeomInteraction(const mjtNum* geom_fluid_coefs, mjtNum* geom_fluid_coef, @@ -990,7 +982,6 @@ void readFluidGeomInteraction(const mjtNum* geom_fluid_coefs, } - // write components into geom_fluid_coefs array void writeFluidGeomInteraction (mjtNum* geom_fluid_coefs, const mjtNum* geom_fluid_coef, diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index ae02c9f4..c83a5123 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -119,7 +119,6 @@ static void printArray2dInt(const char* str, int nr, int nc, const int* data, FI } - // print sparse matrix static void printSparse(const char* str, const mjtNum* mat, int nr, const int* rownnz, const int* rowadr, @@ -193,7 +192,6 @@ static void printBlockArray(const char* str, const mjtNum* data, int nr, int nc, } - // print sparse inertia-like matrix static void printInertia(const char* str, const mjtNum* mat, const mjModel* m, FILE* fp, const char* float_format) { @@ -230,7 +228,6 @@ static void printInertia(const char* str, const mjtNum* mat, const mjModel* m, } - // print sparse matrix structure void mj_printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag, const int* rownnz, const int* rowsuper, const int* colind, FILE* fp) { @@ -268,7 +265,6 @@ void mj_printSparsity(const char* str, int nr, int nc, const int* rowadr, const } - // print block-diagonal sparse matrix structure void mj_printBlockSparsity(const char* str, int nr, int nc, int nisland, const int* island_block_ncols, @@ -328,7 +324,6 @@ void mj_printBlockSparsity(const char* str, int nr, int nc, int nisland, } - // print vector static void printVector(const char* str, const mjtNum* data, int n, FILE* fp, const char* float_format) { @@ -347,7 +342,6 @@ static void printVector(const char* str, const mjtNum* data, int n, FILE* fp, } - // print human readable memory size static const char* memorySize(size_t nbytes) { static mjTHREADLOCAL char message[20]; @@ -367,7 +361,6 @@ static const char* memorySize(size_t nbytes) { } - // return memory footprint of all significant mesh-related arrays static size_t sizeMesh(const mjModel* m) { size_t nbytes = 0; @@ -382,7 +375,6 @@ static size_t sizeMesh(const mjModel* m) { } - // return memory footprint of all significant skin-related arrays static size_t sizeSkin(const mjModel* m) { size_t nbytes = 0; @@ -400,7 +392,6 @@ static size_t sizeSkin(const mjModel* m) { } - // return whether float_format is a valid format string for a single float static bool validateFloatFormat(const char* float_format) { // check for nullptr; diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index dc274fca..2f44cc6b 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -51,21 +51,18 @@ static void ray_map(const mjtNum* pos, const mjtNum* mat, const mjtNum* pnt, con } - // map to azimuth angle in spherical coordinates static mjtNum longitude(const mjtNum vec[3]) { return mju_atan2(vec[1], vec[0]); } - // map to elevation angle in spherical coordinates static mjtNum latitude(const mjtNum vec[3]) { return mju_atan2(mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]), vec[2]); } - // eliminate geom static int ray_eliminate(const mjModel* m, const mjData* d, int geomid, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude) { @@ -101,7 +98,6 @@ static int ray_eliminate(const mjModel* m, const mjData* d, int geomid, } - // compute solution from quadratic: a*x^2 + 2*b*x + c = 0 static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum* x) { // compute determinant and check @@ -128,7 +124,6 @@ static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum* x) { } - // intersect ray with triangle mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec, const mjtNum* b0, const mjtNum* b1) { @@ -218,7 +213,6 @@ static mjtNum ray_plane(const mjtNum* pos, const mjtNum* mat, const mjtNum* size } - // sphere static mjtNum ray_sphere(const mjtNum* pos, const mjtNum* mat, mjtNum dist_sqr, const mjtNum* pnt, const mjtNum* vec) { @@ -234,7 +228,6 @@ static mjtNum ray_sphere(const mjtNum* pos, const mjtNum* mat, mjtNum dist_sqr, } - // capsule static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* pnt, const mjtNum* vec) { @@ -301,7 +294,6 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si } - // ellipsoid static mjtNum ray_ellipsoid(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* pnt, const mjtNum* vec) { @@ -323,7 +315,6 @@ static mjtNum ray_ellipsoid(const mjtNum* pos, const mjtNum* mat, const mjtNum* } - // cylinder static mjtNum ray_cylinder(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* pnt, const mjtNum* vec) { @@ -383,7 +374,6 @@ static mjtNum ray_cylinder(const mjtNum* pos, const mjtNum* mat, const mjtNum* s } - // box static mjtNum ray_box(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* pnt, const mjtNum* vec, mjtNum* all) { @@ -449,7 +439,6 @@ static mjtNum ray_box(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, } - // intersect ray with hfield mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const mjtNum* vec) { @@ -597,7 +586,6 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id, } - // ray vs axis-aligned bounding box using slab method // see Ericson, Real-time Collision Detection section 5.3.3. int mju_raySlab(const mjtNum aabb[6], const mjtNum xpos[3], @@ -732,7 +720,6 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, } - // intersect ray with signed distance field mjtNum ray_sdf(const mjModel* m, const mjData* d, int g, const mjtNum* pnt, const mjtNum* vec) { @@ -793,7 +780,6 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g, } - // intersect ray with mesh mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, const mjtNum* pnt, const mjtNum* vec) { @@ -811,7 +797,6 @@ mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id, } - // intersect ray with pure geom, no meshes or hfields mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* pnt, const mjtNum* vec, int geomtype) { @@ -841,7 +826,6 @@ mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size, } - // intersect ray with flex, return nearest vertex id mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, @@ -1019,7 +1003,6 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl } - // intersect ray with skin, return nearest vertex id mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, const mjtNum* pnt, const mjtNum* vec, int vertid[1]) { @@ -1112,7 +1095,6 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, } - // return 1 if point is inside object-aligned bounding box, 0 otherwise static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3], const mjtNum xmat[9], const mjtNum pnt[3]) { @@ -1134,7 +1116,6 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3], } - //---------------------------- main entry point ---------------------------------------------------- // intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms on bodyexclude diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 9cd67584..42f9873c 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -91,7 +91,6 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) { } - // get xpos and xmat pointers to an object in mjData static void get_xpos_xmat(const mjData* d, mjtObj type, int id, int sensor_id, mjtNum **xpos, mjtNum **xmat) { @@ -122,7 +121,6 @@ static void get_xpos_xmat(const mjData* d, mjtObj type, int id, int sensor_id, } - // get global quaternion of an object in mjData static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, int sensor_id, mjtNum *quat) { @@ -148,7 +146,6 @@ static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, in } - static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], const mjtNum cam_xpos[3], const mjtNum cam_xmat[9], const int cam_res[2], mjtNum cam_fovy, @@ -241,7 +238,6 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3], } - // check if a contact body/geom matches a sensor spec (type, id) static int checkMatch(const mjModel* m, int body, int geom, mjtObj type, int id) { if (type == mjOBJ_UNKNOWN) return 1; @@ -314,7 +310,6 @@ static int matchContact(const mjModel* m, const mjData* d, int conid, } - // fill in output data for contact sensor for all fields // if flg_flip > 0, normal/tangent rotate 180 about frame[2] // force/torque flip-z s.t. force is equal-and-opposite in new contact frame @@ -361,7 +356,6 @@ static void copySensorData(const mjModel* m, const mjData* d, } - // compute total wrench about one point, in the global frame static void total_wrench(mjtNum force[3], mjtNum torque[3], const mjtNum point[3], int n, const mjtNum *wrench, const mjtNum *pos, const mjtNum *frame) { @@ -388,7 +382,6 @@ static void total_wrench(mjtNum force[3], mjtNum torque[3], const mjtNum point[3 } - //-------------------------------- sensor ---------------------------------------------------------- // position-dependent sensors @@ -692,7 +685,6 @@ void mj_sensorPos(const mjModel* m, mjData* d) { } - // velocity-dependent sensors void mj_sensorVel(const mjModel* m, mjData* d) { int objtype, objid, reftype, refid, adr, nusersensor = 0; @@ -875,7 +867,6 @@ void mj_sensorVel(const mjModel* m, mjData* d) { } - // acceleration/force-dependent sensors void mj_sensorAcc(const mjModel* m, mjData* d) { int rootid, bodyid, objtype, objid, adr, nusersensor = 0; @@ -1400,7 +1391,6 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { } - //-------------------------------- energy ---------------------------------------------------------- // position-dependent energy (potential) @@ -1491,7 +1481,6 @@ void mj_energyPos(const mjModel* m, mjData* d) { } - // velocity-dependent energy (kinetic) void mj_energyVel(const mjModel* m, mjData* d) { mj_markStack(d); diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 545ef6f1..a6379ef8 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -59,7 +59,6 @@ static void mj_setM0(mjModel* m, mjData* d) { } - // set quantities that depend on qpos0 static void set0(mjModel* m, mjData* d) { int nv = m->nv; @@ -415,7 +414,6 @@ static void set0(mjModel* m, mjData* d) { } - // accumulate bounding box static void updateBox(mjtNum* xmin, mjtNum* xmax, mjtNum* pos, mjtNum radius) { for (int i=0; i < 3; i++) { @@ -555,7 +553,6 @@ static void setStat(mjModel* m, mjData* d) { } - // set quantities that depend on qpos_spring static void setSpring(mjModel* m, mjData* d) { // run computations in qpos_spring @@ -575,7 +572,6 @@ static void setSpring(mjModel* m, mjData* d) { } - // entry point: set all constant fields of mjModel, except for lengthrange void mj_setConst(mjModel* m, mjData* d) { // compute subtreemass @@ -593,7 +589,6 @@ void mj_setConst(mjModel* m, mjData* d) { } - //----------------------------- actuator length range computation ---------------------------------- // evaluate actuator length, advance special dynamics @@ -634,7 +629,6 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side, } - // Set length range for specified actuator, return 1 if ok, 0 if error. int mj_setLengthRange(mjModel* m, mjData* d, int index, const mjLROpt* opt, char* error, int error_sz) { diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 2a596bd3..ec16ee53 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -65,7 +65,6 @@ static void saveStats(const mjModel* m, mjData* d, int island, int iter, } - // finalize dual solver: map to joint space // TODO: b/295296178 - add island support to Dual solvers static void dualFinish(const mjModel* m, mjData* d) { @@ -78,7 +77,6 @@ static void dualFinish(const mjModel* m, mjData* d) { } - // compute 1/diag(AR) // TODO: b/295296178 - add island support to Dual solvers static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, int flg_subR) { @@ -114,7 +112,6 @@ static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, int flg_su } - // extract diagonal block from AR, clamp diag to 1e-10 if flg_subR // TODO: b/295296178 - add island support to Dual solvers static void extractBlock(const mjModel* m, const mjData* d, mjtNum* Ac, @@ -175,7 +172,6 @@ static void extractBlock(const mjModel* m, const mjData* d, mjtNum* Ac, } - // compute residual for one block // TODO: b/295296178 - add island support to Dual solvers static void residual(const mjModel* m, const mjData* d, mjtNum* res, int i, int dim, int flg_subR) { @@ -206,7 +202,6 @@ static void residual(const mjModel* m, const mjData* d, mjtNum* res, int i, int } - // compute cost change // TODO: b/295296178 - add island support to Dual solvers static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, @@ -233,7 +228,6 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, } - // set efc_state to dual constraint state; return nactive // TODO: b/295296178 - add island support to Dual solvers static int dualState(const mjModel* m, const mjData* d, int* state) { @@ -320,7 +314,6 @@ static int dualState(const mjModel* m, const mjData* d, int* state) { } - //---------------------------- PGS solver ---------------------------------------------------------- // TODO: b/295296178 - add island support to Dual solvers @@ -542,7 +535,6 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { } - //---------------------------- NoSlip solver ------------------------------------------------------- // TODO: b/295296178 - add island support to Dual solvers @@ -759,7 +751,6 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { } - //------------------------- CG and Newton solver -------------------------------------------------- // CG context @@ -964,7 +955,6 @@ static void CGpointers(const mjModel* m, const mjData* d, mjCGContext* ctx, int } - // allocate fixed-size arrays in mjCGContext // mj_{mark/free}Stack in calling function! static void CGallocate(mjData* d, mjCGContext* ctx, int flg_Newton) { @@ -1013,7 +1003,6 @@ static void CGallocate(mjData* d, mjCGContext* ctx, int flg_Newton) { } - // update efc_force, qfrc_constraint, cost-related static void CGupdateConstraint(mjCGContext* ctx, int flg_HessianCone) { int nefc = ctx->nefc, nv = ctx->nv; @@ -1051,7 +1040,6 @@ static void CGupdateConstraint(mjCGContext* ctx, int flg_HessianCone) { } - // update grad, Mgrad static void CGupdateGradient(mjCGContext* ctx, int flg_Newton) { int nv = ctx->nv; @@ -1080,7 +1068,6 @@ static void CGupdateGradient(mjCGContext* ctx, int flg_Newton) { } - // prepare quadratic polynomials and contact cone quantities static void CGprepare(mjCGContext* ctx) { int nv = ctx->nv, nefc = ctx->nefc; @@ -1157,7 +1144,6 @@ static void CGprepare(mjCGContext* ctx) { } - // linesearch evaluation point struct _mjCGPnt { mjtNum alpha; @@ -1306,7 +1292,6 @@ static void CGeval(mjCGContext* ctx, mjCGPnt* p) { } - // update bracket point given 3 candidate points static int updateBracket(mjCGContext* ctx, mjCGPnt* p, const mjCGPnt candidates[3], mjCGPnt* pnext) { @@ -1337,7 +1322,6 @@ static int updateBracket(mjCGContext* ctx, } - // line search static mjtNum CGsearch(mjCGContext* ctx, mjtNum tolerance, mjtNum ls_iterations) { int nv = ctx->nv, nefc = ctx->nefc; @@ -1527,7 +1511,6 @@ static mjtNum CGsearch(mjCGContext* ctx, mjtNum tolerance, mjtNum ls_iterations) } - // allocate and compute Hessian given efc_state // mj_{mark/free}Stack in caller function! static void MakeHessian(mjData* d, mjCGContext* ctx) { @@ -1637,7 +1620,6 @@ static void MakeHessian(mjData* d, mjCGContext* ctx) { } - // forward declaration of HessianCone (readability) static void HessianCone(mjData* d, mjCGContext* ctx); @@ -1716,7 +1698,6 @@ static void FactorizeHessian(mjData* d, mjCGContext* ctx, int flg_recompute) { } - // elliptic case: Hcone = H + cone_contributions static void HessianCone(mjData* d, mjCGContext* ctx) { int nv = ctx->nv, nefc = ctx->nefc; @@ -1795,7 +1776,6 @@ static void HessianCone(mjData* d, mjCGContext* ctx) { } - // incremental update to Hessian factor due to changes in efc_state static void HessianIncremental(mjData* d, mjCGContext* ctx, const int* oldstate) { int rank, nv = ctx->nv, nefc = ctx->nefc; @@ -1863,7 +1843,6 @@ static void HessianIncremental(mjData* d, mjCGContext* ctx, const int* oldstate) } - // driver static void mj_solCGNewton(const mjModel* m, mjData* d, int island, int maxiter, int flg_Newton) { int iter = 0; @@ -2025,28 +2004,24 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int island, int maxiter, } - // CG entry point void mj_solCG(const mjModel* m, mjData* d, int maxiter) { mj_solCGNewton(m, d, /*island=*/-1, maxiter, /*flg_Newton=*/0); } - // CG entry point (one island) void mj_solCG_island(const mjModel* m, mjData* d, int island, int maxiter) { mj_solCGNewton(m, d, island, maxiter, /*flg_Newton=*/0); } - // Newton entry point void mj_solNewton(const mjModel* m, mjData* d, int maxiter) { mj_solCGNewton(m, d, /*island=*/-1, maxiter, /*flg_Newton=*/1); } - // Newton entry point (one island) void mj_solNewton_island(const mjModel* m, mjData* d, int island, int maxiter) { mj_solCGNewton(m, d, island, maxiter, /*flg_Newton=*/1); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index f14a992b..d1fc645f 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -135,7 +135,6 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState sig) { } - // return pointer to a single state element static inline mjtNum* mj_stateElemPtr(const mjModel* m, mjData* d, mjtState sig) { switch (sig) { @@ -158,13 +157,11 @@ static inline mjtNum* mj_stateElemPtr(const mjModel* m, mjData* d, mjtState sig) } - static inline const mjtNum* mj_stateElemConstPtr(const mjModel* m, const mjData* d, mjtState sig) { return mj_stateElemPtr(m, (mjData*) d, sig); // discard const qualifier from d } - // get size of state signature int mj_stateSize(const mjModel* m, unsigned int sig) { if (sig >= (1<= (1<= (1<M, vec, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind); } - // multiply vector by M^(1/2) void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) { int nv = m->nv; @@ -324,7 +315,6 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) } - // add inertia matrix to destination matrix // destination can be sparse or dense when all int* are NULL void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, @@ -350,7 +340,6 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, } - //-------------------------- perturbations --------------------------------------------------------- // add Cartesian force and torque to qfrc_target @@ -412,7 +401,6 @@ void mj_applyFT(const mjModel* m, mjData* d, } - // accumulate xfrc_applied in qfrc void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) { for (int i=1; i < m->nbody; i++) { @@ -424,7 +412,6 @@ void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) { - //-------------------------- miscellaneous --------------------------------------------------------- // returns the smallest distance between two geoms (using nativeccd) @@ -456,7 +443,6 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, const mjData* d, int g1, int } - // returns the smallest distance between two geoms mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, mjtNum distmax, mjtNum fromto[6]) { @@ -509,7 +495,6 @@ mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, } - // compute velocity by finite-differencing two positions void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, const mjtNum* qpos1, const mjtNum* qpos2) { @@ -544,7 +529,6 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, } - // integrate qpos with given qvel void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt) { // loop over joints @@ -579,7 +563,6 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum } - // normalize all quaternions in qpos-type vector void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) { // find quaternion fields and normalize @@ -591,7 +574,6 @@ void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) { } - // return 1 if actuator i is disabled, 0 otherwise int mj_actuatorDisabled(const mjModel* m, int i) { int group = m->actuator_group[i]; @@ -614,7 +596,6 @@ mjtNum mj_getTotalmass(const mjModel* m) { } - // scale all body masses and inertias to achieve specified total mass void mj_setTotalmass(mjModel* m, mjtNum newmass) { // compute scale factor, avoid zeros @@ -632,14 +613,12 @@ void mj_setTotalmass(mjModel* m, mjtNum newmass) { } - // version number int mj_version(void) { return mjVERSION; } - // current version of MuJoCo as a null-terminated string const char* mj_versionString(void) { static const char versionstring[] = mjVERSIONSTRING; @@ -647,7 +626,6 @@ const char* mj_versionString(void) { } - // return total size of data in a contact sensor bitfield specification int mju_condataSize(int dataspec) { int size = 0; diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 95055d72..24e7b05a 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -37,7 +37,6 @@ void mju_zero3(mjtNum res[3]) { } - // vec1 == vec2 int mju_equal3(const mjtNum vec1[3], const mjtNum vec2[3]) { return mju_abs(vec1[0] - vec2[0]) < mjMINVAL && @@ -46,7 +45,6 @@ int mju_equal3(const mjtNum vec1[3], const mjtNum vec2[3]) { } - // res = vec void mju_copy3(mjtNum res[3], const mjtNum data[3]) { res[0] = data[0]; @@ -55,7 +53,6 @@ void mju_copy3(mjtNum res[3], const mjtNum data[3]) { } - // res = vec*scl void mju_scl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl) { res[0] = vec[0] * scl; @@ -64,7 +61,6 @@ void mju_scl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl) { } - // res = vec1 + vec2 void mju_add3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3]) { res[0] = vec1[0] + vec2[0]; @@ -73,7 +69,6 @@ void mju_add3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3]) { } - // res = vec1 - vec2 void mju_sub3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3]) { res[0] = vec1[0] - vec2[0]; @@ -82,7 +77,6 @@ void mju_sub3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3]) { } - // res += vec void mju_addTo3(mjtNum res[3], const mjtNum vec[3]) { res[0] += vec[0]; @@ -91,7 +85,6 @@ void mju_addTo3(mjtNum res[3], const mjtNum vec[3]) { } - // res -= vec void mju_subFrom3(mjtNum res[3], const mjtNum vec[3]) { res[0] -= vec[0]; @@ -100,7 +93,6 @@ void mju_subFrom3(mjtNum res[3], const mjtNum vec[3]) { } - // res += vec*scl void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl) { res[0] += vec[0] * scl; @@ -109,7 +101,6 @@ void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl) { } - // res = vec1 + vec2*scl void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl) { res[0] = vec1[0] + scl*vec2[0]; @@ -118,7 +109,6 @@ void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtN } - // normalize vector, return length before normalization mjtNum mju_normalize3(mjtNum vec[3]) { mjtNum norm = mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]); @@ -138,21 +128,18 @@ mjtNum mju_normalize3(mjtNum vec[3]) { } - // compute vector length (without normalizing) mjtNum mju_norm3(const mjtNum vec[3]) { return mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]); } - // vector dot-product mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]) { return vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]; } - // Cartesian distance between 3D vectors mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]) { mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]}; @@ -160,7 +147,6 @@ mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]) { } - // multiply 3-by-3 matrix by vector void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) { mjtNum tmp[3] = { @@ -174,7 +160,6 @@ void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) { } - // multiply transposed 3-by-3 matrix by vector void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) { mjtNum tmp[3] = { @@ -188,7 +173,6 @@ void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]) { } - // multiply 3x3 matrices, void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[3] + mat1[2]*mat2[6]; @@ -203,7 +187,6 @@ void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { } - // multiply 3x3 matrices, first argument transposed void mju_mulMatTMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { res[0] = mat1[0]*mat2[0] + mat1[3]*mat2[3] + mat1[6]*mat2[6]; @@ -218,7 +201,6 @@ void mju_mulMatTMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) } - // multiply 3x3 matrices, second argument transposed void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[1] + mat1[2]*mat2[2]; @@ -233,7 +215,6 @@ void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) } - //------------------------------ 4D vector and matrix-vector operations ---------------------------- // res = 0 @@ -245,7 +226,6 @@ void mju_zero4(mjtNum res[4]) { } - // res = (1,0,0,0) void mju_unit4(mjtNum res[4]) { res[0] = 1; @@ -264,7 +244,6 @@ void mju_copy4(mjtNum res[4], const mjtNum data[4]) { } - // normalize vector, return length before normalization mjtNum mju_normalize4(mjtNum vec[4]) { mjtNum norm = mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2] + vec[3]*vec[3]); @@ -286,7 +265,6 @@ mjtNum mju_normalize4(mjtNum vec[4]) { } - //------------------------------ vector operations ------------------------------------------------- // res = 0 @@ -295,7 +273,6 @@ void mju_zero(mjtNum* res, int n) { } - // res = val void mju_fill(mjtNum* res, mjtNum val, int n) { for (int i=0; i < n; i++) { @@ -304,14 +281,12 @@ void mju_fill(mjtNum* res, mjtNum val, int n) { } - // res = vec void mju_copy(mjtNum* res, const mjtNum* vec, int n) { memcpy(res, vec, n*sizeof(mjtNum)); } - // sum(vec) mjtNum mju_sum(const mjtNum* vec, int n) { mjtNum res = 0; @@ -324,7 +299,6 @@ mjtNum mju_sum(const mjtNum* vec, int n) { } - // sum(abs(vec)) mjtNum mju_L1(const mjtNum* vec, int n) { mjtNum res = 0; @@ -337,7 +311,6 @@ mjtNum mju_L1(const mjtNum* vec, int n) { } - // res = vec*scl void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { int i = 0; @@ -382,7 +355,6 @@ void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { } - // res = vec1 + vec2 void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { int i = 0; @@ -425,7 +397,6 @@ void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { } - // res = vec1 - vec2 void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { int i = 0; @@ -468,7 +439,6 @@ void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { } - // res += vec void mju_addTo(mjtNum* res, const mjtNum* vec, int n) { int i = 0; @@ -511,7 +481,6 @@ void mju_addTo(mjtNum* res, const mjtNum* vec, int n) { } - // res -= vec void mju_subFrom(mjtNum* res, const mjtNum* vec, int n) { int i = 0; @@ -554,7 +523,6 @@ void mju_subFrom(mjtNum* res, const mjtNum* vec, int n) { } - // res += vec*scl void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { int i = 0; @@ -646,7 +614,6 @@ void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, } - // normalize vector, return length before normalization mjtNum mju_normalize(mjtNum* res, int n) { mjtNum norm = (mjtNum)mju_sqrt(mju_dot(res, res, n)); @@ -668,14 +635,12 @@ mjtNum mju_normalize(mjtNum* res, int n) { } - // compute vector length (without normalizing) mjtNum mju_norm(const mjtNum* res, int n) { return mju_sqrt(mju_dot(res, res, n)); } - // vector dot-product mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) { mjtNum res = 0; @@ -751,7 +716,6 @@ void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, in } - // multiply transposed matrix and vector void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc) { mjtNum tmp; @@ -765,7 +729,6 @@ void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, i } - // multiply square matrix with vectors on both sides: return vec1'*mat*vec2 mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n) { mjtNum res = 0; @@ -776,7 +739,6 @@ mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec } - //------------------------------ matrix operations ------------------------------------------------- // transpose matrix @@ -789,7 +751,6 @@ void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc) { } - // symmetrize square matrix res = (mat + mat')/2 void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n) { for (int i=0; i < n; i++) { @@ -801,7 +762,6 @@ void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n) { } - // identity matrix void mju_eye(mjtNum* mat, int n) { mju_zero(mat, n*n); @@ -811,7 +771,6 @@ void mju_eye(mjtNum* mat, int n) { } - //------------------------------ matrix-matrix operations ------------------------------------------ // multiply matrices, exploit sparsity of mat1 @@ -831,7 +790,6 @@ void mju_mulMatMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, } - // multiply matrices, second argument transposed void mju_mulMatMatT(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, int r1, int c1, int r2) { @@ -887,7 +845,6 @@ void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, in } - // multiply matrices, first argument transposed void mju_mulMatTMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, int r1, int c1, int c2) { diff --git a/src/engine/engine_util_container.c b/src/engine/engine_util_container.c index 7a44e257..204e9a51 100644 --- a/src/engine/engine_util_container.c +++ b/src/engine/engine_util_container.c @@ -39,7 +39,6 @@ mjArrayList* mju_arrayListCreate(mjData* d, size_t element_size, size_t initial_ } - // returns total number of elements in mjArrayList size_t mju_arrayListSize(const mjArrayList* array_list) { const mjArrayList* cursor = array_list; @@ -52,7 +51,6 @@ size_t mju_arrayListSize(const mjArrayList* array_list) { } - // copies one element into an mjArrayList void mju_arrayListAdd(mjArrayList* array_list, void* element) { mjArrayList* cursor = array_list; @@ -73,7 +71,6 @@ void mju_arrayListAdd(mjArrayList* array_list, void* element) { } - // returns pointer to element at index, NULL if out of bounds void* mju_arrayListAt(const mjArrayList* array_list, size_t index) { // if the index is larger than the current capacity, then it is in a later segment diff --git a/src/engine/engine_util_errmem.c b/src/engine/engine_util_errmem.c index 04e98009..cbd373d3 100644 --- a/src/engine/engine_util_errmem.c +++ b/src/engine/engine_util_errmem.c @@ -114,7 +114,6 @@ void mju_writeLog(const char* type, const char* msg) { } - void mju_error_raw(const char* msg) { if (_mjPRIVATE_tls_error_fn) { _mjPRIVATE_tls_error_fn(msg); @@ -131,7 +130,6 @@ void mju_error_raw(const char* msg) { } - void mju_error_v(const char* msg, va_list args) { // Format msg into errmsg char errmsg[1024]; @@ -140,7 +138,6 @@ void mju_error_v(const char* msg, va_list args) { } - // write message to logfile and console, pause and exit void mju_error(const char* msg, ...) { va_list args; @@ -150,7 +147,6 @@ void mju_error(const char* msg, ...) { } - // write message to logfile and console void mju_warning(const char* msg, ...) { char wrnmsg[1024]; @@ -197,7 +193,6 @@ void mju_warning_s(const char* msg, const char* text) { } - //------------------------------ malloc and free --------------------------------------------------- // allocate memory; byte-align on 64; pad size to multiple of 64 diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 0267acb3..9bc2a19a 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -51,7 +51,6 @@ static mjtByte is_intersect(const mjtNum* p1, const mjtNum* p2, } - // curve length along circle static mjtNum length_circle(const mjtNum* p0, const mjtNum* p1, int ind, mjtNum radius) { mjtNum p0n[2] = {p0[0], p0[1]}; @@ -72,7 +71,6 @@ static mjtNum length_circle(const mjtNum* p0, const mjtNum* p1, int ind, mjtNum } - // 2D circle wrap // input: pair of 2D endpoints in end[4], optional 2D side point in side[2], radius // output: return length of circular wrap or -1 @@ -154,7 +152,6 @@ static mjtNum wrap_circle(mjtNum pnt[4], const mjtNum end[4], const mjtNum* side } - // 2D inside wrap // input: pair of 2D endpoints in end[4], radius // output: pair of 2D points in pnt[4]; return 0 if wrap, -1 if no wrap @@ -274,7 +271,6 @@ static mjtNum wrap_inside(mjtNum pnt[4], const mjtNum end[4], mjtNum radius) { } - // wrap tendons around spheres and cylinders // input: x0, x1: pair of 3D endpoints // xpos, xmat, radius: position, orientation and radius of geom @@ -421,7 +417,6 @@ mjtNum mju_wrap(mjtNum wpnt[6], const mjtNum x0[3], const mjtNum x1[3], } - // all 3 semi-axes of a geom void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type) { switch (type) { @@ -543,7 +538,6 @@ void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int or } - //------------------------------ actuator models --------------------------------------------------- // normalized muscle length-gain curve @@ -572,7 +566,6 @@ mjtNum mju_muscleGainLength(mjtNum length, mjtNum lmin, mjtNum lmax) { } - // muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax) mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], mjtNum acc0, const mjtNum prm[9]) { @@ -618,7 +611,6 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], } - // muscle passive force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax) mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], mjtNum acc0, const mjtNum prm[9]) { @@ -654,7 +646,6 @@ mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], } - // muscle time constant with optional smoothing mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum tau_deact, mjtNum smoothing_width) { @@ -674,7 +665,6 @@ mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum tau_deac } - // muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width) mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) { // clamp control @@ -696,7 +686,6 @@ mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) { } - //---------------------------------------- Base64 -------------------------------------------------- // decoding function for Base64 @@ -725,7 +714,6 @@ static uint32_t _decode(char ch) { } - // encode data as Base64 into buf (including padding and null char) // returns number of chars written in buf: 4 * [(ndata + 2) / 3] + 1 size_t mju_encodeBase64(char* buf, const uint8_t* data, size_t ndata) { @@ -779,7 +767,6 @@ size_t mju_encodeBase64(char* buf, const uint8_t* data, size_t ndata) { } - // return size in decoded bytes if s is a valid Base64 encoding // return 0 if s is empty or invalid Base64 encoding size_t mju_isValidBase64(const char* s) { @@ -810,7 +797,6 @@ size_t mju_isValidBase64(const char* s) { } - // decode valid Base64 in string s into buf, undefined behavior if s is not valid Base64 // returns number of bytes decoded (upper limit of 3 * (strlen(s) / 4)) size_t mju_decodeBase64(uint8_t* buf, const char* s) { @@ -841,7 +827,6 @@ size_t mju_decodeBase64(uint8_t* buf, const char* s) { } - //------------------------------ miscellaneous ----------------------------------------------------- // convert contact force to pyramid representation @@ -861,7 +846,6 @@ void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, i } - // convert pyramid representation to contact force void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, int dim) { // special handling of frictionless contacts @@ -883,7 +867,6 @@ void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, i } - // integrate spring-damper analytically, return pos(t) mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) { mjtNum det, c1, c2, r1, r2, w; @@ -936,7 +919,6 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t) } - // return 1 if point is outside box given by pos, mat, size * inflate // return -1 if point is inside box given by pos, mat, size / inflate // return 0 if point is between the inflated and deflated boxes @@ -982,7 +964,6 @@ int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[ } - // print matrix to screen void mju_printMat(const mjtNum* mat, int nr, int nc) { for (int r=0; r < nr; r++) { @@ -995,7 +976,6 @@ void mju_printMat(const mjtNum* mat, int nr, int nc) { } - // print sparse matrix to screen void mju_printMatSparse(const mjtNum* mat, int nr, const int* rownnz, const int* rowadr, @@ -1010,7 +990,6 @@ void mju_printMatSparse(const mjtNum* mat, int nr, } - // min function, avoid re-evaluation mjtNum mju_min(mjtNum a, mjtNum b) { if (a <= b) { @@ -1021,7 +1000,6 @@ mjtNum mju_min(mjtNum a, mjtNum b) { } - // max function, avoid re-evaluation mjtNum mju_max(mjtNum a, mjtNum b) { if (a >= b) { @@ -1032,7 +1010,6 @@ mjtNum mju_max(mjtNum a, mjtNum b) { } - // clip x to the range [min, max] mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) { if (x < min) { @@ -1045,7 +1022,6 @@ mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) { } - // sign function mjtNum mju_sign(mjtNum x) { if (x < 0) { @@ -1058,7 +1034,6 @@ mjtNum mju_sign(mjtNum x) { } - // round to nearest integer int mju_round(mjtNum x) { mjtNum lower = floor(x); @@ -1072,7 +1047,6 @@ int mju_round(mjtNum x) { } - // convert type id to type name const char* mju_type2Str(int type) { switch ((mjtObj) type) { @@ -1160,7 +1134,6 @@ const char* mju_type2Str(int type) { } - // convert type id to type name int mju_str2Type(const char* str) { if (!strcmp(str, "body")) { @@ -1269,7 +1242,6 @@ int mju_str2Type(const char* str) { } - // return human readable number of bytes using standard letter suffix const char* mju_writeNumBytes(size_t nbytes) { int i; @@ -1290,7 +1262,6 @@ const char* mju_writeNumBytes(size_t nbytes) { } - // warning text const char* mju_warningText(int warning, size_t info) { static mjTHREADLOCAL char str[1000]; @@ -1341,14 +1312,12 @@ const char* mju_warningText(int warning, size_t info) { } - // return 1 if nan or abs(x)>mjMAXVAL, 0 otherwise int mju_isBad(mjtNum x) { return (x != x || x > mjMAXVAL || x < -mjMAXVAL); } - // return 1 if all elements are 0 int mju_isZero(const mjtNum* vec, int n) { for (int i=0; i < n; i++) { @@ -1361,7 +1330,6 @@ int mju_isZero(const mjtNum* vec, int n) { } - // return 1 if all elements are 0 int mju_isZeroByte(const unsigned char* vec, int n) { size_t i = 0; @@ -1387,21 +1355,18 @@ int mju_isZeroByte(const unsigned char* vec, int n) { } - // set integer vector to 0 void mju_zeroInt(int* res, int n) { memset(res, 0, n*sizeof(int)); } - // copy int vector vec into res void mju_copyInt(int* res, const int* vec, int n) { memcpy(res, vec, n*sizeof(int)); } - // standard normal random number generator (optional second number) mjtNum mju_standardNormal(mjtNum* num2) { const mjtNum scale = 2.0/((mjtNum)RAND_MAX); @@ -1422,7 +1387,6 @@ mjtNum mju_standardNormal(mjtNum* num2) { } - // convert from float to mjtNum void mju_f2n(mjtNum* res, const float* vec, int n) { for (int i=0; i < n; i++) { @@ -1431,7 +1395,6 @@ void mju_f2n(mjtNum* res, const float* vec, int n) { } - // convert from mjtNum to float void mju_n2f(float* res, const mjtNum* vec, int n) { for (int i=0; i < n; i++) { @@ -1448,7 +1411,6 @@ void mju_d2n(mjtNum* res, const double* vec, int n) { } - // convert from mjtNum to double void mju_n2d(double* res, const mjtNum* vec, int n) { for (int i=0; i < n; i++) { @@ -1457,7 +1419,6 @@ void mju_n2d(double* res, const mjtNum* vec, int n) { } - // gather void mju_gather(mjtNum* restrict res, const mjtNum* restrict vec, const int* restrict ind, int n) { for (int i=0; i < n; i++) { @@ -1466,7 +1427,6 @@ void mju_gather(mjtNum* restrict res, const mjtNum* restrict vec, const int* res } - // masked gather (set to 0 at negative indices) void mju_gatherMasked(mjtNum* restrict res, const mjtNum* restrict vec, const int* restrict ind, int n) { @@ -1476,7 +1436,6 @@ void mju_gatherMasked(mjtNum* restrict res, const mjtNum* restrict vec, } - // scatter void mju_scatter(mjtNum* restrict res, const mjtNum* restrict vec, const int* restrict ind, int n) { for (int i=0; i < n; i++) { @@ -1485,7 +1444,6 @@ void mju_scatter(mjtNum* restrict res, const mjtNum* restrict vec, const int* re } - // gather integers void mju_gatherInt(int* restrict res, const int* restrict vec, const int* restrict ind, int n) { for (int i=0; i < n; i++) { @@ -1494,7 +1452,6 @@ void mju_gatherInt(int* restrict res, const int* restrict vec, const int* restri } - // scatter integers void mju_scatterInt(int* restrict res, const int* restrict vec, const int* restrict ind, int n) { for (int i=0; i < n; i++) { @@ -1503,7 +1460,6 @@ void mju_scatterInt(int* restrict res, const int* restrict vec, const int* restr } - // build gather indices mapping src to res, assumes pattern(res) \subseteq pattern(src) void mju_sparseMap(int* map, int nr, const int* res_rowadr, const int* res_rownnz, const int* res_colind, @@ -1527,7 +1483,6 @@ void mju_sparseMap(int* map, int nr, } - // build masked-gather map to copy a lower-triangular src into symmetric res // `cursor` is a preallocated buffer of size `nr` void mju_lower2SymMap(int* map, int nr, @@ -1594,7 +1549,6 @@ void mju_lower2SymMap(int* map, int nr, } - // insertion sort, increasing order void mju_insertionSort(mjtNum* list, int n) { for (int i=1; i < n; i++) { @@ -1609,7 +1563,6 @@ void mju_insertionSort(mjtNum* list, int n) { } - // integer insertion sort, increasing order void mju_insertionSortInt(int* list, int n) { for (int i=1; i < n; i++) { @@ -1624,7 +1577,6 @@ void mju_insertionSortInt(int* list, int n) { } - // Halton sequence mjtNum mju_Halton(int index, int base) { int n0 = index; @@ -1643,7 +1595,6 @@ mjtNum mju_Halton(int index, int base) { } - // Call strncpy, then set dst[n-1] = 0. char* mju_strncpy(char *dst, const char *src, int n) { if (dst && src && n > 0) { @@ -1655,7 +1606,6 @@ char* mju_strncpy(char *dst, const char *src, int n) { } - // sigmoid function over 0<=x<=1 using quintic polynomial mjtNum mju_sigmoid(mjtNum x) { // fast return diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 324a7cc9..79f1730b 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -61,7 +61,6 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) { } - // Cholesky solve void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n) { // copy if source and destination are different @@ -92,7 +91,6 @@ void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n) { } - // Cholesky rank-one update: L*L' +/- x*x'; return rank int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) { int rank = n; @@ -137,7 +135,6 @@ int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) { } - //---------------------------- sparse Cholesky ----------------------------------------------------- // sparse reverse-order Cholesky decomposition: mat = L'*L; return 'rank' @@ -190,7 +187,6 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, } - // precount row non-zeros of reverse-Cholesky factor L, return total non-zeros // based on ldl_symbolic from 'Algorithm 8xx: a concise sparse Cholesky factorization package' // reads pattern from upper triangle @@ -244,7 +240,6 @@ int mju_cholFactorCount(int* L_rownnz, const int* rownnz, const int* rowadr, con } - // sparse reverse-order Cholesky solve void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n, const int* rownnz, const int* rowadr, const int* colind) { @@ -288,7 +283,6 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int } - // sparse reverse-order Cholesky rank-one update: L'*L +/- x*x'; return rank // x is sparse, change in sparsity pattern of mat is not allowed int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, @@ -444,7 +438,6 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense, } - // solve with band-Cholesky decomposition void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int ntotal, int nband, int ndense) { @@ -509,7 +502,6 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec, } - // address of diagonal element i in band-dense matrix representation int mju_bandDiag(int i, int ntotal, int nband, int ndense) { int nsparse = ntotal-ndense; @@ -526,7 +518,6 @@ int mju_bandDiag(int i, int ntotal, int nband, int ndense) { } - // convert band matrix to dense matrix void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int ndense, mjtByte flg_sym) { @@ -560,7 +551,6 @@ void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n } - // convert dense matrix to band matrix void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int ndense) { int nsparse = ntotal-ndense; @@ -581,7 +571,6 @@ void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n } - // multiply band-diagonal matrix with vector void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int ntotal, int nband, int ndense, int nvec, mjtByte flg_sym) { @@ -697,7 +686,6 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, } - // solve mat*res=vec given LU factorization of mat void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n, const int* rownnz, const int* rowadr, const int* diag, const int* colind) { @@ -729,7 +717,6 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n, } - //--------------------------- eigen decomposition -------------------------------------------------- // eigenvalue decomposition of symmetric 3x3 matrix @@ -830,7 +817,6 @@ int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum ma } - //---------------------------------- QCQP ---------------------------------------------------------- // solve QCQP in 2 dimensions: @@ -900,7 +886,6 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, } - // solve QCQP in 3 dimensions: // min 0.5*x'*A*x + x'*b s.t. sum (xi/di)^2 <= r^2 // return 0 if unconstrained, 1 if constrained @@ -989,7 +974,6 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, } - // solve QCQP in n dimensions: // min 0.5*x'*A*x + x'*b s.t. sum (xi/di)^2 <= r^2 // return 0 if unconstrained, 1 if constrained @@ -1062,7 +1046,6 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin, } - //--------------------------- box-constrained quadratic program ------------------------------------ // minimize 0.5*x'*H*x + x'*g s.t. lower <= x <= upper, return rank or -1 if failed @@ -1104,7 +1087,6 @@ int mju_boxQP(mjtNum* res, mjtNum* R, int* index, } - // allocate heap memory for box-constrained Quadratic Program // as in mju_boxQP, index, lower and upper are optional // free all pointers with mju_free() @@ -1124,7 +1106,6 @@ void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, } - // local enum encoding mju_boxQP solver status (purely for readability) enum mjtStatusBoxQP { mjBOXQP_NOT_SPD = -1, // Hessian is not positive definite diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index f4f73c7a..599f1e01 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -58,7 +58,6 @@ void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2, } - // dot-product, both vectors are sparse mjtNum mju_dotSparse2(const mjtNum* vec1, const int* ind1, int nnz1, const mjtNum* vec2, const int* ind2, int nnz2) { @@ -91,7 +90,6 @@ mjtNum mju_dotSparse2(const mjtNum* vec1, const int* ind1, int nnz1, } - // convert matrix from dense to sparse // nnz is size of res and colind, return 1 if too small, 0 otherwise int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, @@ -129,7 +127,6 @@ int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, } - // convert matrix from sparse to dense void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind) { @@ -145,7 +142,6 @@ void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, } - // multiply sparse matrix and dense vector: res = mat * vec. void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, const int* rownnz, const int* rowadr, @@ -161,7 +157,6 @@ void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, } - // multiply transposed sparse matrix and dense vector: res = mat' * vec. void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind) { @@ -221,7 +216,6 @@ void mju_addToSymSparse(mjtNum* res, const mjtNum* mat, int n, } - // multiply symmetric matrix (only lower triangle represented) by vector: // res = (mat + strict_upper(mat')) * vec void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat, @@ -252,7 +246,6 @@ void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat, } - // count the number of non-zeros in the sum of two sparse vectors int mju_combineSparseCount(int a_nnz, int b_nnz, const int* a_ind, const int* b_ind) { int a = 0, b = 0, c_nnz = 0; @@ -279,7 +272,6 @@ int mju_combineSparseCount(int a_nnz, int b_nnz, const int* a_ind, const int* b_ } - // incomplete combine sparse: dst = a*dst + b*src at common indices void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, const int* dst_ind, const int* src_ind) { @@ -327,7 +319,6 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu } - // dst += scl*src, only at common non-zero indices void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src, int nnzdst, const int* inddst, @@ -379,7 +370,6 @@ void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src, } - // add to sparse matrix: dst = dst + scl*src, return nnz of result int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum scl, int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind, @@ -447,7 +437,6 @@ int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum s } - // add(merge) two chains int mju_addChains(int* res, int n, int NV1, int NV2, const int* chain1, const int* chain2) { @@ -497,7 +486,6 @@ int mju_addChains(int* res, int n, int NV1, int NV2, } - // compress sparse matrix, remove elements with abs(value) <= minval, return total non-zeros int mju_compressSparse(mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind, mjtNum minval) { @@ -529,7 +517,6 @@ int mju_compressSparse(mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, in } - // transpose sparse matrix, optionally compute row supernodes void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind, int* res_rowsuper, @@ -606,7 +593,6 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc, } - // construct row supernodes void mju_superSparse(int nr, int* rowsuper, const int* rownnz, const int* rowadr, const int* colind) { @@ -743,7 +729,6 @@ void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc) { } - // max number of supernodes handled #define mjMAXSUPER 8 @@ -1092,7 +1077,6 @@ void mju_sqrMatTDSparse_row(mjtNum* res, const mjtNum* mat, const mjtNum* matT, } - // block-diagonalize a dense matrix // res output matrix // mat input matrix diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 6c0dceb6..1e433467 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -52,7 +52,6 @@ void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) { } - // negate quaternion void mju_negQuat(mjtNum res[4], const mjtNum quat[4]) { res[0] = quat[0]; @@ -62,7 +61,6 @@ void mju_negQuat(mjtNum res[4], const mjtNum quat[4]) { } - // multiply quaternions void mju_mulQuat(mjtNum res[4], const mjtNum qa[4], const mjtNum qb[4]) { mjtNum tmp[4] = { @@ -78,7 +76,6 @@ void mju_mulQuat(mjtNum res[4], const mjtNum qa[4], const mjtNum qb[4]) { } - // multiply quaternion and axis void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]) { mjtNum tmp[4] = { @@ -94,7 +91,6 @@ void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]) } - // convert axisAngle to quaternion void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle) { // zero angle: null quat @@ -116,7 +112,6 @@ void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle) { } - // convert quaternion (corresponding to orientation difference) to 3D velocity void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) { mjtNum axis[3] = {quat[1], quat[2], quat[3]}; @@ -133,7 +128,6 @@ void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) { } - // Subtract quaternions, express as 3D velocity: qb*quat(res) = qa. void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) { // qdif = neg(qb)*qa @@ -146,7 +140,6 @@ void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) { } - // convert quaternion to 3D rotation matrix void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) { // null quat: identity @@ -189,7 +182,6 @@ void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) { } - // convert 3D rotation matrix to quaternion void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) { // q0 largest @@ -228,7 +220,6 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) { } - // time-derivative of quaternion, given 3D rotational velocity void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) { res[0] = 0.5*(-vel[0]*quat[1] - vel[1]*quat[2] - vel[2]*quat[3]); @@ -238,7 +229,6 @@ void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) { } - // integrate quaternion given 3D angular velocity void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) { mjtNum angle, tmp[4], qrot[4]; @@ -252,7 +242,6 @@ void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) { } - // compute quaternion performing rotation from z-axis to given vector void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { mjtNum axis[3], a, vn[3] = {vec[0], vec[1], vec[2]}, z[3] = {0, 0, 1}; @@ -287,7 +276,6 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { } - // extract 3D rotation from an arbitrary 3x3 matrix static const mjtNum rotEPS = 1e-9; int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) { @@ -328,7 +316,6 @@ int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) { } - //------------------------------ pose operations (quat, pos) --------------------------------------- // multiply two poses @@ -345,7 +332,6 @@ void mju_mulPose(mjtNum posres[3], mjtNum quatres[4], } - // negate pose void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const mjtNum quat[4]) { // qres = neg(quat) @@ -357,7 +343,6 @@ void mju_negPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos[3], const } - // transform vector by pose void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], const mjtNum vec[3]) { // res = quat*vec + pos @@ -366,7 +351,6 @@ void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], co } - //------------------------------ spatial algebra --------------------------------------------------- // vector cross-product, 3D @@ -382,7 +366,6 @@ void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]) { } - // cross-product for motion vector void mju_crossMotion(mjtNum res[6], const mjtNum vel[6], const mjtNum v[6]) { res[0] = -vel[2]*v[1] + vel[1]*v[2]; @@ -398,7 +381,6 @@ void mju_crossMotion(mjtNum res[6], const mjtNum vel[6], const mjtNum v[6]) { } - // cross-product for force vectors void mju_crossForce(mjtNum res[6], const mjtNum vel[6], const mjtNum f[6]) { res[0] = -vel[2]*f[1] + vel[1]*f[2]; @@ -414,7 +396,6 @@ void mju_crossForce(mjtNum res[6], const mjtNum vel[6], const mjtNum f[6]) { } - // express inertia in com-based frame void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9], const mjtNum dif[3], mjtNum mass) { @@ -449,7 +430,6 @@ void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9], } - // multiply 6D vector (rotation, translation) by 6D inertia matrix void mju_mulInertVec(mjtNum res[6], const mjtNum i[10], const mjtNum v[6]) { res[0] = i[0]*v[0] + i[3]*v[1] + i[4]*v[2] - i[8]*v[4] + i[7]*v[5]; @@ -461,7 +441,6 @@ void mju_mulInertVec(mjtNum res[6], const mjtNum i[10], const mjtNum v[6]) { } - // express motion axis in com-based frame void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]) { // hinge @@ -478,7 +457,6 @@ void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]) { } - // multiply dof matrix (6-by-n, transposed) by vector (n-by-1) void mju_mulDofVec(mjtNum* res, const mjtNum* dof, const mjtNum* vec, int n) { if (n == 1) { @@ -491,7 +469,6 @@ void mju_mulDofVec(mjtNum* res, const mjtNum* dof, const mjtNum* vec, int n) { } - // transform 6D motion or force vector between frames // flg_force: determines vector type (motion or force) // rotnew2old: rotation that maps vectors from new to old frame, @@ -526,7 +503,6 @@ void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force, } - // make 3D frame given X axis (normal) and possibly Y axis (tangent 1) void mju_makeFrame(mjtNum frame[9]) { mjtNum tmp[3]; @@ -557,7 +533,6 @@ void mju_makeFrame(mjtNum frame[9]) { } - // convert sequence of Euler angles (radians) to quaternion // seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq) { diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c index 41b3fa2e..14b9ebb5 100644 --- a/src/engine/engine_vis_init.c +++ b/src/engine/engine_vis_init.c @@ -275,7 +275,6 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) { } - // free abstract scene void mjv_freeScene(mjvScene* scn) { // free buffers allocated by mjv_makeScene @@ -306,14 +305,12 @@ void mjv_freeScene(mjvScene* scn) { } - // set default scene void mjv_defaultScene(mjvScene* scn) { memset(scn, 0, sizeof(mjvScene)); } - // set default visualization options void mjv_defaultOption(mjvOption* vopt) { vopt->label = mjLABEL_NONE; @@ -339,7 +336,6 @@ void mjv_defaultOption(mjvOption* vopt) { } - // set default camera void mjv_defaultCamera(mjvCamera* cam) { memset(cam, 0, sizeof(mjvCamera)); @@ -353,7 +349,6 @@ void mjv_defaultCamera(mjvCamera* cam) { } - // set default free camera void mjv_defaultFreeCamera(const mjModel* m, mjvCamera* cam) { memset(cam, 0, sizeof(mjvCamera)); @@ -371,7 +366,6 @@ void mjv_defaultFreeCamera(const mjModel* m, mjvCamera* cam) { } - // set default perturbation void mjv_defaultPerturb(mjvPerturb* pert) { memset(pert, 0, sizeof(mjvPerturb)); @@ -383,7 +377,6 @@ void mjv_defaultPerturb(mjvPerturb* pert) { } - // predefined line colors static const float _linergb[8][3] = { {1.0, 0.3, 0.3}, @@ -453,7 +446,6 @@ void mjv_defaultFigure(mjvFigure* fig) { } - // compute rbound for mjvGeom float mjv_rbound(const mjvGeom* geom) { // model geom: return diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 09ffc7d9..537efb47 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -63,7 +63,6 @@ void mjv_room2model(mjtNum* modelpos, mjtNum* modelquat, const mjtNum* roompos, } - // transform pose from model to room space void mjv_model2room(mjtNum* roompos, mjtNum* roomquat, const mjtNum* modelpos, const mjtNum* modelquat, const mjvScene* scn) { @@ -95,7 +94,6 @@ void mjv_model2room(mjtNum* roompos, mjtNum* roomquat, const mjtNum* modelpos, } - // get camera info in model space: average left and right OpenGL cameras void mjv_cameraInModel(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvScene* scn) { mjtNum pos[3], fwd[3], u[3], quat[4]; @@ -169,7 +167,6 @@ void mjv_cameraInModel(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSc } - // get camera info in room space: average left and right OpenGL cameras void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvScene* scn) { mjtNum pos[3], fwd[3], u[3]; @@ -219,7 +216,6 @@ void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSce } - // get frustum height at unit distance from camera; average left and right OpenGL cameras mjtNum mjv_frustumHeight(const mjvScene* scn) { const mjvGLCamera* cam1 = scn->camera; @@ -251,7 +247,6 @@ mjtNum mjv_frustumHeight(const mjvScene* scn) { } - // rotate 3D vec in horizontal plane by angle between (0,1) and (forward_x,forward_y) void mjv_alignToCamera(mjtNum* res, const mjtNum* vec, const mjtNum* forward) { mjtNum xaxis[2], yaxis[2]; @@ -271,7 +266,6 @@ void mjv_alignToCamera(mjtNum* res, const mjtNum* vec, const mjtNum* forward) { } - // convert 2D mouse motion to z-aligned 3D world coordinates static void convert2D(mjtNum* res, int action, mjtNum dx, mjtNum dy, const mjtNum* forward) { mjtNum vec[3]; @@ -316,7 +310,6 @@ static void convert2D(mjtNum* res, int action, mjtNum dx, mjtNum dy, const mjtNu } - // move camera with mouse; action is mjtMouse void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, const mjvScene* scn, mjvCamera* cam) { @@ -407,7 +400,6 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, } - // move perturb object with mouse; action is mjtMouse void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx, mjtNum reldy, const mjvScene* scn, mjvPerturb* pert) { @@ -498,7 +490,6 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx } - // move model with mouse; action is mjtMouse void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, const mjtNum roomup[3], mjvScene* scn) { @@ -576,7 +567,6 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, } - // copy perturb pos,quat from selected body; set scale for perturbation void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert) { mj_markStack(d); @@ -636,7 +626,6 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur } - // set perturb pos,quat in d->mocap when selected body is mocap, and in d->qpos otherwise // d->qpos written only if flg_paused and subtree root for selected body has free joint void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, int flg_paused) { @@ -690,7 +679,6 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, i } - // set perturb force,torque in d->xfrc_applied, if selected body is dynamic void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) { int sel = pert->select; @@ -763,7 +751,6 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) } - // return the average of two OpenGL cameras mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2) { mjtNum pos[3], forward[3], up[3], projection, tmp1[3], tmp2[3]; @@ -812,7 +799,6 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2) } - // Select geom, flex or skin with mouse, return bodyid; -1: none selected. int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, mjtNum aspectratio, mjtNum relx, mjtNum rely, diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 3aa3687b..a80755a4 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -36,6 +36,7 @@ #include "engine/engine_vis_init.h" #include "engine/engine_vis_interact.h" + //----------------------------- utility functions and macros --------------------------------------- static const mjtNum IDENTITY[9] = {1, 0, 0, @@ -49,7 +50,6 @@ static void f2f(float* dest, const float* src, int n) { } - // make text label static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) { const char* typestr = mju_type2Str(type); @@ -71,7 +71,6 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) { } - // convert HSV to RGB static void hsv2rgb(float *RGB, float H, float S, float V) { float R, G, B; @@ -106,7 +105,6 @@ static void hsv2rgb(float *RGB, float H, float S, float V) { } - static const float kIslandSaturation = 0.8; static const float kIslandValue = 0.7; @@ -136,7 +134,6 @@ static void mixcolor(float rgba[4], const float ref[4], int flg1, int flg2) { } - // a body is static if it is welded to the world and is not a mocap body static int bodycategory(const mjModel* m, int bodyid) { if (m->body_weldid[bodyid] == 0 && m->body_mocapid[bodyid] == -1) { @@ -180,7 +177,6 @@ void releaseGeom(mjvGeom** geom, mjvScene* scn) { } - // make a triangle in thisgeom at coordinates v0, v1, v2 with a given color static void makeTriangle(mjvGeom* thisgeom, const mjtNum v0[3], const mjtNum v1[3], const mjtNum v2[3], const float rgba[4]) { @@ -196,7 +192,6 @@ static void makeTriangle(mjvGeom* thisgeom, const mjtNum v0[3], const mjtNum v1[ } - // copy material fields from model to visual geom static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* rgba, const mjtByte* flags) { @@ -226,7 +221,6 @@ static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* } - // set (type, size, pos, mat) connector-type geom between given points // assume that mjv_initGeom was already called to set all other properties void mjv_connector(mjvGeom* geom, int type, mjtNum width, @@ -269,7 +263,6 @@ void mjv_connector(mjvGeom* geom, int type, mjtNum width, } - // initialize given fields when not NULL, set the rest to their default values void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size, const mjtNum* pos, const mjtNum* mat, const float* rgba) { @@ -353,7 +346,6 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size, } - // mark geom as selected static void markselected(const mjVisual* vis, mjvGeom* geom) { // add emission @@ -361,7 +353,6 @@ static void markselected(const mjVisual* vis, mjvGeom* geom) { } - // draw 3 cylinders representing a "frame" decor element void addFrameGeoms(mjvScene* scn, int i, mjtNum* pos, mjtNum* rot, float length, float width) { // draw separate geoms for each axis @@ -393,7 +384,6 @@ void addFrameGeoms(mjvScene* scn, int i, mjtNum* pos, mjtNum* rot, float length, } - //----------------------------- camera functions -------------------------------------------------- // computes the camera frustum @@ -410,7 +400,6 @@ static void getFrustum(float zver[2], float zhor[2], float znear, } - void mjv_cameraFrame(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], mjtNum right[3], const mjData* d, const mjvCamera* cam) { switch (cam->type) { @@ -534,7 +523,6 @@ void mjv_cameraFrustum(float zver[2], float zhor[2], float zclip[2], const mjMod } - //----------------------------- main API functions ------------------------------------------------- // add contact-related geoms in mjvObject @@ -2506,7 +2494,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } - // make list of lights only void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn) { mjvLight* thislight; @@ -2582,7 +2569,6 @@ void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn) { } - // update camera only void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn) { // return if nothing to do @@ -2659,7 +2645,6 @@ void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScen } - // construct face, flat normals static void makeFace(float* _face, float* _normal, mjtNum radius, const mjtNum* vertxpos, int nface, int i0, int i1, int i2) { @@ -2692,7 +2677,6 @@ static void makeFace(float* _face, float* _normal, mjtNum radius, const mjtNum* } - // add face normal to vertices static void addNormal(mjtNum* vertnorm, const mjtNum* vertxpos, int i0, int i1, int i2) { @@ -2713,7 +2697,6 @@ static void addNormal(mjtNum* vertnorm, const mjtNum* vertxpos, } - // construct face, smooth normals static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_flat, const mjtNum* vertnorm, const mjtNum* vertxpos, @@ -2761,7 +2744,6 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_ } - // construct side in 2D face static void makeSide(float* _face, float* _normal, mjtNum radius, const mjtNum* vertnorm, const mjtNum* vertxpos, @@ -2798,7 +2780,6 @@ static void makeSide(float* _face, float* _normal, mjtNum radius, } - // copy texcoord for face static void copyTex(float* dst, const float* src, int nface, int i0, int i1, int i2) { if (!dst || !src) { @@ -2814,7 +2795,6 @@ static void copyTex(float* dst, const float* src, int nface, int i0, int i1, int } - // update visible flexes only void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) { // save flex visualization flags in scene (needed by renderer) @@ -2974,7 +2954,6 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO } - // update all skins, here for backward API compatibility void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn) { mjvOption opt; @@ -2984,7 +2963,6 @@ void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn) { } - // update visible skins only void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, const mjvOption* opt) { // process skins @@ -3113,7 +3091,6 @@ void mjv_updateActiveSkin(const mjModel* m, const mjData* d, mjvScene* scn, cons } - // update entire scene void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, const mjvPerturb* pert, mjvCamera* cam, int catmask, mjvScene* scn) { @@ -3163,7 +3140,6 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, } - //----------------------------------- catenary functions ------------------------------------------- // returns hyperbolic cosine and optionally computes hyperbolic sine @@ -3176,14 +3152,12 @@ static inline mjtNum cosh_sinh(mjtNum x, mjtNum* sinh) { } - // returns intercept of the catenary equation static inline mjtNum catenary_intercept(mjtNum v, mjtNum h, mjtNum length) { return 1/mju_sqrt(mju_sqrt(length*length - v*v)/h - 1); } - // returns residual of catenary equation and optionally computes its gradient w.r.t b static inline mjtNum catenary_residual(mjtNum b, mjtNum intercept, mjtNum* grad) { mjtNum a = 0.5 / b; @@ -3195,7 +3169,6 @@ static inline mjtNum catenary_residual(mjtNum b, mjtNum intercept, mjtNum* grad) } - // convergence tolerance for catenary solver static const mjtNum tolerance = 1e-9; @@ -3240,7 +3213,6 @@ static inline mjtNum solve_catenary(mjtNum v, mjtNum h, mjtNum length) { } - // points along catenary of given length between x0 and x1, returns number of points int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], mjtNum length, mjtNum* catenary, int ncatenary) {