chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Change MuJoCo engine source code function-spacing convention from 3 blank lines to 2

Browse Source 
PiperOrigin-RevId: 813754244
Change-Id: I6836e41c3b021cb727e922c25c60f629b9814c93
This commit is contained in:
Yuval Tassa
2025-10-01 07:57:17 -07:00
committed by Copybara-Service
parent c439628f82
commit edbdb5195c
36 changed files with 8 additions and 651 deletions
Download Patch File Download Diff File Expand all files Collapse all files
STYLEGUIDE.md
+7 -3
View File
@@ -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`.
src/engine/engine_collision_box.c
-3
View File
@@ -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)
{
src/engine/engine_collision_convex.c
-34
View File
@@ -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) {
src/engine/engine_collision_driver.c
-31
View File
@@ -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]);
src/engine/engine_collision_gjk.c
-51
View File
@@ -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, v2 - v1> / <v2 - v1, v2 - v1> * (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
src/engine/engine_collision_primitive.c
-14
View File
@@ -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,
src/engine/engine_collision_sdf.c
-1
View File
@@ -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
src/engine/engine_core_constraint.c
-34
View File
@@ -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,
src/engine/engine_core_smooth.c
-21
View File
@@ -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);
src/engine/engine_core_util.c
-21
View File
@@ -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],
}
src/engine/engine_derivative.c
-29
View File
@@ -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:
src/engine/engine_derivative_fd.c
-11
View File
@@ -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
src/engine/engine_forward.c
-23
View File
@@ -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;
src/engine/engine_init.c
-6
View File
@@ -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
src/engine/engine_inverse.c
-6
View File
@@ -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)
src/engine/engine_io.c
-31
View File
@@ -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:
src/engine/engine_island.c
-7
View File
@@ -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:
src/engine/engine_memory.c
-11
View File
@@ -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))) {
src/engine/engine_name.c
-1
View File
@@ -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) {
src/engine/engine_passive.c
-9
View File
@@ -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,
src/engine/engine_print.c
-9
View File
@@ -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;
src/engine/engine_ray.c
-19
View File
@@ -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
src/engine/engine_sensor.c
-11
View File
@@ -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);
src/engine/engine_setconst.c
-6
View File
@@ -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) {
src/engine/engine_solver.c
-25
View File
@@ -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);
src/engine/engine_support.c
-22
View File
@@ -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<<mjNSTATE)) {
@@ -183,7 +180,6 @@ int mj_stateSize(const mjModel* m, unsigned int sig) {
}
// get state
void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int sig) {
if (sig >= (1<<mjNSTATE)) {
@@ -215,7 +211,6 @@ void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int
}
// set state
void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int sig) {
if (sig >= (1<<mjNSTATE)) {
@@ -247,7 +242,6 @@ void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int
}
// copy current state to the k-th model keyframe
void mj_setKeyframe(mjModel* m, const mjData* d, int k) {
// check keyframe index
@@ -269,7 +263,6 @@ void mj_setKeyframe(mjModel* m, const mjData* d, int k) {
}
//-------------------------- inertia functions -----------------------------------------------------
// convert sparse inertia matrix M into full matrix
@@ -289,14 +282,12 @@ void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M) {
}
// multiply vector by inertia matrix
void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) {
mju_mulSymVecSparse(res, d->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;
src/engine/engine_util_blas.c
-43
View File
@@ -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) {
src/engine/engine_util_container.c
-3
View File
@@ -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
src/engine/engine_util_errmem.c
-5
View File
@@ -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
src/engine/engine_util_misc.c
-50
View File
@@ -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
src/engine/engine_util_solve.c
-19
View File
@@ -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
src/engine/engine_util_sparse.c
-16
View File
@@ -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
src/engine/engine_util_spatial.c
-25
View File
@@ -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) {
src/engine/engine_vis_init.c
-8
View File
@@ -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
src/engine/engine_vis_interact.c
-14
View File
@@ -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,
src/engine/engine_vis_visualize.c
+1 -29
View File
@@ -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) {