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

Add spaces around comparison operators.

Browse Source 
PiperOrigin-RevId: 573620198
Change-Id: Icf295cc0dd381a4a7f0e2c94f2e12b499193e862
This commit is contained in:
Yuval Tassa
2023-10-15 07:39:20 -07:00
committed by Copybara-Service
parent a1b6026b8c
commit a9ee497e33
21 changed files with 670 additions and 667 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+19 -19
View File
@@ -39,12 +39,12 @@ void mjccd_center(const void *obj, ccd_vec3_t *center) {
int v = ccd->vert;
// return geom position
if (g>=0) {
if (g >= 0) {
mju_copy3(center->v, ccd->data->geom_xpos + 3*g);
}
// return flex element position
else if (e>=0) {
else if (e >= 0) {
mju_copy3(center->v, ccd->data->flexelem_aabb + 6*(ccd->model->flex_elemadr[f]+e));
}
@@ -64,14 +64,14 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
int g = ccd->geom;
//-------------------------- flex element or vertex -----------------------------
if (g<0) {
if (g < 0) {
int f = ccd->flex;
int dim = m->flex_dim[f];
mjtNum *res = vec->v;
const mjtNum *dir = _dir->v;
// flex element
if (ccd->elem>=0) {
if (ccd->elem >= 0) {
int e = ccd->elem;
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
@@ -79,11 +79,11 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
// find element vertex with largest projection along dir
mju_copy3(res, vert+3*edata[0]);
mjtNum best = mju_dot3(res, dir);
for (int i=1; i<=dim; i++) {
for (int i=1; i <= dim; i++) {
mjtNum dot = mju_dot3(vert+3*edata[i], dir);
// better vertex found: assign
if (dot>best) {
if (dot > best) {
best = dot;
mju_copy3(res, vert+3*edata[i]);
}
@@ -285,7 +285,7 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
mju_zero3(con->frame+3);
// both geoms: fix contact frame normal
if (obj1->geom>=0 && obj2->geom>=0) {
if (obj1->geom >= 0 && obj2->geom >= 0) {
mjc_fixNormal(m, d, con, obj1->geom, obj2->geom);
}
@@ -1130,7 +1130,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
int dim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
mjtNum* evert[4] = {NULL, NULL, NULL, NULL};
for (int i=0; i<=dim; i++) {
for (int i=0; i <= dim; i++) {
evert[i] = d->flexvert_xpos + 3*(m->flex_vertadr[f] + edata[i]);
}
mjtNum* ecenter = d->flexelem_aabb + 6*(m->flex_elemadr[f]+e);
@@ -1145,7 +1145,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// save elem vertices, transform to hfield frame
mjtNum savevert[4][3];
for (int i=0; i<=dim; i++) {
for (int i=0; i <= dim; i++) {
mju_copy3(savevert[i], evert[i]);
mju_sub3(vec, evert[i], hpos);
mju_mulMatTVec(evert[i], hmat, vec, 3, 3);
@@ -1161,7 +1161,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
xmin = xmax = evert[0][0];
ymin = ymax = evert[0][1];
zmin = zmax = evert[0][2];
for (int i=1; i<=dim; i++) {
for (int i=1; i <= dim; i++) {
xmin = mju_min(xmin, evert[i][0]);
xmax = mju_max(xmax, evert[i][0]);
ymin = mju_min(ymin, evert[i][1]);
@@ -1175,7 +1175,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
(ymin-margin > hsize[1]) || (ymax+margin < -hsize[1]) ||
(zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) {
// restore vertices and center
for (int i=0; i<=dim; i++) {
for (int i=0; i <= dim; i++) {
mju_copy3(evert[i], savevert[i]);
}
mju_copy3(ecenter, savecenter);
@@ -1217,23 +1217,23 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// process all prisms in sub-grid
cnt = 0;
for (int r=rmin; r<rmax; r++) {
for (int r=rmin; r < rmax; r++) {
int nvert = 0;
for (int c=cmin; c<=cmax; c++) {
for (int k=0; k<2; k++) {
for (int c=cmin; c <= cmax; c++) {
for (int k=0; k < 2; k++) {
// send vertex to prism constructor
addVert(&nvert, &prism, dx*c-hsize[0], dy*(r+dr[k])-hsize[1],
hdata[(r+dr[k])*ncol+c]*hsize[2]+margin);
// check for enough vertices
if (nvert>2) {
if (nvert > 2) {
// prism height test
if (prism.v[3][2]<zmin && prism.v[4][2]<zmin && prism.v[5][2]<zmin) {
if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) {
continue;
}
// run MPR, save contact
if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd)==0) {
if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) {
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
@@ -1244,7 +1244,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// count, stop if max number reached
cnt++;
if (cnt>=mjMAXCONPAIR) {
if (cnt >= mjMAXCONPAIR) {
r = rmax+1;
c = cmax+1;
k = 3;
@@ -1258,7 +1258,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
}
// restore elem vertices and center
for (int i=0; i<=dim; i++) {
for (int i=0; i <= dim; i++) {
mju_copy3(evert[i], savevert[i]);
}
mju_copy3(ecenter, savecenter);
src/engine/engine_collision_driver.c
+173 -171
View File
@@ -61,7 +61,7 @@ static inline void resetArena(mjData* d) {
#ifdef ADDRESS_SANITIZER
if (!d->threadpool) {
ASAN_POISON_MEMORY_REGION(
(char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
(char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
}
#endif
}
@@ -145,19 +145,19 @@ 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
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) {
if (m->geom_rbound[g1] > 0 && m->geom_rbound[g2] > 0) {
return filterSphere(d->geom_xpos + 3*g1, d->geom_xpos + 3*g2,
m->geom_rbound[g1] + m->geom_rbound[g2] + margin);
}
// one geom is a plane
if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0
if (m->geom_type[g1] == mjGEOM_PLANE && m->geom_rbound[g2] > 0
&& planeGeomDist(m, d, g1, g2) > margin + m->geom_rbound[g2]) {
return 1;
return 1;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
if (m->geom_type[g2] == mjGEOM_PLANE && m->geom_rbound[g1] > 0
&& planeGeomDist(m, d, g2, g1) > margin + m->geom_rbound[g1]) {
return 1;
return 1;
}
return 0;
}
@@ -186,7 +186,7 @@ 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) {
if (bf < m->nbody) {
return (m->body_contype[bf] || m->body_conaffinity[bf]);
} else {
int f = bf - m->nbody;
@@ -199,10 +199,10 @@ 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;
int contype1 = (bf1<nbody) ? m->body_contype[bf1] : m->flex_contype[bf1-nbody];
int conaffinity1 = (bf1<nbody) ? m->body_conaffinity[bf1] : m->flex_conaffinity[bf1-nbody];
int contype2 = (bf2<nbody) ? m->body_contype[bf2] : m->flex_contype[bf2-nbody];
int conaffinity2 = (bf2<nbody) ? m->body_conaffinity[bf2] : m->flex_conaffinity[bf2-nbody];
int contype1 = (bf1 < nbody) ? m->body_contype[bf1] : m->flex_contype[bf1-nbody];
int conaffinity1 = (bf1 < nbody) ? m->body_conaffinity[bf1] : m->flex_conaffinity[bf1-nbody];
int contype2 = (bf2 < nbody) ? m->body_contype[bf2] : m->flex_contype[bf2-nbody];
int conaffinity2 = (bf2 < nbody) ? m->body_conaffinity[bf2] : m->flex_conaffinity[bf2-nbody];
// opposite of bitmask filter
return (!filterBitmask(contype1, conaffinity1, contype2, conaffinity2));
@@ -212,7 +212,7 @@ 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) {
if (m->flex_dim[f] < 3) {
return 1;
} else {
return (m->flex_elemlayer[m->flex_elemadr[f]+e] < m->flex_activelayers[f]);
@@ -226,19 +226,19 @@ int mj_isElemActive(const mjModel* m, int f, int e) {
// compare contact pairs by their geom/elem/vert IDs
quicksortfunc(contactcompare, context, el1, el2) {
const mjModel* m = (const mjModel*) context;
mjContact* con1 = (mjContact*)el1;
mjContact* con2 = (mjContact*)el2;
mjContact* c1 = (mjContact*)el1;
mjContact* c2 = (mjContact*)el2;
// get colliding object ids
int con1_obj1 = con1->geom[0]>=0 ? con1->geom[0] : (con1->elem[0]>=0 ? con1->elem[0] : con1->vert[0]);
int con1_obj2 = con1->geom[1]>=0 ? con1->geom[1] : (con1->elem[1]>=0 ? con1->elem[1] : con1->vert[1]);
int con2_obj1 = con2->geom[0]>=0 ? con2->geom[0] : (con2->elem[0]>=0 ? con2->elem[0] : con2->vert[0]);
int con2_obj2 = con2->geom[1]>=0 ? con2->geom[1] : (con2->elem[1]>=0 ? con2->elem[1] : con2->vert[1]);
int con1_obj1 = c1->geom[0] >= 0 ? c1->geom[0] : (c1->elem[0] >= 0 ? c1->elem[0] : c1->vert[0]);
int con1_obj2 = c1->geom[1] >= 0 ? c1->geom[1] : (c1->elem[1] >= 0 ? c1->elem[1] : c1->vert[1]);
int con2_obj1 = c2->geom[0] >= 0 ? c2->geom[0] : (c2->elem[0] >= 0 ? c2->elem[0] : c2->vert[0]);
int con2_obj2 = c2->geom[1] >= 0 ? c2->geom[1] : (c2->elem[1] >= 0 ? c2->elem[1] : c2->vert[1]);
// for geom:geom, reproduce the order of contacts without mj_collideTree
// normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type
// here we undo this swapping for the purpose of sorting - needs to be done for each mjContact
if (con1->geom[0]>=0 && con1->geom[1] && con2->geom[0]>=0 && con2->geom[1]) {
if (c1->geom[0] >= 0 && c1->geom[1] && c2->geom[0] >= 0 && c2->geom[1]) {
if (m->geom_type[con1_obj1] > m->geom_type[con1_obj2]) {
int tmp = con1_obj1;
con1_obj1 = con1_obj2;
@@ -298,7 +298,7 @@ void mj_collision(const mjModel* m, mjData* d) {
// process bodyflex pairs returned by broadphase, merge with predefined geom pairs
int pairadr = 0;
for (int i=0; i<nbfpair; i++) {
for (int i=0; i < nbfpair; i++) {
// reconstruct bodyflex pair ids
int bf1 = (broadphasepair[i]>>16) & 0xFFFF;
int bf2 = broadphasepair[i] & 0xFFFF;
@@ -317,8 +317,8 @@ void mj_collision(const mjModel* m, mjData* d) {
int startadr = pairadr;
if (npair) {
// test all predefined pairs for which pair_signature<=signature
while (pairadr<npair && m->pair_signature[pairadr]<=signature) {
if (m->pair_signature[pairadr]==signature) {
while (pairadr < npair && m->pair_signature[pairadr] <= signature) {
if (m->pair_signature[pairadr] == signature) {
merged = 1;
}
mj_collideGeoms(m, d, pairadr++, -1);
@@ -334,30 +334,31 @@ void mj_collision(const mjModel* m, mjData* d) {
int exadr = 0;
if (nexclude) {
// advance exadr while exclude_signature < signature
while (exadr<nexclude && m->exclude_signature[exadr]<signature) {
while (exadr < nexclude && m->exclude_signature[exadr] < signature) {
exadr++;
}
// skip this bodyflex pair if its signature is found in exclude array
if (exadr<nexclude && m->exclude_signature[exadr]==signature) {
if (exadr < nexclude && m->exclude_signature[exadr] == signature) {
continue;
}
}
// get bodyflex info
int isbody1 = (bf1<nbody);
int isbody2 = (bf2<nbody);
int isbody1 = (bf1 < nbody);
int isbody2 = (bf2 < nbody);
int bvh1 = (isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[bf1-nbody]);
int bvh2 = (isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[bf2-nbody]);
int geomadr1 = (isbody1 ? m->body_geomadr[bf1] : -1);
int geomadr2 = (isbody2 ? m->body_geomadr[bf2] : -1);
// process bodyflex pair: two single-geom bodies
if (isbody1 && isbody2 && m->body_geomnum[bf1]==1 && m->body_geomnum[bf2]==1) {
mj_collideGeomPair(m, d, m->body_geomadr[bf1], m->body_geomadr[bf2],
merged, startadr, pairadr);
if (isbody1 && isbody2 && m->body_geomnum[bf1] == 1 && m->body_geomnum[bf2] == 1) {
mj_collideGeomPair(m, d, geomadr1, geomadr2, merged, startadr, pairadr);
}
// process bodyflex pair: midphase
else if (!mjDISABLED(mjDSBL_MIDPHASE) && bvh1>=0 && bvh2>=0) {
else if (!mjDISABLED(mjDSBL_MIDPHASE) && bvh1 >= 0 && bvh2 >= 0) {
int ncon_before = d->ncon;
mj_collideTree(m, d, bf1, bf2, merged, startadr, pairadr);
int ncon_after = d->ncon;
@@ -369,13 +370,13 @@ void mj_collision(const mjModel* m, mjData* d) {
// process bodyflex pair: all-to-all
else {
int geomadr_end1 = m->body_geomadr[bf1] + m->body_geomnum[bf1];
int geomadr_end2 = m->body_geomadr[bf2] + m->body_geomnum[bf2];
int geomadr_end1 = geomadr1 + m->body_geomnum[bf1];
int geomadr_end2 = geomadr2 + m->body_geomnum[bf2];
// body : body
if (isbody1 && isbody2) {
for (int g1=m->body_geomadr[bf1]; g1<geomadr_end1; g1++) {
for (int g2=m->body_geomadr[bf2]; g2<geomadr_end2; g2++) {
for (int g1=geomadr1; g1 < geomadr_end1; g1++) {
for (int g2=geomadr2; g2 < geomadr_end2; g2++) {
mj_collideGeomPair(m, d, g1, g2, merged, startadr, pairadr);
}
}
@@ -386,7 +387,7 @@ void mj_collision(const mjModel* m, mjData* d) {
int f = bf2 - nbody;
// process body geoms
for (int g=m->body_geomadr[bf1]; g<geomadr_end1; g++) {
for (int g=m->body_geomadr[bf1]; g < geomadr_end1; g++) {
// bitmask filtering at the geom-flex level
if (filterBitmask(m->geom_contype[g], m->geom_conaffinity[g],
m->flex_contype[f], m->flex_conaffinity[f])) {
@@ -394,13 +395,14 @@ void mj_collision(const mjModel* m, mjData* d) {
}
// plane special processing
if (m->geom_type[g]==mjGEOM_PLANE) {
if (m->geom_type[g] == mjGEOM_PLANE) {
mj_collidePlaneFlex(m, d, g, f);
continue;
}
// collide geom with flex elements
for (int e=0; e<m->flex_elemnum[f]; e++) {
int elemnum = m->flex_elemnum[f];
for (int e=0; e < elemnum; e++) {
mj_collideGeomElem(m, d, g, f, e);
}
}
@@ -412,8 +414,8 @@ void mj_collision(const mjModel* m, mjData* d) {
int f2 = bf2 - nbody;
// collide elements of two flexes
for (int e1=0; e1<m->flex_elemnum[f1]; e1++) {
for (int e2=0; e2<m->flex_elemnum[f2]; e2++) {
for (int e1=0; e1 < m->flex_elemnum[f1]; e1++) {
for (int e2=0; e2 < m->flex_elemnum[f2]; e2++) {
mj_collideElems(m, d, f1, e1, f2, e2);
}
}
@@ -423,13 +425,13 @@ void mj_collision(const mjModel* m, mjData* d) {
// finish merging predefined geom pairs
if (npair) {
while (pairadr<npair) {
while (pairadr < npair) {
mj_collideGeoms(m, d, pairadr++, -1);
}
}
// flex self-collisions
for (int f=0; f<m->nflex; f++) {
for (int f=0; f < m->nflex; f++) {
if (!m->flex_rigid[f] && (m->flex_contype[f] & m->flex_conaffinity[f])) {
// internal collisions
if (m->flex_internal[f]) {
@@ -437,14 +439,14 @@ void mj_collision(const mjModel* m, mjData* d) {
}
// active element collisions
if (m->flex_selfcollide[f]!=mjFLEXSELF_NONE) {
if (m->flex_selfcollide[f] != mjFLEXSELF_NONE) {
// element-element: midphase
if (!mjDISABLED(mjDSBL_MIDPHASE) &&
m->flex_selfcollide[f]!=mjFLEXSELF_NARROW &&
m->flex_bvhadr[f]>=0) {
m->flex_selfcollide[f] != mjFLEXSELF_NARROW &&
m->flex_bvhadr[f] >= 0) {
// select midphase mode
if (m->flex_selfcollide[f]==mjFLEXSELF_BVH ||
(m->flex_selfcollide[f]==mjFLEXSELF_AUTO && m->flex_dim[f]==3)) {
if (m->flex_selfcollide[f] == mjFLEXSELF_BVH ||
(m->flex_selfcollide[f] == mjFLEXSELF_AUTO && m->flex_dim[f] == 3)) {
mj_collideTree(m, d, nbody+f, nbody+f, 0, 0, 0);
} else {
mj_collideFlexSAP(m, d, f);
@@ -454,9 +456,9 @@ void mj_collision(const mjModel* m, mjData* d) {
// element-element: direct
else {
int flex_elemnum = m->flex_elemnum[f];
for (int e1=0; e1<flex_elemnum; e1++) {
for (int e1=0; e1 < flex_elemnum; e1++) {
if (mj_isElemActive(m, f, e1)) {
for (int e2=e1+1; e2<flex_elemnum; e2++) {
for (int e2=e1+1; e2 < flex_elemnum; e2++) {
if (mj_isElemActive(m, f, e2)) {
mj_collideElems(m, d, f, e1, f, e2);
}
@@ -497,7 +499,7 @@ typedef struct mjCollisionTree_ mjCollisionTree;
// collision tree allocation
static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) {
return (mjCollisionTree*) mj_stackAllocByte(
d, max_stack * sizeof(mjCollisionTree), _Alignof(mjCollisionTree));
d, max_stack * sizeof(mjCollisionTree), _Alignof(mjCollisionTree));
}
@@ -576,7 +578,7 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
if (xmat[i]) {
normal[i][j][k] = xmat[i][3*k+j];
} else {
normal[i][j][k] = (j==k);
normal[i][j][k] = (j == k);
}
}
}
@@ -669,9 +671,9 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
stack[0].node1 = stack[0].node2 = 0;
// for body:flex, if body has planes, call mj_collidePlaneFlex directly
if (isbody1 && !isbody2 && m->body_weldid[bf1]==0) {
for (int i=m->body_geomadr[bf1]; i<m->body_geomadr[bf1]+m->body_geomnum[bf1]; i++) {
if (m->geom_type[i]==mjGEOM_PLANE) {
if (isbody1 && !isbody2 && m->body_weldid[bf1] == 0) {
for (int i=m->body_geomadr[bf1]; i < m->body_geomadr[bf1]+m->body_geomnum[bf1]; i++) {
if (m->geom_type[i] == mjGEOM_PLANE) {
mj_collidePlaneFlex(m, d, i, f2);
}
}
@@ -683,18 +685,18 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
nstack--;
int node1 = stack[nstack].node1;
int node2 = stack[nstack].node2;
mjtByte isleaf1 = (child1[2*node1]<0) && (child1[2*node1+1]<0);
mjtByte isleaf2 = (child2[2*node2]<0) && (child2[2*node2+1]<0);
mjtByte isleaf1 = (child1[2*node1] < 0) && (child1[2*node1+1] < 0);
mjtByte isleaf2 = (child2[2*node2] < 0) && (child2[2*node2+1] < 0);
int nodeid1 = m->bvh_nodeid[bvhadr1 + node1];
int nodeid2 = m->bvh_nodeid[bvhadr2 + node2];
// SHOULD NOT OCCUR
if ((isleaf1 && nodeid1<0) || (isleaf2 && nodeid2<0)) {
if ((isleaf1 && nodeid1 < 0) || (isleaf2 && nodeid2 < 0)) {
mju_error("BVH leaf has invalid node id");
}
// self-collision: avoid repeated pairs
if (bf1==bf2 && node1>node2) {
if (bf1 == bf2 && node1 > node2) {
continue;
}
@@ -702,8 +704,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
if (isbody1 && isbody2) {
// both are leaves
if (isleaf1 && isleaf2) {
mjtNum margin = mj_assignMargin(m,
mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2]));
mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
@@ -719,8 +721,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
}
// if no intersection at intermediate levels, stop
mjtNum margin = mj_assignMargin(m,
mju_max(m->body_margin[bf1], m->body_margin[bf2]));
mjtNum maxmargin = mju_max(m->body_margin[bf1], m->body_margin[bf2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
d->xipos + 3*bf1, d->ximat + 9*bf1,
d->xipos + 3*bf2, d->ximat + 9*bf2,
@@ -733,8 +735,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
else if (isbody1 && !isbody2) {
// both are leaves
if (isleaf1 && isleaf2) {
mjtNum margin = mj_assignMargin(m,
mju_max(m->geom_margin[nodeid1], m->flex_margin[f2]));
mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->flex_margin[f2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1],
m->flex_contype[f2], m->flex_conaffinity[f2]) &&
@@ -745,7 +747,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
NULL, NULL,
margin, NULL, NULL, &initialize)) {
// collide unless geom is plane (plane:flex handled separately)
if (m->geom_type[nodeid1]!=mjGEOM_PLANE) {
if (m->geom_type[nodeid1] != mjGEOM_PLANE) {
mj_collideGeomElem(m, d, nodeid1, f2, nodeid2);
}
d->bvh_active[node1 + bvhadr1] = 1;
@@ -756,12 +758,12 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
}
// if no intersection at intermediate levels, stop
mjtNum margin = mj_assignMargin(m,
mju_max(m->body_margin[bf1], m->flex_margin[f2]));
mjtNum maxmargin = mju_max(m->body_margin[bf1], m->flex_margin[f2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
d->xipos + 3*bf1, d->ximat + 9*bf1,
NULL, NULL,
margin, product, offset, &initialize)) {
d->xipos + 3*bf1, d->ximat + 9*bf1,
NULL, NULL,
margin, product, offset, &initialize)) {
continue;
}
}
@@ -783,8 +785,8 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
}
// if no intersection at intermediate levels, stop
mjtNum margin = mj_assignMargin(m,
mju_max(m->flex_margin[f1], m->flex_margin[f2]));
mjtNum maxmargin = mju_max(m->flex_margin[f1], m->flex_margin[f2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
if (filterBox(bvh1 + 6*node1, bvh2 + 6*node2, margin)) {
continue;
}
@@ -863,28 +865,28 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
// make AAMM (xmin[3], xmax[3]) for one bodyflex
static void makeAAMM(const mjModel* m, mjData* d, mjtNum* aamm, int bf, const mjtNum* frame) {
// body
if (bf<m->nbody) {
if (bf < m->nbody) {
int body = bf;
int body_geomnum = m->body_geomnum[body];
// process all body geoms (body is collidable, should have geoms)
for (int i=0; i<body_geomnum; i++) {
for (int i=0; i < body_geomnum; i++) {
int geom = m->body_geomadr[body]+i;
mjtNum margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5*m->opt.o_margin : m->geom_margin[geom];
mjtNum _aamm[6];
// set _aamm for this geom
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
mjtNum cen = mju_dot3(d->geom_xpos+3*geom, frame+3*j);
_aamm[j] = cen - m->geom_rbound[geom] - margin;
_aamm[j+3] = cen + m->geom_rbound[geom] + margin;
}
// update body aamm
if (i==0) {
if (i == 0) {
mju_copy(aamm, _aamm, 6);
} else {
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
aamm[j] = mju_min(aamm[j], _aamm[j]);
aamm[j+3] = mju_max(aamm[j+3], _aamm[j+3]);
}
@@ -906,11 +908,11 @@ static void makeAAMM(const mjModel* m, mjData* d, mjtNum* aamm, int bf, const mj
mju_mulMatVec(v, frame, vbase+3*i, 3, 3);
// update aamm
if (i==0) {
if (i == 0) {
mju_copy3(aamm, v);
mju_copy3(aamm+3, v);
} else {
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
aamm[j] = mju_min(aamm[j], v[j]);
aamm[j+3] = mju_max(aamm[j+3], v[j]);
}
@@ -946,7 +948,7 @@ static void add_pair(const mjModel* m, int bf1, int bf2,
int body_geomadr1 = m->body_geomadr[bf1];
int body_geomnum1 = m->body_geomnum[bf1];
contype1 = conaffinity1 = 0;
for (int i=body_geomadr1; i<body_geomadr1+body_geomnum1; i++) {
for (int i=body_geomadr1; i < body_geomadr1+body_geomnum1; i++) {
contype1 |= m->geom_contype[i];
conaffinity1 |= m->geom_conaffinity[i];
}
@@ -960,7 +962,7 @@ static void add_pair(const mjModel* m, int bf1, int bf2,
int body_geomadr2 = m->body_geomadr[bf2];
int body_geomnum2 = m->body_geomnum[bf2];
contype2 = conaffinity2 = 0;
for (int i=body_geomadr2; i<body_geomadr2+body_geomnum2; i++) {
for (int i=body_geomadr2; i < body_geomadr2+body_geomnum2; i++) {
contype2 |= m->geom_contype[i];
conaffinity2 |= m->geom_conaffinity[i];
}
@@ -1021,7 +1023,7 @@ quicksortfunc(SAPcompare, context, el1, el2) {
// using sweep-and-prune along specified axis (0-2).
static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int maxpair) {
// check inputs
if (n>=0x10000 || axis<0 || axis>2 || maxpair<1) {
if (n >= 0x10000 || axis < 0 || axis > 2 || maxpair < 1) {
return -1;
}
@@ -1030,7 +1032,7 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int
mjtSAP* activebuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP));
// init sortbuf with specified axis
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
sortbuf[2*i].id_ismax = i;
sortbuf[2*i].value = (float)aamm[6*i+axis];
sortbuf[2*i+1].id_ismax = i + 0x10000;
@@ -1042,10 +1044,10 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int
// define the other two axes
int axisA, axisB;
if (axis==0) {
if (axis == 0) {
axisA = 1;
axisB = 2;
} else if (axis==1) {
} else if (axis == 1) {
axisA = 0;
axisB = 2;
} else {
@@ -1056,10 +1058,10 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int
// sweep and prune
int cnt = 0; // size of active list
int npair = 0; // number of pairs added
for (int i=0; i<2*n; i++) {
for (int i=0; i < 2*n; i++) {
// min value: collide with all in list, add
if (!(sortbuf[i].id_ismax & 0x10000)) {
for (int j=0; j<cnt; j++) {
for (int j=0; j < cnt; j++) {
// get ids: no need to mask ismax because activebuf entries never have the ismax bit,
// and sortbuf[i].id_ismax is tested above
int id1 = activebuf[j].id_ismax;
@@ -1075,7 +1077,7 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int
// add pair, check buffer size
pair[npair++] = (id1<<16) + id2;
if (npair>=maxpair) {
if (npair >= maxpair) {
return maxpair;
}
}
@@ -1088,9 +1090,9 @@ static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int
// max value: remove corresponding min value from list
else {
int toremove = sortbuf[i].id_ismax & 0xFFFF;
for (int j=0; j<cnt; j++) {
if (activebuf[j].id_ismax==toremove) {
if (j<cnt-1) {
for (int j=0; j < cnt; j++) {
if (activebuf[j].id_ismax == toremove) {
if (j < cnt-1) {
memmove(activebuf+j, activebuf+j+1, sizeof(mjtSAP)*(cnt-1-j));
}
cnt--;
@@ -1159,9 +1161,9 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
// b1 is world body with geoms, or world-welded body with plane
if ((b1 == 0 && m->body_geomnum[b1] > 0) ||
(m->body_weldid[b1]==0 && hasPlane(m, b1))) {
(m->body_weldid[b1] == 0 && hasPlane(m, b1))) {
// add b1:body pairs that are not welded together
for (int b2=0; b2<nbody; b2++) {
for (int b2=0; b2 < nbody; b2++) {
// cannot colide
if (!canCollide(m, b2)) {
continue;
@@ -1179,7 +1181,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
}
// add all b1:flex pairs
for (int f=0; f<nflex; f++) {
for (int f=0; f < nflex; f++) {
add_pair(m, b1, nbody+f, &npair, bfpair, maxpair);
}
}
@@ -1200,7 +1202,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
cnt++;
}
}
if (cnt==0) {
if (cnt == 0) {
return npair;
}
mju_scl3(cen, cen, 1.0/cnt);
@@ -1226,20 +1228,20 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
mj_markStack(d);
int* bfid = mj_stackAllocInt(d, nbodyflex);
int ncollide = 0;
for (int i=1; i<nbodyflex; i++) {
for (int i=1; i < nbodyflex; i++) {
if (canCollide(m, i)) {
bfid[ncollide++] = i;
}
}
// nothing collidable
if (ncollide<2) {
if (ncollide < 2) {
goto endbroad;
}
// allocate and construct AAMMs for collidable only
mjtNum* aamm = mj_stackAllocNum(d, 6*ncollide);
for (int i=0; i<ncollide; i++) {
for (int i=0; i < ncollide; i++) {
makeAAMM(m, d, aamm+6*i, bfid[i], frame);
}
@@ -1247,17 +1249,17 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
int maxsappair = ncollide*(ncollide-1)/2;
int* sappair = mj_stackAllocInt(d, maxsappair);
int nsappair = mj_SAP(d, aamm, ncollide, 0, sappair, maxsappair);
if (nsappair<0) {
if (nsappair < 0) {
mjERROR("SAP failed");
}
// filter SAP pairs, convert to bodyflex pairs
for (int i=0; i<nsappair; i++) {
for (int i=0; i < nsappair; i++) {
int bf1 = bfid[sappair[i] >> 16];
int bf2 = bfid[sappair[i] & 0xFFFF];
// body pair: prune based on weld filter
if (bf1<nbody && bf2<nbody) {
if (bf1 < nbody && bf2 < nbody) {
int weld1 = m->body_weldid[bf1];
int weld2 = m->body_weldid[bf2];
int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
@@ -1295,34 +1297,34 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
mjtNum fri[3];
// get parameters from geom1 or flex1
int priority1 = (f1<0) ? m->geom_priority[g1] : m->flex_priority[f1];
int condim1 = (f1<0) ? m->geom_condim[g1] : m->flex_condim[f1];
mjtNum gap1 = (f1<0) ? m->geom_gap[g1] : m->flex_gap[f1];
mjtNum solmix1 = (f1<0) ? m->geom_solmix[g1] : m->flex_solmix[f1];
const mjtNum* solref1 = (f1<0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF;
const mjtNum* solimp1 = (f1<0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP;
const mjtNum* friction1 = (f1<0) ? m->geom_friction+g1*3 : m->flex_friction+f1*3;
int priority1 = (f1 < 0) ? m->geom_priority[g1] : m->flex_priority[f1];
int condim1 = (f1 < 0) ? m->geom_condim[g1] : m->flex_condim[f1];
mjtNum gap1 = (f1 < 0) ? m->geom_gap[g1] : m->flex_gap[f1];
mjtNum solmix1 = (f1 < 0) ? m->geom_solmix[g1] : m->flex_solmix[f1];
const mjtNum* solref1 = (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF;
const mjtNum* solimp1 = (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP;
const mjtNum* friction1 = (f1 < 0) ? m->geom_friction+g1*3 : m->flex_friction+f1*3;
// get parameters from geom2 or flex2
int priority2 = (f2<0) ? m->geom_priority[g2] : m->flex_priority[f2];
int condim2 = (f2<0) ? m->geom_condim[g2] : m->flex_condim[f2];
mjtNum gap2 = (f2<0) ? m->geom_gap[g2] : m->flex_gap[f2];
mjtNum solmix2 = (f2<0) ? m->geom_solmix[g2] : m->flex_solmix[f2];
const mjtNum* solref2 = (f2<0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF;
const mjtNum* solimp2 = (f2<0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP;
const mjtNum* friction2 = (f2<0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3;
int priority2 = (f2 < 0) ? m->geom_priority[g2] : m->flex_priority[f2];
int condim2 = (f2 < 0) ? m->geom_condim[g2] : m->flex_condim[f2];
mjtNum gap2 = (f2 < 0) ? m->geom_gap[g2] : m->flex_gap[f2];
mjtNum solmix2 = (f2 < 0) ? m->geom_solmix[g2] : m->flex_solmix[f2];
const mjtNum* solref2 = (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF;
const mjtNum* solimp2 = (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP;
const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3;
// gap: max
*gap = mju_max(gap1, gap2);
// different priority: copy from item with higher priority
if (priority1>priority2) {
if (priority1 > priority2) {
*condim = condim1;
mju_copy(solref, solref1, mjNREF);
mju_copy(solimp, solimp1, mjNIMP);
mju_copy(fri, friction1, 3);
}
else if (priority1<priority2) {
else if (priority1 < priority2) {
*condim = condim2;
mju_copy(solref, solref2, mjNREF);
mju_copy(solimp, solimp2, mjNIMP);
@@ -1336,37 +1338,37 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
// compute solver mix factor
mjtNum mix;
if (solmix1>=mjMINVAL && solmix2>=mjMINVAL) {
if (solmix1 >= mjMINVAL && solmix2 >= mjMINVAL) {
mix = solmix1 / (solmix1 + solmix2);
} else if (solmix1<mjMINVAL && solmix2<mjMINVAL) {
} else if (solmix1 < mjMINVAL && solmix2 < mjMINVAL) {
mix = 0.5;
} else if (solmix1<mjMINVAL) {
} else if (solmix1 < mjMINVAL) {
mix = 0.0;
} else {
mix = 1.0;
}
// reference standard: mix
if (solref1[0]>0 && solref2[0]>0) {
for (int i=0; i<mjNREF; i++) {
if (solref1[0] > 0 && solref2[0] > 0) {
for (int i=0; i < mjNREF; i++) {
solref[i] = mix*solref1[i] + (1-mix)*solref2[i];
}
}
// reference direct: min
else {
for (int i=0; i<mjNREF; i++) {
for (int i=0; i < mjNREF; i++) {
solref[i] = mju_min(solref1[i], solref2[i]);
}
}
// impedance: mix
for (int i=0; i<mjNIMP; i++) {
for (int i=0; i < mjNIMP; i++) {
solimp[i] = mix*solimp1[i] + (1-mix)*solimp2[i];
}
// friction: max
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
fri[i] = mju_max(friction1[i], friction2[i]);
}
}
@@ -1379,7 +1381,7 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap,
friction[4] = fri[2];
// SHOULD NOT OCCUR
if (*condim>6 || *condim<1) {
if (*condim > 6 || *condim < 1) {
mjERROR("Invalid condim value: %d", *condim);
}
}
@@ -1501,7 +1503,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
// allocate mjContact[mjMAXCONPAIR] on the arena
mjContact* con =
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
if (!con) {
mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
return;
@@ -1578,7 +1580,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
}
// add contacts returned by collision detector
for (int i=0; i<num; i++) {
for (int i=0; i < num; i++) {
// set contact ids
con[i].geom[0] = g1;
con[i].geom[1] = g2;
@@ -1622,7 +1624,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f);
// collide all flex vertices with plane
for (int i=0; i<flex_vertnum; i++) {
for (int i=0; i < flex_vertnum; i++) {
mjtNum* v = d->flexvert_xpos + 3*(m->flex_vertadr[f]+i);
// distance from plane to vertex
@@ -1678,7 +1680,7 @@ static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad,
// project, check distance
mjtNum dst = mju_dot3(ev, nrm);
if (dst<=-2*rad) {
if (dst <= -2*rad) {
return 0;
}
@@ -1698,13 +1700,13 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
int flex_evpairnum = m->flex_evpairnum[f];
// predefined element-vertex
for (int i=0; i<flex_evpairnum; i++) {
for (int i=0; i < flex_evpairnum; i++) {
const int* ev = m->flex_evpair + 2*m->flex_evpairadr[f] + 2*i;
mj_collideElemVert(m, d, f, ev[0], ev[1]);
}
// within-element for tetrahedral only
if (m->flex_dim[f]!=3) {
if (m->flex_dim[f] != 3) {
return;
}
@@ -1725,7 +1727,7 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
// process all elements
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
for (int e=0; e<flex_elemnum; e++) {
for (int e=0; e < flex_elemnum; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*4;
con.elem[0] = e;
@@ -1766,14 +1768,14 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) {
int* elid = mj_stackAllocInt(d, m->flex_elemnum[f]);
int nactive = 0;
int flex_elemnum = m->flex_elemnum[f];
for (int i=0; i<flex_elemnum; i++) {
for (int i=0; i < flex_elemnum; i++) {
if (mj_isElemActive(m, f, i)) {
elid[nactive++] = i;
}
}
// nothing active
if (nactive<2) {
if (nactive < 2) {
mj_freeStack(d);
return;
}
@@ -1781,25 +1783,25 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) {
// allocate and construct AAMMs for active elements
mjtNum* aamm = mj_stackAllocNum(d, 6*nactive);
const mjtNum* elemaabb = d->flexelem_aabb + 6*m->flex_elemadr[f];
for (int i=0; i<nactive; i++) {
for (int i=0; i < nactive; i++) {
mju_sub3(aamm+6*i+0, elemaabb+6*elid[i], elemaabb+6*elid[i]+3);
mju_add3(aamm+6*i+3, elemaabb+6*elid[i], elemaabb+6*elid[i]+3);
}
// select largest axis from flex bvh
const mjtNum* bvh = d->bvh_aabb_dyn + 6*(m->flex_bvhadr[f] - m->nbvhstatic);
int axis = (bvh[3]>bvh[4] && bvh[3]>bvh[5]) ? 0 : (bvh[4]>bvh[5] ? 1 : 2);
int axis = (bvh[3] > bvh[4] && bvh[3] > bvh[5]) ? 0 : (bvh[4] > bvh[5] ? 1 : 2);
// call SAP; hard limit on number of pairs to avoid out-of-memory
int maxsappair = mjMIN(nactive*(nactive-1)/2, 1000000);
int* sappair = mj_stackAllocInt(d, maxsappair);
int nsappair = mj_SAP(d, aamm, nactive, axis, sappair, maxsappair);
if (nsappair<0) {
if (nsappair < 0) {
mjERROR("SAP failed");
}
// send SAP pairs to nearphase
for (int i=0; i<nsappair; i++) {
for (int i=0; i < nsappair; i++) {
int e1 = elid[sappair[i] >> 16];
int e2 = elid[sappair[i] & 0xFFFF];
mj_collideElems(m, d, f, e1, f, e2);
@@ -1829,62 +1831,62 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
int b = m->geom_bodyid[g];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const int* bdata = m->flex_vertbodyid + m->flex_vertadr[f];
for (int i=0; i<=dim; i++) {
if (b==bdata[edata[i]]) {
for (int i=0; i <= dim; i++) {
if (b == bdata[edata[i]]) {
return;
}
}
// allocate mjContact[mjMAXCONPAIR] on the arena
mjContact* con =
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
if (!con) {
mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
return;
}
// sphere/capsule/box : capsule
if (dim==1 && (type==mjGEOM_SPHERE || type==mjGEOM_CAPSULE || type==mjGEOM_BOX)) {
if (dim == 1 && (type == mjGEOM_SPHERE || type == mjGEOM_CAPSULE || type == mjGEOM_BOX)) {
// make capsule from vertices
mjtNum pos[3], mat[9], size[2];
mj_makeCapsule(m, d, f, m->flex_elem + m->flex_elemdataadr[f] + e*2,
pos, mat, size);
// call raw primitive for corresponding geom type
if (type==mjGEOM_SPHERE) {
if (type == mjGEOM_SPHERE) {
num = mjraw_SphereCapsule(con, margin,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
pos, mat, size);
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
pos, mat, size);
}
else if (type==mjGEOM_CAPSULE) {
else if (type == mjGEOM_CAPSULE) {
num = mjraw_CapsuleCapsule(con, margin,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
pos, mat, size);
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g,
pos, mat, size);
}
else {
num = mjraw_CapsuleBox(con, margin,
pos, mat, size,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g);
pos, mat, size,
d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g);
// reverse contact normals, since box geom is second
for (int i=0; i<num; i++) {
for (int i=0; i < num; i++) {
mju_scl3(con[i].frame, con[i].frame, -1);
}
}
}
// heightfield : elem
else if (type==mjGEOM_HFIELD) {
else if (type == mjGEOM_HFIELD) {
num = mjc_HFieldElem(m, d, con, g, f, e, margin);
}
// sphere : triangle
else if (type==mjGEOM_SPHERE && dim==2) {
else if (type == mjGEOM_SPHERE && dim == 2) {
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
num = mjraw_SphereTriangle(con, margin,
d->geom_xpos+3*g, m->geom_size[3*g],
vertxpos + 3*edata[0], vertxpos + 3*edata[1],
vertxpos + 3*edata[2], m->flex_radius[f]);
d->geom_xpos+3*g, m->geom_size[3*g],
vertxpos + 3*edata[0], vertxpos + 3*edata[1],
vertxpos + 3*edata[2], m->flex_radius[f]);
}
// general geom : elem
@@ -1905,7 +1907,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f);
// add contacts
for (int i=0; i<num; i++) {
for (int i=0; i < num; i++) {
// set contact ids
con[i].geom[0] = g;
con[i].geom[1] = -1;
@@ -1937,7 +1939,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
int num;
// ignore margin in self-collisions
if (f1==f2) {
if (f1 == f2) {
margin = 0;
}
@@ -1952,10 +1954,10 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
const int* edata2 = m->flex_elem + m->flex_elemdataadr[f2] + e2*(dim2+1);
const int* bdata1 = m->flex_vertbodyid + m->flex_vertadr[f1];
const int* bdata2 = m->flex_vertbodyid + m->flex_vertadr[f2];
for (int i1=0; i1<=dim1; i1++) {
for (int i1=0; i1 <= dim1; i1++) {
int b1 = bdata1[edata1[i1]];
for (int i2=0; i2<=dim2; i2++) {
if (b1==bdata2[edata2[i2]]) {
for (int i2=0; i2 <= dim2; i2++) {
if (b1 == bdata2[edata2[i2]]) {
return;
}
}
@@ -1963,14 +1965,14 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
// allocate mjContact[mjMAXCONPAIR] on the arena
mjContact* con =
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
if (!con) {
mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
return;
}
// capsule : capsule
if (dim1==1 && dim2==1) {
if (dim1 == 1 && dim2 == 1) {
// make capsules from vertices
mjtNum pos1[3], mat1[9], size1[2];
mjtNum pos2[3], mat2[9], size2[2];
@@ -2001,12 +2003,12 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f1, f2);
// ignore gap in self collision, since margin is ignored
if (f1==f2) {
if (f1 == f2) {
gap = 0;
}
// add contacts
for (int i=0; i<num; i++) {
for (int i=0; i < num; i++) {
// set contact ids
con[i].geom[0] = -1;
con[i].geom[1] = -1;
@@ -2054,14 +2056,14 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {
// allocate mjContact[mjMAXCONPAIR] on the arena
mjContact* con =
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
(mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
if (!con) {
mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
return;
}
// sphere : capsule
if (dim==1) {
if (dim == 1) {
mjtNum pos[3], mat[9], size[2];
mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
mj_makeCapsule(m, d, f, edata, pos, mat, size);
@@ -2069,11 +2071,11 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {
}
// sphere : triangle
else if (dim==2) {
else if (dim == 2) {
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
num = mjraw_SphereTriangle(con, 0, vert, radius,
vertxpos + 3*edata[0], vertxpos + 3*edata[1],
vertxpos + 3*edata[2], radius);
vertxpos + 3*edata[0], vertxpos + 3*edata[1],
vertxpos + 3*edata[2], radius);
}
// sphere : tetrahdron
@@ -2094,7 +2096,7 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {
mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f);
// add contacts
for (int i=0; i<num; i++) {
for (int i=0; i < num; i++) {
// set contact ids
con[i].geom[0] = -1;
con[i].geom[1] = -1;
src/engine/engine_collision_primitive.c
+5 -5
View File
@@ -520,10 +520,10 @@ static mjtNum pointSegment(mjtNum res[2], const mjtNum p[2],
mjtNum a = mju_dot(uv, up, 2) / mju_max(mjMINVAL, mju_dot(uv, uv, 2));
// find nearest point to p, clamp to u or v if a is not in (0,1)
if (a<=0) {
if (a <= 0) {
res[0] = u[0];
res[1] = u[1];
} else if (a>=1) {
} else if (a >= 1) {
res[0] = v[0];
res[1] = v[1];
} else {
@@ -582,7 +582,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
mjtNum sign3 = areaSign(p, b, o);
// p is inside triangle
if (sign1==sign2 && sign2==sign3) {
if (sign1 == sign2 && sign2 == sign3) {
// P is nearest point to S within triangle
mju_copy3(X, P);
}
@@ -596,7 +596,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
dstx[2] = pointSegment(x[2], p, b, o);
// select minimum
int best = (dstx[0]<dstx[1] && dstx[0]<dstx[2]) ? 0 : (dstx[1]<dstx[2] ? 1 : 2);
int best = (dstx[0] < dstx[1] && dstx[0] < dstx[2]) ? 0 : (dstx[1] < dstx[2] ? 1 : 2);
// convert x[best] to 3D
mju_scl3(X, V1, x[best][0]);
@@ -609,7 +609,7 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
mjtNum dst = mju_normalize3(nrm);
// exit if too far
if (dst>rbound) {
if (dst > rbound) {
return 0;
}
src/engine/engine_collision_sdf.c
+178 -176
View File
@@ -43,44 +43,44 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p
// see https://iquilezles.org/articles/distfunctions/
switch (type) {
case mjGEOM_PLANE:
return x[2];
case mjGEOM_SPHERE:
return mju_norm3(x) - size[0];
case mjGEOM_BOX:
a[0] = mju_abs(x[0]) - size[0];
a[1] = mju_abs(x[1]) - size[1];
a[2] = mju_abs(x[2]) - size[2];
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
b[2] = mju_max(a[2], 0);
return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0);
case mjGEOM_CAPSULE:
a[0] = x[0];
a[1] = x[1];
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
return mju_norm3(a) - size[0];
case mjGEOM_ELLIPSOID:
a[0] = x[0] / size[0];
a[1] = x[1] / size[1];
a[2] = x[2] / size[2];
b[0] = a[0] / size[0];
b[1] = a[1] / size[1];
b[2] = a[2] / size[2];
mjtNum k0 = mju_norm3(a);
mjtNum k1 = mju_norm3(b);
return k0 * (k0 - 1.0) / k1;
case mjGEOM_CYLINDER:
a[0] = mju_sqrt(x[0]*x[0]+x[1]*x[1]) - size[0];
a[1] = mju_abs(x[2]) - size[1];
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2);
case mjGEOM_SDF:
return p->sdf_distance(x, d, i);
default:
mjERROR("sdf collisions not available for geom type %d", type);
return 0;
case mjGEOM_PLANE:
return x[2];
case mjGEOM_SPHERE:
return mju_norm3(x) - size[0];
case mjGEOM_BOX:
a[0] = mju_abs(x[0]) - size[0];
a[1] = mju_abs(x[1]) - size[1];
a[2] = mju_abs(x[2]) - size[2];
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
b[2] = mju_max(a[2], 0);
return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0);
case mjGEOM_CAPSULE:
a[0] = x[0];
a[1] = x[1];
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
return mju_norm3(a) - size[0];
case mjGEOM_ELLIPSOID:
a[0] = x[0] / size[0];
a[1] = x[1] / size[1];
a[2] = x[2] / size[2];
b[0] = a[0] / size[0];
b[1] = a[1] / size[1];
b[2] = a[2] / size[2];
mjtNum k0 = mju_norm3(a);
mjtNum k1 = mju_norm3(b);
return k0 * (k0 - 1.0) / k1;
case mjGEOM_CYLINDER:
a[0] = mju_sqrt(x[0]*x[0]+x[1]*x[1]) - size[0];
a[1] = mju_abs(x[2]) - size[1];
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2);
case mjGEOM_SDF:
return p->sdf_distance(x, d, i);
default:
mjERROR("sdf collisions not available for geom type %d", type);
return 0;
}
}
@@ -92,83 +92,83 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
// see https://iquilezles.org/articles/distfunctions/
switch (type) {
case mjGEOM_PLANE:
mju_zero3(gradient);
gradient[2] = 1;
break;
case mjGEOM_SPHERE:
mju_copy3(gradient, x);
c = mju_norm3(x);
gradient[0] *= 1. / c;
gradient[1] *= 1. / c;
gradient[2] *= 1. / c;
break;
case mjGEOM_BOX:
mju_zero3(gradient);
a[0] = mju_abs(x[0]) - size[0];
a[1] = mju_abs(x[1]) - size[1];
a[2] = mju_abs(x[2]) - size[2];
int k = a[0] > a[1] ? 0 : 1;
int l = a[2] > a[k] ? 2 : k;
if (a[l]<0) {
gradient[l] = x[l] / mju_abs(x[l]);
} else {
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
b[2] = mju_max(a[2], 0);
c = mju_norm3(b);
gradient[0] = a[0]>0 ? b[0] / c * x[0] / mju_abs(x[0]) : 0;
gradient[1] = a[1]>0 ? b[1] / c * x[1] / mju_abs(x[1]) : 0;
gradient[2] = a[2]>0 ? b[2] / c * x[2] / mju_abs(x[2]) : 0;
}
break;
case mjGEOM_CAPSULE:
a[0] = x[0];
a[1] = x[1];
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
c = mju_norm3(a);
gradient[0] = a[0] / c;
gradient[1] = a[1] / c;
gradient[2] = a[2] / c;
break;
case mjGEOM_ELLIPSOID:
a[0] = x[0] / size[0];
a[1] = x[1] / size[1];
a[2] = x[2] / size[2];
b[0] = a[0] / size[0];
b[1] = a[1] / size[1];
b[2] = a[2] / size[2];
mjtNum k0 = mju_norm3(a);
mjtNum k1 = mju_norm3(b);
gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1);
gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1);
gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1);
break;
case mjGEOM_CYLINDER:
c = mju_sqrt(x[0]*x[0]+x[1]*x[1]);
e = mju_abs(x[2]);
a[0] = c - size[0];
a[1] = e - size[1];
mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e};
int j = a[0] > a[1] ? 0 : 1;
if (a[j] < 0) {
gradient[0] = j==0 ? grada[0] : 0;
gradient[1] = j==0 ? grada[1] : 0;
gradient[2] = j==1 ? grada[2] : 0;
} else {
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
mjtNum bnorm = mju_norm(b, 2);
gradient[0] = grada[0] * b[0] / bnorm;
gradient[1] = grada[1] * b[0] / bnorm;
gradient[2] = grada[2] * b[1] / bnorm;
}
break;
case mjGEOM_SDF:
p->sdf_gradient(gradient, x, d, i);
break;
default:
mjERROR("sdf collisions not available for geom type %d", type);
case mjGEOM_PLANE:
mju_zero3(gradient);
gradient[2] = 1;
break;
case mjGEOM_SPHERE:
mju_copy3(gradient, x);
c = mju_norm3(x);
gradient[0] *= 1. / c;
gradient[1] *= 1. / c;
gradient[2] *= 1. / c;
break;
case mjGEOM_BOX:
mju_zero3(gradient);
a[0] = mju_abs(x[0]) - size[0];
a[1] = mju_abs(x[1]) - size[1];
a[2] = mju_abs(x[2]) - size[2];
int k = a[0] > a[1] ? 0 : 1;
int l = a[2] > a[k] ? 2 : k;
if (a[l] < 0) {
gradient[l] = x[l] / mju_abs(x[l]);
} else {
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
b[2] = mju_max(a[2], 0);
c = mju_norm3(b);
gradient[0] = a[0] > 0 ? b[0] / c * x[0] / mju_abs(x[0]) : 0;
gradient[1] = a[1] > 0 ? b[1] / c * x[1] / mju_abs(x[1]) : 0;
gradient[2] = a[2] > 0 ? b[2] / c * x[2] / mju_abs(x[2]) : 0;
}
break;
case mjGEOM_CAPSULE:
a[0] = x[0];
a[1] = x[1];
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
c = mju_norm3(a);
gradient[0] = a[0] / c;
gradient[1] = a[1] / c;
gradient[2] = a[2] / c;
break;
case mjGEOM_ELLIPSOID:
a[0] = x[0] / size[0];
a[1] = x[1] / size[1];
a[2] = x[2] / size[2];
b[0] = a[0] / size[0];
b[1] = a[1] / size[1];
b[2] = a[2] / size[2];
mjtNum k0 = mju_norm3(a);
mjtNum k1 = mju_norm3(b);
gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1);
gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1);
gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1);
break;
case mjGEOM_CYLINDER:
c = mju_sqrt(x[0]*x[0]+x[1]*x[1]);
e = mju_abs(x[2]);
a[0] = c - size[0];
a[1] = e - size[1];
mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e};
int j = a[0] > a[1] ? 0 : 1;
if (a[j] < 0) {
gradient[0] = j == 0 ? grada[0] : 0;
gradient[1] = j == 0 ? grada[1] : 0;
gradient[2] = j == 1 ? grada[2] : 0;
} else {
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
mjtNum bnorm = mju_norm(b, 2);
gradient[0] = grada[0] * b[0] / bnorm;
gradient[1] = grada[1] * b[0] / bnorm;
gradient[2] = grada[2] * b[1] / bnorm;
}
break;
case mjGEOM_SDF:
p->sdf_gradient(gradient, x, d, i);
break;
default:
mjERROR("sdf collisions not available for geom type %d", type);
}
}
@@ -179,16 +179,16 @@ mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjt
mjtNum y[3];
switch (s->type) {
case mjSDFTYPE_SINGLE:
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]),
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]));
default:
mjERROR("SDF type not available");
return 0;
case mjSDFTYPE_SINGLE:
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]),
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]));
default:
mjERROR("SDF type not available");
return 0;
}
}
@@ -199,33 +199,33 @@ void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
const mjtNum* point[2] = {x, y};
switch (s->type) {
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) >
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1;
geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]);
if (i==1) {
mju_rotVecMatT(gradient, gradient, s->relmat);
}
break;
case mjSDFTYPE_AVERAGE:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
mjtNum grad1[3], grad2[3];
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mju_normalize3(grad1);
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
mju_rotVecMatT(grad2, grad2, s->relmat);
mju_normalize3(grad2);
mju_sub3(gradient, grad1, grad2);
mju_normalize3(gradient);
break;
case mjSDFTYPE_SINGLE:
geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]);
break;
default:
mjERROR("SDF type not available");
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) >
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1;
geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]);
if (i == 1) {
mju_rotVecMatT(gradient, gradient, s->relmat);
}
break;
case mjSDFTYPE_AVERAGE:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
mjtNum grad1[3], grad2[3];
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mju_normalize3(grad1);
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
mju_rotVecMatT(grad2, grad2, s->relmat);
mju_normalize3(grad2);
mju_sub3(gradient, grad1, grad2);
mju_normalize3(gradient);
break;
case mjSDFTYPE_SINGLE:
geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]);
break;
default:
mjERROR("SDF type not available");
}
}
@@ -257,7 +257,7 @@ static void undoTransformation(const mjModel* m, const mjData* d, int g,
mjtNum sdf_xpos[3], mjtNum sdf_quat[4]) {
mjtNum* xpos = d->geom_xpos + 3 * g;
mjtNum* xmat = d->geom_xmat + 9 * g;
if (m->geom_type[g]==mjGEOM_MESH || m->geom_type[g]==mjGEOM_SDF) {
if (m->geom_type[g] == mjGEOM_MESH || m->geom_type[g] == mjGEOM_SDF) {
mjtNum negpos[3], negquat[4], xquat[4];
mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[g];
mjtNum* quat = m->mesh_quat + 4 * m->geom_dataid[g];
@@ -328,19 +328,19 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
// finds minimum of Frank-Wolfe objective
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
const mjModel* m, const mjSDF* sdf, mjData* d) {
for (int step=0; step<m->opt.sdf_iterations; step++) {
for (int step=0; step < m->opt.sdf_iterations; step++) {
mjtNum best = 1e10, fun, s[3], grad[3];
// evaluate gradient
mjc_gradient(m, d, sdf, grad, x);
// evaluate all corners
for (int i=0; i<ncorners; i++) {
for (int i=0; i < ncorners; i++) {
// compute sdf
fun = mju_dot3(corners + 3*i, grad);
// save argmin
if (fun<best) {
if (fun < best) {
best = fun;
mju_copy3(s, corners + 3*i);
}
@@ -360,16 +360,16 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
mjData* d) {
mjtNum alpha = 0.2; // step along the gradient direction
for (int step=0; step<m->opt.sdf_iterations; step++) {
for (int step=0; step < m->opt.sdf_iterations; step++) {
mjtNum grad[3];
// evaluate gradient
mjc_gradient(m, d, s, grad, x);
// sanity check
if (isnan(grad[0]) || grad[0]>mjMAXVAL || grad[0]<-mjMAXVAL ||
isnan(grad[1]) || grad[1]>mjMAXVAL || grad[1]<-mjMAXVAL ||
isnan(grad[2]) || grad[2]>mjMAXVAL || grad[2]<-mjMAXVAL) {
if (isnan(grad[0]) || grad[0] > mjMAXVAL || grad[0] < -mjMAXVAL ||
isnan(grad[1]) || grad[1] > mjMAXVAL || grad[1] < -mjMAXVAL ||
isnan(grad[2]) || grad[2] > mjMAXVAL || grad[2] < -mjMAXVAL) {
return mjMAXVAL;
}
@@ -422,7 +422,7 @@ static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
// coordinate change
mju_addTo3(center, p);
return mjc_distance(m, d, sdf, center)<r;
return mjc_distance(m, d, sdf, center) < r;
}
// intersect with circumsphere of bounding box
@@ -436,7 +436,7 @@ static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
mju_addTo3(candidate, offset);
// check if inside the bounding box
return mjc_distance(m, d, s, candidate)<r;
return mjc_distance(m, d, s, candidate) < r;
}
//---------------------------- mesh vs sdf broad phase --------------------------------------------
@@ -460,7 +460,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
};
typedef struct CollideTreeArgs_ CollideTreeArgs;
CollideTreeArgs* stack = (CollideTreeArgs*) mj_stackAllocByte(
d, max_stack * sizeof(CollideTreeArgs), _Alignof(CollideTreeArgs));
d, max_stack * sizeof(CollideTreeArgs), _Alignof(CollideTreeArgs));
int nstack = 0;
stack[nstack].node = 0;
@@ -480,7 +480,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
}
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
faces[*npoints] = faceid[node];
if (++(*npoints)==MAXSDFFACE) {
if (++(*npoints) == MAXSDFFACE) {
mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed");
mj_freeStack(d);
return;
@@ -498,7 +498,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
visited[node] = 1;
// recursive call
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
if (child[2*node+i] != -1) {
if (nstack >= max_stack) mjERROR("BVH stack depth exceeded.");
stack[nstack].node = child[2*node+i];
@@ -551,9 +551,9 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf);
// Frank-Wolfe algorithm
for (int i=0; i<npoints; i++) {
for (int i=0; i < npoints; i++) {
int face = faceadr + faces[i];
for (int v=0; v<3; v++) {
for (int v=0; v < 3; v++) {
mjtNum vec[3] = {
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0],
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1],
@@ -576,13 +576,13 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
x[2] = (corners[2]+corners[5]+corners[8])/3;
// SHOULD NOT OCCUR
if (ncandidate==MAXMESHPNT) mjERROR("too many contact points");
if (ncandidate == MAXMESHPNT)mjERROR("too many contact points");
// Frank-Wolfe
depth = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d);
// store candidate if there is penetration
if (depth<0) {
if (depth < 0) {
mju_copy3(candidate + 3*ncandidate, x);
index[ncandidate] = ncandidate;
dist[ncandidate++] = depth;
@@ -593,7 +593,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
mjQUICKSORT(index, ncandidate, sizeof(int), distcompare, dist);
// add only the first mjMAXCONPAIR pairs
for (int i=0; i<mju_min(ncandidate, mjMAXCONPAIR); i++) {
for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) {
cnt = addContact(points, con, candidate + 3*index[i], pos2true, sdf_quat,
dist[index[i]], cnt, m, &sdf, (mjData*)d);
}
@@ -633,7 +633,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
mapPose(pos1true, squat1, pos2true, squat2, offset12, rotation12);
// axis-aligned bounding boxes in g1 frame
for (int i=0; i<8; i++) {
for (int i=0; i < 8; i++) {
vec1[0] = (i&1 ? size1[0]+size1[3] : size1[0]-size1[3]);
vec1[1] = (i&2 ? size1[1]+size1[4] : size1[1]-size1[4]);
vec1[2] = (i&4 ? size1[2]+size1[5] : size1[2]-size1[5]);
@@ -645,7 +645,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
mju_rotVecMat(vec2, vec2, rotation1);
mju_addTo3(vec2, offset1);
for (int k=0; k<3; k++) {
for (int k=0; k < 3; k++) {
aabb1[0+k] = mju_min(aabb1[0+k], vec1[k]);
aabb1[3+k] = mju_max(aabb1[3+k], vec1[k]);
aabb2[0+k] = mju_min(aabb2[0+k], vec2[k]);
@@ -654,13 +654,13 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
}
// intersection of aabbs
for (int k=0; k<3; k++) {
for (int k=0; k < 3; k++) {
aabb[0+k] = mju_max(aabb1[0+k], aabb2[0+k]);
aabb[3+k] = mju_min(aabb1[3+k], aabb2[3+k]);
}
// no intersection if max < min
if (aabb[3]<aabb[0] || aabb[4]<aabb[1] || aabb[5]<aabb[2]) {
if (aabb[3] < aabb[0] || aabb[4] < aabb[1] || aabb[5] < aabb[2]) {
return cnt;
}
@@ -696,7 +696,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
mjtNum contacts[3*mjMAXCONPAIR];
int i = 0, j = 0;
while (i<m->opt.sdf_initpoints) {
while (i < m->opt.sdf_initpoints) {
x[0] = aabb[0] + (aabb[3]-aabb[0]) * mju_Halton(j, 2);
x[1] = aabb[1] + (aabb[4]-aabb[1]) * mju_Halton(j, 3);
x[2] = aabb[2] + (aabb[5]-aabb[2]) * mju_Halton(j, 5);
@@ -723,7 +723,9 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d);
// SHOULD NOT OCCUR
if (cnt>mjMAXCONPAIR) mjERROR("too many contact points");
if (cnt > mjMAXCONPAIR) {
mjERROR("too many contact points");
}
}
return cnt;
src/engine/engine_core_constraint.c
+35 -33
View File
@@ -124,11 +124,11 @@ 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)) {
for (int i=0; i<5; i++) {
for (int i=0; i < 5; i++) {
target[i] = mju_max(mjMINMU, m->opt.o_friction[i]);
}
} else {
for (int i=0; i<5; i++) {
for (int i=0; i < 5; i++) {
target[i] = mju_max(mjMINMU, source[i]);
}
}
@@ -181,20 +181,20 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
// compute inverse distances from contact point to element vertices
// save body ids, find vertex v in element
int vid = -1;
for (int i=0; i<=dim; i++) {
for (int i=0; i <= dim; i++) {
mjtNum dist = mju_dist3(point, vert+3*edata[i]);
weight[i] = 1.0/(mju_max(mjMINVAL, dist));
body[i] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[i]];
// check if element vertex matches v
if (edata[i]==v) {
if (edata[i] == v) {
vid = i;
}
}
// v found in e: skip and shift remaining
if (vid>=0) {
while (vid<dim) {
if (vid >= 0) {
while (vid < dim) {
weight[vid] = weight[vid+1];
body[vid] = body[vid+1];
vid++;
@@ -242,9 +242,9 @@ 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,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain) {
const mjtNum* jac, const mjtNum* pos,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain) {
int empty, nv = m->nv, nefc = d->nefc;
int *nnz = d->efc_J_rownnz, *adr = d->efc_J_rowadr, *ind = d->efc_J_colind;
mjtNum *J = d->efc_J;
@@ -665,7 +665,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
flex_edgenum = m->flex_edgenum[id[0]];
// add one constraint per edge
for (int e=flex_edgeadr; e<flex_edgeadr+flex_edgenum; e++) {
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
// position error
cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e];
@@ -930,14 +930,14 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
con->efc_address = d->nefc;
// special case: single body on each side
if ((con->geom[0]>=0 || con->vert[0]>=0) &&
(con->geom[1]>=0 || con->vert[1]>=0)) {
if ((con->geom[0] >= 0 || con->vert[0] >= 0) &&
(con->geom[1] >= 0 || con->vert[1] >= 0)) {
// get bodies
int bid[2];
for (int side=0; side < 2; side++) {
bid[side] = (con->geom[side]>=0) ?
m->geom_bodyid[con->geom[side]] :
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bid[side] = (con->geom[side] >= 0) ?
m->geom_bodyid[con->geom[side]] :
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
}
// compute Jacobian differences
@@ -956,16 +956,16 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
int nb = 0;
int bid[8];
mjtNum bweight[8];
for (int side=0; side<2; side++) {
for (int side=0; side < 2; side++) {
// geom
if (con->geom[side]>=0) {
if (con->geom[side] >= 0) {
bid[nb] = m->geom_bodyid[con->geom[side]];
bweight[nb] = side ? +1 : -1;
nb++;
}
// flex vert
else if (con->vert[side]>=0) {
else if (con->vert[side] >= 0) {
bid[nb] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[nb] = side ? +1 : -1;
nb++;
@@ -977,7 +977,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
con->vert[1-side], con->pos, bid+nb, bweight+nb);
// negative sign for first side of contact
if (side==0) {
if (side == 0) {
mju_scl(bweight+nb, bweight+nb, -1, nw);
}
@@ -986,7 +986,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
}
// combine weighted Jacobians
NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim>3);
NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3);
}
// skip contact if no DOFs affected
@@ -1068,7 +1068,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
id = d->efc_id[i];
// clear edge counter
if (d->efc_type[i]!=mjEQ_FLEX) {
if (d->efc_type[i] != mjEQ_FLEX) {
edgecnt = 0;
}
@@ -1139,31 +1139,33 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// add the average translation and rotation components from both sides
tran = rot = 0;
for (int side=0; side<2; side++) {
for (int side=0; side < 2; side++) {
// get bodies and weights
int nb, bid[4];
mjtNum bweight[4];
// geom
if (con->geom[side]>=0) {
if (con->geom[side] >= 0) {
bid[0] = m->geom_bodyid[con->geom[side]];
bweight[0] = 1;
nb = 1;
}
// flex vert
} else if (con->vert[side]>=0) {
else if (con->vert[side] >= 0) {
bid[0] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[0] = 1;
nb = 1;
}
// flex elem
} else {
else {
nb = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid, bweight);
}
// add weighted average over bodies
for (int k=0; k<nb; k++) {
for (int k=0; k < nb; k++) {
tran += m->body_invweight0[2*bid[k]] * bweight[k];
rot += m->body_invweight0[2*bid[k]+1] * bweight[k];
}
@@ -1188,7 +1190,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
else {
for (int j=0; j < dim-1; j++) {
fri = con->friction[j];
dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j<2 ? tran : rot);
dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j < 2 ? tran : rot);
}
// processed 2*dim-2 elements in one i-loop iteration; advance counter
@@ -1401,7 +1403,7 @@ void mj_makeImpedance(const mjModel* m, mjData* d) {
// elliptic contacts use solreffriction in non-normal directions, if non-zero
int elliptic_friction = (tp == mjCNSTR_CONTACT_ELLIPTIC) && (j > 0);
mjtNum* ref = elliptic_friction && (solreffriction[0] || solreffriction[1]) ?
solreffriction : solref;
solreffriction : solref;
// friction: K = 0
if (tp == mjCNSTR_FRICTION_DOF || tp == mjCNSTR_FRICTION_TENDON || elliptic_friction) {
@@ -1526,7 +1528,7 @@ static int mj_jacSumCount(const mjModel* m, mjData* d, int* chain,
NV = mj_bodyChain(m, body[0], chain);
// accumulate remaining
for (int i=1; i<n; i++) {
for (int i=1; i < n; i++) {
// get body chain
int bodyNV = mj_bodyChain(m, body[i], bodychain);
if (!bodyNV) {
@@ -1649,7 +1651,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
flex_edgenum = m->flex_edgenum[id[0]];
// process edges of this flex
for (int e=flex_edgeadr; e<flex_edgeadr+flex_edgenum; e++) {
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
int b1 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e+1]];
@@ -1665,7 +1667,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
// accumulate counts; flex NV already accumulated
ne += mj_addConstraintCount(m, size, NV);
nnze += (m->eq_type[i]==mjEQ_FLEX) ? NV : size*NV;
nnze += (m->eq_type[i] == mjEQ_FLEX) ? NV : size*NV;
}
}
@@ -1805,7 +1807,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int nb = 0, bid[8];
for (int side=0; side < 2; side++) {
// geom
if (con->geom[side]>=0) {
if (con->geom[side] >= 0) {
bid[nb++] = m->geom_bodyid[con->geom[side]];
}
@@ -1819,7 +1821,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int f = con->flex[side];
int fdim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1);
for (int k=0; k<=fdim; k++) {
for (int k=0; k <= fdim; k++) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[k]];
}
}
src/engine/engine_core_smooth.c
+25 -25
View File
@@ -284,7 +284,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
// adjust for mode
switch ((mjtCamLight) m->cam_mode[i]) {
case mjCAMLIGHT_FIXED:
break;
break;
case mjCAMLIGHT_TRACK:
case mjCAMLIGHT_TRACKCOM:
// fixed global orientation
@@ -393,15 +393,15 @@ void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) {
mju_zeroInt(modified, bvhnum);
// mark leafs as modified
for (int i=0; i<bvhnum; i++) {
if (m->bvh_nodeid[bvhadr+i]>=0) {
for (int i=0; i < bvhnum; i++) {
if (m->bvh_nodeid[bvhadr+i] >= 0) {
modified[i] = 1;
}
}
// update non-leafs in backward pass (parents come before children)
for (int i=bvhnum-1; i>=0; i--) {
if (m->bvh_nodeid[bvhadr+i]<0) {
for (int i=bvhnum-1; i >= 0; i--) {
if (m->bvh_nodeid[bvhadr+i] < 0) {
int child1 = m->bvh_child[2*(bvhadr+i)];
int child2 = m->bvh_child[2*(bvhadr+i)+1];
@@ -413,13 +413,13 @@ void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) {
// compute new (min, max)
mjtNum xmin[3], xmax[3];
for (int k=0; k<3; k++) {
for (int k=0; k < 3; k++) {
xmin[k] = mju_min(aabb1[k] - aabb1[k+3], aabb2[k] - aabb2[k+3]);
xmax[k] = mju_max(aabb1[k] + aabb1[k+3], aabb2[k] + aabb2[k+3]);
}
// convert to (center, size)
for (int k=0; k<3; k++) {
for (int k=0; k < 3; k++) {
aabb[k] = 0.5*(xmax[k]+xmin[k]);
aabb[k+3] = 0.5*(xmax[k]-xmin[k]);
}
@@ -446,20 +446,20 @@ void mj_flex(const mjModel* m, mjData* d) {
}
// compute Cartesian positions of flex vertices
for (int f=0; f<m->nflex; f++) {
for (int f=0; f < m->nflex; f++) {
int vstart = m->flex_vertadr[f];
int vend = m->flex_vertadr[f] + m->flex_vertnum[f];
// centered: copy body position
if (m->flex_centered[f]) {
for (int i=vstart; i<vend; i++) {
for (int i=vstart; i < vend; i++) {
mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
// non-centered: map from local to global
else {
for (int i=vstart; i<vend; i++) {
for (int i=vstart; i < vend; i++) {
mju_rotVecMat(d->flexvert_xpos+3*i, m->flex_vert+3*i, d->xmat+9*m->flex_vertbodyid[i]);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
@@ -467,11 +467,11 @@ void mj_flex(const mjModel* m, mjData* d) {
}
// compute flex element aabb
for (int f=0; f<m->nflex; f++) {
for (int f=0; f < m->nflex; f++) {
int dim = m->flex_dim[f];
// process elements of this flex
for (int e=0; e<m->flex_elemnum[f]; e++) {
for (int e=0; e < m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
@@ -479,8 +479,8 @@ void mj_flex(const mjModel* m, mjData* d) {
mjtNum xmin[3], xmax[3];
mju_copy3(xmin, vert+3*edata[0]);
mju_copy3(xmax, vert+3*edata[0]);
for (int i=1; i<=dim; i++) {
for (int j=0; j<3; j++) {
for (int i=1; i <= dim; i++) {
for (int j=0; j < 3; j++) {
mjtNum value = vert[3*edata[i]+j];
xmin[j] = mju_min(xmin[j], value);
xmax[j] = mju_max(xmax[j], value);
@@ -500,14 +500,14 @@ void mj_flex(const mjModel* m, mjData* d) {
// update flex bhv_aabb_dyn if needed
if (!mjDISABLED(mjDSBL_MIDPHASE)) {
for (int f=0; f<m->nflex; f++) {
if (m->flex_bvhadr[f]>=0) {
for (int f=0; f < m->nflex; f++) {
if (m->flex_bvhadr[f] >= 0) {
int flex_bvhadr = m->flex_bvhadr[f];
int flex_bvhnum = m->flex_bvhnum[f];
// copy element aabbs to bhv leaf aabbs
for (int i=flex_bvhadr; i<flex_bvhadr+flex_bvhnum; i++) {
if (m->bvh_nodeid[i]>=0) {
for (int i=flex_bvhadr; i < flex_bvhadr+flex_bvhnum; i++) {
if (m->bvh_nodeid[i] >= 0) {
mju_copy(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic),
d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i]), 6);
}
@@ -535,18 +535,18 @@ void mj_flex(const mjModel* m, mjData* d) {
}
// compute lengths and Jacobians of edges
for (int f=0; f<m->nflex; f++) {
for (int f=0; f < m->nflex; f++) {
// skip if edges cannot generate forces
if (m->flex_rigid[f] ||
(m->flex_edgeequality[f]==0 &&
m->flex_edgestiffness[f]==0 && m->flex_edgedamping[f]==0)) {
(m->flex_edgeequality[f] == 0 &&
m->flex_edgestiffness[f] == 0 && m->flex_edgedamping[f] == 0)) {
continue;
}
// process edges of this flex
int vbase = m->flex_vertadr[f];
int ebase = m->flex_edgeadr[f];
for (int e=0; e<m->flex_edgenum[f]; e++) {
for (int e=0; e < m->flex_edgenum[f]; e++) {
int v1 = m->flex_edge[2*(ebase+e)];
int v2 = m->flex_edge[2*(ebase+e)+1];
int b1 = m->flex_vertbodyid[vbase+v1];
@@ -562,7 +562,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// sparse edge Jacobian
if (issparse) {
// set rowadr
if (ebase+e>0) {
if (ebase+e > 0) {
rowadr[ebase+e] = rowadr[ebase+e-1] + rownnz[ebase+e-1];
}
@@ -1110,7 +1110,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
const mjContact* con = d->contact+j;
// contact involving flex, continue
if (con->geom[0]<0 || con->geom[1]<0) {
if (con->geom[0] < 0 || con->geom[1] < 0) {
continue;
}
@@ -1772,7 +1772,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
con = d->contact+i;
// skip contact involving flex
if (con->geom[0]<0 || con->geom[1]<0) {
if (con->geom[0] < 0 || con->geom[1] < 0) {
continue;
}
src/engine/engine_derivative.c
+2 -2
View File
@@ -1471,12 +1471,12 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
// flex edge damping
for (int f=0; f<m->nflex; f++) {
for (int f=0; f < m->nflex; f++) {
if (!m->flex_rigid[f] && m->flex_edgedamping[f]) {
mjtNum B = -m->flex_edgedamping[f];
// process edges of this flex
for (int e=m->flex_edgeadr[f]; e<m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->flexedge_J, &B, 1, e,
src/engine/engine_forward.c
+2 -2
View File
@@ -215,7 +215,7 @@ 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) {
int actuator_id, int act_adr, mjtNum act_dot) {
mjtNum act = d->act[act_adr];
if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) {
@@ -569,7 +569,7 @@ void mj_solCG_island_multithreaded(const mjModel* m, mjData* d) {
mj_markStack(d);
// allocate array of arguments to be passed to threads
mjSolIslandArgs* sol_cg_island_args =
mj_stackAllocByte(d, sizeof(mjSolIslandArgs) * d->nisland, _Alignof(mjSolIslandArgs));
mj_stackAllocByte(d, sizeof(mjSolIslandArgs) * d->nisland, _Alignof(mjSolIslandArgs));
mjTask* tasks = mj_stackAllocByte(d, sizeof(mjTask) * d->nisland, _Alignof(mjTask));
for (int island = 0; island < d->nisland; ++island) {
src/engine/engine_io.c
+33 -36
View File
@@ -444,19 +444,16 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
// allocate and initialize mjModel structure
mjModel* mj_makeModel(
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic,
int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam, int nlight,
int nflex, int nflexvert, int nflexedge, int nflexelem, int nflexelemdata,
int nflexshelldata, int nflexevpair, int nflextexcoord, int nmesh,
int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata, int ntex,
int ntexdata, int nmat, int npair, int nexclude, int neq, int ntendon,
int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext,
int ntextdata, int ntuple, int ntupledata, int nkey, int nmocap,
int nplugin, int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths) {
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, int njnt,
int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert, int nflexedge,
int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair, int nflextexcoord,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, int nmeshgraph,
int nskin, int nskinvert, int nskintexvert, int nskinface, int nskinbone, int nskinbonevert,
int nhfield, int nhfielddata, int ntex, int ntexdata, int nmat, int npair, int nexclude, int neq,
int ntendon, int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin, int npluginattr, int nuser_body,
int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam, int nuser_tendon,
int nuser_actuator, int nuser_sensor, int nnames, int npaths) {
intptr_t offset = 0;
// allocate mjModel
@@ -602,21 +599,21 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
// allocate new model if needed
if (!dest) {
dest = mj_makeModel(
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
src->nflexelem, src->nflexelemdata, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric,
src->nnumericdata, src->ntext, src->ntextdata, src->ntuple,
src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr,
src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site,
src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths);
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
src->nflexelem, src->nflexelemdata, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric,
src->nnumericdata, src->ntext, src->ntextdata, src->ntuple,
src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr,
src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site,
src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths);
}
if (!dest) {
mjERROR("failed to make mjModel. Invalid sizes.");
@@ -1333,8 +1330,8 @@ static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_
char* prev_pstack_ptr = (char*)(stack_info->top);
size_t prev_misalign = (uintptr_t)prev_pstack_ptr % _Alignof(size_t);
size_t* prev_usage_ptr =
(size_t*)(prev_pstack_ptr +
(prev_misalign ? _Alignof(size_t) - prev_misalign : 0));
(size_t*)(prev_pstack_ptr +
(prev_misalign ? _Alignof(size_t) - prev_misalign : 0));
ASAN_UNPOISON_MEMORY_REGION(prev_usage_ptr, sizeof(size_t));
usage = current_alloc_usage + *prev_usage_ptr;
ASAN_POISON_MEMORY_REGION(prev_usage_ptr, sizeof(size_t));
@@ -1343,7 +1340,7 @@ static inline void* stackallocinternal(mjData* d, mjStackInfo* stack_info, size_
// store new stack usage in the red zone
size_t misalign = new_top_ptr % _Alignof(size_t);
size_t* usage_ptr =
(size_t*)(new_top_ptr + (misalign ? _Alignof(size_t) - misalign : 0));
(size_t*)(new_top_ptr + (misalign ? _Alignof(size_t) - misalign : 0));
ASAN_UNPOISON_MEMORY_REGION(usage_ptr, sizeof(size_t));
*usage_ptr = usage;
ASAN_POISON_MEMORY_REGION(usage_ptr, sizeof(size_t));
@@ -1403,7 +1400,7 @@ __attribute__((always_inline))
static inline void markstackinternal(mjData* d, mjStackInfo* stack_info) {
size_t top_old = stack_info->top;
mjStackFrame* s =
(mjStackFrame*) stackallocinternal(d, stack_info, sizeof(mjStackFrame), _Alignof(mjStackFrame));
(mjStackFrame*) stackallocinternal(d, stack_info, sizeof(mjStackFrame), _Alignof(mjStackFrame));
s->pbase = stack_info->stack_base;
s->pstack = top_old;
#ifdef ADDRESS_SANITIZER
@@ -1447,8 +1444,8 @@ static inline void freestackinternal(mjStackInfo* stack_info) {
#define mjSYMBOLIZELEN 256
char dbginfo[mjSYMBOLIZELEN];
__sanitizer_symbolize_pc(
s->pc, "mj_markStack %F at %S has no corresponding mj_freeStack",
dbginfo, sizeof(dbginfo));
s->pc, "mj_markStack %F at %S has no corresponding mj_freeStack",
dbginfo, sizeof(dbginfo));
dbginfo[mjSYMBOLIZELEN - 1] = '\0';
mjERROR("%s", dbginfo);
#undef mjSYMBOLIZELEN
@@ -1584,7 +1581,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
mju_zero(d->qvel, m->nv);
mju_zero(d->act, m->na);
mju_zero(d->ctrl, m->nu);
for (int i=0; i<m->neq; i++) d->eq_active[i] = m->eq_active0[i];
for (int i=0; i < m->neq; i++) d->eq_active[i] = m->eq_active0[i];
mju_zero(d->qfrc_applied, m->nv);
mju_zero(d->xfrc_applied, 6*m->nbody);
mju_zero(d->qacc, m->nv);
@@ -1966,7 +1963,7 @@ const char* mj_validateReferences(const mjModel* m) {
if (m->geom_dataid[i] >= m->nhfield || m->geom_dataid[i] < -1) {
return "Invalid model: geom_dataid out of bounds.";
}
} else if ((m->geom_type[i]==mjGEOM_MESH) || (m->geom_type[i]==mjGEOM_SDF)) {
} else if ((m->geom_type[i] == mjGEOM_MESH) || (m->geom_type[i] == mjGEOM_SDF)) {
if (m->geom_dataid[i] >= m->nmesh || m->geom_dataid[i] < -1) {
return "Invalid model: geom_dataid out of bounds.";
}
src/engine/engine_passive.c
+2 -2
View File
@@ -52,7 +52,7 @@ void mj_passive(const mjModel* m, mjData* d) {
stiffness = m->jnt_stiffness[i];
// disabled : nothing to do
if (stiffness==0) {
if (stiffness == 0) {
continue;
}
@@ -102,7 +102,7 @@ void mj_passive(const mjModel* m, mjData* d) {
damping = m->flex_edgedamping[f];
// disabled or rigid: nothing to do
if (m->flex_rigid[f] || (stiffness==0 && damping==0)) {
if (m->flex_rigid[f] || (stiffness == 0 && damping == 0)) {
continue;
}
src/engine/engine_print.c
+2 -2
View File
@@ -362,7 +362,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nbody) fprintf(fp, "\n");
// BVHs
for (int i=0; i<m->nbvh; i++) {
for (int i=0; i < m->nbvh; i++) {
fprintf(fp, "\nBVH %d:\n", i);
object_class = &m->nbvh;
MJMODEL_POINTERS
@@ -428,7 +428,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nlight) fprintf(fp, "\n");
// flexes
for (int i=0; i<m->nflex; i++) {
for (int i=0; i < m->nflex; i++) {
fprintf(fp, "\nFLEX %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_flexadr[i]);
src/engine/engine_ray.c
+25 -25
View File
@@ -844,15 +844,15 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
// compute bounding box
mjtNum box[3][2] = {{0, 0}, {0, 0}, {0, 0}};
mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[flexid];
for (int i=0; i<m->flex_vertnum[flexid]; i++) {
for (int j=0; j<3; j++) {
for (int i=0; i < m->flex_vertnum[flexid]; i++) {
for (int j=0; j < 3; j++) {
// update minimum along side j
if (box[j][0]>vert[3*i+j] || i==0) {
if (box[j][0] > vert[3*i+j] || i == 0) {
box[j][0] = vert[3*i+j];
}
// update maximum along side j
if (box[j][1]<vert[3*i+j] || i==0) {
if (box[j][1] < vert[3*i+j] || i == 0) {
box[j][1] = vert[3*i+j];
}
}
@@ -860,28 +860,28 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
// adjust box for radius
mjtNum radius = m->flex_radius[flexid];
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
box[j][0] -= radius;
box[j][1] += radius;
}
// construct box geom
mjtNum pos[3], size[3], mat[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
pos[j] = 0.5*(box[j][0]+box[j][1]);
size[j] = 0.5*(box[j][1]-box[j][0]);
}
// apply bounding-box filter
if (ray_box(pos, mat, size, pnt, vec, NULL)<0) {
if (ray_box(pos, mat, size, pnt, vec, NULL) < 0) {
return -1;
}
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
if (mju_abs(vec[0])>=mju_abs(vec[1]) && mju_abs(vec[0])>=mju_abs(vec[2])) {
if (mju_abs(vec[0]) >= mju_abs(vec[1]) && mju_abs(vec[0]) >= mju_abs(vec[2])) {
b0[0] = 0;
} else if (mju_abs(vec[1])>=mju_abs(vec[2])) {
} else if (mju_abs(vec[1]) >= mju_abs(vec[2])) {
b0[1] = 0;
} else {
b0[2] = 0;
@@ -895,9 +895,9 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
mjtNum x = -1;
// check edges if rendered, or if skin
if (flg_edge || (dim>1 && flg_skin)) {
for (int e=m->flex_edgeadr[flexid];
e<m->flex_edgeadr[flexid]+m->flex_edgenum[flexid]; e++) {
if (flg_edge || (dim > 1 && flg_skin)) {
int edge_end = m->flex_edgeadr[flexid]+m->flex_edgenum[flexid];
for (int e=m->flex_edgeadr[flexid]; e < edge_end; e++) {
// get vertices for this edge
mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e]);
mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e+1]);
@@ -916,7 +916,7 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE);
// update
if (sol>=0 && (x<0 || sol<x)) {
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
// construct intersection point
@@ -935,8 +935,8 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
}
// check vertices if rendered (and edges not checked)
else if (flg_vert && !(dim>1 && flg_skin)) {
for (int v=0; v<m->flex_vertnum[flexid]; v++) {
else if (flg_vert && !(dim > 1 && flg_skin)) {
for (int v=0; v < m->flex_vertnum[flexid]; v++) {
// get vertex
mjtNum* vpos = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + v);
@@ -947,7 +947,7 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE);
// update
if (sol>=0 && (x<0 || sol<x)) {
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
*vertid = v;
}
@@ -955,11 +955,11 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
}
// check faces if rendered
if (dim>1 && (flg_face || flg_skin)) {
for (int e=0; e<m->flex_elemnum[flexid]; e++) {
if (dim > 1 && (flg_face || flg_skin)) {
for (int e=0; e < m->flex_elemnum[flexid]; e++) {
// skip if 3D element is not visible
int elayer = m->flex_elemlayer[m->flex_elemadr[flexid]+e];
if (dim==3 && ((flg_skin && elayer>0) || (!flg_skin && elayer!=flex_layer))) {
if (dim == 3 && ((flg_skin && elayer > 0) || (!flg_skin && elayer != flex_layer))) {
continue;
}
@@ -968,22 +968,22 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[0]);
mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[1]);
mjtNum* v3 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[2]);
mjtNum* v4 = dim==2 ? NULL : d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[3]);
mjtNum* v4 = dim == 2 ? NULL : d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[3]);
mjtNum* vptr[4][3] = {{v1, v2, v3}, {v1, v2, v4}, {v1, v3, v4}, {v2, v3, v4}};
int vid[4][3] = {{0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3}};
// process triangles of this element
for (int i=0; i<(dim==2?1:4); i++) {
for (int i=0; i < (dim == 2?1:4); i++) {
// copy vertices into triangle representation
mjtNum v[3][3];
for (int j=0; j<3; j++)
for (int j=0; j < 3; j++)
mju_copy3(v[j], vptr[i][j]);
// intersect ray with triangle
mjtNum sol = ray_triangle(v, pnt, vec, b0, b1);
// update
if (sol>=0 && (x<0 || sol<x)) {
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
// construct intersection point
@@ -996,9 +996,9 @@ mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte fl
mju_dist3(v[1], intersect),
mju_dist3(v[2], intersect)
};
if (dist[0]<=dist[1] && dist[0]<=dist[2]) {
if (dist[0] <= dist[1] && dist[0] <= dist[2]) {
*vertid = edata[vid[i][0]];
} else if (dist[1]<=dist[2]){
} else if (dist[1] <= dist[2]){
*vertid = edata[vid[i][1]];
} else {
*vertid = edata[vid[i][2]];
src/engine/engine_sensor.c
+16 -16
View File
@@ -208,8 +208,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
rotation[1][1] = 1;
rotation[2][2] = 1;
rotation[3][3] = 1;
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
for (int i=0; i < 3; i++) {
for (int j=0; j < 3; j++) {
rotation[i][j] = cam_xmat[j*3+i];
}
}
@@ -237,11 +237,11 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
// projection matrix (3x4): product of all 4 matrices
mjtNum proj[3][4] = {0};
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
for (int k=0; k<4; k++) {
for (int l=0; l<4; l++) {
for (int n=0; n<4; n++) {
for (int i=0; i < 3; i++) {
for (int j=0; j < 3; j++) {
for (int k=0; k < 4; k++) {
for (int l=0; l < 4; l++) {
for (int n=0; n < 4; n++) {
proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n];
}
}
@@ -256,10 +256,10 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
// project world coordinates into pixel space, see:
// https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula
mjtNum pixel_coord_hom[3] = {0};
for (int i=0; i<3; i++) {
for (int j=0; j<4; j++) {
pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
}
for (int i=0; i < 3; i++) {
for (int j=0; j < 4; j++) {
pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
}
}
// avoid dividing by tiny numbers
@@ -716,8 +716,8 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// contact pointer, contacting bodies (-1 for flex)
con = d->contact + j;
int conbody[2];
for (int k=0; k<2; k++) {
conbody[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] : -1;
for (int k=0; k < 2; k++) {
conbody[k] = (con->geom[k] >= 0) ? m->geom_bodyid[con->geom[k]] : -1;
}
// select contacts involving sensorized body
@@ -950,14 +950,14 @@ void mj_energyPos(const mjModel* m, mjData* d) {
// add flex-level springs
if (!mjDISABLED(mjDSBL_PASSIVE)) {
for (int i=0; i<m->nflex; i++) {
for (int i=0; i < m->nflex; i++) {
stiffness = m->flex_edgestiffness[i];
if (m->flex_rigid[i] || stiffness==0) {
if (m->flex_rigid[i] || stiffness == 0) {
continue;
}
// process edges of this flex
for (int e=m->flex_edgeadr[i]; e<m->flex_edgeadr[i]+m->flex_edgenum[i]; e++) {
for (int e=m->flex_edgeadr[i]; e < m->flex_edgeadr[i]+m->flex_edgenum[i]; e++) {
mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
d->energy[0] += 0.5*stiffness*displacement*displacement;
}
src/engine/engine_setconst.c
+10 -10
View File
@@ -115,14 +115,14 @@ static void set0(mjModel* m, mjData* d) {
// compute body_invweight0
m->body_invweight0[0] = m->body_invweight0[1] = 0.0;
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// static bodies: zero invweight0
if (m->body_weldid[i] == 0) {
m->body_invweight0[2*i] = m->body_invweight0[2*i+1] = 0;
}
// accelerate simple bodies with no rotations
else if (m->body_simple[i]==2) {
else if (m->body_simple[i] == 2) {
mjtNum mass = m->body_mass[i];
if (!mass) { // SHOULD NOT OCCUR
mjERROR("moving body %d has 0 mass", i);
@@ -147,7 +147,7 @@ static void set0(mjModel* m, mjData* d) {
}
// compute dof_invweight0
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
// simple body with no rotations: no off-diagonal inertia
if (m->body_simple[m->jnt_bodyid[i]] == 2) {
int id = m->jnt_dofadr[i];
@@ -200,14 +200,14 @@ static void set0(mjModel* m, mjData* d) {
// compute flexedge_invweight0, tendon_invweight0, actuator_acc0
if (nv) {
// compute flexedge_invweight0
for (int f=0; f<m->nflex; f++) {
for (int i=m->flex_edgeadr[f]; i<m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) {
for (int f=0; f < m->nflex; f++) {
for (int i=m->flex_edgeadr[f]; i < m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) {
// bodies connected by edge
int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i+1]];
// accelerate edges that connect simple bodies with no rotations
if (m->body_simple[b1]==2 && m->body_simple[b2]==2) {
if (m->body_simple[b1] == 2 && m->body_simple[b2] == 2) {
m->flexedge_invweight0[i] = (1/m->body_mass[b1] + 1/m->body_mass[b2])/2;
}
@@ -217,7 +217,7 @@ static void set0(mjModel* m, mjData* d) {
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
int end = d->flexedge_J_rowadr[i] + d->flexedge_J_rownnz[i];
for (int j=d->flexedge_J_rowadr[i]; j<end; j++) {
for (int j=d->flexedge_J_rowadr[i]; j < end; j++) {
tmp[d->flexedge_J_colind[j]] = d->flexedge_J[j];
}
} else {
@@ -232,7 +232,7 @@ static void set0(mjModel* m, mjData* d) {
}
// compute tendon_invweight0
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
// make dense vector into tmp
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
@@ -422,8 +422,8 @@ static void setStat(mjModel* m, mjData* d) {
}
// adjust body size for flex edges involving body
for (int f=0; f<m->nflex; f++) {
for (int e=m->flex_edgeadr[f]; e<m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
for (int f=0; f < m->nflex; f++) {
for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]];
src/engine/engine_support.c
+48 -48
View File
@@ -100,22 +100,22 @@ const char* mjTIMERSTRING[mjNTIMER]= {
// return size of a single state element
static inline int mj_stateElemSize(const mjModel* m, mjtState spec) {
switch (spec) {
case mjSTATE_TIME: return 1;
case mjSTATE_QPOS: return m->nq;
case mjSTATE_QVEL: return m->nv;
case mjSTATE_ACT: return m->na;
case mjSTATE_WARMSTART: return m->nv;
case mjSTATE_CTRL: return m->nu;
case mjSTATE_QFRC_APPLIED: return m->nv;
case mjSTATE_XFRC_APPLIED: return 6*m->nbody;
case mjSTATE_EQ_ACTIVE: return m->neq; // mjtByte, stored as mjtNum in state vector
case mjSTATE_MOCAP_POS: return 3*m->nmocap;
case mjSTATE_MOCAP_QUAT: return 4*m->nmocap;
case mjSTATE_USERDATA: return m->nuserdata;
case mjSTATE_PLUGIN: return m->npluginstate;
default:
mjERROR("invalid state element %u", spec);
return 0;
case mjSTATE_TIME: return 1;
case mjSTATE_QPOS: return m->nq;
case mjSTATE_QVEL: return m->nv;
case mjSTATE_ACT: return m->na;
case mjSTATE_WARMSTART: return m->nv;
case mjSTATE_CTRL: return m->nu;
case mjSTATE_QFRC_APPLIED: return m->nv;
case mjSTATE_XFRC_APPLIED: return 6*m->nbody;
case mjSTATE_EQ_ACTIVE: return m->neq; // mjtByte, stored as mjtNum in state vector
case mjSTATE_MOCAP_POS: return 3*m->nmocap;
case mjSTATE_MOCAP_QUAT: return 4*m->nmocap;
case mjSTATE_USERDATA: return m->nuserdata;
case mjSTATE_PLUGIN: return m->npluginstate;
default:
mjERROR("invalid state element %u", spec);
return 0;
}
}
@@ -124,21 +124,21 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState spec) {
// return pointer to a single state element
static inline mjtNum* mj_stateElemPtr(const mjModel* m, mjData* d, mjtState spec) {
switch (spec) {
case mjSTATE_TIME: return &d->time;
case mjSTATE_QPOS: return d->qpos;
case mjSTATE_QVEL: return d->qvel;
case mjSTATE_ACT: return d->act;
case mjSTATE_WARMSTART: return d->qacc_warmstart;
case mjSTATE_CTRL: return d->ctrl;
case mjSTATE_QFRC_APPLIED: return d->qfrc_applied;
case mjSTATE_XFRC_APPLIED: return d->xfrc_applied;
case mjSTATE_MOCAP_POS: return d->mocap_pos;
case mjSTATE_MOCAP_QUAT: return d->mocap_quat;
case mjSTATE_USERDATA: return d->userdata;
case mjSTATE_PLUGIN: return d->plugin_state;
default:
mjERROR("invalid state element %u", spec);
return NULL;
case mjSTATE_TIME: return &d->time;
case mjSTATE_QPOS: return d->qpos;
case mjSTATE_QVEL: return d->qvel;
case mjSTATE_ACT: return d->act;
case mjSTATE_WARMSTART: return d->qacc_warmstart;
case mjSTATE_CTRL: return d->ctrl;
case mjSTATE_QFRC_APPLIED: return d->qfrc_applied;
case mjSTATE_XFRC_APPLIED: return d->xfrc_applied;
case mjSTATE_MOCAP_POS: return d->mocap_pos;
case mjSTATE_MOCAP_QUAT: return d->mocap_quat;
case mjSTATE_USERDATA: return d->userdata;
case mjSTATE_PLUGIN: return d->plugin_state;
default:
mjERROR("invalid state element %u", spec);
return NULL;
}
}
@@ -248,7 +248,7 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) {
}
// neither body is movable: empty chain
if (b1==0 && b2==0) {
if (b1 == 0 && b2 == 0) {
return 0;
}
@@ -257,19 +257,19 @@ int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) {
da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1;
// merge chains
while (da1>=0 || da2>=0) {
while (da1 >= 0 || da2 >= 0) {
chain[NV] = mjMAX(da1, da2);
if (da1==chain[NV]) {
if (da1 == chain[NV]) {
da1 = m->dof_parentid[da1];
}
if (da2==chain[NV]) {
if (da2 == chain[NV]) {
da2 = m->dof_parentid[da2];
}
NV++;
}
// reverse order of chain: make it increasing
for (int i=0; i<NV/2; i++) {
for (int i=0; i < NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
@@ -283,7 +283,7 @@ 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
if (b1>b2) {
if (b1 > b2) {
int tmp = b1;
b1 = b2;
b2 = tmp;
@@ -293,17 +293,17 @@ int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
int n1 = m->body_dofnum[b1], n2 = m->body_dofnum[b2];
// both fixed: nothing to do
if (n1==0 && n2==0) {
if (n1 == 0 && n2 == 0) {
return 0;
}
// copy b1 dofs
for (int i=0; i<n1; i++) {
for (int i=0; i < n1; i++) {
chain[i] = m->body_dofadr[b1] + i;
}
// copy b2 dofs
for (int i=0; i<n2; i++) {
for (int i=0; i < n2; i++) {
chain[n1+i] = m->body_dofadr[b2] + i;
}
@@ -317,7 +317,7 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) {
// simple body
if (m->body_simple[body]) {
int dofnum = m->body_dofnum[body];
for (int i=0; i<dofnum; i++) {
for (int i=0; i < dofnum; i++) {
chain[i] = m->body_dofadr[body] + i;
}
return dofnum;
@@ -331,7 +331,7 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) {
}
// not movable: empty chain
if (body==0) {
if (body == 0) {
return 0;
}
@@ -340,13 +340,13 @@ int mj_bodyChain(const mjModel* m, int body, int* chain) {
int NV = 0;
// construct chain from child to parent
while (da>=0) {
while (da >= 0) {
chain[NV++] = da;
da = m->dof_parentid[da];
}
// reverse order of chain: make it increasing
for (int i=0; i<NV/2; i++) {
for (int i=0; i < NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
@@ -737,7 +737,7 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain,
}
// accumulate remaining
for (int i=1; i<n; i++) {
for (int i=1; i < n; i++) {
// get body chain and Jacobian
int bodyNV = mj_bodyChain(m, body[i], bodychain);
if (!bodyNV) {
@@ -762,7 +762,7 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain,
mju_scl(jac, jac, weight[0], flg_rot ? 6*nv : 3*nv);
// accumulate remaining
for (int i=1; i<n; i++) {
for (int i=1; i < n; i++) {
mj_jac(m, d, jp, jr, point, body[i]);
mju_addToScl(jac, jtmp, weight[i], flg_rot ? 6*nv : 3*nv);
}
@@ -1477,13 +1477,13 @@ void mj_applyFT(const mjModel* m, mjData* d,
// compute J'*f and accumulate
if (force) {
mju_mulMatTVec(qforce, jacp, force, 3, NV);
for (int i=0; i<NV; i++) {
for (int i=0; i < NV; i++) {
qfrc_target[chain[i]] += qforce[i];
}
}
if (torque) {
mju_mulMatTVec(qforce, jacr, torque, 3, NV);
for (int i=0; i<NV; i++) {
for (int i=0; i < NV; i++) {
qfrc_target[chain[i]] += qforce[i];
}
}
src/engine/engine_util_container.c
+3 -3
View File
@@ -25,7 +25,7 @@
// stack allocate and initialize new mjArrayList
mjArrayList* mju_arrayListCreate(mjData* d, size_t element_size, size_t initial_capacity) {
mjArrayList* array_list = (mjArrayList*) mj_stackAllocByte(
d, sizeof(mjArrayList), _Alignof(mjArrayList));
d, sizeof(mjArrayList), _Alignof(mjArrayList));
initial_capacity = mjMAX(1, initial_capacity);
array_list->d = d;
array_list->element_size = element_size;
@@ -35,7 +35,7 @@ mjArrayList* mju_arrayListCreate(mjData* d, size_t element_size, size_t initial_
// allocate array list buffer
array_list->buffer = (void*) mj_stackAllocByte(
d, element_size * initial_capacity, _Alignof(mjtMaxAlign));
d, element_size * initial_capacity, _Alignof(mjtMaxAlign));
return array_list;
}
@@ -63,7 +63,7 @@ void mju_arrayListAdd(mjArrayList* array_list, void* element) {
if (cursor->next_segment == NULL) {
// add a new segment with twice the capacity of the last segment
cursor->next_segment = mju_arrayListCreate(
cursor->d, cursor->element_size, 2 * cursor->capacity);
cursor->d, cursor->element_size, 2 * cursor->capacity);
}
cursor = cursor->next_segment;
}
src/engine/engine_util_misc.c
+1 -1
View File
@@ -630,7 +630,7 @@ static uint32_t _decode(char ch) {
// 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) {
static const char *table =
"ABCDEFGHIJKLMNOPQRSTUBWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
"ABCDEFGHIJKLMNOPQRSTUBWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
int i = 0, j = 0;
src/engine/engine_util_sparse.c
+27 -27
View File
@@ -379,19 +379,19 @@ void mju_addToSparseInc(mjtNum* dst, const mjtNum* src,
int adrs = 0, adrd = 0, inds = indsrc[0], indd = inddst[0];
while (1) {
// common non-zero index
if (inds==indd) {
if (inds == indd) {
// add
dst[adrd] += src[adrs];
// advance src
if (++adrs<nnzsrc) {
if (++adrs < nnzsrc) {
inds = indsrc[adrs];
} else {
return;
}
// advance dst
if (++adrd<nnzdst) {
if (++adrd < nnzdst) {
indd = inddst[adrd];
} else {
return;
@@ -399,8 +399,8 @@ void mju_addToSparseInc(mjtNum* dst, const mjtNum* src,
}
// src non-zero index smaller: advance src
else if (inds<indd) {
if (++adrs<nnzsrc) {
else if (inds < indd) {
if (++adrs < nnzsrc) {
inds = indsrc[adrs];
} else {
return;
@@ -409,7 +409,7 @@ void mju_addToSparseInc(mjtNum* dst, const mjtNum* src,
// dst non-zero index smaller: advance dst
else {
if (++adrd<nnzdst) {
if (++adrd < nnzdst) {
indd = inddst[adrd];
} else {
return;
@@ -425,8 +425,8 @@ int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum s
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind,
mjtNum* buf, int* buf_ind) {
// check for identical pattern
if (dst_nnz==src_nnz) {
if (dst_nnz==0) {
if (dst_nnz == src_nnz) {
if (dst_nnz == 0) {
return 0;
}
if (mju_compare(dst_ind, src_ind, dst_nnz)) {
@@ -442,40 +442,40 @@ int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum s
int dadr = dst_nnz ? dst_ind[0] : n+1;
// merge matrices
while (si<src_nnz || di<dst_nnz) {
while (si < src_nnz || di < dst_nnz) {
// both
if (sadr==dadr) {
for (int k=0; k<nrow; k++) {
if (sadr == dadr) {
for (int k=0; k < nrow; k++) {
buf[nrow*nnz + k] = dst[di + k*dst_nnz] + scl*src[si + k*src_nnz];
}
buf_ind[nnz++] = sadr;
si++;
di++;
sadr = si<src_nnz ? src_ind[si] : n+1;
dadr = di<dst_nnz ? dst_ind[di] : n+1;
sadr = si < src_nnz ? src_ind[si] : n+1;
dadr = di < dst_nnz ? dst_ind[di] : n+1;
}
// dst only
else if (dadr<sadr) {
for (int k=0; k<nrow; k++) {
else if (dadr < sadr) {
for (int k=0; k < nrow; k++) {
buf[nrow*nnz + k] = dst[di + k*dst_nnz];
}
buf_ind[nnz++] = dadr;
di++;
dadr = di<dst_nnz ? dst_ind[di] : n+1;
dadr = di < dst_nnz ? dst_ind[di] : n+1;
}
// src only
else {
for (int k=0; k<nrow; k++) {
for (int k=0; k < nrow; k++) {
buf[nrow*nnz + k] = scl*src[si + k*src_nnz];
}
buf_ind[nnz++] = sadr;
si++;
sadr = si<src_nnz ? src_ind[si] : n+1;
sadr = si < src_nnz ? src_ind[si] : n+1;
}
}
@@ -492,8 +492,8 @@ int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum s
int mju_addChains(int* res, int n, int NV1, int NV2,
const int* chain1, const int* chain2) {
// check for identical pattern
if (NV1==NV2) {
if (NV1==0) {
if (NV1 == NV2) {
if (NV1 == 0) {
return 0;
}
if (mju_compare(chain1, chain2, NV1)) {
@@ -508,28 +508,28 @@ int mju_addChains(int* res, int n, int NV1, int NV2,
int adr2 = NV2 ? chain2[0] : n+1;
// merge chains
while (i1<NV1 || i2<NV2) {
while (i1 < NV1 || i2 < NV2) {
// both
if (adr1==adr2) {
if (adr1 == adr2) {
res[NV++] = adr1;
i1++;
i2++;
adr1 = i1<NV1 ? chain1[i1] : n+1;
adr2 = i2<NV2 ? chain2[i2] : n+1;
adr1 = i1 < NV1 ? chain1[i1] : n+1;
adr2 = i2 < NV2 ? chain2[i2] : n+1;
}
// chain1 only
else if (adr1<adr2) {
else if (adr1 < adr2) {
res[NV++] = adr1;
i1++;
adr1 = i1<NV1 ? chain1[i1] : n+1;
adr1 = i1 < NV1 ? chain1[i1] : n+1;
}
// chain2 only
else {
res[NV++] = adr2;
i2++;
adr2 = i2<NV2 ? chain2[i2] : n+1;
adr2 = i2 < NV2 ? chain2[i2] : n+1;
}
}
src/engine/engine_vis_init.c
+8 -8
View File
@@ -169,15 +169,15 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
// count max number of flex faces to be rendered (depending on vis options)
int nface = 0;
for (int f=0; f<nflex; f++) {
for (int f=0; f < nflex; f++) {
// 1D : 0
if (m->flex_dim[f]==0) {
if (m->flex_dim[f] == 0) {
scn->flexfacenum[f] = 0;
}
// 2D: 2*fragments + 2*elements
else if (m->flex_dim[f]==2) {
scn->flexfacenum[f] = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f];
else if (m->flex_dim[f] == 2) {
scn->flexfacenum[f] = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f];
}
// 3D: max(fragments, 4*maxlayer)
@@ -187,8 +187,8 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
while (nlayer) {
// count elements in this layer
nlayer = 0;
for (int e=0; e<m->flex_elemnum[f]; e++) {
if (m->flex_elemlayer[m->flex_elemadr[f]+e]==layer) {
for (int e=0; e < m->flex_elemnum[f]; e++) {
if (m->flex_elemlayer[m->flex_elemadr[f]+e] == layer) {
nlayer++;
}
}
@@ -234,8 +234,8 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
memcpy(scn->flexedge, m->flex_edge, 2*m->nflexedge*sizeof(int));
// compute flexfaceadr
for (int f=0; f<nflex; f++) {
scn->flexfaceadr[f] = f==0 ? 0 : scn->flexfaceadr[f-1]+scn->flexfacenum[f-1];
for (int f=0; f < nflex; f++) {
scn->flexfaceadr[f] = f == 0 ? 0 : scn->flexfaceadr[f-1]+scn->flexfacenum[f-1];
}
}
src/engine/engine_vis_interact.c
+6 -6
View File
@@ -546,7 +546,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
pert->localmass = 3 / mju_max(invmass, mjMINVAL);
// scale localmass with flex average number of edges per vertex
if (pert->flexselect>=0 && !m->flex_rigid[pert->flexselect]) {
if (pert->flexselect >= 0 && !m->flex_rigid[pert->flexselect]) {
pert->localmass *= (2.0*m->flex_edgenum[pert->flexselect]) /
(mjtNum)m->flex_vertnum[pert->flexselect];
}
@@ -773,7 +773,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
*flexid = -1;
if (vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] ||
vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) {
for (int i=0; i<m->nflex; i++) {
for (int i=0; i < m->nflex; i++) {
// process one flex
int vertid;
mjtNum newdist = mju_rayFlex(m, d, vopt->flex_layer,
@@ -782,7 +782,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
i, pos, ray, &vertid);
// update if closer intersection found
if (newdist>=0 && (newdist<flexdist || flexdist<0)) {
if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) {
flexdist = newdist;
flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
*flexid = i;
@@ -842,9 +842,9 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// find smallest non-negative distance
mjtNum raydist[3] = {geomdist, flexdist, skindist};
int best = -1;
for (int i=0; i<3; i++) {
if (raydist[i]>=0) {
if (best<0 || raydist[best]>raydist[i]) {
for (int i=0; i < 3; i++) {
if (raydist[i] >= 0) {
if (best < 0 || raydist[best] > raydist[i]) {
best = i;
}
}
src/engine/engine_vis_visualize.c
+50 -50
View File
@@ -148,9 +148,9 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// label contacting geom names or ids
if (vopt->label == mjLABEL_CONTACTPOINT) {
char contactlabel[2][48];
for (int k=0; k<2; k++) {
for (int k=0; k < 2; k++) {
// make geom label
if (con->geom[k]>=0) {
if (con->geom[k] >= 0) {
const char* geomname = mj_id2name(m, mjOBJ_GEOM, con->geom[k]);
if (geomname) {
mjSNPRINTF(contactlabel[k], "%s", geomname);
@@ -164,7 +164,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
else {
const char* flexname = mj_id2name(m, mjOBJ_FLEX, con->flex[k]);
if (flexname) {
if (con->elem[k]>=0) {
if (con->elem[k] >= 0) {
mjSNPRINTF(contactlabel[k], "%s.e%d", flexname, con->elem[k]);
}
else {
@@ -172,7 +172,7 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
}
}
else {
if (con->elem[k]>=0) {
if (con->elem[k] >= 0) {
mjSNPRINTF(contactlabel[k], "f%d.e%d", con->flex[k], con->elem[k]);
}
else {
@@ -265,9 +265,9 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// get bodyflex ids
int bf[2];
for (int k=0; k<2; k++) {
bf[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] :
m->nbody + con->flex[k];
for (int k=0; k < 2; k++) {
bf[k] = (con->geom[k] >= 0) ? m->geom_bodyid[con->geom[k]] :
m->nbody + con->flex[k];
}
// make sure arrow points towards bodyflex with higher id
@@ -599,7 +599,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if ((vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] ||
vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) &&
(category & catmask)) {
for (int i=0; i<m->nflex; i++) {
for (int i=0; i < m->nflex; i++) {
if (vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[i]))]) {
START
@@ -614,7 +614,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
setMaterial(m, thisgeom, m->flex_matid[i], m->flex_rgba+4*i, vopt->flags);
// set texcoord
if (m->flex_texcoordadr[i]>=0) {
if (m->flex_texcoordadr[i] >= 0) {
thisgeom->texcoord = 1;
}
else {
@@ -622,17 +622,17 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
// glow flex if selected
if (pert->flexselect==i) {
if (pert->flexselect == i) {
markselected(&m->vis, thisgeom);
}
// skip if alpha is 0
if (thisgeom->rgba[3]==0) {
if (thisgeom->rgba[3] == 0) {
continue;
}
// vopt->label
if (vopt->label==mjLABEL_FLEX) {
if (vopt->label == mjLABEL_FLEX) {
makeLabel(m, mjOBJ_FLEX, i, thisgeom->label);
}
@@ -723,11 +723,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// flex BVH
if (vopt->flags[mjVIS_FLEXBVH]) {
float rgba[] = {1, 0, 0, 0.1};
for (int f=0; f<m->nflex; f++) {
for (int f=0; f < m->nflex; f++) {
if (m->flex_bvhnum[f] &&
vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
for (int i=m->flex_bvhadr[f]; i<m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1;
for (int i=m->flex_bvhadr[f]; i < m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
if (scn->ngeom >= scn->maxgeom) break;
if (m->bvh_depth[i] != vopt->bvh_depth) {
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
@@ -743,7 +743,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// b/304453879 : add LINEBOX geom for bounding box visualization
START
mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba);
mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba);
FINISH
}
}
@@ -1351,7 +1351,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) {
thisgeom->dataid *= 2;
if (m->mesh_graphadr[m->geom_dataid[i]] >= 0 && vopt->flags[mjVIS_CONVEXHULL] &&
(m->geom_contype[i] || m->geom_conaffinity[i])) {
(m->geom_contype[i] || m->geom_conaffinity[i])) {
thisgeom->dataid += 1;
}
}
@@ -1646,7 +1646,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_addToScl3(vfar[3], y, zver[1]);
// triangulation and wireframe of the frustum
for (int e=0; e<4; e++) {
for (int e=0; e < 4; e++) {
START
mju_sub3(x, vfar[e], vnear[e]);
mju_sub3(y, vnear[(e+1)%4], vnear[e]);
@@ -1839,7 +1839,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int j=0; j < npoints-1; j++) {
START
sz[0] = m->tendon_width[i];
sz[0] = m->tendon_width[i];
// construct geom
mjv_connector(thisgeom, mjGEOM_CAPSULE, sz[0], catenary+3*j, catenary+3*j+3);
@@ -2300,7 +2300,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
int ind[3] = {i0, i1, i2};
int sign = radius>0 ? 1 : -1;
int sign = radius > 0 ? 1 : -1;
// flat shading
if (flg_flat) {
@@ -2315,7 +2315,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_
mju_normalize3(nrm);
// set all vertex normals equal to face normal
for (int k=0; k<3; k++){
for (int k=0; k < 3; k++){
normal[3*k+0] = (float) (sign*nrm[0]);
normal[3*k+1] = (float) (sign*nrm[1]);
normal[3*k+2] = (float) (sign*nrm[2]);
@@ -2324,7 +2324,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_
// smooth shading
else {
for (int k=0; k<3; k++){
for (int k=0; k < 3; k++){
normal[3*k+0] = (float) (sign*vertnorm[3*ind[k]+0]);
normal[3*k+1] = (float) (sign*vertnorm[3*ind[k]+1]);
normal[3*k+2] = (float) (sign*vertnorm[3*ind[k]+2]);
@@ -2332,7 +2332,7 @@ static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_
}
// set positions: vertices offset by radius*normal
for (int k=0; k<3; k++){
for (int k=0; k < 3; k++){
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + radius*vertnorm[3*ind[k]+2]);
@@ -2354,13 +2354,13 @@ static void makeSide(float* _face, float* _normal, mjtNum radius,
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, vertnorm+3*i1);
if (radius<0) {
if (radius < 0) {
mju_scl3(nrm, nrm, -1);
}
mju_normalize3(nrm);
// set normals
for (int k=0; k<3; k++){
for (int k=0; k < 3; k++){
normal[3*k+0] = (float) nrm[0];
normal[3*k+1] = (float) nrm[1];
normal[3*k+2] = (float) nrm[2];
@@ -2368,8 +2368,8 @@ static void makeSide(float* _face, float* _normal, mjtNum radius,
// set positions
int ind[3] = {i0, i1, i1};
for (int k=0; k<3; k++){
mjtNum sign = (k==1 ? -1 : +1);
for (int k=0; k < 3; k++){
mjtNum sign = (k == 1 ? -1 : +1);
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + sign*radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + sign*radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + sign*radius*vertnorm[3*ind[k]+2]);
@@ -2403,39 +2403,39 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
scn->flexskinopt = opt->flags[mjVIS_FLEXSKIN];
// convert vertex positions from mjtNum to float
for (int v=0; v<3*m->nflexvert; v++) {
for (int v=0; v < 3*m->nflexvert; v++) {
scn->flexvert[v] = (float) d->flexvert_xpos[v];
}
// construct faces
for (int f=0; f<m->nflex; f++) {
for (int f=0; f < m->nflex; f++) {
int dim = m->flex_dim[f];
mjtNum radius = m->flex_radius[f];
mjtByte flg_flat = m->flex_flatskin[f];
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
float* face = scn->flexface + 9*scn->flexfaceadr[f];
float* normal = scn->flexnormal + 9*scn->flexfaceadr[f];
float* texdst = m->flex_texcoordadr[f]>=0 ?
scn->flextexcoord + 6*scn->flexfaceadr[f] : NULL;
const float* texsrc = m->flex_texcoordadr[f]>=0 ?
m->flex_texcoord + 2*m->flex_texcoordadr[f] : NULL;
float* texdst = m->flex_texcoordadr[f] >= 0 ?
scn->flextexcoord + 6*scn->flexfaceadr[f] : NULL;
const float* texsrc = m->flex_texcoordadr[f] >= 0 ?
m->flex_texcoord + 2*m->flex_texcoordadr[f] : NULL;
// 1D, or face and skin disabled: no faces
if (dim==1 || (!opt->flags[mjVIS_FLEXFACE] && !opt->flags[mjVIS_FLEXSKIN])) {
if (dim == 1 || (!opt->flags[mjVIS_FLEXFACE] && !opt->flags[mjVIS_FLEXSKIN])) {
scn->flexfaceused[f] = 0;
}
// 2D or 3D face: faces from elements, flat normals, texture
else if (!opt->flags[mjVIS_FLEXSKIN]) {
int nface = 0;
for (int e=0; e<m->flex_elemnum[f]; e++) {
for (int e=0; e < m->flex_elemnum[f]; e++) {
// in 3D, show only elements in selected layer
if (dim==2 || m->flex_elemlayer[m->flex_elemadr[f]+e]==opt->flex_layer) {
if (dim == 2 || m->flex_elemlayer[m->flex_elemadr[f]+e] == opt->flex_layer) {
// get element data
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
// triangles: two faces per element
if (dim==2) {
if (dim == 2) {
makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]);
nface++;
@@ -2482,27 +2482,27 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
mju_zero(vertnorm, 3*m->flex_vertnum[f]);
// add vertex normals: top element sides in 2D, shell fragments in 3D
if (dim==2) {
for (int e=0; e<m->flex_elemnum[f]; e++) {
if (dim == 2) {
for (int e=0; e < m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
addNormal(vertnorm, vertxpos, edata[0], edata[1], edata[2]);
}
} else {
for (int s=0; s<m->flex_shellnum[f]; s++) {
for (int s=0; s < m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
addNormal(vertnorm, vertxpos, sdata[0], sdata[1], sdata[2]);
}
}
// normalize vertex normals
for (int i=0; i<m->flex_vertnum[f]; i++) {
for (int i=0; i < m->flex_vertnum[f]; i++) {
mju_normalize3(vertnorm+3*i);
}
// create faces, offset along smoothed vertex normals, and texcoord
int nface = 0;
if (dim==2) {
for (int e=0; e<m->flex_elemnum[f]; e++) {
if (dim == 2) {
for (int e=0; e < m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, edata[0], edata[1], edata[2]);
@@ -2514,7 +2514,7 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
nface++;
}
} else {
for (int s=0; s<m->flex_shellnum[f]; s++) {
for (int s=0; s < m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, sdata[0], sdata[1], sdata[2]);
@@ -2524,8 +2524,8 @@ void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
}
// 2D: close sides using shell fragments
if (dim==2) {
for (int s=0; s<m->flex_shellnum[f]; s++) {
if (dim == 2) {
for (int s=0; s < m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSide(face, normal, radius, vertnorm, vertxpos,
nface, sdata[0], sdata[1]);
@@ -2665,10 +2665,10 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
// normalize normals
for (int k=vertadr; k < vertadr+vertnum; k++) {
float s = sqrtf(
scn->skinnormal[3*k]*scn->skinnormal[3*k] +
scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] +
scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2]
);
scn->skinnormal[3*k+0]*scn->skinnormal[3*k+0] +
scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] +
scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2]
);
float scl = 1/mjMAX(mjMINVAL, s);
scn->skinnormal[3*k] *= scl;