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

Add spaces around comparison operators in engine source files.

Browse Source 
PiperOrigin-RevId: 535989348
Change-Id: I883f7e82351299933c49b35a31842b5d8d6aea04
This commit is contained in:
Yuval Tassa
2023-05-28 05:01:55 -07:00
committed by Copybara-Service
parent d40c395917
commit 455b1cd2e2
29 changed files with 2224 additions and 2219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_callback.c
+5 -5
View File
@@ -21,11 +21,11 @@
mjfGeneric mjcb_passive = 0;
mjfGeneric mjcb_control = 0;
mjfConFilt mjcb_contactfilter = 0;
mjfSensor mjcb_sensor = 0;
mjfTime mjcb_time = 0;
mjfAct mjcb_act_bias = 0;
mjfAct mjcb_act_gain = 0;
mjfAct mjcb_act_dyn = 0;
mjfSensor mjcb_sensor = 0;
mjfTime mjcb_time = 0;
mjfAct mjcb_act_bias = 0;
mjfAct mjcb_act_gain = 0;
mjfAct mjcb_act_dyn = 0;
src/engine/engine_collision_convex.c
+58 -58
View File
@@ -73,13 +73,13 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
case mjGEOM_ELLIPSOID:
// find support point on unit sphere: scale dir by ellipsoid sizes and renormalize
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
res[i] = dir[i] * size[i];
}
mju_normalize3(res);
// transform to ellipsoid
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
res[i] *= size[i];
}
break;
@@ -87,7 +87,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
case mjGEOM_CYLINDER:
// set result in XY plane: support on circle
tmp = mju_sqrt(dir[0]*dir[0] + dir[1]*dir[1]);
if (tmp>mjMINVAL) {
if (tmp > mjMINVAL) {
res[0] = dir[0]/tmp*size[0];
res[1] = dir[1]/tmp*size[0];
} else {
@@ -99,7 +99,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
break;
case mjGEOM_BOX:
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
res[i] = mju_sign(dir[i]) * size[i];
}
break;
@@ -111,16 +111,16 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
ibest = -1;
// no graph data: exhaustive search
if (m->mesh_graphadr[m->geom_dataid[g]]<0) {
if (m->mesh_graphadr[m->geom_dataid[g]] < 0) {
// search all vertices, find best
for (int i=0; i<m->mesh_vertnum[m->geom_dataid[g]]; i++) {
for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) {
// vdot = dot(vertex, dir)
vdot = dir[0] * (mjtNum)vertdata[3*i] +
dir[1] * (mjtNum)vertdata[3*i+1] +
dir[2] * (mjtNum)vertdata[3*i+2];
// update best
if (vdot>tmp) {
if (vdot > tmp) {
tmp = vdot;
ibest = i;
}
@@ -158,7 +158,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2];
// update best
if (vdot>tmp) {
if (vdot > tmp) {
tmp = vdot;
ibest = locid;
change = 1;
@@ -177,14 +177,14 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
}
// sanity check, SHOULD NOT OCCUR
if (ibest<0) {
if (ibest < 0) {
mju_warning("mesh_support could not find support vertex");
mju_zero3(res);
}
// copy best vertex
else {
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
res[i] = (mjtNum)vertdata[3*ibest + i];
}
}
@@ -195,7 +195,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
}
// add dir*margin/2 to result
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
res[i] += dir[i] * ccd->margin/2;
}
@@ -214,7 +214,7 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
mjContact* con, int g1, int g2, mjtNum margin) {
ccd_vec3_t dir, pos;
ccd_real_t depth;
if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos)==0) {
if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
// contact is found but normal is undefined
if (ccdVec3Eq(&dir, ccd_vec3_origin)) {
return 0;
@@ -242,7 +242,7 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
// compare new contact to previous contacts, return 1 if it is far from all of them
static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) {
for (int i=0; i<ncon-1; i++) {
for (int i=0; i < ncon-1; i++) {
if (mju_dist3(con[i].pos, con[ncon - 1].pos) <= tolerance) {
return 0;
}
@@ -380,7 +380,7 @@ static int addplanemesh(mjContact* con, const float vertex[3],
mju_addTo3(pnt, pos2);
// skip if too close to first contact
if (mju_dist3(pnt, first)<tolplanemesh*rbound) {
if (mju_dist3(pnt, first) < tolplanemesh*rbound) {
return 0;
}
@@ -419,7 +419,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
// compute normal distance, return if too far
mju_sub3(dif, vec.v, pos1);
dist = mju_dot3(normal, dif);
if (dist>margin) {
if (dist > margin) {
return 0;
}
@@ -454,16 +454,16 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
mjtNum threshold = mju_dot3(normal, dif) - margin;
// no graph data: exhaustive search
if (m->mesh_graphadr[m->geom_dataid[g]]<0) {
if (m->mesh_graphadr[m->geom_dataid[g]] < 0) {
// search all vertices, find best
for (int i=0; i<m->mesh_vertnum[m->geom_dataid[g]] && count<maxplanemesh; i++) {
for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]] && count < maxplanemesh; i++) {
// vdot = dot(vertex, dir)
vdot = locdir[0] * (mjtNum)vertdata[3*i] +
locdir[1] * (mjtNum)vertdata[3*i+1] +
locdir[2] * (mjtNum)vertdata[3*i+2];
// detect contact, skip best
if (vdot>threshold && i!=obj.meshindex) {
if (vdot > threshold && i != obj.meshindex) {
count += addplanemesh(con+count, vertdata+3*i,
pos1, normal, pos2, mat2,
con->pos, m->geom_rbound[g2]);
@@ -472,7 +472,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
}
// use graph data
else if (obj.meshindex>=0) {
else if (obj.meshindex >= 0) {
// get info
graphadr = m->mesh_graphadr[m->geom_dataid[g]];
numvert = m->mesh_graph[graphadr];
@@ -482,14 +482,14 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
// look for contacts in ibest neighborhood
int i = vert_edgeadr[obj.meshindex];
while ((locid=edge_localid[i])>=0 && count<maxplanemesh) {
while ((locid=edge_localid[i]) >= 0 && count < maxplanemesh) {
// vdot = dot(vertex, dir)
vdot = locdir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] +
locdir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] +
locdir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2];
// detect contact
if (vdot>threshold) {
if (vdot > threshold) {
count += addplanemesh(con+count, vertdata+3*vert_globalid[locid],
pos1, normal, pos2, mat2,
con->pos, m->geom_rbound[g2]);
@@ -522,10 +522,10 @@ static void prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *ve
const mjtPrism* p = (const mjtPrism*)obj;
// find best vertex in halfspace determined by dir.z
istart = dir->v[2]<0 ? 0 : 3;
istart = dir->v[2] < 0 ? 0 : 3;
ibest = istart;
best = mju_dot3(p->v[istart], dir->v);
for (int i=istart+1; i<istart+3; i++) {
for (int i=istart+1; i < istart+3; i++) {
if ((tmp = mju_dot3(p->v[i], dir->v)) > best) {
ibest = i;
best = tmp;
@@ -543,7 +543,7 @@ static void prism_center(const void *obj, ccd_vec3_t *center) {
// compute mean
mju_zero3(center->v);
for (int i=0; i<6; i++) {
for (int i=0; i < 6; i++) {
mju_addTo3(center->v, p->v[i]);
}
mju_scl3(center->v, center->v, 1.0/6.0);
@@ -607,7 +607,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
r2 = m->geom_rbound[g2];
// box-sphere test: horizontal plane
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
if ((size1[i] < pos[i]-r2-margin) || (-size1[i] > pos[i]+r2+margin)) {
return 0;
}
@@ -710,23 +710,23 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// process all prisms in sub-grid
cnt = 0;
for (int r=rmin; r<rmax; r++) {
for (int r=rmin; r < rmax; r++) {
nvert = 0;
for (int c=cmin; c<=cmax; c++) {
for (int i=0; i<2; i++) {
for (int c=cmin; c <= cmax; c++) {
for (int i=0; i < 2; i++) {
// send vertex to prism constructor
addVert(&nvert, &prism, dx*c-size1[0], dy*(r+dr[i])-size1[1],
data[(r+dr[i])*ncol+c]*size1[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 &&
!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
@@ -737,7 +737,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// count, stop if max number reached
cnt++;
if (cnt>=mjMAXCONPAIR) {
if (cnt >= mjMAXCONPAIR) {
r = rmax+1;
c = cmax+1;
i = 3;
@@ -754,7 +754,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
mju_copy3(pos2, savepos2);
// fix contact normals
for (int i=0; i<cnt; i++) {
for (int i=0; i < cnt; i++) {
mjc_fixNormal(m, d, con+i, g1, g2);
}
@@ -774,7 +774,7 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
// precompute quantities
mjtNum S2inv[3] = {1/(size[0]*size[0]), 1/(size[1]*size[1]), 1/(size[2]*size[2])};
mjtNum C = pos[0]*pos[0]*S2inv[0] + pos[1]*pos[1]*S2inv[1] + pos[2]*pos[2]*S2inv[2] - 1;
if (C>0) {
if (C > 0) {
return 0;
}
@@ -783,19 +783,19 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
// main iteration
int iter;
for (iter=0; iter<maxiter; iter++) {
for (iter=0; iter < maxiter; iter++) {
// coefficients and determinant of quadratic
mjtNum A = nrm[0]*nrm[0]*S2inv[0] + nrm[1]*nrm[1]*S2inv[1] + nrm[2]*nrm[2]*S2inv[2];
mjtNum B = pos[0]*nrm[0]*S2inv[0] + pos[1]*nrm[1]*S2inv[1] + pos[2]*nrm[2]*S2inv[2];
mjtNum det = B*B - A*C;
if (det<mjMINVAL || A<mjMINVAL) {
return (iter>0);
if (det < mjMINVAL || A < mjMINVAL) {
return (iter > 0);
}
// ray intersection with ellipse: pos + x*nrm, x>=0
mjtNum x = (-B + mju_sqrt(det))/A;
if (x<0) {
return (iter>0);
if (x < 0) {
return (iter > 0);
}
// new point on ellipsoid
@@ -811,7 +811,7 @@ static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
mju_copy3(nrm, newnrm);
// terminate if converged
if (change<tolerance) {
if (change < tolerance) {
break;
}
}
@@ -834,25 +834,25 @@ static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum
// main iteration
mjtNum la = 0;
int iter;
for (iter=0; iter<maxiter; iter++) {
for (iter=0; iter < maxiter; iter++) {
// precompute 1/(s^2+la)
mjtNum R[3] = {1/(S2[0]+la), 1/(S2[1]+la), 1/(S2[2]+la)};
// value
mjtNum val = PS2[0]*R[0]*R[0] + PS2[1]*R[1]*R[1] + PS2[2]*R[2]*R[2] - 1;
if (val<tolerance) {
if (val < tolerance) {
break;
}
// derivative
mjtNum deriv = -2*(PS2[0]*R[0]*R[0]*R[0] + PS2[1]*R[1]*R[1]*R[1] + PS2[2]*R[2]*R[2]*R[2]);
if (deriv>-mjMINVAL) {
if (deriv > -mjMINVAL) {
break;
}
// delta
mjtNum delta = -val/deriv;
if (delta<tolerance) {
if (delta < tolerance) {
break;
}
@@ -878,20 +878,20 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
// get geom ids and types
int gid[2] = {g1, g2};
int type[2];
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
type[i] = m->geom_type[gid[i]];
// set to -1 if type cannot be processed
if (type[i]!=mjGEOM_SPHERE &&
type[i]!=mjGEOM_CAPSULE &&
type[i]!=mjGEOM_ELLIPSOID &&
type[i]!=mjGEOM_CYLINDER) {
if (type[i] != mjGEOM_SPHERE &&
type[i] != mjGEOM_CAPSULE &&
type[i] != mjGEOM_ELLIPSOID &&
type[i] != mjGEOM_CYLINDER) {
type[i] = -1;
}
}
// neither type can be processed: nothing to do
if (type[0]<0 && type[1]<0) {
if (type[0] < 0 && type[1] < 0) {
return;
}
@@ -903,8 +903,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
// process geoms in type range
int processed[2] = {0, 0};
for (int i=0; i<2; i++) {
if (type[i]>=0) {
for (int i=0; i < 2; i++) {
if (type[i] >= 0) {
// get geom mat and size
mjtNum* mat = d->geom_xmat + 9*gid[i];
mjtNum* size = m->geom_size + 3*gid[i];
@@ -924,12 +924,12 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
case mjGEOM_CAPSULE:
// Z: bottom cap
if (pos[2]<-size[1]) {
if (pos[2] < -size[1]) {
nrm[2] = pos[2]+size[1];
}
// Z: top cap
else if (pos[2]>size[1]) {
else if (pos[2] > size[1]) {
nrm[2] = pos[2]-size[1];
}
@@ -946,7 +946,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
case mjGEOM_ELLIPSOID:
// guard against invalid ellipsoid size (just in case)
if (size[0]<mjMINVAL || size[1]<mjMINVAL || size[2]<mjMINVAL) {
if (size[0] < mjMINVAL || size[1] < mjMINVAL || size[2] < mjMINVAL) {
break;
}
@@ -956,7 +956,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
pos[2]*pos[2]/(size[2]*size[2]);
// dispatch to inside or outside solver
if (dst1<=1) {
if (dst1 <= 1) {
processed[i] = mjc_ellipsoidInside(nrm, pos, size);
} else {
processed[i] = mjc_ellipsoidOutside(nrm, pos, size);
@@ -965,7 +965,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
case mjGEOM_CYLINDER:
// skip if within 5% length of flat wall
if (mju_abs(pos[2])>0.95*size[1]) {
if (mju_abs(pos[2]) > 0.95*size[1]) {
break;
}
@@ -974,7 +974,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
dst2 = mju_abs(size[0]-mju_norm(pos, 2));
// require 4x closer to round than flat wall
if (dst1<0.25*dst2) {
if (dst1 < 0.25*dst2) {
break;
}
src/engine/engine_collision_driver.c
+105 -104
View File
@@ -68,7 +68,7 @@ static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
// bounding-sphere collision
static int mj_collideSphere(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) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return 0;
@@ -76,13 +76,13 @@ static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum
}
// 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
&& plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) {
return 0;
return 0;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
if (m->geom_type[g2] == mjGEOM_PLANE && m->geom_rbound[g1] > 0
&& plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) {
return 0;
return 0;
}
return 1;
}
@@ -97,9 +97,9 @@ void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
if (merged) {
// find matching pair
int found = 0;
for (int k=startadr; k<pairadr; k++) {
if ((m->pair_geom1[k]==g1 && m->pair_geom2[k]==g2) ||
(m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) {
for (int k=startadr; k < pairadr; k++) {
if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) ||
(m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) {
found = 1;
break;
}
@@ -139,9 +139,9 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
mjtByte infinite[2] = {inf1[0] || inf1[1] || inf1[2], inf2[0] || inf2[1] || inf2[2]};
// compute centers in local coordinates
if (product==NULL) {
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // axes
if (product == NULL) {
for (int i=0; i < 2; i++) { // bounding boxes
for (int j=0; j < 3; j++) { // axes
mju_rotVecMat(xcenter[i], aabb[i], xmat[i]);
mju_addTo3(xcenter[i], xpos[i]);
}
@@ -149,9 +149,9 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
}
// compute normals in global coordinates
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // faces
for (int k=0; k<3; k++) { // world axes
for (int i=0; i < 2; i++) { // bounding boxes
for (int j=0; j < 3; j++) { // faces
for (int k=0; k < 3; k++) { // world axes
normal[i][j][k] = xmat[i][3*k+j];
}
}
@@ -159,10 +159,10 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
// precompute dot products
if (product && offset && *initialize) {
for (int i=0; i<2; i++) { // bodies
for (int j=0; j<2; j++) { // bodies
for (int k=0; k<3; k++) { // axes
for (int l=0; l<3; l++) { // axes
for (int i=0; i < 2; i++) { // bodies
for (int j=0; j < 2; j++) { // bodies
for (int k=0; k < 3; k++) { // axes
for (int l=0; l < 3; l++) { // axes
product[18*i + 9*j + 3*k + l] = mju_dot3(normal[i][l], normal[j][k]);
}
offset[6*i + 3*j + k] = mju_dot3(xpos[i], normal[j][k]);
@@ -173,13 +173,13 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
}
// check intersections
for (int j=0; j<2; j++) { // bounding boxes
for (int j=0; j < 2; j++) { // bounding boxes
if (infinite[1-j]) {
continue; // skip test against an infinite body
}
for (int k=0; k<3; k++) { // face
for (int i=0; i<2; i++) { // bounding boxes
if (product==NULL) {
for (int k=0; k < 3; k++) { // face
for (int i=0; i < 2; i++) { // bounding boxes
if (product == NULL) {
proj[i] = mju_dot3(xcenter[i], normal[j][k]);
radius[i] = fabs(aabb[i][3]*mju_dot3(normal[i][0], normal[j][k])) +
fabs(aabb[i][4]*mju_dot3(normal[i][1], normal[j][k])) +
@@ -210,7 +210,7 @@ static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) {
_Static_assert(sizeof(mjCollisionTree*) % sizeof(mjtNum) == 0,
"mjCollisionTree has a different size from mjtNum");
return (mjCollisionTree*)mj_stackAlloc(
d, max_stack * sizeof(mjCollisionTree*) / sizeof(mjtNum));
d, max_stack * sizeof(mjCollisionTree*) / sizeof(mjtNum));
}
// binary search between two body trees
@@ -245,7 +245,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
int nodeid2 = m->bvh_geomid[bvhadr2 + node2];
// both are leaves
if (isleaf1 && isleaf2 && nodeid1!=-1 && nodeid2!=-1) {
if (isleaf1 && isleaf2 && nodeid1 != -1 && nodeid2 != -1) {
if (mj_collideSphere(m, d, nodeid1, nodeid2, /*margin=*/ 0)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
@@ -272,7 +272,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
// keep traversing the tree
if (!isleaf1 && isleaf2) {
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
if (child1[2*node1+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2*node1+i];
@@ -281,7 +281,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
}
}
} else if (isleaf1 && !isleaf2) {
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
if (child2[2*node2+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
@@ -380,14 +380,14 @@ void mj_collision(const mjModel* m, mjData* d) {
// return if disabled
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)
|| m->nconmax==0 || m->nbody < 2) {
|| m->nconmax == 0 || m->nbody < 2) {
return;
}
// predefined only; ignore exclude
if (m->opt.collision==mjCOL_PAIR) {
if (m->opt.collision == mjCOL_PAIR) {
d->nbodypair_broad = npair;
for (pairadr=0; pairadr<npair; pairadr++) {
for (pairadr=0; pairadr < npair; pairadr++) {
int ngeompair_narrow_before = d->ngeompair_narrow;
int ngeompair_mid_before = d->ngeompair_mid;
mj_collideGeoms(m, d, pairadr, -1, 0, 0);
@@ -405,7 +405,7 @@ void mj_collision(const mjModel* m, mjData* d) {
unsigned int last_signature = -1;
// loop over body pairs (broadphase or all)
for (int i=0; i<nbodypair; i++) {
for (int i=0; i < nbodypair; i++) {
// reconstruct body pair ids
b1 = (broadphasepair[i]>>16) & 0xFFFF;
b2 = broadphasepair[i] & 0xFFFF;
@@ -422,10 +422,10 @@ void mj_collision(const mjModel* m, mjData* d) {
// merge predefined pairs
merged = 0;
startadr = pairadr;
if (npair && m->opt.collision==mjCOL_ALL) {
if (npair && m->opt.collision == mjCOL_ALL) {
// 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, 0, 0);
@@ -435,12 +435,12 @@ void mj_collision(const mjModel* m, mjData* d) {
// handle exclusion
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 body 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;
}
}
@@ -450,7 +450,7 @@ void mj_collision(const mjModel* m, mjData* d) {
// test all geom pairs within this body pair
if (m->body_geomnum[b1] && m->body_geomnum[b2]) {
if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2]>1) {
if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2] > 1) {
int ncon_before = d->ncon;
mj_collideTree(m, d, b1, b2, merged, startadr, pairadr);
int ncon_after = d->ncon;
@@ -458,8 +458,8 @@ void mj_collision(const mjModel* m, mjData* d) {
mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before,
sizeof(mjContact), contactcompare, context);
} else {
for (g1=m->body_geomadr[b1]; g1<m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
for (g2=m->body_geomadr[b2]; g2<m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
for (g1=m->body_geomadr[b1]; g1 < m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
for (g2=m->body_geomadr[b2]; g2 < m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
mj_collidePair(m, d, g1, g2, merged, startadr, pairadr);
}
}
@@ -470,8 +470,8 @@ void mj_collision(const mjModel* m, mjData* d) {
}
// finish merging predefined pairs
if (npair && m->opt.collision==mjCOL_ALL) {
while (pairadr<npair) {
if (npair && m->opt.collision == mjCOL_ALL) {
while (pairadr < npair) {
mj_collideGeoms(m, d, pairadr++, -1, 0, 0);
}
}
@@ -498,28 +498,28 @@ static void makeAABB(const mjModel* m, mjData* d, mjtNum* aabb, int body, const
mjtNum _aabb[6], cen;
// no geoms attached to body: set to 0
if (m->body_geomnum[body]==0) {
if (m->body_geomnum[body] == 0) {
mju_zero(aabb, 6);
return;
}
// process all body geoms
for (int i=0; i<m->body_geomnum[body]; i++) {
for (int i=0; i < m->body_geomnum[body]; i++) {
// get geom id
geom = m->body_geomadr[body]+i;
// set _aabb for this geom
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
cen = mju_dot3(d->geom_xpos+3*geom, frame+3*j);
_aabb[2*j] = cen - m->geom_rbound[geom] - m->geom_margin[geom];
_aabb[2*j+1] = cen + m->geom_rbound[geom] + m->geom_margin[geom];
}
// update body aabb
if (i==0) {
if (i == 0) {
mju_copy(aabb, _aabb, 6);
} else {
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
aabb[2*j] = mju_min(aabb[2*j], _aabb[2*j]);
aabb[2*j+1] = mju_max(aabb[2*j+1], _aabb[2*j+1]);
}
@@ -536,8 +536,8 @@ static int has_plane_or_hfield(const mjModel* m, int body) {
// scan geoms belonging to body
int g;
for (g=start; g<end; g++) {
if (m->geom_type[g]==mjGEOM_PLANE || m->geom_type[g]==mjGEOM_HFIELD) {
for (g=start; g < end; g++) {
if (m->geom_type[g] == mjGEOM_PLANE || m->geom_type[g] == mjGEOM_HFIELD) {
return 1;
}
}
@@ -549,7 +549,7 @@ static int has_plane_or_hfield(const mjModel* m, int body) {
static int body_pair_filter(int weldbody1, int weldparent1, int weldbody2,
int weldparent2, int dsbl_filterparent) {
// same weldbody check
if (weldbody1==weldbody2) {
if (weldbody1 == weldbody2) {
return 1;
}
@@ -566,9 +566,9 @@ static int body_pair_filter(int weldbody1, int weldparent1, int weldbody2,
// add body pair in buffer
static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, int maxpair) {
// add pair if there is room in buffer
if ((*npair)<maxpair) {
if ((*npair) < maxpair) {
// exlude based on contype and conaffinity
if (m && m->body_geomnum[b1]==1 && m->body_geomnum[b2]==1) {
if (m && m->body_geomnum[b1] == 1 && m->body_geomnum[b2] == 1) {
// get contypes and conaffinities
int contype1 = m->geom_contype[m->body_geomadr[b1]];
int conaffinity1 = m->geom_conaffinity[m->body_geomadr[b1]];
@@ -582,7 +582,7 @@ static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, in
}
// add pair
if (b1<b2) {
if (b1 < b2) {
pair[*npair] = (b1<<16) + b2;
} else {
pair[*npair] = (b2<<16) + b1;
@@ -601,9 +601,9 @@ quicksortfunc(broadcompare, context, el1, el2) {
mjtBroadphase* b1 = (mjtBroadphase*)el1;
mjtBroadphase* b2 = (mjtBroadphase*)el2;
if (b1->value<b2->value) {
if (b1->value < b2->value) {
return -1;
} else if (b1->value==b2->value) {
} else if (b1->value == b2->value) {
return 0;
} else {
return 1;
@@ -617,9 +617,9 @@ quicksortfunc(paircompare, context, el1, el2) {
int signature1 = *(int*)el1;
int signature2 = *(int*)el2;
if (signature1<signature2) {
if (signature1 < signature2) {
return -1;
} else if (signature1==signature2) {
} else if (signature1 == signature2) {
return 0;
} else {
return 1;
@@ -633,7 +633,7 @@ static int can_collide(const mjModel* m, int b) {
int g;
// scan geoms; return if collidable
for (g=0; g<m->body_geomnum[b]; g++) {
for (g=0; g < m->body_geomnum[b]; g++) {
int ind = m->body_geomadr[b] + g;
if (m->geom_contype[ind] || m->geom_conaffinity[ind]) {
return 1;
@@ -656,17 +656,18 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT);
// world with geoms, and body with plane or hfield, can collide all bodies
for (b1=0; b1<nbody; b1++) {
for (b1=0; b1 < nbody; b1++) {
// cannot colide
if (!can_collide(m, b1)) {
continue;
}
// world with geoms, or welded body with plane or hfield
if ((b1==0 && m->body_geomnum[b1]>0) || (m->body_weldid[b1]==0 && has_plane_or_hfield(m, b1))) {
if ((b1 == 0 && m->body_geomnum[b1] > 0) ||
(m->body_weldid[b1] == 0 && has_plane_or_hfield(m, b1))) {
int weld1 = 0;
int parent_weld1 = 0;
for (b2=0; b2<nbody; b2++) {
for (b2=0; b2 < nbody; b2++) {
int weld2 = m->body_weldid[b2];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
if (!body_pair_filter(weld1, parent_weld1, weld2, parent_weld2,
@@ -680,23 +681,23 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
// find center of non-world geoms; return if none
cnt = 0;
mju_zero3(cen);
for (int i=0; i<ngeom; i++) {
for (int i=0; i < ngeom; i++) {
if (m->geom_bodyid[i]) {
mju_addTo3(cen, d->geom_xpos+3*i);
cnt++;
}
}
if (cnt==0) {
if (cnt == 0) {
return npair;
} else {
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
cen[i] /= cnt;
}
}
// compute covariance
mju_zero(cov, 9);
for (int i=0; i<ngeom; i++) {
for (int i=0; i < ngeom; i++) {
if (m->geom_bodyid[i]) {
mju_sub3(dif, d->geom_xpos+3*i, cen);
mjtNum D00 = dif[0]*dif[0];
@@ -716,7 +717,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
cov[8] += D22;
}
}
for (int i=0; i<9; i++) {
for (int i=0; i < 9; i++) {
cov[i] /= cnt;
}
@@ -729,7 +730,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
// construct body AABB for the aligned frame, count collidable
int bufcnt = 0;
for (int i=1; i<nbody; i++) {
for (int i=1; i < nbody; i++) {
makeAABB(m, d, aabb+6*i, i, frame);
if (can_collide(m, i)) {
@@ -750,7 +751,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
// init sortbuf with axis0
int k = 0;
for (int i=1; i<nbody; i++) {
for (int i=1; i < nbody; i++) {
// cannot colide
if (!can_collide(m, i)) {
continue;
@@ -765,7 +766,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
}
// sanity check; SHOULD NOT OCCUR
if (k!=bufcnt) {
if (k != bufcnt) {
mju_error("Internal error in broadphase: unexpected bufcnt");
}
@@ -774,10 +775,10 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
// sweep and prune
cnt = 0; // size of active list
for (int i=0; i<2*bufcnt; i++) {
for (int i=0; i < 2*bufcnt; i++) {
// min value: collide with all in list, add
if (!(sortbuf[i].body_ismax & 0x10000)) {
for (int j=0; j<cnt; j++) {
for (int j=0; j < cnt; j++) {
// get body ids: no need to mask ismax because activebuf entries never have the ismax bit,
// and sortbuf[i].body_ismax is tested above
b1 = activebuf[j].body_ismax;
@@ -813,9 +814,9 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
// max value: remove corresponding min value from list
else {
toremove = sortbuf[i].body_ismax & 0xFFFF;
for (int j=0; j<cnt; j++) {
if (activebuf[j].body_ismax==toremove) {
if (j<cnt-1) {
for (int j=0; j < cnt; j++) {
if (activebuf[j].body_ismax == toremove) {
if (j < cnt-1) {
memmove(activebuf+j, activebuf+j+1, sizeof(mjtBroadphase)*(cnt-1-j));
}
cnt--;
@@ -847,10 +848,10 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
int num, type1, type2, condim;
mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP];
mjContact con[mjMAXCONPAIR];
int ipair = (g2<0 ? g1 : -1);
int ipair = (g2 < 0 ? g1 : -1);
// get explicit geom ids from pair
if (ipair>=0) {
if (ipair >= 0) {
g1 = m->pair_geom1[ipair];
g2 = m->pair_geom2[ipair];
}
@@ -872,7 +873,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// apply filters if not predefined pair and not flg_user
if (ipair<0 && !flg_user) {
if (ipair < 0 && !flg_user) {
// user filter if defined
if (mjcb_contactfilter) {
if (mjcb_contactfilter(m, d, g1, g2)) {
@@ -888,14 +889,14 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// set margin, gap, condim: dynamic
if (ipair<0) {
if (ipair < 0) {
// margin and gap: max
margin = mju_max(m->geom_margin[g1], m->geom_margin[g2]);
gap = mju_max(m->geom_gap[g1], m->geom_gap[g2]);
// condim: priority or max
if (m->geom_priority[g1]!=m->geom_priority[g2]) {
int gp = (m->geom_priority[g1]>m->geom_priority[g2] ? g1 : g2);
if (m->geom_priority[g1] != m->geom_priority[g2]) {
int gp = (m->geom_priority[g1] > m->geom_priority[g2] ? g1 : g2);
condim = m->geom_condim[gp];
} else {
condim = mjMAX(m->geom_condim[g1], m->geom_condim[g2]);
@@ -936,23 +937,23 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
d->ngeompair_narrow++;
// check number of contacts, SHOULD NOT OCCUR
if (num>mjMAXCONPAIR) {
if (num > mjMAXCONPAIR) {
mju_error("Too many contacts returned by collision function");
}
// remove repeated contacts in box-box
if (type1==mjGEOM_BOX && type2==mjGEOM_BOX) {
if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) {
// use dim field to mark: -1: bad, 0: good
for (int i=0; i<num; i++) {
for (int i=0; i < num; i++) {
con[i].dim = 0;
}
// find bad
for (int i=0; i<num-1; i++) {
for (int j=i+1; j<num; j++) {
if (con[i].pos[0]==con[j].pos[0] &&
con[i].pos[1]==con[j].pos[1] &&
con[i].pos[2]==con[j].pos[2]) {
for (int i=0; i < num-1; i++) {
for (int j=i+1; j < num; j++) {
if (con[i].pos[0] == con[j].pos[0] &&
con[i].pos[1] == con[j].pos[1] &&
con[i].pos[2] == con[j].pos[2]) {
con[i].dim = -1;
break;
}
@@ -961,10 +962,10 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
// consolidate good
int i = 0;
for (int j=0; j<num; j++) {
if (con[j].dim==0) {
for (int j=0; j < num; j++) {
if (con[j].dim == 0) {
// different: copy
if (i<j) {
if (i < j) {
con[i] = con[j];
}
@@ -978,13 +979,13 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// set friction, solref, solimp: dynamic
if (ipair<0) {
if (ipair < 0) {
// different priority
if (m->geom_priority[g1]!=m->geom_priority[g2]) {
int gp = (m->geom_priority[g1]>m->geom_priority[g2] ? g1 : g2);
if (m->geom_priority[g1] != m->geom_priority[g2]) {
int gp = (m->geom_priority[g1] > m->geom_priority[g2] ? g1 : g2);
// friction
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
friction[2*i] = m->geom_friction[3*gp+i];
}
@@ -998,31 +999,31 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
// same priority
else {
// friction: max
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
friction[2*i] = mju_max(m->geom_friction[3*g1+i], m->geom_friction[3*g2+i]);
}
// solver mix factor
if (m->geom_solmix[g1]>=mjMINVAL && m->geom_solmix[g2]>=mjMINVAL) {
if (m->geom_solmix[g1] >= mjMINVAL && m->geom_solmix[g2] >= mjMINVAL) {
mix = m->geom_solmix[g1] / (m->geom_solmix[g1] + m->geom_solmix[g2]);
} else if (m->geom_solmix[g1]<mjMINVAL && m->geom_solmix[g2]<mjMINVAL) {
} else if (m->geom_solmix[g1] < mjMINVAL && m->geom_solmix[g2] < mjMINVAL) {
mix = 0.5;
} else if (m->geom_solmix[g1]<mjMINVAL) {
} else if (m->geom_solmix[g1] < mjMINVAL) {
mix = 0.0;
} else {
mix = 1.0;
}
// reference standard: mix
if (m->geom_solref[mjNREF*g1]>0 && m->geom_solref[mjNREF*g2]>0) {
for (int i=0; i<mjNREF; i++) {
if (m->geom_solref[mjNREF*g1] > 0 && m->geom_solref[mjNREF*g2] > 0) {
for (int i=0; i < mjNREF; i++) {
solref[i] = mix*m->geom_solref[mjNREF*g1+i] + (1-mix)*m->geom_solref[mjNREF*g2+i];
}
}
// reference direct: min
else {
for (int i=0; i<mjNREF; i++) {
for (int i=0; i < mjNREF; i++) {
solref[i] = mju_min(m->geom_solref[mjNREF*g1+i], m->geom_solref[mjNREF*g2+i]);
}
}
@@ -1040,7 +1041,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
// set friction, solref, solimp: pair
else {
// friction
for (int i=0; i<5; i++) {
for (int i=0; i < 5; i++) {
friction[i] = m->pair_friction[5*ipair+i];
}
@@ -1052,12 +1053,12 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// clamp friction to mjMINMU
for (int i=0; i<5; i++) {
for (int i=0; i < 5; i++) {
friction[i] = mju_max(mjMINMU, friction[i]);
}
// add contact returned by collision detector
for (int i=0; i<num; i++) {
for (int i=0; i < num; i++) {
// set contact data
if (condim > 6 || condim < 1) { // SHOULD NOT OCCUR
mju_error("Invalid condim value: %d", i);
@@ -1071,7 +1072,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
mj_assignImp(m, con[i].solimp, solimp);
// exclude in gap
if (con[i].dist<con[i].includemargin) {
if (con[i].dist < con[i].includemargin) {
con[i].exclude = 0;
} else {
con[i].exclude = 1;
src/engine/engine_collision_primitive.c
+4 -4
View File
@@ -217,7 +217,7 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d,
dist = mju_dot3(dif, norm);
// test all corners, pick bottom 4
for (int i=0; i<8; i++) {
for (int i=0; i < 8; i++) {
// get corner in local coordinates
vec[0] = (i&1 ? size2[0] : -size2[0]);
vec[1] = (i&2 ? size2[1] : -size2[1]);
@@ -298,7 +298,7 @@ static int _SphereSphere(mjContact* con, mjtNum margin,
int mjc_SphereSphere(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return _SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
return _SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
@@ -423,7 +423,7 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
n2 = _SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2>=2) {
if (n1+n2 >= 2) {
return n1+n2;
}
@@ -440,7 +440,7 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2+n3>=2) {
if (n1+n2+n3 >= 2) {
return n1+n2+n3;
}
src/engine/engine_core_constraint.c
+189 -189
View File
File diff suppressed because it is too large Load Diff
src/engine/engine_core_smooth.c
+104 -104
View File
@@ -50,14 +50,14 @@ void mj_kinematics(const mjModel* m, mjData* d) {
mj_normalizeQuat(m, d->qpos);
// normalize mocap quaternions
for (int i=0; i<m->nmocap; i++) {
for (int i=0; i < m->nmocap; i++) {
mju_normalize4(d->mocap_quat+4*i);
}
// compute global cartesian positions and orientations of all bodies
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// free joint
if (m->body_jntnum[i]==1 && m->jnt_type[m->body_jntadr[i]]==mjJNT_FREE) {
if (m->body_jntnum[i] == 1 && m->jnt_type[m->body_jntadr[i]] == mjJNT_FREE) {
// get addresses
int jid = m->body_jntadr[i];
int qadr = m->jnt_qposadr[jid];
@@ -81,7 +81,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
int pid = m->body_parentid[i];
// get body pos and quat: from model or mocap
if (m->body_mocapid[i]>=0) {
if (m->body_mocapid[i] >= 0) {
bodypos = d->mocap_pos + 3*m->body_mocapid[i];
bodyquat = d->mocap_quat + 4*m->body_mocapid[i];
} else {
@@ -95,7 +95,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
mju_mulQuat(quat, d->xquat+4*pid, bodyquat);
// accumulate joints, compute pos and quat for this body
for (int j=0; j<m->body_jntnum[i]; j++) {
for (int j=0; j < m->body_jntnum[i]; j++) {
// get joint id, qpos address, joint type
int jid = m->body_jntadr[i] + j;
int qadr = m->jnt_qposadr[jid];
@@ -117,7 +117,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
case mjJNT_BALL:
case mjJNT_HINGE:
// compute local quaternion rotation (qloc)
if (jtype==mjJNT_BALL) {
if (jtype == mjJNT_BALL) {
mju_copy4(qloc, d->qpos+qadr);
} else {
mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]);
@@ -152,21 +152,21 @@ void mj_kinematics(const mjModel* m, mjData* d) {
}
// compute/copy Cartesian positions and orientations of body inertial frames
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
mj_local2Global(d, d->xipos+3*i, d->ximat+9*i,
m->body_ipos+3*i, m->body_iquat+4*i,
i, m->body_sameframe[i]);
}
// compute/copy Cartesian positions and orientations of geoms
for (int i=0; i<m->ngeom; i++) {
for (int i=0; i < m->ngeom; i++) {
mj_local2Global(d, d->geom_xpos+3*i, d->geom_xmat+9*i,
m->geom_pos+3*i, m->geom_quat+4*i,
m->geom_bodyid[i], m->geom_sameframe[i]);
}
// compute/copy Cartesian positions and orientations of sites
for (int i=0; i<m->nsite; i++) {
for (int i=0; i < m->nsite; i++) {
mj_local2Global(d, d->site_xpos+3*i, d->site_xmat+9*i,
m->site_pos+3*i, m->site_quat+4*i,
m->site_bodyid[i], m->site_sameframe[i]);
@@ -186,7 +186,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
mju_zero(d->subtree_com, m->nbody*3);
// backwards pass over bodies: compute subtree_com and mass_subtree
for (int i=m->nbody-1; i>=0; i--) {
for (int i=m->nbody-1; i >= 0; i--) {
// add local info
mju_addToScl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]);
mass_subtree[i] += m->body_mass[i];
@@ -199,7 +199,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
}
// compute local com
if (mass_subtree[i]<mjMINVAL) {
if (mass_subtree[i] < mjMINVAL) {
mju_copy3(d->subtree_com+3*i, d->xipos+3*i);
} else {
mju_scl3(d->subtree_com+3*i, d->subtree_com+3*i,
@@ -208,14 +208,14 @@ void mj_comPos(const mjModel* m, mjData* d) {
}
// map inertias to frame centered at subtree_com
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
mju_sub3(offset, d->xipos+3*i, d->subtree_com+3*m->body_rootid[i]);
mju_inertCom(d->cinert+10*i, m->body_inertia+3*i, d->ximat+9*i,
offset, m->body_mass[i]);
}
// map motion dofs to global frame centered at subtree_com
for (int j=0; j<m->njnt; j++) {
for (int j=0; j < m->njnt; j++) {
// get dof address, body index
int da = 6*m->jnt_dofadr[j];
int bi = m->jnt_bodyid[j];
@@ -229,7 +229,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
case mjJNT_FREE:
// translation components: x, y, z in global frame
mju_zero(d->cdof+da, 18);
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
d->cdof[da+3+7*i] = 1;
}
@@ -238,7 +238,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
mjFALLTHROUGH;
case mjJNT_BALL:
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
// I_3 rotation in child frame (assume no subsequent rotations)
axis[0] = d->xmat[9*bi+i+0];
axis[1] = d->xmat[9*bi+i+3];
@@ -268,7 +268,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
mjtNum pos[3], matT[9];
// compute Cartesian positions and orientations of cameras
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
// default processing for fixed mode
mj_local2Global(d, d->cam_xpos+3*i, d->cam_xmat+9*i,
m->cam_pos+3*i, m->cam_quat+4*i, m->cam_bodyid[i], 0);
@@ -285,7 +285,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
mju_copy(d->cam_xmat+9*i, m->cam_mat0+9*i, 9);
// position: track camera body
if (m->cam_mode[i]==mjCAMLIGHT_TRACK) {
if (m->cam_mode[i] == mjCAMLIGHT_TRACK) {
mju_add3(d->cam_xpos+3*i, d->xpos+3*id, m->cam_pos0+3*i);
}
@@ -298,9 +298,9 @@ void mj_camlight(const mjModel* m, mjData* d) {
case mjCAMLIGHT_TARGETBODY:
case mjCAMLIGHT_TARGETBODYCOM:
// only if target body is specified
if (id1>=0) {
if (id1 >= 0) {
// get position to look at
if (m->cam_mode[i]==mjCAMLIGHT_TARGETBODY) {
if (m->cam_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(pos, d->xpos+3*id1);
} else {
mju_copy3(pos, d->subtree_com+3*id1);
@@ -328,7 +328,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
}
// compute Cartesian positions and directions of lights
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
// default processing for fixed mode
mj_local2Global(d, d->light_xpos+3*i, 0, m->light_pos+3*i, 0, m->light_bodyid[i], 0);
mju_rotVecQuat(d->light_xdir+3*i, m->light_dir+3*i, d->xquat+4*m->light_bodyid[i]);
@@ -345,7 +345,7 @@ void mj_camlight(const mjModel* m, mjData* d) {
mju_copy3(d->light_xdir+3*i, m->light_dir0+3*i);
// position: track light body
if (m->light_mode[i]==mjCAMLIGHT_TRACK) {
if (m->light_mode[i] == mjCAMLIGHT_TRACK) {
mju_add3(d->light_xpos+3*i, d->xpos+3*id, m->light_pos0+3*i);
}
@@ -358,9 +358,9 @@ void mj_camlight(const mjModel* m, mjData* d) {
case mjCAMLIGHT_TARGETBODY:
case mjCAMLIGHT_TARGETBODYCOM:
// only if target body is specified
if (id1>=0) {
if (id1 >= 0) {
// get position to look at
if (m->light_mode[i]==mjCAMLIGHT_TARGETBODY) {
if (m->light_mode[i] == mjCAMLIGHT_TARGETBODY) {
mju_copy3(pos, d->xpos+3*id1);
} else {
mju_copy3(pos, d->subtree_com+3*id1);
@@ -416,7 +416,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// loop over tendons
for (int i=0; i<nten; i++) {
for (int i=0; i < nten; i++) {
// initialize tendon path
adr = m->tendon_adr[i];
d->ten_wrapadr[i] = wcnt;
@@ -424,13 +424,13 @@ void mj_tendon(const mjModel* m, mjData* d) {
// sparse Jacobian row init
if (issparse) {
rowadr[i] = (i>0 ? rowadr[i-1] + rownnz[i-1] : 0);
rowadr[i] = (i > 0 ? rowadr[i-1] + rownnz[i-1] : 0);
}
// process joint tendon
if (m->wrap_type[adr]==mjWRAP_JOINT) {
if (m->wrap_type[adr] == mjWRAP_JOINT) {
// process all defined joints
for (int j=0; j<m->tendon_num[i]; j++) {
for (int j=0; j < m->tendon_num[i]; j++) {
// get joint id
int k = m->wrap_objid[adr+j];
@@ -455,11 +455,11 @@ void mj_tendon(const mjModel* m, mjData* d) {
int x, *list = colind+rowadr[i];
mjtNum y, *listy = J+rowadr[i];
for (int k=1; k<rownnz[i]; k++) {
for (int k=1; k < rownnz[i]; k++) {
x = list[k];
y = listy[k];
int j = k-1;
while (j>=0 && list[j]>x) {
while (j >= 0 && list[j] > x) {
list[j+1] = list[j];
listy[j+1] = listy[j];
j--;
@@ -475,7 +475,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
// process spatial tendon
divisor = 1;
int j = 0;
while (j<m->tendon_num[i]-1) {
while (j < m->tendon_num[i]-1) {
// get 1st and 2nd object
tp0 = m->wrap_type[adr+j];
id0 = m->wrap_objid[adr+j];
@@ -483,9 +483,9 @@ void mj_tendon(const mjModel* m, mjData* d) {
id1 = m->wrap_objid[adr+j+1];
// pulley
if (tp0==mjWRAP_PULLEY || tp1==mjWRAP_PULLEY) {
if (tp0 == mjWRAP_PULLEY || tp1 == mjWRAP_PULLEY) {
// get divisor, insert obj=-2
if (tp0==mjWRAP_PULLEY) {
if (tp0 == mjWRAP_PULLEY) {
divisor = m->wrap_prm[adr+j];
mju_zero3(d->wrap_xpos+wcnt*3);
d->wrap_obj[wcnt] = -2;
@@ -504,7 +504,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
wbody[0] = m->site_bodyid[id0];
// second object is geom: process site-geom-site
if (tp1==mjWRAP_SPHERE || tp1==mjWRAP_CYLINDER) {
if (tp1 == mjWRAP_SPHERE || tp1 == mjWRAP_CYLINDER) {
// reassign, get 2nd site info
tpw = tp1;
idw = id1;
@@ -513,19 +513,19 @@ void mj_tendon(const mjModel* m, mjData* d) {
// do wrapping, possibly get 2 extra points (wlen>=0)
sideid = mju_round(m->wrap_prm[adr+j+1]);
if (sideid<-1 || sideid>=m->nsite) {
if (sideid < -1 || sideid >= m->nsite) {
mju_error("Invalid sideid %d in wrap_prm", sideid); // SHOULD NOT OCCUR
}
wlen = mju_wrap(wpnt+3, d->site_xpos+3*id0, d->site_xpos+3*id1,
d->geom_xpos+3*idw, d->geom_xmat+9*idw, m->geom_size+3*idw, tpw,
(sideid>=0 ? d->site_xpos+3*sideid : 0));
(sideid >= 0 ? d->site_xpos+3*sideid : 0));
} else {
tpw = mjWRAP_NONE;
}
// complete sequence, accumulate lengths
if (wlen<0) {
if (wlen < 0) {
mju_copy3(wpnt+3, d->site_xpos+3*id1);
wbody[1] = m->site_bodyid[id1];
L[i] += mju_dist3(wpnt, wpnt+3)/divisor;
@@ -537,8 +537,8 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// accumulate moments if consequtive points are in different bodies
for (int k=0; k<(wlen<0 ? 1 : 3); k++) {
if (wbody[k]!=wbody[k+1]) {
for (int k=0; k < (wlen < 0 ? 1 : 3); k++) {
if (wbody[k] != wbody[k+1]) {
// get 3D position difference, normalize
mju_sub3(dif, wpnt+3*k+3, wpnt+3*k);
mju_normalize3(dif);
@@ -581,19 +581,19 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// assign to wrap
mju_copy(d->wrap_xpos+wcnt*3, wpnt, (wlen<0 ? 3:9));
mju_copy(d->wrap_xpos+wcnt*3, wpnt, (wlen < 0 ? 3:9));
d->wrap_obj[wcnt] = -1;
if (wlen>=0) {
if (wlen >= 0) {
d->wrap_obj[wcnt+1] = d->wrap_obj[wcnt+2] = idw;
}
d->ten_wrapnum[i] += (wlen<0 ? 1:3);
wcnt += (wlen<0 ? 1:3);
d->ten_wrapnum[i] += (wlen < 0 ? 1:3);
wcnt += (wlen < 0 ? 1:3);
// advance
j += (tpw!=mjWRAP_NONE ? 2 : 1);
j += (tpw != mjWRAP_NONE ? 2 : 1);
// assign last site before pulley or tendon end
if (j==m->tendon_num[i]-1 || m->wrap_type[adr+j+1]==mjWRAP_PULLEY) {
if (j == m->tendon_num[i]-1 || m->wrap_type[adr+j+1] == mjWRAP_PULLEY) {
mju_copy3(d->wrap_xpos+wcnt*3, d->site_xpos+3*id1);
d->wrap_obj[wcnt] = -1;
d->ten_wrapnum[i]++;
@@ -634,7 +634,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
int *chain;
// compute lengths and moments
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
// extract info
id = m->actuator_trnid[2*i];
idslider = m->actuator_trnid[2*i+1]; // for slider-crank only
@@ -645,13 +645,13 @@ void mj_transmission(const mjModel* m, mjData* d) {
case mjTRN_JOINT: // joint
case mjTRN_JOINTINPARENT: // joint, force in parent frame
// slide and hinge joint: scalar gear
if (m->jnt_type[id]==mjJNT_SLIDE || m->jnt_type[id]==mjJNT_HINGE) {
if (m->jnt_type[id] == mjJNT_SLIDE || m->jnt_type[id] == mjJNT_HINGE) {
length[i] = d->qpos[m->jnt_qposadr[id]]*gear[0];
moment[i*nv + m->jnt_dofadr[id]] = gear[0];
}
// ball joint: 3D wrench gear
else if (m->jnt_type[id]==mjJNT_BALL) {
else if (m->jnt_type[id] == mjJNT_BALL) {
// j: qpos start address
int j = m->jnt_qposadr[id];
@@ -659,7 +659,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_quat2Vel(axis, d->qpos+j, 1);
// gearAxis: rotate to parent frame if necessary
if (m->actuator_trntype[i]==mjTRN_JOINT) {
if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear);
} else {
mju_negQuat(quat, d->qpos+j);
@@ -691,7 +691,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_quat2Vel(axis, d->qpos+j+3, 1);
// gearAxis: rotate to world frame if necessary
if (m->actuator_trntype[i]==mjTRN_JOINT) {
if (m->actuator_trntype[i] == mjTRN_JOINT) {
mju_copy3(gearAxis, gear+3);
} else {
mju_negQuat(quat, d->qpos+j+3);
@@ -720,7 +720,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
av = mju_dot3(vec, axis);
det = av*av + rod*rod - mju_dot3(vec, vec);
ok = 1;
if (det<=0) {
if (det <= 0) {
ok = 0;
sdet = 0;
length[i] = av;
@@ -748,15 +748,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_subFrom(jac, jacS, 3*nv);
// apply chain rule
for (int j=0; j<nv; j++) {
for (int k=0; k<3; k++) {
for (int j=0; j < nv; j++) {
for (int k=0; k < 3; k++) {
moment[i*nv+j] += dlda[k]*jacA[k*nv+j] + dldv[k]*jac[k*nv+j];
}
}
// scale by gear ratio
length[i] *= gear[0];
for (int j = 0; j<nv; j++) {
for (int j = 0; j < nv; j++) {
moment[i*nv + j] *= gear[0];
}
break;
@@ -767,7 +767,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// moment: sparse or dense
if (mj_isSparse(m)) {
int end = d->ten_J_rowadr[id] + d->ten_J_rownnz[id];
for (int j=d->ten_J_rowadr[id]; j<end; j++) {
for (int j=d->ten_J_rowadr[id]; j < end; j++) {
moment[i*nv + d->ten_J_colind[j]] = d->ten_J[j] * gear[0];
}
} else {
@@ -878,7 +878,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// count all relevant contacts, accumulate Jacobians
int counter = 0;
for (int j=0; j<d->ncon; j++) {
for (int j=0; j < d->ncon; j++) {
const mjContact* con = d->contact+j;
int b1 = m->geom_bodyid[con->geom1];
int b2 = m->geom_bodyid[con->geom2];
@@ -893,14 +893,14 @@ void mj_transmission(const mjModel* m, mjData* d) {
counter++;
// condim 1 or elliptic cones: normal is in the first row
if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
if (con->dim == 1 || m->opt.cone == mjCONE_ELLIPTIC) {
efc_force[con->efc_address] = 1;
}
// pyramidal cones: average all pyramid directions
else {
int npyramid = con->dim-1; // number of frictional directions
for (int k=0; k<2*npyramid; k++) {
for (int k=0; k < 2*npyramid; k++) {
efc_force[con->efc_address+k] = 0.5/npyramid;
}
}
@@ -919,7 +919,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// accumulate in moment_exclude
if (issparse) {
for (int k=0; k<NV; k++) {
for (int k=0; k < NV; k++) {
moment_exclude[chain[k]] += jac[k];
}
} else {
@@ -963,8 +963,8 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) {
mju_copy(crb, d->cinert, 10*m->nbody);
// backward pass over bodies, accumulate composite inertias
for (int i=m->nbody-1; i>0; i--) {
if (m->body_parentid[i]>0) {
for (int i=m->nbody-1; i > 0; i--) {
if (m->body_parentid[i] > 0) {
mju_addTo(crb+10*m->body_parentid[i], crb+10*i, 10);
}
}
@@ -973,7 +973,7 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) {
mju_zero(d->qM, m->nM);
// dense backward pass over dofs
for (int i=m->nv-1; i>=0; i--) {
for (int i=m->nv-1; i >= 0; i--) {
// copy
if (skipsimple && m->dof_simplenum[i]) {
d->qM[m->dof_Madr[i]] = m->dof_M0[i];
@@ -990,7 +990,7 @@ void mj_crbSkip(const mjModel* m, mjData* d, int skipsimple) {
// sparse backward pass over ancestors
int j = i;
while (j>=0) {
while (j >= 0) {
// M(i,j) += cdof_j * crb_body(i) * cdof_i = cdof_j * tmp
d->qM[Madr_ij] += mju_dot(d->cdof+6*j, tmp, 6);
@@ -1027,12 +1027,12 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
mju_copy(qLD, M, m->nM);
// dense backward loop over dofs (regular only, simple diagonal already copied)
for (int k=nv-1; k>=0; k--) {
for (int k=nv-1; k >= 0; k--) {
// get address of M(k,k)
Madr_kk = dof_Madr[k];
// check for small/negative numbers on diagonal
if (qLD[Madr_kk]<mjMINVAL) {
if (qLD[Madr_kk] < mjMINVAL) {
mj_warning(d, mjWARN_INERTIA, k);
qLD[Madr_kk] = mjMINVAL;
}
@@ -1045,11 +1045,11 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
// sparse backward loop over ancestors of k (excluding k)
Madr_ki = Madr_kk + 1;
int i = dof_parentid[k];
while (i>=0) {
while (i >= 0) {
tmp = qLD[Madr_ki] / qLD[Madr_kk]; // tmp = M(k,i) / M(k,k)
// get number of ancestors of i (including i)
if (i<nv-1) {
if (i < nv-1) {
cnt = dof_Madr[i+1] - dof_Madr[i];
} else {
cnt = m->nM - dof_Madr[i+1];
@@ -1067,7 +1067,7 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
}
// compute 1/diag(D), 1/sqrt(diag(D))
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
mjtNum qLDi = qLD[dof_Madr[i]];
qLDiagInv[i] = 1.0/qLDi;
if (qLDiagSqrtInv) {
@@ -1096,9 +1096,9 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
int nv = m->nv;
// single vector
if (n==1) {
if (n == 1) {
// x <- inv(L') * x; skip simple, exploit sparsity of input vector
for (int i=nv-1; i>=0; i--) {
for (int i=nv-1; i >= 0; i--) {
if (!m->dof_simplenum[i] && x[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
@@ -1106,7 +1106,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
// traverse ancestors backwards
// read directly from x[i] since i cannot be a parent of itself
while (j>=0) {
while (j >= 0) {
x[j] -= qLD[Madr_ij++]*x[i]; // x(j) -= L(i,j) * x(i)
// advance to parent
@@ -1116,12 +1116,12 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
}
// x <- inv(D) * x
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
x[i] *= qLDiagInv[i]; // x(i) /= L(i,i)
}
// x <- inv(L) * x; skip simple
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
if (!m->dof_simplenum[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
@@ -1129,7 +1129,7 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
// traverse ancestors backwards
// write directly in x[i] since i cannot be a parent of itself
while (j>=0) {
while (j >= 0) {
x[i] -= qLD[Madr_ij++]*x[j]; // x(i) -= L(i,j) * x(j)
// advance to parent
@@ -1145,16 +1145,16 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
mjtNum tmp;
// x <- inv(L') * x; skip simple
for (int i=nv-1; i>=0; i--) {
for (int i=nv-1; i >= 0; i--) {
if (!m->dof_simplenum[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
int j = dof_parentid[i];
// traverse ancestors backwards
while (j>=0) {
while (j >= 0) {
// process all vectors, exploit sparsity
for (offset=0; offset<n*nv; offset+=nv)
for (offset=0; offset < n*nv; offset+=nv)
if ((tmp = x[i+offset])) {
x[j+offset] -= qLD[Madr_ij]*tmp; // x(j) -= L(i,j) * x(i)
}
@@ -1167,14 +1167,14 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
}
// x <- inv(D) * x
for (int i=0; i<nv; i++) {
for (offset=0; offset<n*nv; offset+=nv) {
for (int i=0; i < nv; i++) {
for (offset=0; offset < n*nv; offset+=nv) {
x[i+offset] *= qLDiagInv[i]; // x(i) /= L(i,i)
}
}
// x <- inv(L) * x; skip simple
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
if (!m->dof_simplenum[i]) {
// init
int Madr_ij = dof_Madr[i]+1;
@@ -1182,9 +1182,9 @@ void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
// traverse ancestors backwards
tmp = x[i+offset];
while (j>=0) {
while (j >= 0) {
// process all vectors
for (offset=0; offset<n*nv; offset+=nv) {
for (offset=0; offset < n*nv; offset+=nv) {
x[i+offset] -= qLD[Madr_ij]*x[j+offset]; // x(i) -= L(i,j) * x(j)
}
@@ -1222,11 +1222,11 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n)
mju_copy(x, y, n * nv);
// loop over the n input vectors
for (int ivec=0; ivec<n; ivec++) {
for (int ivec=0; ivec < n; ivec++) {
int offset = ivec*nv;
// x <- inv(L') * x; skip simple, exploit sparsity of input vector
for (int i=nv-1; i>=0; i--) {
for (int i=nv-1; i >= 0; i--) {
mjtNum tmp;
if (!m->dof_simplenum[i] && (tmp = x[i+offset])) {
// init
@@ -1234,7 +1234,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n)
int j = dof_parentid[i];
// traverse ancestors backwards
while (j>=0) {
while (j >= 0) {
x[j+offset] -= qLD[Madr_ij++] * tmp; // x(j) -= L(i,j) * x(i)
// advance to parent
@@ -1244,7 +1244,7 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n)
}
// x <- sqrt(inv(D)) * x
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
x[i+offset] *= qLDiagSqrtInv[i]; // x(i) /= sqrt(L(i,i))
}
}
@@ -1262,7 +1262,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
mju_zero(d->cvel, 6);
// forward pass over bodies
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// get body's first dof address
int bda = m->body_dofadr[i];
@@ -1270,7 +1270,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
mju_copy(cvel, d->cvel+6*m->body_parentid[i], 6);
// cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof
for (int j=0; j<m->body_dofnum[i]; j++) {
for (int j=0; j < m->body_dofnum[i]; j++) {
// compute cvel and cdofdot
switch (m->jnt_type[m->dof_jntid[bda+j]]) {
case mjJNT_FREE:
@@ -1287,7 +1287,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
case mjJNT_BALL:
// compute all 3 cdofdots using parent velocity
for (int k=0; k<3; k++) {
for (int k=0; k < 3; k++) {
mju_crossMotion(cdofdot+6*(j+k), cvel, d->cdof+6*(bda+j+k));
}
@@ -1326,7 +1326,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
mjtNum* body_vel = mj_stackAlloc(d, 6*m->nbody);
// bodywise quantities
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
// compute and save body velocity
mj_objectVelocity(m, d, mjOBJ_BODY, i, body_vel+6*i, 0);
@@ -1342,7 +1342,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
}
// subtree linvel
for (int i=m->nbody-1; i>=0; i--) {
for (int i=m->nbody-1; i >= 0; i--) {
// non-world: add linear momentum to parent
if (i) {
mju_addTo3(d->subtree_linvel+3*m->body_parentid[i], d->subtree_linvel+3*i);
@@ -1354,7 +1354,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
}
// subtree angmom
for (int i=m->nbody-1; i>0; i--) {
for (int i=m->nbody-1; i > 0; i--) {
int parent = m->body_parentid[i];
// momentum wrt body i
@@ -1399,7 +1399,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
}
// forward pass over bodies: accumulate cacc, set cfrc_body
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// get body's first dof address
int bda = m->body_dofadr[i];
@@ -1424,13 +1424,13 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
mju_zero(loc_cfrc_body, 6);
// backward pass over bodies: accumulate cfrc_body from children
for (int i=m->nbody-1; i>0; i--)
for (int i=m->nbody-1; i > 0; i--)
if (m->body_parentid[i]) {
mju_addTo(loc_cfrc_body+6*m->body_parentid[i], loc_cfrc_body+6*i, 6);
}
// result = cdof * cfrc_body
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6);
}
@@ -1453,7 +1453,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// cfrc_ext = perturb
mju_zero(d->cfrc_ext, 6*nbody);
for (int i=1; i<nbody; i++)
for (int i=1; i < nbody; i++)
if (!mju_isZero(d->xfrc_applied+6*i, 6)) {
// rearrange as torque:force
mju_copy3(cfrc, d->xfrc_applied+6*i+3);
@@ -1467,8 +1467,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// cfrc_ext += contacts
for (int i=0; i<d->ncon; i++)
if (d->contact[i].efc_address>=0) {
for (int i=0; i < d->ncon; i++)
if (d->contact[i].efc_address >= 0) {
// get contact pointer
con = d->contact+i;
@@ -1502,7 +1502,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// cfrc_ext += connect and weld constraints
int i = 0;
while (i < d->ne) {
if (d->efc_type[i]!=mjCNSTR_EQUALITY)
if (d->efc_type[i] != mjCNSTR_EQUALITY)
mju_error("Row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR
int id = d->efc_id[i];
@@ -1514,7 +1514,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
case mjEQ_WELD:
// cfrc = world-oriented torque:force vector
mju_copy3(cfrc + 3, d->efc_force + i);
if (m->eq_type[id]==mjEQ_WELD) {
if (m->eq_type[id] == mjEQ_WELD) {
mju_copy3(cfrc, d->efc_force + i + 3);
} else {
mju_zero3(cfrc); // no torque from connect
@@ -1523,7 +1523,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1
if ((k = m->eq_obj1id[id])) {
// transform point on body1: local -> global
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_WELD), 0, k, 0);
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_WELD), 0, k, 0);
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
@@ -1535,7 +1535,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 2
if ((k = m->eq_obj2id[id])) {
// transform point on body2: local -> global
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_CONNECT), 0, k, 0);
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_CONNECT), 0, k, 0);
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
@@ -1545,7 +1545,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// increment rows
i += m->eq_type[id]==mjEQ_WELD ? 6 : 3;
i += m->eq_type[id] == mjEQ_WELD ? 6 : 3;
break;
case mjEQ_JOINT:
@@ -1562,7 +1562,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// forward pass over bodies: compute cacc, cfrc_int
mjtNum cacc[6], cfrc_body[6], cfrc_corr[6];
mju_zero(d->cfrc_int, 6);
for (int j=1; j<m->nbody; j++) {
for (int j=1; j < m->nbody; j++) {
// get body's first dof address
int bda = m->body_dofadr[j];
@@ -1583,7 +1583,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// backward pass over bodies: accumulate cfrc_int from children
for (int j=m->nbody-1; j>0; j--) {
for (int j=m->nbody-1; j > 0; j--) {
mju_addTo(d->cfrc_int+6*m->body_parentid[j], d->cfrc_int+6*j, 6);
}
}
src/engine/engine_derivative.c
+98 -96
View File
@@ -280,19 +280,19 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt
mju_zero(Dcvel, nbody*6*nv);
// forward pass over bodies: accumulate Dcvel, set Dcdofdot
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
// Dcvel = Dcvel_parent
mju_copy(Dcvel+i*6*nv, Dcvel+m->body_parentid[i]*6*nv, 6*nv);
// Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof)
for (int j=m->body_dofadr[i]; j<m->body_dofadr[i]+m->body_dofnum[i]; j++) {
for (int j=m->body_dofadr[i]; j < m->body_dofadr[i]+m->body_dofnum[i]; j++) {
switch (m->jnt_type[m->dof_jntid[j]]) {
case mjJNT_FREE:
// Dcdofdot = 0
mju_zero(Dcdofdot+j*6*nv, 18*nv);
// Dcvel += cdof * (D qvel)
for (int k=0; k<6; k++) {
for (int k=0; k < 6; k++) {
Dcvel[i*6*nv + k*nv + j+0] += d->cdof[(j+0)*6 + k];
Dcvel[i*6*nv + k*nv + j+1] += d->cdof[(j+1)*6 + k];
Dcvel[i*6*nv + k*nv + j+2] += d->cdof[(j+2)*6 + k];
@@ -304,13 +304,13 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt
case mjJNT_BALL:
// Dcdofdot = D crossMotion(cvel, cdof)
for (int k=0; k<3; k++) {
for (int k=0; k < 3; k++) {
mjd_crossMotion_vel(mat, d->cdof+6*(j+k));
mju_mulMatMat(Dcdofdot+(j+k)*6*nv, mat, Dcvel+i*6*nv, 6, 6, nv);
}
// Dcvel += cdof * (D qvel)
for (int k=0; k<6; k++) {
for (int k=0; k < 6; k++) {
Dcvel[i*6*nv + k*nv + j+0] += d->cdof[(j+0)*6 + k];
Dcvel[i*6*nv + k*nv + j+1] += d->cdof[(j+1)*6 + k];
Dcvel[i*6*nv + k*nv + j+2] += d->cdof[(j+2)*6 + k];
@@ -326,7 +326,7 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt
mju_mulMatMat(Dcdofdot+j*6*nv, mat, Dcvel+i*6*nv, 6, 6, nv);
// Dcvel += cdof * (D qvel)
for (int k=0; k<6; k++) {
for (int k=0; k < 6; k++) {
Dcvel[i*6*nv + k*nv + j] += d->cdof[j*6 + k];
}
}
@@ -355,14 +355,14 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
mju_zero(Dcacc, nbody*6*nv);
// forward pass over bodies: accumulate Dcacc, set Dcfrcbody
for (int i=1; i<nbody; i++) {
for (int i=1; i < nbody; i++) {
// Dcacc = Dcacc_parent
mju_copy(Dcacc + i*6*nv, Dcacc + m->body_parentid[i]*6*nv, 6*nv);
// Dcacc += D(cdofdot * qvel)
for (int j=m->body_dofadr[i]; j<m->body_dofadr[i]+m->body_dofnum[i]; j++) {
for (int j=m->body_dofadr[i]; j < m->body_dofadr[i]+m->body_dofnum[i]; j++) {
// Dcacc += cdofdot * (D qvel)
for (int k=0; k<6; k++) {
for (int k=0; k < 6; k++) {
Dcacc[i*6*nv + k*nv + j] += d->cdof_dot[j*6 + k];
}
@@ -392,21 +392,21 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
mju_zero(Dcfrcbody, 6*nv);
// backward pass over bodies: accumulate Dcfrcbody
for (int i=m->nbody-1; i>0; i--) {
for (int i=m->nbody-1; i > 0; i--) {
if (m->body_parentid[i]) {
mju_addTo(Dcfrcbody+m->body_parentid[i]*6*nv, Dcfrcbody+i*6*nv, 6*nv);
}
}
// qDeriv -= D(cdof * cfrc_body)
for (int i=0; i<nv; i++) {
for (int k=0; k<6; k++) {
for (int i=0; i < nv; i++) {
for (int k=0; k < 6; k++) {
// compute D(cdof * cfrc_body), store in row
mju_scl(row, Dcfrcbody + (m->dof_bodyid[i]*6+k)*nv, d->cdof[i*6+k], nv);
// dense to sparse: qDeriv -= row
int end = d->D_rowadr[i] + d->D_rownnz[i];
for (int adr=d->D_rowadr[i]; adr<end; adr++) {
for (int adr=d->D_rowadr[i]; adr < end; adr++) {
d->qDeriv[adr] -= row[d->D_colind[adr]];
}
}
@@ -430,7 +430,7 @@ static void copyFromParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
// count dofs in ancestors
int ndof = 0;
int np = m->body_weldid[m->body_parentid[n]];
while (np>0) {
while (np > 0) {
// add self dofs
ndof += m->body_dofnum[np];
@@ -454,7 +454,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
// find matching nonzeros
int np = m->body_parentid[n];
int i = 0, ip = 0;
while (i<d->B_rownnz[n] && ip<d->B_rownnz[np]) {
while (i < d->B_rownnz[n] && ip < d->B_rownnz[np]) {
// columns match
if (d->B_colind[d->B_rowadr[n] + i] == d->B_colind[d->B_rowadr[np] + ip]) {
mju_addTo(mat + 6*(d->B_rowadr[np] + ip), mat + 6*(d->B_rowadr[n] + i), 6);
@@ -485,15 +485,15 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D
mjtNum mat[36], matT[36]; // 6x6 matrices
// forward pass over bodies: accumulate Dcvel, set Dcdofdot
for (int i = 1; i<nbody; i++) {
for (int i = 1; i < nbody; i++) {
// Dcvel = Dcvel_parent
copyFromParent(m, d, Dcvel, i);
// process all dofs of this body
int doflast = m->body_dofadr[i] + m->body_dofnum[i];
for (int j = m->body_dofadr[i]; j<doflast; j++) {
for (int j = m->body_dofadr[i]; j < doflast; j++) {
// number of dof ancestors of dof j
int Jadr = (j<nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1);
int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1);
// Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof)
switch (m->jnt_type[m->dof_jntid[j]]) {
@@ -512,7 +512,7 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D
case mjJNT_BALL:
// Dcdofdot = Dcvel * D crossMotion(cvel, cdof)
for (int dj=0; dj<3; dj++) {
for (int dj=0; dj < 3; dj++) {
mjd_crossMotion_vel(mat, d->cdof + 6 * (j + dj));
mju_transpose(matT, mat, 6, 6);
mju_mulMatMat(Dcdofdot + 6*Dadr[j + dj], Dcvel + 6*Badr[i], matT, Jadr + dj, 6, 6);
@@ -573,13 +573,13 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
mjd_comVel_vel(m, d, Dcvel, Dcdofdot);
// forward pass over bodies: accumulate Dcacc, set Dcfrcbody
for (int i=1; i<nbody; i++) {
for (int i=1; i < nbody; i++) {
// Dcacc = Dcacc_parent
copyFromParent(m, d, Dcacc, i);
// process all dofs of this body
int doflast = m->body_dofadr[i] + m->body_dofnum[i];
for (int j=m->body_dofadr[i]; j<doflast; j++) {
for (int j=m->body_dofadr[i]; j < doflast; j++) {
// number of dof ancestors of dof j
int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1);
@@ -615,12 +615,12 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
mju_zero(Dcfrcbody, 6*Bnnz[0]);
// backward pass over bodies: accumulate Dcfrcbody
for (int i=m->nbody-1; i>0; i--) {
for (int i=m->nbody-1; i > 0; i--) {
addToParent(m, d, Dcfrcbody, i);
}
// process all dofs, update qDeriv
for (int j=0; j<nv; j++) {
for (int j=0; j < nv; j++) {
// get body index
int i = m->dof_bodyid[j];
@@ -639,19 +639,19 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
// construct sparse Jacobian structure of body; return nnz
static int bodyJacSparse(const mjModel* m, int body, int* ind) {
// skip fixed bodies
while (body>0 && m->body_dofnum[body]==0) {
while (body > 0 && m->body_dofnum[body] == 0) {
body = m->body_parentid[body];
}
// body is not movable: empty chain
if (body==0) {
if (body == 0) {
return 0;
}
// count dofs
int nnz = 0;
int dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof>=0) {
while (dof >= 0) {
nnz++;
dof = m->dof_parentid[dof];
}
@@ -659,7 +659,7 @@ static int bodyJacSparse(const mjModel* m, int body, int* ind) {
// fill array in reverse (increasing dof)
int cnt = 0;
dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof>=0) {
while (dof >= 0) {
ind[nnz-cnt-1] = dof;
cnt++;
dof = m->dof_parentid[dof];
@@ -680,20 +680,20 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum*
mjtNum* row = mj_stackAlloc(d, nv);
// process non-zero elements of B
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
for (int i=0; i < n; i++) {
for (int j=0; j < n; j++) {
if (!B[i*n+j]) {
continue;
}
// process non-zero elements of J(i,:)
for (int k=0; k<nv; k++) {
for (int k=0; k < nv; k++) {
if (J[i*nv+k]) {
// row = J(i,k)*B(i,j)*J(j,:)
mju_scl(row, J+j*nv, J[i*nv+k] * B[i*n+j], nv);
// add row to qDeriv(k,:)
int rownnz_k = d->D_rownnz[k];
for (int s=0; s<rownnz_k; s++) {
for (int s=0; s < rownnz_k; s++) {
int adr = d->D_rowadr[k] + s;
d->qDeriv[adr] += row[d->D_colind[adr]];
}
@@ -709,9 +709,9 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum*
// add J'*B*J to qDeriv, sparse version
static void addJTBJSparse(
const mjModel* m, mjData* d, const mjtNum* J,
const mjtNum* B, int n, int offset,
const int* J_rownnz, const int* J_rowadr, const int* J_colind) {
const mjModel* m, mjData* d, const mjtNum* J,
const mjtNum* B, int n, int offset,
const int* J_rownnz, const int* J_rowadr, const int* J_colind) {
int nv = m->nv;
// allocate row
@@ -770,7 +770,7 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang
mjtNum fvmax = prm[8];
// scale force if negative
if (force<0) {
if (force < 0) {
force = scale / mjMAX(mjMINVAL, acc0);
}
@@ -788,16 +788,16 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang
// length curve
mjtNum FL = 0;
if (L>=lmin && L<=a) {
if (L >= lmin && L <= a) {
x = (L-lmin) / mjMAX(mjMINVAL, a-lmin);
FL = 0.5*x*x;
} else if (L<=1) {
} else if (L <= 1) {
x = (1-L) / mjMAX(mjMINVAL, 1-a);
FL = 1 - 0.5*x*x;
} else if (L<=b) {
} else if (L <= b) {
x = (L-1) / mjMAX(mjMINVAL, b-1);
FL = 1 - 0.5*x*x;
} else if (L<=lmax) {
} else if (L <= lmax) {
x = (lmax-L) / mjMAX(mjMINVAL, lmax-b);
FL = 0.5*x*x;
}
@@ -805,13 +805,13 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang
// velocity curve
mjtNum dFV;
mjtNum y = fvmax-1;
if (V<=-1) {
if (V <= -1) {
// FV = 0
dFV = 0;
} else if (V<=0) {
} else if (V <= 0) {
// FV = (V+1)*(V+1)
dFV = 2*V + 2;
} else if (V<=y) {
} else if (V <= y) {
// FV = fvmax - (y-V)*(y-V) / mjMAX(mjMINVAL, y)
dFV = (-2*V + 2*y) / mjMAX(mjMINVAL, y);
} else {
@@ -835,23 +835,23 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
}
// process actuators
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
mjtNum bias_vel = 0, gain_vel = 0;
// affine bias
if (m->actuator_biastype[i]==mjBIAS_AFFINE) {
if (m->actuator_biastype[i] == mjBIAS_AFFINE) {
// extract bias info: prm = [const, kp, kv]
bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2];
}
// affine gain
if (m->actuator_gaintype[i]==mjGAIN_AFFINE) {
if (m->actuator_gaintype[i] == mjGAIN_AFFINE) {
// extract bias info: prm = [const, kp, kv]
gain_vel = (m->actuator_gainprm + mjNGAIN*i)[2];
}
// muscle gain
else if (m->actuator_gaintype[i]==mjGAIN_MUSCLE) {
else if (m->actuator_gaintype[i] == mjGAIN_MUSCLE) {
gain_vel = mjd_muscleGain_vel(d->actuator_length[i],
d->actuator_velocity[i],
m->actuator_lengthrange+2*i,
@@ -860,8 +860,8 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
}
// force = gain .* [ctrl/act]
if (gain_vel!=0) {
if (m->actuator_dyntype[i]==mjDYN_NONE) {
if (gain_vel != 0) {
if (m->actuator_dyntype[i] == mjDYN_NONE) {
bias_vel += gain_vel * d->ctrl[i];
} else {
bias_vel += gain_vel * d->act[i-(m->nu - m->na)];
@@ -869,7 +869,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
}
// add
if (bias_vel!=0) {
if (bias_vel != 0) {
addJTBJ(m, d, d->actuator_moment+i*nv, &bias_vel, 1);
}
}
@@ -915,8 +915,8 @@ static void addToQuadrant(mjtNum* restrict B, const mjtNum D[9], int col_quad, i
// forces due to fluid mass moving with the body, B is 6x6
static void mjd_addedMassForces(
mjtNum* restrict B, const mjtNum local_vels[6], const mjtNum fluid_density,
const mjtNum virtual_mass[3], const mjtNum virtual_inertia[3]) {
mjtNum* restrict B, const mjtNum local_vels[6], const mjtNum fluid_density,
const mjtNum virtual_mass[3], const mjtNum virtual_inertia[3]) {
const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]};
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};
const mjtNum virtual_lin_mom[3] = {
@@ -935,7 +935,7 @@ static void mjd_addedMassForces(
// force[:3] += cross(virtual_ang_mom, ang_vel)
mjd_cross(virtual_ang_mom, ang_vel, Da, Db);
addToQuadrant(B, Db, 0, 0);
for (int i=0; i<9; ++i) {
for (int i=0; i < 9; ++i) {
Da[i] *= fluid_density * virtual_inertia[i % 3];
}
addToQuadrant(B, Da, 0, 0);
@@ -943,7 +943,7 @@ static void mjd_addedMassForces(
// force[:3] += cross(virtual_lin_mom, lin_vel)
mjd_cross(virtual_lin_mom, lin_vel, Da, Db);
addToQuadrant(B, Db, 0, 1);
for (int i=0; i<9; ++i) {
for (int i=0; i < 9; ++i) {
Da[i] *= fluid_density * virtual_mass[i % 3];
}
addToQuadrant(B, Da, 0, 1);
@@ -951,7 +951,7 @@ static void mjd_addedMassForces(
// force[3:] += cross(virtual_lin_mom, ang_vel)
mjd_cross(virtual_lin_mom, ang_vel, Da, Db);
addToQuadrant(B, Db, 1, 0);
for (int i=0; i<9; ++i) {
for (int i=0; i < 9; ++i) {
Da[i] *= fluid_density * virtual_mass[i % 3];
}
addToQuadrant(B, Da, 1, 1);
@@ -961,9 +961,9 @@ static void mjd_addedMassForces(
// torque due to motion in the fluid, D is 3x3
static inline void mjd_viscous_torque(
mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum fluid_viscosity, const mjtNum size[3],
const mjtNum slender_drag_coef, const mjtNum ang_drag_coef)
mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum fluid_viscosity, const mjtNum size[3],
const mjtNum slender_drag_coef, const mjtNum ang_drag_coef)
{
const mjtNum d_max = mju_max(mju_max(size[0], size[1]), size[2]);
const mjtNum d_min = mju_min(mju_min(size[0], size[1]), size[2]);
@@ -1016,9 +1016,9 @@ static inline void mjd_viscous_torque(
// drag due to motion in the fluid, D is 3x3
static inline void mjd_viscous_drag(
mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum fluid_viscosity, const mjtNum size[3],
const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef) {
mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum fluid_viscosity, const mjtNum size[3],
const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef) {
const mjtNum d_max = mju_max(mju_max(size[0], size[1]), size[2]);
const mjtNum d_min = mju_min(mju_min(size[0], size[1]), size[2]);
const mjtNum d_mid = size[0] + size[1] + size[2] - d_max - d_min;
@@ -1046,7 +1046,7 @@ static inline void mjd_viscous_drag(
const mjtNum lin_coef = fluid_viscosity * 3.0 * mjPI * eq_sphere_D;
const mjtNum quad_coef = fluid_density * (
A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj));
A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj));
const mjtNum Aproj_coef = fluid_density * norm * (blunt_drag_coef - slender_drag_coef);
const mjtNum dAproj_dv[3] = {
@@ -1084,8 +1084,8 @@ static inline void mjd_viscous_drag(
// Kutta lift due to motion in the fluid, D is 3x3
static inline void mjd_kutta_lift(
mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum size[3], const mjtNum kutta_lift_coef) {
mjtNum* restrict D, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum size[3], const mjtNum kutta_lift_coef) {
const mjtNum a = pow2(size[1] * size[2]);
const mjtNum b = pow2(size[2] * size[0]);
const mjtNum c = pow2(size[0] * size[1]);
@@ -1097,7 +1097,7 @@ static inline void mjd_kutta_lift(
const mjtNum proj_num = a * xx + b * yy + c * zz;
const mjtNum norm2 = xx + yy + zz;
const mjtNum df_denom = mjPI * kutta_lift_coef * fluid_density / mju_max(
mjMINVAL, mju_sqrt(proj_denom * proj_num * norm2));
mjMINVAL, mju_sqrt(proj_denom * proj_num * norm2));
const mjtNum dfx_coef = yy * (a - b) + zz * (a - c);
const mjtNum dfy_coef = xx * (b - a) + zz * (b - c);
@@ -1138,8 +1138,8 @@ static inline void mjd_kutta_lift(
// Magnus force due to motion in the fluid, B is 6x6
static inline void mjd_magnus_force(
mjtNum* restrict B, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum size[3], const mjtNum magnus_lift_coef) {
mjtNum* restrict B, const mjtNum lvel[6], const mjtNum fluid_density,
const mjtNum size[3], const mjtNum magnus_lift_coef) {
const mjtNum volume = 4.0/3.0 * mjPI * size[0] * size[1] * size[2];
// magnus_coef = magnus_lift_coef * fluid_density * volume
@@ -1149,9 +1149,11 @@ static inline void mjd_magnus_force(
// premultiply by magnus_coef
const mjtNum lin_vel[3] = {
magnus_coef * lvel[3], magnus_coef * lvel[4], magnus_coef * lvel[5]};
magnus_coef * lvel[3], magnus_coef * lvel[4], magnus_coef * lvel[5]
};
const mjtNum ang_vel[3] = {
magnus_coef * lvel[0], magnus_coef * lvel[1], magnus_coef * lvel[2]};
magnus_coef * lvel[0], magnus_coef * lvel[1], magnus_coef * lvel[2]
};
// force[3:] += magnus_coef * cross(ang_vel, lin_vel)
mjd_cross(ang_vel, lin_vel, D_ang, D_lin);
@@ -1186,25 +1188,25 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
nnz = bodyJacSparse(m, bodyid, colind);
// prepare rownnz, rowadr, colind for all 6 rows
for (int i=0; i<6; i++) {
for (int i=0; i < 6; i++) {
rownnz[i] = nnz;
rowadr[i] = i == 0 ? 0 : rowadr[i-1] + nnz;
for (int k=0; k<nnz; k++) {
for (int k=0; k < nnz; k++) {
colind_compressed[i*nnz+k] = colind[k];
}
}
}
for (int j=0; j<m->body_geomnum[bodyid]; j++) {
for (int j=0; j < m->body_geomnum[bodyid]; j++) {
const int geomid = m->body_geomadr[bodyid] + j;
mju_geomSemiAxes(m, geomid, semiaxes);
readFluidGeomInteraction(
m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef,
&blunt_drag_coef, &slender_drag_coef, &ang_drag_coef,
&kutta_lift_coef, &magnus_lift_coef,
virtual_mass, virtual_inertia);
m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef,
&blunt_drag_coef, &slender_drag_coef, &ang_drag_coef,
&kutta_lift_coef, &magnus_lift_coef,
virtual_mass, virtual_inertia);
// scales all forces, read from MJCF as boolean (0.0 or 1.0)
if (geom_interaction_coef == 0.0) {
@@ -1228,8 +1230,8 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
// compress geom Jacobian in-place
if (mj_isSparse(m)) {
for (int i=0; i<6; i++) {
for (int k=0; k<nnz; k++) {
for (int i=0; i < 6; i++) {
for (int k=0; k < nnz; k++) {
J[i*nnz+k] = J[i*nv+colind[k]];
}
}
@@ -1291,11 +1293,11 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
// equivalent inertia box
box[0] = mju_sqrt(mju_max(mjMINVAL,
(inertia[1] + inertia[2] - inertia[0])) / m->body_mass[i] * 6.0);
(inertia[1] + inertia[2] - inertia[0])) / m->body_mass[i] * 6.0);
box[1] = mju_sqrt(mju_max(mjMINVAL,
(inertia[0] + inertia[2] - inertia[1])) / m->body_mass[i] * 6.0);
(inertia[0] + inertia[2] - inertia[1])) / m->body_mass[i] * 6.0);
box[2] = mju_sqrt(mju_max(mjMINVAL,
(inertia[0] + inertia[1] - inertia[2])) / m->body_mass[i] * 6.0);
(inertia[0] + inertia[1] - inertia[2])) / m->body_mass[i] * 6.0);
// map from CoM-centered to local body-centered 6D velocity
mj_objectVelocity(m, d, mjOBJ_BODY, i, lvel, 1);
@@ -1321,8 +1323,8 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
nnz = bodyJacSparse(m, i, colind);
// compress body Jacobian in-place
for (int j=0; j<6; j++) {
for (int k=0; k<nnz; k++) {
for (int j=0; j < 6; j++) {
for (int k=0; k < nnz; k++) {
J[j*nnz+k] = J[j*nv+colind[k]];
}
}
@@ -1330,10 +1332,10 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
// prepare rownnz, rowadr, colind for all 6 rows
rownnz[0] = nnz;
rowadr[0] = 0;
for (int j=1; j<6; j++) {
for (int j=1; j < 6; j++) {
rownnz[j] = nnz;
rowadr[j] = rowadr[j-1] + nnz;
for (int k=0; k<nnz; k++) {
for (int k=0; k < nnz; k++) {
colind[j*nnz+k] = colind[k];
}
}
@@ -1346,13 +1348,13 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
mju_copy(J+3*nnz, tmp, 3*nnz);
// add viscous force and torque
if (m->opt.viscosity>0) {
if (m->opt.viscosity > 0) {
// diameter of sphere approximation
mjtNum diam = (box[0] + box[1] + box[2])/3.0;
// mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity)
B = -mjPI*diam*diam*diam*m->opt.viscosity;
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
if (mj_isSparse(m)) {
addJTBJSparse(m, d, J, &B, 1, j, rownnz, rowadr, colind);
} else {
@@ -1362,7 +1364,7 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
// mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity);
B = -3.0*mjPI*diam*m->opt.viscosity;
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
if (mj_isSparse(m)) {
addJTBJSparse(m, d, J, &B, 1, 3+j, rownnz, rowadr, colind);
} else {
@@ -1372,7 +1374,7 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
}
// add lift and drag force and torque
if (m->opt.density>0) {
if (m->opt.density > 0) {
// lfrc[0] -= m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])*
// mju_abs(lvel[0])*lvel[0]/64.0;
B = -m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])*
@@ -1445,9 +1447,9 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
// dof damping
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
int nnz_i = d->D_rownnz[i];
for (int j=0; j<nnz_i; j++) {
for (int j=0; j < nnz_i; j++) {
int ij = d->D_rowadr[i] + j;
// identify diagonal element
@@ -1459,8 +1461,8 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
// tendon damping
for (int i=0; i<m->ntendon; i++) {
if (m->tendon_damping[i]>0) {
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_damping[i] > 0) {
mjtNum B = -m->tendon_damping[i];
// add sparse or dense
@@ -1474,15 +1476,15 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
// fluid drag model, either body-level (inertia box) or geom-level (ellipsoid)
if (m->opt.viscosity>0 || m->opt.density>0) {
for (int i=1; i<nbody; i++) {
if (m->body_mass[i]<mjMINVAL) {
if (m->opt.viscosity > 0 || m->opt.density > 0) {
for (int i=1; i < nbody; i++) {
if (m->body_mass[i] < mjMINVAL) {
continue;
}
int use_ellipsoid_model = 0;
// if any child geom uses the ellipsoid model, inertia-box model is disabled for parent body
for (int j=0; j<m->body_geomnum[i] && use_ellipsoid_model==0; j++) {
for (int j=0; j < m->body_geomnum[i] && use_ellipsoid_model == 0; j++) {
const int geomid = m->body_geomadr[i] + j;
use_ellipsoid_model += (m->geom_fluid[mjNFLUID*geomid] > 0);
}
src/engine/engine_derivative_fd.c
+26 -26
View File
@@ -68,7 +68,7 @@ static void setState(const mjModel* m, mjData* d, mjtNum time, const mjtNum* sta
// dx = (x2 - x1) / h
static void diff(mjtNum* restrict dx, const mjtNum* x1, const mjtNum* x2, mjtNum h, int n) {
mjtNum inv_h = 1/h;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
dx[i] = inv_h * (x2[i] - x1[i]);
}
}
@@ -154,22 +154,22 @@ void mj_stepSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) {
// use selected integrator
switch (m->opt.integrator) {
case mjINT_EULER:
mj_EulerSkip(m, d, skipstage >= mjSTAGE_POS);
break;
case mjINT_EULER:
mj_EulerSkip(m, d, skipstage >= mjSTAGE_POS);
break;
case mjINT_RK4:
// ignore skipstage
mj_RungeKutta(m, d, 4);
break;
case mjINT_RK4:
// ignore skipstage
mj_RungeKutta(m, d, 4);
break;
case mjINT_IMPLICIT:
case mjINT_IMPLICITFAST:
mj_implicitSkip(m, d, skipstage >= mjSTAGE_VEL);
break;
case mjINT_IMPLICIT:
case mjINT_IMPLICITFAST:
mj_implicitSkip(m, d, skipstage >= mjSTAGE_VEL);
break;
default:
mju_error("Invalid integrator");
default:
mju_error("Invalid integrator");
}
TM_END(mjTIMER_STEP);
@@ -208,7 +208,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
mju_copy(qfrc_passive, d->qfrc_passive, nv);
// loop over dofs
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
// save qvel[i]
mjtNum saveqvel = d->qvel[i];
@@ -224,9 +224,9 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
mju_scl(fd, fd, 1/eps, nv);
// copy to i-th column of qDeriv
for (int j=0; j<nv; j++) {
for (int j=0; j < nv; j++) {
int adr = d->D_rowadr[j] + cnt[j];
if (cnt[j]<d->D_rownnz[j] && d->D_colind[adr] == i) {
if (cnt[j] < d->D_rownnz[j] && d->D_colind[adr] == i) {
d->qDeriv[adr] = fd[j];
cnt[j]++;
}
@@ -257,7 +257,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
memset(cnt, 0, nv*sizeof(int));
// loop over dofs
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
// save qvel[i]
mjtNum saveqvel = d->qvel[i];
@@ -283,8 +283,8 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
mju_scl(fd, fd, 0.5/eps, nv);
// copy to sparse qDeriv
for (int j=0; j<nv; j++) {
if (cnt[j]<d->D_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]]==i) {
for (int j=0; j < nv; j++) {
if (cnt[j] < d->D_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]] == i) {
d->qDeriv[d->D_rowadr[j]+cnt[j]] = fd[j];
cnt[j]++;
}
@@ -292,8 +292,8 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
}
// make sure final row counters equal rownnz
for (int i=0; i<nv; i++) {
if (cnt[i]!=d->D_rownnz[i]) {
for (int i=0; i < nv; i++) {
if (cnt[i] != d->D_rownnz[i]) {
mju_error("error in constructing FD sparse derivative");
}
}
@@ -368,7 +368,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
// finite-difference controls: skip=mjSTAGE_VEL, handle ctrl at range limits
if (DyDu || DsDu) {
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
int limited = m->actuator_ctrllimited[i];
// nudge forward, if possible given ctrlrange
int nudge_fwd = !limited || inRange(ctrl[i], ctrl[i]+eps, m->actuator_ctrlrange+2*i);
@@ -415,7 +415,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
// finite-difference activations: skip=mjSTAGE_VEL
if (DyDa || DsDa) {
for (int i=0; i<na; i++) {
for (int i=0; i < na; i++) {
// nudge forward
d->act[i] += eps;
@@ -462,7 +462,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
// finite-difference velocities: skip=mjSTAGE_POS
if (DyDv || DsDv) {
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
// nudge forward
d->qvel[i] += eps;
@@ -509,7 +509,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
// finite-difference positions: skip=mjSTAGE_NONE
if (DyDq || DsDq) {
mjtNum *dpos = mj_stackAlloc(d, nv); // allocate position perturbation
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
// nudge forward
mju_zero(dpos, nv);
dpos[i] = 1;
src/engine/engine_forward.c
+42 -42
View File
@@ -46,7 +46,7 @@
// check positions, reset if bad
void mj_checkPos(const mjModel* m, mjData* d) {
for (int i=0; i<m->nq; i++) {
for (int i=0; i < m->nq; i++) {
if (mju_isBad(d->qpos[i])) {
mj_warning(d, mjWARN_BADQPOS, i);
mj_resetData(m, d);
@@ -61,7 +61,7 @@ void mj_checkPos(const mjModel* m, mjData* d) {
// check velocities, reset if bad
void mj_checkVel(const mjModel* m, mjData* d) {
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
if (mju_isBad(d->qvel[i])) {
mj_warning(d, mjWARN_BADQVEL, i);
mj_resetData(m, d);
@@ -76,7 +76,7 @@ void mj_checkVel(const mjModel* m, mjData* d) {
// check accelerations, reset if bad
void mj_checkAcc(const mjModel* m, mjData* d) {
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
if (mju_isBad(d->qacc[i])) {
mj_warning(d, mjWARN_BADQACC, i);
mj_resetData(m, d);
@@ -166,7 +166,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mju_zero(d->actuator_force, nu);
// disabled or no actuation: return
if (nu==0 || mjDISABLED(mjDSBL_ACTUATION)) {
if (nu == 0 || mjDISABLED(mjDSBL_ACTUATION)) {
return;
}
@@ -176,7 +176,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
if (mjDISABLED(mjDSBL_CLAMPCTRL)) {
mju_copy(ctrl, d->ctrl, nu);
} else {
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
// clamp ctrl
if (m->actuator_ctrllimited[i]) {
mjtNum *ctrlrange = m->actuator_ctrlrange + 2*i;
@@ -188,7 +188,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// check controls, set all to 0 if any are bad
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
if (mju_isBad(ctrl[i])) {
mj_warning(d, mjWARN_BADCTRL, i);
mju_zero(ctrl, nu);
@@ -197,7 +197,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// force = gain .* [ctrl/act] + bias
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
// skip actuator plugins -- these are handled after builtin actuator types
if (m->actuator_plugin[i] >= 0) {
continue;
@@ -275,7 +275,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// handle actuator plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
for (int i=0; i < m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
@@ -291,7 +291,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// clamp actuator_force
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
if (m->actuator_forcelimited[i]) {
mjtNum *forcerange = m->actuator_forcerange + 2*i;
force[i] = mju_clip(force[i], forcerange[0], forcerange[1]);
@@ -302,7 +302,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// act_dot for stateful actuators
for (int i=0; i<nu; i++) {
for (int i=0; i < nu; i++) {
if (m->actuator_plugin[i] >= 0) {
continue;
}
@@ -392,7 +392,7 @@ static void warmstart(const mjModel* m, mjData* d) {
mj_constraintUpdate(m, d, jar, &cost_warmstart, 0);
// PGS
if (m->opt.solver==mjSOL_PGS) {
if (m->opt.solver == mjSOL_PGS) {
// cost(force_warmstart)
mjtNum PGS_warmstart = mju_dot(d->efc_force, d->efc_b, nefc);
mjtNum* ARf = mj_stackAlloc(d, nefc);
@@ -406,7 +406,7 @@ static void warmstart(const mjModel* m, mjData* d) {
PGS_warmstart += 0.5*mju_dot(d->efc_force, ARf, nefc);
// use zero if better
if (PGS_warmstart>0) {
if (PGS_warmstart > 0) {
mju_zero(d->efc_force, nefc);
mju_zero(d->qfrc_constraint, nv);
}
@@ -417,7 +417,7 @@ static void warmstart(const mjModel* m, mjData* d) {
// add Gauss to cost(qacc_warmstart)
mjtNum* Ma = mj_stackAlloc(d, nv);
mj_mulM(m, d, Ma, d->qacc_warmstart);
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
cost_warmstart += 0.5*(Ma[i]-d->qfrc_smooth[i])*(d->qacc_warmstart[i]-d->qacc_smooth[i]);
}
@@ -426,7 +426,7 @@ static void warmstart(const mjModel* m, mjData* d) {
mj_constraintUpdate(m, d, d->efc_b, &cost_smooth, 0);
// use qacc_smooth if better
if (cost_warmstart>cost_smooth) {
if (cost_warmstart > cost_smooth) {
mju_copy(d->qacc, d->qacc_smooth, nv);
}
}
@@ -488,7 +488,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
mju_copy(d->qacc_warmstart, d->qacc, nv);
// run noslip solver if enabled
if (m->opt.noslip_iterations>0) {
if (m->opt.noslip_iterations > 0) {
mj_solNoSlip(m, d, m->opt.noslip_iterations);
}
@@ -507,11 +507,11 @@ static void mj_advance(const mjModel* m, mjData* d,
mju_addToScl(d->act, act_dot, m->opt.timestep, m->na);
// clamp activations
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
int j = m->actuator_actadr[i];
if (j > -1 && m->actuator_actlimited[i]) {
mjtNum* actrange = m->actuator_actrange + 2*i;
for (int k=0; k<m->actuator_actnum[i]; k++) {
for (int k=0; k < m->actuator_actnum[i]; k++) {
d->act[j+k] = mju_clip(d->act[j+k], actrange[0], actrange[1]);
}
}
@@ -552,8 +552,8 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// check for dof damping
int dof_damping = 0;
for (int i=0; i<nv; i++) {
if (m->dof_damping[i]>0) {
for (int i=0; i < nv; i++) {
if (m->dof_damping[i] > 0) {
dof_damping = 1;
break;
}
@@ -571,7 +571,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// MhB = M + h*diag(B)
mju_copy(MhB, d->qM, m->nM);
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
MhB[m->dof_Madr[i]] += m->opt.timestep * m->dof_damping[i];
}
@@ -618,8 +618,8 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
int nv = m->nv, nq = m->nq, na = m->na;
mjtNum h = m->opt.timestep, time = d->time;
mjtNum C[9], T[9], *X[10], *F[10], *dX;
const mjtNum* A = (N==4 ? RK4_A : 0);
const mjtNum* B = (N==4 ? RK4_B : 0);
const mjtNum* A = (N == 4 ? RK4_A : 0);
const mjtNum* B = (N == 4 ? RK4_B : 0);
mjMARKSTACK;
// check order
@@ -629,16 +629,16 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
// allocate space for intermediate solutions
dX = mj_stackAlloc(d, 2*nv+na);
for (int i=0; i<N; i++) {
for (int i=0; i < N; i++) {
X[i] = mj_stackAlloc(d, nq+nv+na);
F[i] = mj_stackAlloc(d, nv+na);
}
// precompute C and T; C,T,A have size (N-1)
for (int i=1; i<N; i++) {
for (int i=1; i < N; i++) {
// C(i) = sum_j A(i,j)
C[i-1] = 0;
for (int j=0; j<i; j++) {
for (int j=0; j < i; j++) {
C[i-1] += A[(i-1)*(N-1)+j];
}
@@ -656,10 +656,10 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
}
// compute the remaining X[i], F[i]
for (int i=1; i<N; i++) {
for (int i=1; i < N; i++) {
// compute dX
mju_zero(dX, 2*nv+na);
for (int j=0; j<i; j++) {
for (int j=0; j < i; j++) {
mju_addToScl(dX, X[j]+nq, A[(i-1)*(N-1)+j], nv);
mju_addToScl(dX+nv, F[j], A[(i-1)*(N-1)+j], nv+na);
}
@@ -687,7 +687,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
// compute dX for final update (using B instead of A)
mju_zero(dX, 2*nv+na);
for (int j=0; j<N; j++) {
for (int j=0; j < N; j++) {
mju_addToScl(dX, X[j]+nq, B[j], nv);
mju_addToScl(dX+nv, F[j], B[j], nv+na);
}
@@ -783,7 +783,7 @@ void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor)
TM_START;
// position-dependent
if (skipstage<mjSTAGE_POS) {
if (skipstage < mjSTAGE_POS) {
mj_fwdPosition(m, d);
if (!skipsensor) {
mj_sensorPos(m, d);
@@ -794,7 +794,7 @@ void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor)
}
// velocity-dependent
if (skipstage<mjSTAGE_VEL) {
if (skipstage < mjSTAGE_VEL) {
mj_fwdVelocity(m, d);
if (!skipsensor) {
mj_sensorVel(m, d);
@@ -845,21 +845,21 @@ void mj_step(const mjModel* m, mjData* d) {
// use selected integrator
switch (m->opt.integrator) {
case mjINT_EULER:
mj_Euler(m, d);
break;
case mjINT_EULER:
mj_Euler(m, d);
break;
case mjINT_RK4:
mj_RungeKutta(m, d, 4);
break;
case mjINT_RK4:
mj_RungeKutta(m, d, 4);
break;
case mjINT_IMPLICIT:
case mjINT_IMPLICITFAST:
mj_implicit(m, d);
break;
case mjINT_IMPLICIT:
case mjINT_IMPLICITFAST:
mj_implicit(m, d);
break;
default:
mju_error("Invalid integrator");
default:
mju_error("Invalid integrator");
}
TM_END(mjTIMER_STEP);
src/engine/engine_inverse.c
+3 -3
View File
@@ -123,7 +123,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
int nv = m->nv;
// position-dependent
if (skipstage<mjSTAGE_POS) {
if (skipstage < mjSTAGE_POS) {
mj_invPosition(m, d);
if (!skipsensor) {
mj_sensorPos(m, d);
@@ -134,7 +134,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
}
// velocity-dependent
if (skipstage<mjSTAGE_VEL) {
if (skipstage < mjSTAGE_VEL) {
mj_invVelocity(m, d);
if (!skipsensor) {
mj_sensorVel(m, d);
@@ -152,7 +152,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
}
// qfrc_inverse += artmature*qacc - qfrc_passive - qfrc_constraint
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
d->qfrc_inverse[i] += m->dof_armature[i]*d->qacc[i]
- d->qfrc_passive[i] - d->qfrc_constraint[i];
}
src/engine/engine_io.c
+152 -152
View File
@@ -285,7 +285,7 @@ static void bufwrite(const void* src, int num, int szbuf, void* buf, int* ptrbuf
}
// check size
if (*ptrbuf+num>szbuf) {
if (*ptrbuf+num > szbuf) {
mju_error("Attempting to write outside model buffer");
}
@@ -304,7 +304,7 @@ static void bufread(void* dest, int num, int szbuf, const void* buf, int* ptrbuf
}
// check size
if (*ptrbuf+num>szbuf) {
if (*ptrbuf+num > szbuf) {
mju_error("Attempting to read outside model buffer");
}
@@ -362,7 +362,7 @@ static int safeAddToBufferSize(intptr_t* offset, int* nbuffer, size_t type_size,
return 0;
}
#if (__has_builtin(__builtin_add_overflow) && __has_builtin(__builtin_mul_overflow)) \
|| (defined(__GNUC__) && __GNUC__ >= 5)
|| (defined(__GNUC__) && __GNUC__ >= 5)
// supported by GCC and Clang
int to_add = 0;
if (__builtin_mul_overflow(nc, nr, &to_add)) return 0;
@@ -654,8 +654,8 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
bufread(header, 4*sizeof(int), buffer_sz, buffer, &ptrbuf);
// check header
for (int i=0; i<4; i++) {
if (header[i]!=expected_header[i]) {
for (int i=0; i < 4; i++) {
if (header[i] != expected_header[i]) {
switch (i) {
case 0:
mju_warning("Model missing header ID");
@@ -692,7 +692,7 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
info[35], info[36], info[37], info[38], info[39], info[40], info[41],
info[42], info[43], info[44], info[45], info[46], info[47], info[48],
info[49], info[50], info[51], info[52]);
if (!m || m->nbuffer!=info[getnint()-1]) {
if (!m || m->nbuffer != info[getnint()-1]) {
mju_closeResource(r);
mju_warning("Corrupted model, wrong size parameters");
mj_deleteModel(m);
@@ -775,7 +775,7 @@ int mj_sizeModel(const mjModel* m) {
+ sizeof(mjVisual)
+ sizeof(mjStatistic));
MJMODEL_POINTERS_PREAMBLE(m)
MJMODEL_POINTERS_PREAMBLE(m)
#define X(type, name, nr, nc) \
size += sizeof(type)*(m->nr)*(nc);
MJMODEL_POINTERS
@@ -1367,16 +1367,16 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
// set mocap_pos/quat = body_pos/quat for mocap bodies
if (m->body_mocapid) {
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
int id = m->body_mocapid[i];
if (id>=0) {
if (id >= 0) {
mju_copy3(d->mocap_pos+3*id, m->body_pos+3*i);
mju_copy4(d->mocap_quat+4*id, m->body_quat+4*i);
}
}
} else {
// set the mocap_quats to {1, 0, 0, 0}
for (int i=0; i<m->nmocap; i++) {
for (int i=0; i < m->nmocap; i++) {
d->mocap_quat[4*i] = 1.0;
}
}
@@ -1426,7 +1426,7 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
_resetData(m, d, 0);
// copy keyframe data if key is valid
if (key>=0 && key<m->nkey) {
if (key >= 0 && key < m->nkey) {
d->time = m->key_time[key];
mju_copy(d->qpos, m->key_qpos+key*m->nq, m->nq);
mju_copy(d->qvel, m->key_qvel+key*m->nv, m->nv);
@@ -1510,7 +1510,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_PLUGIN:
return -1;
// don't use a 'default' case, so compiler warns about missing values
// don't use a 'default' case, so compiler warns about missing values
}
return -1;
}
@@ -1520,55 +1520,55 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
// -2: invalid objtype
static int numObjects(const mjModel* m, mjtObj objtype) {
switch (objtype) {
case mjOBJ_UNKNOWN:
return -1;
case mjOBJ_BODY:
case mjOBJ_XBODY:
return m->nbody;
case mjOBJ_JOINT:
return m->njnt;
case mjOBJ_DOF:
return m->nv;
case mjOBJ_GEOM:
return m->ngeom;
case mjOBJ_SITE:
return m->nsite;
case mjOBJ_CAMERA:
return m->ncam;
case mjOBJ_LIGHT:
return m->nlight;
case mjOBJ_MESH:
return m->nmesh;
case mjOBJ_SKIN:
return m->nskin;
case mjOBJ_HFIELD:
return m->nhfield;
case mjOBJ_TEXTURE:
return m->ntex;
case mjOBJ_MATERIAL:
return m->nmat;
case mjOBJ_PAIR:
return m->npair;
case mjOBJ_EXCLUDE:
return m->nexclude;
case mjOBJ_EQUALITY:
return m->neq;
case mjOBJ_TENDON:
return m->ntendon;
case mjOBJ_ACTUATOR:
return m->nu;
case mjOBJ_SENSOR:
return m->nsensor;
case mjOBJ_NUMERIC:
return m->nnumeric;
case mjOBJ_TEXT:
return m->ntext;
case mjOBJ_TUPLE:
return m->ntuple;
case mjOBJ_KEY:
return m->nkey;
case mjOBJ_PLUGIN:
return m->nplugin;
case mjOBJ_UNKNOWN:
return -1;
case mjOBJ_BODY:
case mjOBJ_XBODY:
return m->nbody;
case mjOBJ_JOINT:
return m->njnt;
case mjOBJ_DOF:
return m->nv;
case mjOBJ_GEOM:
return m->ngeom;
case mjOBJ_SITE:
return m->nsite;
case mjOBJ_CAMERA:
return m->ncam;
case mjOBJ_LIGHT:
return m->nlight;
case mjOBJ_MESH:
return m->nmesh;
case mjOBJ_SKIN:
return m->nskin;
case mjOBJ_HFIELD:
return m->nhfield;
case mjOBJ_TEXTURE:
return m->ntex;
case mjOBJ_MATERIAL:
return m->nmat;
case mjOBJ_PAIR:
return m->npair;
case mjOBJ_EXCLUDE:
return m->nexclude;
case mjOBJ_EQUALITY:
return m->neq;
case mjOBJ_TENDON:
return m->ntendon;
case mjOBJ_ACTUATOR:
return m->nu;
case mjOBJ_SENSOR:
return m->nsensor;
case mjOBJ_NUMERIC:
return m->nnumeric;
case mjOBJ_TEXT:
return m->ntext;
case mjOBJ_TUPLE:
return m->ntuple;
case mjOBJ_KEY:
return m->nkey;
case mjOBJ_PLUGIN:
return m->nplugin;
}
return -2;
}
@@ -1675,7 +1675,7 @@ const char* mj_validateReferences(const mjModel* m) {
#undef MJMODEL_REFERENCES
// special logic that doesn't fit in the macro:
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
if (i > 0 && m->body_parentid[i] >= i) {
return "Invalid model: bad body_parentid.";
}
@@ -1686,7 +1686,7 @@ const char* mj_validateReferences(const mjModel* m) {
return "Invalid model: bad body_weldid.";
}
}
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
if (m->jnt_type[i] >= 4 || m->jnt_type[i] < 0) {
return "Invalid model: jnt_type out of bounds.";
}
@@ -1699,12 +1699,12 @@ const char* mj_validateReferences(const mjModel* m) {
return "Invalid model: jnt_dofadr out of bounds.";
}
}
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
if (m->dof_parentid[i] >= i) {
return "Invalid model: bad dof_parentid.";
}
}
for (int i=0; i<m->ngeom; i++) {
for (int i=0; i < m->ngeom; i++) {
if (m->geom_condim[i] > 6 || m->geom_condim[i] < 0) {
return "Invalid model: geom_condim out of bounds.";
}
@@ -1718,19 +1718,19 @@ const char* mj_validateReferences(const mjModel* m) {
}
}
}
for (int i=0; i<m->nhfield; i++) {
for (int i=0; i < m->nhfield; i++) {
int hfield_adr = m->hfield_adr[i] + m->hfield_nrow[i]*m->hfield_ncol[i];
if (hfield_adr > m->nhfielddata || m->hfield_adr[i] < 0) {
return "Invalid model: hfield_adr out of bounds.";
}
}
for (int i=0; i<m->ntex; i++) {
for (int i=0; i < m->ntex; i++) {
int tex_adr = m->tex_adr[i] + 3*m->tex_height[i]*m->tex_width[i];
if (tex_adr > m->ntexdata || m->tex_adr[i] < 0) {
return "Invalid model: tex_adr out of bounds.";
}
}
for (int i=0; i<m->npair; i++) {
for (int i=0; i < m->npair; i++) {
int pair_body1 = (m->pair_signature[i] & 0xFFFF) - 1;
if (pair_body1 >= m->nbody || pair_body1 < 0) {
return "Invalid model: pair_body1 out of bounds.";
@@ -1740,104 +1740,104 @@ const char* mj_validateReferences(const mjModel* m) {
return "Invalid model: pair_body2 out of bounds.";
}
}
for (int i=0; i<m->neq; i++) {
for (int i=0; i < m->neq; i++) {
int obj1id = m->eq_obj1id[i];
int obj2id = m->eq_obj2id[i];
switch (m->eq_type[i]) {
case mjEQ_JOINT:
if (obj1id >= m->njnt || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
// -1 is the value used if second object is omitted.
if (obj2id >= m->njnt || obj2id < -1) {
return "Invalid model: eq_obj2id out of bounds.";
}
break;
case mjEQ_JOINT:
if (obj1id >= m->njnt || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
// -1 is the value used if second object is omitted.
if (obj2id >= m->njnt || obj2id < -1) {
return "Invalid model: eq_obj2id out of bounds.";
}
break;
case mjEQ_TENDON:
if (obj1id >= m->ntendon || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
// -1 is the value used if second object is omitted.
if (obj2id >= m->ntendon || obj2id < -1) {
return "Invalid model: eq_obj2id out of bounds.";
}
break;
case mjEQ_TENDON:
if (obj1id >= m->ntendon || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
// -1 is the value used if second object is omitted.
if (obj2id >= m->ntendon || obj2id < -1) {
return "Invalid model: eq_obj2id out of bounds.";
}
break;
case mjEQ_WELD:
case mjEQ_CONNECT:
if (obj1id >= m->nbody || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
if (obj2id >= m->nbody || obj2id < 0) {
return "Invalid model: eq_obj2id out of bounds.";
}
break;
case mjEQ_WELD:
case mjEQ_CONNECT:
if (obj1id >= m->nbody || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
if (obj2id >= m->nbody || obj2id < 0) {
return "Invalid model: eq_obj2id out of bounds.";
}
break;
default:
mju_error("mj_validateReferences: unknown equality constraint type.");
default:
mju_error("mj_validateReferences: unknown equality constraint type.");
}
}
for (int i=0; i<m->nwrap; i++) {
for (int i=0; i < m->nwrap; i++) {
int wrap_objid = m->wrap_objid[i];
switch (m->wrap_type[i]) {
case mjWRAP_NONE:
case mjWRAP_PULLEY:
// wrap_objid not used.
break;
case mjWRAP_JOINT:
if (wrap_objid >= m->njnt || wrap_objid < 0) {
return "Invalid model: wrap_objid out of bounds.";
}
break;
case mjWRAP_SITE:
if (wrap_objid >= m->nsite || wrap_objid < 0) {
return "Invalid model: wrap_objid out of bounds.";
}
break;
case mjWRAP_SPHERE:
case mjWRAP_CYLINDER:
if (wrap_objid >= m->ngeom || wrap_objid < 0) {
return "Invalid model: wrap_objid out of bounds.";
}
break;
case mjWRAP_NONE:
case mjWRAP_PULLEY:
// wrap_objid not used.
break;
case mjWRAP_JOINT:
if (wrap_objid >= m->njnt || wrap_objid < 0) {
return "Invalid model: wrap_objid out of bounds.";
}
break;
case mjWRAP_SITE:
if (wrap_objid >= m->nsite || wrap_objid < 0) {
return "Invalid model: wrap_objid out of bounds.";
}
break;
case mjWRAP_SPHERE:
case mjWRAP_CYLINDER:
if (wrap_objid >= m->ngeom || wrap_objid < 0) {
return "Invalid model: wrap_objid out of bounds.";
}
break;
}
}
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
int actuator_trntype = m->actuator_trntype[i];
int id = m->actuator_trnid[2*i];
int idslider = m->actuator_trnid[2*i+1];
switch (actuator_trntype) {
case mjTRN_JOINT:
case mjTRN_JOINTINPARENT:
if (id < 0 || id >= m->njnt) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_TENDON:
if (id < 0 || id >= m->ntendon) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_SITE:
if (id < 0 || id >= m->nsite) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_SLIDERCRANK:
if (id < 0 || id >= m->nsite) {
return "Invalid model: actuator_trnid out of bounds.";
}
if (idslider < 0 || idslider >= m->nsite) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_UNDEFINED:
// actuator_trnid not used.
break;
case mjTRN_JOINT:
case mjTRN_JOINTINPARENT:
if (id < 0 || id >= m->njnt) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_TENDON:
if (id < 0 || id >= m->ntendon) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_SITE:
if (id < 0 || id >= m->nsite) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_SLIDERCRANK:
if (id < 0 || id >= m->nsite) {
return "Invalid model: actuator_trnid out of bounds.";
}
if (idslider < 0 || idslider >= m->nsite) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_UNDEFINED:
// actuator_trnid not used.
break;
}
}
for (int i=0; i<m->nsensor; i++) {
for (int i=0; i < m->nsensor; i++) {
mjtSensor sensor_type = m->sensor_type[i];
int sensor_size;
if (sensor_type == mjSENS_PLUGIN) {
@@ -1851,7 +1851,7 @@ const char* mj_validateReferences(const mjModel* m) {
sensor_size = sensorSize(sensor_type, m->sensor_dim[i]);
}
if (sensor_size < 0) {
return "Invalid model: Bad sensor_type.";
return "Invalid model: Bad sensor_type.";
}
int sensor_adr = m->sensor_adr[i];
if (sensor_adr < 0 || sensor_adr + sensor_size > m->nsensordata) {
@@ -1872,7 +1872,7 @@ const char* mj_validateReferences(const mjModel* m) {
return "Invalid model: invalid sensor_refid";
}
}
for (int i=0; i<m->nexclude; i++) {
for (int i=0; i < m->nexclude; i++) {
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF) - 1;
if (exclude_body1 >= m->nbody || exclude_body1 < 0) {
return "Invalid model: exclude_body1 out of bounds.";
@@ -1882,8 +1882,8 @@ const char* mj_validateReferences(const mjModel* m) {
return "Invalid model: exclude_body2 out of bounds.";
}
}
for (int i=0; i<m->ntuple; i++) {
for (int j=0; j<m->tuple_size[i]; j++) {
for (int i=0; i < m->ntuple; i++) {
for (int j=0; j < m->tuple_size[i]; j++) {
int adr = m->tuple_adr[i] + j;
int nobj = numObjects(m, m->tuple_objtype[adr]);
if (nobj == -2) {
src/engine/engine_passive.c
+27 -27
View File
@@ -48,7 +48,7 @@ void mj_passive(const mjModel* m, mjData* d) {
}
// joint-level springs
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
stiffness = m->jnt_stiffness[i];
int padr = m->jnt_qposadr[i];
@@ -85,13 +85,13 @@ void mj_passive(const mjModel* m, mjData* d) {
}
// dof-level dampers
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
damping = m->dof_damping[i];
d->qfrc_passive[i] -= damping*d->qvel[i];
}
// tendon-level spring-dampers
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
stiffness = m->tendon_stiffness[i];
damping = m->tendon_damping[i];
@@ -113,7 +113,7 @@ void mj_passive(const mjModel* m, mjData* d) {
// transform to joint torque, add to qfrc_passive: dense or sparse
if (issparse) {
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
for (int j=d->ten_J_rowadr[i]; j<end; j++) {
for (int j=d->ten_J_rowadr[i]; j < end; j++) {
d->qfrc_passive[d->ten_J_colind[j]] += d->ten_J[j] * frc;
}
} else {
@@ -126,7 +126,7 @@ void mj_passive(const mjModel* m, mjData* d) {
mjtNum force[3], torque[3]={0};
// apply per-body gravity compensation
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
if (m->body_gravcomp[i]) {
mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i]));
mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_passive);
@@ -135,15 +135,15 @@ void mj_passive(const mjModel* m, mjData* d) {
}
// body-level viscosity, lift and drag
if (m->opt.viscosity>0 || m->opt.density>0) {
for (int i=1; i<m->nbody; i++) {
if (m->body_mass[i]<mjMINVAL) {
if (m->opt.viscosity > 0 || m->opt.density > 0) {
for (int i=1; i < m->nbody; i++) {
if (m->body_mass[i] < mjMINVAL) {
continue;
}
int use_ellipsoid_model = 0;
// if any child geom uses the ellipsoid model, inertia-box model is disabled for parent body
for (int j=0; j<m->body_geomnum[i] && use_ellipsoid_model==0; j++) {
for (int j=0; j < m->body_geomnum[i] && use_ellipsoid_model == 0; j++) {
const int geomid = m->body_geomadr[i] + j;
use_ellipsoid_model += (m->geom_fluid[mjNFLUID*geomid] > 0);
}
@@ -164,7 +164,7 @@ void mj_passive(const mjModel* m, mjData* d) {
if (m->nplugin) {
const int nslot = mjp_pluginCount();
// iterate over plugins, call compute if type is mjPLUGIN_PASSIVE
for (int i=0; i<m->nplugin; i++) {
for (int i=0; i < m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
@@ -209,7 +209,7 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) {
mju_zero(lfrc, 6);
// set viscous force and torque
if (m->opt.viscosity>0) {
if (m->opt.viscosity > 0) {
// diameter of sphere approximation
diam = (box[0] + box[1] + box[2])/3.0;
@@ -221,7 +221,7 @@ void mj_inertiaBoxFluidModel(const mjModel* m, mjData* d, int i) {
}
// add lift and drag force and torque
if (m->opt.density>0) {
if (m->opt.density > 0) {
// force
lfrc[3] -= 0.5*m->opt.density*box[1]*box[2]*mju_abs(lvel[3])*lvel[3];
lfrc[4] -= 0.5*m->opt.density*box[0]*box[2]*mju_abs(lvel[4])*lvel[4];
@@ -252,16 +252,16 @@ void mj_ellipsoidFluidModel(const mjModel* m, mjData* d, int bodyid) {
mjtNum semiaxes[3], virtual_mass[3], virtual_inertia[3];
mjtNum blunt_drag_coef, slender_drag_coef, ang_drag_coef;
for (int j=0; j<m->body_geomnum[bodyid]; j++) {
for (int j=0; j < m->body_geomnum[bodyid]; j++) {
const int geomid = m->body_geomadr[bodyid] + j;
mju_geomSemiAxes(m, geomid, semiaxes);
readFluidGeomInteraction(
m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef,
&blunt_drag_coef, &slender_drag_coef, &ang_drag_coef,
&kutta_lift_coef, &magnus_lift_coef,
virtual_mass, virtual_inertia);
m->geom_fluid + mjNFLUID*geomid, &geom_interaction_coef,
&blunt_drag_coef, &slender_drag_coef, &ang_drag_coef,
&kutta_lift_coef, &magnus_lift_coef,
virtual_mass, virtual_inertia);
// scales all forces, read from MJCF as boolean (0.0 or 1.0)
if (geom_interaction_coef == 0.0) {
@@ -364,11 +364,11 @@ static inline mjtNum mji_ellipsoid_max_moment(const mjtNum size[3], const int di
// lift and drag forces due to motion in the fluid
void mj_viscousForces(
const mjtNum local_vels[6], const mjtNum fluid_density,
const mjtNum fluid_viscosity, const mjtNum size[3],
const mjtNum magnus_lift_coef, const mjtNum kutta_lift_coef,
const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef,
const mjtNum ang_drag_coef, mjtNum local_force[6])
const mjtNum local_vels[6], const mjtNum fluid_density,
const mjtNum fluid_viscosity, const mjtNum size[3],
const mjtNum magnus_lift_coef, const mjtNum kutta_lift_coef,
const mjtNum blunt_drag_coef, const mjtNum slender_drag_coef,
const mjtNum ang_drag_coef, mjtNum local_force[6])
{
const mjtNum lin_vel[3] = {local_vels[3], local_vels[4], local_vels[5]};
const mjtNum ang_vel[3] = {local_vels[0], local_vels[1], local_vels[2]};
@@ -406,7 +406,7 @@ void mj_viscousForces(
// cosine between velocity and normal to the surface
// divided by proj_denom instead of sqrt(proj_denom) to account for skipped normalization in norm
const mjtNum cos_alpha = proj_num / mju_max(
mjMINVAL, mju_norm3(lin_vel) * proj_denom);
mjMINVAL, mju_norm3(lin_vel) * proj_denom);
mjtNum kutta_circ[3];
mju_cross(kutta_circ, norm, lin_vel);
kutta_circ[0] *= kutta_lift_coef * fluid_density * cos_alpha * A_proj;
@@ -434,11 +434,11 @@ void mj_viscousForces(
};
const mjtNum drag_lin_coef = // linear plus quadratic
fluid_viscosity*lin_visc_force_coef + fluid_density*mju_norm3(lin_vel)*(
A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj));
fluid_viscosity*lin_visc_force_coef + fluid_density*mju_norm3(lin_vel)*(
A_proj*blunt_drag_coef + slender_drag_coef*(A_max - A_proj));
const mjtNum drag_ang_coef = // linear plus quadratic
fluid_viscosity * lin_visc_torq_coef +
fluid_density * mju_norm3(mom_visc);
fluid_viscosity * lin_visc_torq_coef +
fluid_density * mju_norm3(mom_visc);
local_force[0] -= drag_ang_coef * ang_vel[0];
local_force[1] -= drag_ang_coef * ang_vel[1];
src/engine/engine_print.c
+76 -76
View File
@@ -52,9 +52,9 @@ static void printArray(const char* str, int nr, int nc, const mjtNum* data, FILE
}
if (nr && nc) {
fprintf(fp, "%s\n", str);
for (int r=0; r<nr; r++) {
for (int r=0; r < nr; r++) {
fprintf(fp, " ");
for (int c=0; c<nc; c++) {
for (int c=0; c < nc; c++) {
fprintf(fp, " ");
fprintf(fp, float_format, data[c + r*nc]);
}
@@ -72,9 +72,9 @@ static void printArrayInt(const char* str, int nr, int nc, const int* data, FILE
}
if (nr && nc) {
fprintf(fp, "%s\n", str);
for (int r=0; r<nr; r++) {
for (int r=0; r < nr; r++) {
fprintf(fp, " ");
for (int c=0; c<nc; c++) {
for (int c=0; c < nc; c++) {
fprintf(fp, " ");
fprintf(fp, "%d", data[c + r*nc]);
}
@@ -95,9 +95,9 @@ static void printSparse(const char* str, const mjtNum* mat, int nr,
}
fprintf(fp, "%s\n", str);
for (int r=0; r<nr; r++) {
for (int r=0; r < nr; r++) {
fprintf(fp, " ");
for (int adr=rowadr[r]; adr<rowadr[r]+rownnz[r]; adr++) {
for (int adr=rowadr[r]; adr < rowadr[r]+rownnz[r]; adr++) {
fprintf(fp, " ");
fprintf(fp, "%d: ", colind[adr]);
fprintf(fp, float_format, mat[adr]);
@@ -119,7 +119,7 @@ static void printVector(const char* str, const mjtNum* data, int n, FILE* fp,
fprintf(fp, "%s", str);
// print data
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
fprintf(fp, " ");
fprintf(fp, float_format, data[i]);
}
@@ -199,11 +199,11 @@ static bool validateFloatFormat(const char* float_format) {
}
// Clang sometimes goes OOM when the -Wuninitialized warning is enabled for this function
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wuninitialized"
#endif
// Clang sometimes goes OOM when the -Wuninitialized warning is enabled for this function
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wuninitialized"
#endif
// print mjModel to text file, specifying format. float_format must be a
@@ -231,7 +231,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
// compute total body mass
mjtNum totalmass = 0;
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
totalmass += m->body_mass[i];
}
@@ -305,7 +305,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
// qpos0
fprintf(fp, NAME_FORMAT, "qpos0");
for (int i=0; i<m->nq; i++) {
for (int i=0; i < m->nq; i++) {
fprintf(fp, float_format, m->qpos0[i]);
fprintf(fp, " ");
}
@@ -313,7 +313,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
// qpos_spring
fprintf(fp, NAME_FORMAT, "qpos_spring");
for (int i=0; i<m->nq; i++) {
for (int i=0; i < m->nq; i++) {
fprintf(fp, float_format, m->qpos_spring[i]);
fprintf(fp, " ");
}
@@ -347,7 +347,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
// bodies
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
fprintf(fp, "\nBODY %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_bodyadr[i]);
@@ -357,7 +357,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nbody) fprintf(fp, "\n");
// joints
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
fprintf(fp, "\nJOINT %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_jntadr[i]);
@@ -367,7 +367,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->njnt) fprintf(fp, "\n");
// dofs
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
fprintf(fp, "\nDOF %d:\n", i);
object_class = &m->nv;
MJMODEL_POINTERS
@@ -375,7 +375,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nv) fprintf(fp, "\n");
// geoms
for (int i=0; i<m->ngeom; i++) {
for (int i=0; i < m->ngeom; i++) {
fprintf(fp, "\nGEOM %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_geomadr[i]);
@@ -385,7 +385,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->ngeom) fprintf(fp, "\n");
// sites
for (int i=0; i<m->nsite; i++) {
for (int i=0; i < m->nsite; i++) {
fprintf(fp, "\nSITE %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_siteadr[i]);
@@ -395,7 +395,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nsite) fprintf(fp, "\n");
// cameras
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
fprintf(fp, "\nCAMERA %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_camadr[i]);
@@ -405,7 +405,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->ncam) fprintf(fp, "\n");
// lights
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
fprintf(fp, "\nLIGHT %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_lightadr[i]);
@@ -415,13 +415,13 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nlight) fprintf(fp, "\n");
// meshes
for (int i=0; i<m->nmesh; i++) {
for (int i=0; i < m->nmesh; i++) {
fprintf(fp, "\nMESH %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_meshadr[i]);
object_class = &m->nmesh;
MJMODEL_POINTERS
if (m->mesh_graphadr[i]>=0) {
if (m->mesh_graphadr[i] >= 0) {
fprintf(fp, " " NAME_FORMAT, "qhull face");
fprintf(fp, " %d\n", m->mesh_graph[m->mesh_graphadr[i]+1]);
fprintf(fp, " " NAME_FORMAT, "qhull vert");
@@ -431,7 +431,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nmesh) fprintf(fp, "\n");
// skins
for (int i=0; i<m->nskin; i++) {
for (int i=0; i < m->nskin; i++) {
fprintf(fp, "\nSKIN %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_skinadr[i]);
@@ -441,7 +441,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nskin) fprintf(fp, "\n");
// hfields
for (int i=0; i<m->nhfield; i++) {
for (int i=0; i < m->nhfield; i++) {
fprintf(fp, "\nHEIGHTFIELD %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_hfieldadr[i]);
@@ -451,7 +451,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nhfield) fprintf(fp, "\n");
// textures
for (int i=0; i<m->ntex; i++) {
for (int i=0; i < m->ntex; i++) {
fprintf(fp, "\nTEXTURE %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_texadr[i]);
@@ -461,7 +461,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->ntex) fprintf(fp, "\n");
// materials
for (int i=0; i<m->nmat; i++) {
for (int i=0; i < m->nmat; i++) {
fprintf(fp, "\nMATERIAL %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_matadr[i]);
@@ -471,7 +471,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nmat) fprintf(fp, "\n");
// pairs
for (int i=0; i<m->npair; i++) {
for (int i=0; i < m->npair; i++) {
fprintf(fp, "\nPAIR %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_pairadr[i]);
@@ -481,7 +481,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->npair) fprintf(fp, "\n");
// excludes
for (int i=0; i<m->nexclude; i++) {
for (int i=0; i < m->nexclude; i++) {
fprintf(fp, "\nEXCLUDE %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_excludeadr[i]);
@@ -491,7 +491,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nexclude) fprintf(fp, "\n");
// equality constraints
for (int i=0; i<m->neq; i++) {
for (int i=0; i < m->neq; i++) {
fprintf(fp, "\nEQUALITY %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_eqadr[i]);
@@ -501,14 +501,14 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->neq) fprintf(fp, "\n");
// tendons
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
fprintf(fp, "\nTENDON %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_tendonadr[i]);
object_class = &m->ntendon;
MJMODEL_POINTERS
fprintf(fp, " path \n");
for (int j=0; j<m->tendon_num[i]; j++) {
for (int j=0; j < m->tendon_num[i]; j++) {
int k = m->tendon_adr[i]+j;
fprintf(fp, " %d %d ", m->wrap_type[k], m->wrap_objid[k]);
fprintf(fp, float_format, m->wrap_prm[k]);
@@ -519,7 +519,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->ntendon) fprintf(fp, "\n");
// actuators
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
fprintf(fp, "\nACTUATOR %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_actuatoradr[i]);
@@ -529,7 +529,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nu) fprintf(fp, "\n");
// sensors
for (int i=0; i<m->nsensor; i++) {
for (int i=0; i < m->nsensor; i++) {
fprintf(fp, "\nSENSOR %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_sensoradr[i]);
@@ -539,12 +539,12 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nsensor) fprintf(fp, "\n");
// custom numeric parameters
for (int i=0; i<m->nnumeric; i++) {
for (int i=0; i < m->nnumeric; i++) {
fprintf(fp, "\nNUMERIC %d:\n", i);
fprintf(fp, " name %s\n", m->names + m->name_numericadr[i]);
fprintf(fp, " size %d\n", m->numeric_size[i]);
fprintf(fp, " value ");
for (int j=0; j<m->numeric_size[i]; j++) {
for (int j=0; j < m->numeric_size[i]; j++) {
fprintf(fp, float_format, m->numeric_data[m->numeric_adr[i]+j]);
}
fprintf(fp, "\n");
@@ -552,7 +552,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->nnumeric) fprintf(fp, "\n");
// custom text parameters
for (int i=0; i<m->ntext; i++) {
for (int i=0; i < m->ntext; i++) {
fprintf(fp, "\nTEXT %d:\n", i);
fprintf(fp, " name %s\n", m->names + m->name_textadr[i]);
fprintf(fp, " size %d\n", m->text_size[i]);
@@ -561,12 +561,12 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->ntext) fprintf(fp, "\n");
// custom tuple parameters
for (int i=0; i<m->ntuple; i++) {
for (int i=0; i < m->ntuple; i++) {
fprintf(fp, "\nTUPLE %d:\n", i);
fprintf(fp, " name %s\n", m->names + m->name_tupleadr[i]);
fprintf(fp, " size %d\n", m->tuple_size[i]);
fprintf(fp, " elements\n");
for (int j=m->tuple_adr[i]; j<m->tuple_adr[i]+m->tuple_size[i]; j++) {
for (int j=m->tuple_adr[i]; j < m->tuple_adr[i]+m->tuple_size[i]; j++) {
fprintf(fp, " %s %d, prm = ",
mju_type2Str(m->tuple_objtype[j]), m->tuple_objid[j]);
fprintf(fp, float_format, m->tuple_objprm[j]);
@@ -576,58 +576,58 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
if (m->ntuple) fprintf(fp, "\n");
// keyframes (only if different from default)
for (int i=0; i<m->nkey; i++) {
for (int i=0; i < m->nkey; i++) {
// print name
if (m->names[m->name_keyadr[i]]) {
fprintf(fp, "key_name%d %s\n", i, m->names + m->name_keyadr[i]);
}
// print time if non-0
if (m->key_time[i]!=0) {
if (m->key_time[i] != 0) {
fprintf(fp, "key_time%d %.4f\n", i, m->key_time[i]);
}
// check qpos for difference
int k = 0;
for (int j=0; j<m->nq; j++)
for (int j=0; j < m->nq; j++)
if (m->qpos0[j] != m->key_qpos[i*m->nq + j]) {
k = 1;
}
// print if different
if (k==1) {
if (k == 1) {
fprintf(fp, "key_qpos%d ", i);
for (int j=0; j<m->nq; j++) {
for (int j=0; j < m->nq; j++) {
fprintf(fp, float_format, m->key_qpos[i*m->nq + j]);
}
fprintf(fp, "\n");
}
// check qvel for nonzero
for (int j=0; j<m->nv; j++)
for (int j=0; j < m->nv; j++)
if (m->key_qvel[i*m->nv + j]) {
k = 2;
}
// print if nonzero
if (k==2) {
if (k == 2) {
fprintf(fp, "key_qvel%d ", i);
for (int j=0; j<m->nv; j++) {
for (int j=0; j < m->nv; j++) {
fprintf(fp, float_format, m->key_qvel[i*m->nv + j]);
}
fprintf(fp, "\n");
}
// check act for nonzero
for (int j=0; j<m->na; j++)
for (int j=0; j < m->na; j++)
if (m->key_act[i*m->na + j]) {
k = 3;
}
// print if nonzero
if (k==3) {
if (k == 3) {
fprintf(fp, "key_act%d ", i);
for (int j=0; j<m->na; j++) {
for (int j=0; j < m->na; j++) {
fprintf(fp, float_format, m->key_act[i*m->na + j]);
}
fprintf(fp, "\n");
@@ -635,10 +635,10 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
// check mpos for difference
if (m->nmocap) {
for (int j=0; j<m->nbody; j++) {
if (m->body_mocapid[j]>=0) {
for (int j=0; j < m->nbody; j++) {
if (m->body_mocapid[j] >= 0) {
int id = m->body_mocapid[j];
if (m->body_pos[3*j] != m->key_mpos[i*3*m->nmocap + 3*id] ||
if (m->body_pos[3*j] != m->key_mpos[i*3*m->nmocap + 3*id] ||
m->body_pos[3*j+1] != m->key_mpos[i*3*m->nmocap + 3*id+1] ||
m->body_pos[3*j+2] != m->key_mpos[i*3*m->nmocap + 3*id+2]) {
k = 4;
@@ -649,9 +649,9 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
// print if nonzero
if (k==4) {
if (k == 4) {
fprintf(fp, "key_mpos%d ", i);
for (int j=0; j<3*m->nmocap; j++) {
for (int j=0; j < 3*m->nmocap; j++) {
fprintf(fp, float_format, m->key_mpos[i*3*m->nmocap + j]);
}
fprintf(fp, "\n");
@@ -659,10 +659,10 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
// check mquat for difference
if (m->nmocap) {
for (int j=0; j<m->nbody; j++) {
if (m->body_mocapid[j]>=0) {
for (int j=0; j < m->nbody; j++) {
if (m->body_mocapid[j] >= 0) {
int id = m->body_mocapid[j];
if (m->body_quat[4*j] != m->key_mquat[i*4*m->nmocap + 4*id] ||
if (m->body_quat[4*j] != m->key_mquat[i*4*m->nmocap + 4*id] ||
m->body_quat[4*j+1] != m->key_mquat[i*4*m->nmocap + 4*id+1] ||
m->body_quat[4*j+2] != m->key_mquat[i*4*m->nmocap + 4*id+2] ||
m->body_quat[4*j+3] != m->key_mquat[i*4*m->nmocap + 4*id+3]) {
@@ -674,16 +674,16 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
// print if nonzero
if (k==5) {
if (k == 5) {
fprintf(fp, "key_mquat%d ", i);
for (int j=0; j<4*m->nmocap; j++) {
for (int j=0; j < 4*m->nmocap; j++) {
fprintf(fp, float_format, m->key_mquat[i*4*m->nmocap + j]);
}
fprintf(fp, "\n");
}
// check ctrl for nonzero
for (int j=0; j<m->nu; j++) {
for (int j=0; j < m->nu; j++) {
if (m->key_ctrl[i*m->nu + j]) {
k = 6;
break;
@@ -691,9 +691,9 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
// print if nonzero
if (k==6) {
if (k == 6) {
fprintf(fp, "key_ctrl%d ", i);
for (int j=0; j<m->nu; j++) {
for (int j=0; j < m->nu; j++) {
fprintf(fp, float_format, m->key_ctrl[i*m->nu + j]);
}
fprintf(fp, "\n");
@@ -790,12 +790,12 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
// WARNING
int active_warnings = 0;
for (int i=0; i<mjNWARNING; i++) {
for (int i=0; i < mjNWARNING; i++) {
active_warnings += d->warning[i].number;
}
if (active_warnings) {
fprintf(fp, "WARNING\n");
for (int i=0; i<mjNWARNING; i++)
for (int i=0; i < mjNWARNING; i++)
if (d->warning[i].number)
fprintf(fp, " %d: lastinfo = %d number = %d\n",
i, d->warning[i].lastinfo, d->warning[i].number);
@@ -804,12 +804,12 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
// TIMER
mjtNum active_timers = 0;
for (int i=0; i<mjNTIMER; i++) {
for (int i=0; i < mjNTIMER; i++) {
active_timers += d->timer[i].duration;
}
if (active_timers) {
fprintf(fp, "TIMER\n");
for (int i=0; i<mjNTIMER; i++) {
for (int i=0; i < mjNTIMER; i++) {
fprintf(fp, " %d: duration = ", i);
fprintf(fp, float_format, d->timer[i].duration);
fprintf(fp, " number = %d\n", d->timer[i].number);
@@ -822,7 +822,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
fprintf(fp, "SOLVER STAT\n");
fprintf(fp, " solver_iter = %d\n", d->solver_iter);
fprintf(fp, " solver_nnz = %d\n", d->solver_nnz);
for (int i=0; i<mjMIN(mjNSOLVER, d->solver_iter); i++) {
for (int i=0; i < mjMIN(mjNSOLVER, d->solver_iter); i++) {
fprintf(fp, " %d: improvement = ", i);
fprintf(fp, float_format, d->solver[i].improvement);
fprintf(fp, " gradient = ");
@@ -886,11 +886,11 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArrayInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp);
printArrayInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp);
printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz,
d->ten_J_rowadr, d->ten_J_colind, fp, float_format);
d->ten_J_rowadr, d->ten_J_colind, fp, float_format);
}
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
fprintf(fp, "TENDON %d: %d wrap points\n", i, d->ten_wrapnum[i]);
for (int j=0; j<d->ten_wrapnum[i]; j++) {
for (int j=0; j < d->ten_wrapnum[i]; j++) {
fprintf(fp, " %d: ", d->wrap_obj[d->ten_wrapadr[i]+j]);
printVector("", d->wrap_xpos+3*(d->ten_wrapadr[i]+j), 3, fp, float_format);
}
@@ -965,7 +965,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
// contact
fprintf(fp, "CONTACT\n");
for (int i=0; i<d->ncon; i++) {
for (int i=0; i < d->ncon; i++) {
fprintf(fp, " %d:\n dim %d\n geom ", i, d->contact[i].dim);
const char* geom1 = mj_id2name(m, mjOBJ_GEOM, d->contact[i].geom1);
if (geom1) {
@@ -1066,9 +1066,9 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
}
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#ifdef __clang__
#pragma clang diagnostic pop
#endif
// print mjData to text file
src/engine/engine_ray.c
+109 -109
View File
@@ -67,22 +67,22 @@ static mjtNum latitude(const mjtNum vec[3]) {
static int ray_eliminate(const mjModel* m, const mjData* d, int geomid,
const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude) {
// body exclusion
if (m->geom_bodyid[geomid]==bodyexclude) {
if (m->geom_bodyid[geomid] == bodyexclude) {
return 1;
}
// invisible geom exclusion
if (m->geom_matid[geomid]<0 && m->geom_rgba[4*geomid+3]==0) {
if (m->geom_matid[geomid] < 0 && m->geom_rgba[4*geomid+3] == 0) {
return 1;
}
// invisible material exclusion
if (m->geom_matid[geomid]>=0 && m->mat_rgba[4*m->geom_matid[geomid]+3]==0) {
if (m->geom_matid[geomid] >= 0 && m->mat_rgba[4*m->geom_matid[geomid]+3] == 0) {
return 1;
}
// static exclusion
if (!flg_static && m->body_weldid[m->geom_bodyid[geomid]]==0) {
if (!flg_static && m->body_weldid[m->geom_bodyid[geomid]] == 0) {
return 1;
}
@@ -94,7 +94,7 @@ static int ray_eliminate(const mjModel* m, const mjData* d, int geomid,
// group inclusion/exclusion
int groupid = mjMIN(mjNGROUP-1, mjMAX(0, m->geom_group[geomid]));
return (geomgroup[groupid]==0);
return (geomgroup[groupid] == 0);
}
@@ -103,7 +103,7 @@ static int ray_eliminate(const mjModel* m, const mjData* d, int geomid,
static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum* x) {
// compute determinant and check
mjtNum det = b*b - a*c;
if (det<mjMINVAL) {
if (det < mjMINVAL) {
x[0] = -1;
x[1] = -1;
return -1;
@@ -115,9 +115,9 @@ static mjtNum ray_quad(mjtNum a, mjtNum b, mjtNum c, mjtNum* x) {
x[1] = (-b+det)/a;
// finalize result
if (x[0]>=0) {
if (x[0] >= 0) {
return x[0];
} else if (x[1]>=0) {
} else if (x[1] >= 0) {
return x[1];
} else {
return -1;
@@ -131,24 +131,24 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
const mjtNum* b0, const mjtNum* b1) {
// dif = v[i] - lpnt
mjtNum dif[3][3];
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++) {
dif[i][j] = v[i][j] - lpnt[j];
}
}
// project difference vectors in normal plane
mjtNum planar[3][2];
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
planar[i][0] = mju_dot3(b0, dif[i]);
planar[i][1] = mju_dot3(b1, dif[i]);
}
// reject if on the same side of any coordinate axis
if ((planar[0][0]>0 && planar[1][0]>0 && planar[2][0]>0) ||
(planar[0][0]<0 && planar[1][0]<0 && planar[2][0]<0) ||
(planar[0][1]>0 && planar[1][1]>0 && planar[2][1]>0) ||
(planar[0][1]<0 && planar[1][1]<0 && planar[2][1]<0)) {
if ((planar[0][0] > 0 && planar[1][0] > 0 && planar[2][0] > 0) ||
(planar[0][0] < 0 && planar[1][0] < 0 && planar[2][0] < 0) ||
(planar[0][1] > 0 && planar[1][1] > 0 && planar[2][1] > 0) ||
(planar[0][1] < 0 && planar[1][1] < 0 && planar[2][1] < 0)) {
return -1;
}
@@ -158,14 +158,14 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
planar[0][1]-planar[2][1], planar[1][1]-planar[2][1]};
mjtNum b[2] = {-planar[2][0], -planar[2][1]};
mjtNum det = A[0]*A[3] - A[1]*A[2];
if (mju_abs(det)<mjMINVAL) {
if (mju_abs(det) < mjMINVAL) {
return -1;
}
mjtNum t0 = (A[3]*b[0] - A[1]*b[1]) / det;
mjtNum t1 = (-A[2]*b[0] + A[0]*b[1]) / det;
// check if outside
if (t0<0 || t1<0|| t0+t1>1) {
if (t0 < 0 || t1 < 0|| t0+t1 > 1) {
return -1;
}
@@ -176,7 +176,7 @@ mjtNum ray_triangle(mjtNum v[][3], const mjtNum* lpnt, const mjtNum* lvec,
mjtNum nrm[3];
mju_cross(nrm, dif[0], dif[1]); // normal to triangle plane
mjtNum denom = mju_dot3(lvec, nrm);
if (mju_abs(denom)<mjMINVAL) {
if (mju_abs(denom) < mjMINVAL) {
return -1;
}
@@ -193,21 +193,21 @@ static mjtNum ray_plane(const mjtNum* pos, const mjtNum* mat, const mjtNum* size
ray_map(pos, mat, pnt, vec, lpnt, lvec);
// z-vec not pointing towards front face: reject
if (lvec[2]>-mjMINVAL) {
if (lvec[2] > -mjMINVAL) {
return -1;
}
// intersection with plane
const mjtNum x = -lpnt[2]/lvec[2];
if (x<0) {
if (x < 0) {
return -1;
}
mjtNum p0 = lpnt[0] + x*lvec[0];
mjtNum p1 = lpnt[1] + x*lvec[1];
// accept only within rendered rectangle
if ((size[0]<=0 || mju_abs(p0)<=size[0]) &&
(size[1]<=0 || mju_abs(p1)<=size[1])) {
if ((size[0] <= 0 || mju_abs(p0) <= size[0]) &&
(size[1] <= 0 || mju_abs(p1) <= size[1])) {
return x;
} else {
return -1;
@@ -237,7 +237,7 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si
const mjtNum* pnt, const mjtNum* vec) {
// bounding sphere test
mjtNum ssz = size[0] + size[1];
if (ray_sphere(pos, NULL, ssz*ssz, pnt, vec)<0) {
if (ray_sphere(pos, NULL, ssz*ssz, pnt, vec) < 0) {
return -1;
}
@@ -257,8 +257,8 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si
sol = ray_quad(a, b, c, xx);
// make sure round solution is between flat sides
if (sol>=0 && mju_abs(lpnt[2]+sol*lvec[2])<=size[1]) {
if (x<0 || sol<x) {
if (sol >= 0 && mju_abs(lpnt[2]+sol*lvec[2]) <= size[1]) {
if (x < 0 || sol < x) {
x = sol;
}
}
@@ -271,9 +271,9 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si
ray_quad(a, b, c, xx);
// accept only top half of sphere
for (int i=0; i<2; i++) {
if (xx[i]>=0 && lpnt[2]+xx[i]*lvec[2]>=size[1]) {
if (x<0 || xx[i]<x) {
for (int i=0; i < 2; i++) {
if (xx[i] >= 0 && lpnt[2]+xx[i]*lvec[2] >= size[1]) {
if (x < 0 || xx[i] < x) {
x = xx[i];
}
}
@@ -286,9 +286,9 @@ static mjtNum ray_capsule(const mjtNum* pos, const mjtNum* mat, const mjtNum* si
ray_quad(a, b, c, xx);
// accept only bottom half of sphere
for (int i=0; i<2; i++) {
if (xx[i]>=0 && lpnt[2]+xx[i]*lvec[2]<=-size[1]) {
if (x<0 || xx[i]<x) {
for (int i=0; i < 2; i++) {
if (xx[i] >= 0 && lpnt[2]+xx[i]*lvec[2] <= -size[1]) {
if (x < 0 || xx[i] < x) {
x = xx[i];
}
}
@@ -326,7 +326,7 @@ static mjtNum ray_cylinder(const mjtNum* pos, const mjtNum* mat, const mjtNum* s
const mjtNum* pnt, const mjtNum* vec) {
// bounding sphere test
mjtNum ssz = size[0]*size[0] + size[1]*size[1];
if (ray_sphere(pos, NULL, ssz, pnt, vec)<0) {
if (ray_sphere(pos, NULL, ssz, pnt, vec) < 0) {
return -1;
}
@@ -339,20 +339,20 @@ static mjtNum ray_cylinder(const mjtNum* pos, const mjtNum* mat, const mjtNum* s
// flat sides
int side;
if (mju_abs(lvec[2])>mjMINVAL) {
for (side=-1; side<=1; side+=2) {
if (mju_abs(lvec[2]) > mjMINVAL) {
for (side=-1; side <= 1; side+=2) {
// soludion of: lpnt[2] + x*lvec[2] = side*height_size
sol = (side*size[1]-lpnt[2])/lvec[2];
// process if non-negative
if (sol>=0) {
if (sol >= 0) {
// intersection with horizontal face
mjtNum p0 = lpnt[0] + sol*lvec[0];
mjtNum p1 = lpnt[1] + sol*lvec[1];
// accept within radius
if (p0*p0 + p1*p1 <= size[0]*size[0]) {
if (x<0 || sol<x) {
if (x < 0 || sol < x) {
x = sol;
}
}
@@ -370,8 +370,8 @@ static mjtNum ray_cylinder(const mjtNum* pos, const mjtNum* mat, const mjtNum* s
sol = ray_quad(a, b, c, xx);
// make sure round solution is between flat sides
if (sol>=0 && mju_abs(lpnt[2]+sol*lvec[2])<=size[1]) {
if (x<0 || sol<x) {
if (sol >= 0 && mju_abs(lpnt[2]+sol*lvec[2]) <= size[1]) {
if (x < 0 || sol < x) {
x = sol;
}
}
@@ -386,14 +386,14 @@ static mjtNum ray_box(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
const mjtNum* pnt, const mjtNum* vec, mjtNum* all) {
// clear all
if (all) {
for (int i=0; i<6; i++) {
for (int i=0; i < 6; i++) {
all[i] = -1;
}
}
// bounding sphere test
mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2];
if (ray_sphere(pos, NULL, ssz, pnt, vec)<0) {
if (ray_sphere(pos, NULL, ssz, pnt, vec) < 0) {
return -1;
}
@@ -412,23 +412,23 @@ static mjtNum ray_box(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
mjtNum x = -1, sol;
// loop over axes with non-zero vec
for (int i=0; i<3; i++) {
if (mju_abs(lvec[i])>mjMINVAL) {
for (int side=-1; side<=1; side+=2) {
for (int i=0; i < 3; i++) {
if (mju_abs(lvec[i]) > mjMINVAL) {
for (int side=-1; side <= 1; side+=2) {
// soludion of: lpnt[i] + x*lvec[i] = side*size[i]
sol = (side*size[i]-lpnt[i])/lvec[i];
// process if non-negative
if (sol>=0) {
if (sol >= 0) {
// intersection with face
mjtNum p0 = lpnt[iface[i][0]] + sol*lvec[iface[i][0]];
mjtNum p1 = lpnt[iface[i][1]] + sol*lvec[iface[i][1]];
// accept within rectangle
if (mju_abs(p0)<=size[iface[i][0]] &&
mju_abs(p1)<=size[iface[i][1]]) {
if (mju_abs(p0) <= size[iface[i][0]] &&
mju_abs(p1) <= size[iface[i][1]]) {
// update
if (x<0 || sol<x) {
if (x < 0 || sol < x) {
x = sol;
}
@@ -451,7 +451,7 @@ static mjtNum ray_box(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
const mjtNum* pnt, const mjtNum* vec) {
// check geom type
if (m->geom_type[id]!=mjGEOM_HFIELD) {
if (m->geom_type[id] != mjGEOM_HFIELD) {
mju_error("mj_rayHfield: geom with hfield type expected");
}
@@ -484,7 +484,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
// check top box: done if no intersection
mjtNum all[6];
mjtNum top_intersect = ray_box(top_pos, d->geom_xmat+9*id, top_size, pnt, vec, all);
if (top_intersect<0) {
if (top_intersect < 0) {
return x;
}
@@ -494,9 +494,9 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
if (mju_abs(lvec[0])>=mju_abs(lvec[1]) && mju_abs(lvec[0])>=mju_abs(lvec[2])) {
if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) && mju_abs(lvec[0]) >= mju_abs(lvec[2])) {
b0[0] = 0;
} else if (mju_abs(lvec[1])>=mju_abs(lvec[2])) {
} else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) {
b0[1] = 0;
} else {
b0[2] = 0;
@@ -508,8 +508,8 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
// find ray segment intersecting top box
mjtNum seg[2] = {0, top_intersect};
for (int i=0; i<6; i++) {
if (all[i]>seg[1]) {
for (int i=0; i < 6; i++) {
if (all[i] > seg[1]) {
seg[0] = top_intersect;
seg[1] = all[i];
}
@@ -519,7 +519,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
mjtNum dx = (2.0*size[0]) / (ncol-1);
mjtNum dy = (2.0*size[1]) / (nrow-1);
mjtNum SX[2], SY[2];
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
SX[i] = (lpnt[0] + seg[i]*lvec[0] + size[0]) / dx;
SY[i] = (lpnt[1] + seg[i]*lvec[1] + size[1]) / dy;
}
@@ -531,8 +531,8 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
int rmax = mjMIN(nrow-1, (int)mju_ceil(mjMAX(SY[0], SY[1]))+1);
// check triangles within bounds
for (int r=rmin; r<rmax; r++) {
for (int c=cmin; c<cmax; c++) {
for (int r=rmin; r < rmax; r++) {
for (int c=cmin; c < cmax; c++) {
// first triangle
mjtNum va[3][3] = {
{dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},
@@ -540,7 +540,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
{dx*(c+1)-size[0], dy*r-size[1], data[r*ncol+(c+1)]*size[2]}
};
mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1);
if (sol>=0 && (x<0 || sol<x)) {
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
}
@@ -551,15 +551,15 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
{dx*c-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+c]*size[2]}
};
sol = ray_triangle(vb, lpnt, lvec, b0, b1);
if (sol>=0 && (x<0 || sol<x)) {
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
}
}
}
// check viable sides of top box
for (int i=0; i<4; i++) {
if (all[i]>=0 && (all[i]<x || x<0)) {
for (int i=0; i < 4; i++) {
if (all[i] >= 0 && (all[i] < x || x < 0)) {
// normalized height of intersection point
mjtNum z = (lpnt[2] + all[i]*lvec[2]) / size[2];
@@ -567,19 +567,19 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
mjtNum y, y0, z0, z1;
// side normal to x-axis
if (i<2) {
if (i < 2) {
y = (lpnt[1] + all[i]*lvec[1] + size[1]) / dy;
y0 = mjMAX(0, mjMIN(nrow-2, mju_floor(y)));
z0 = (mjtNum)data[mju_round(y0)*nrow + (i==1 ? ncol-1 : 0)];
z1 = (mjtNum)data[mju_round(y0+1)*nrow + (i==1 ? ncol-1 : 0)];
z0 = (mjtNum)data[mju_round(y0)*nrow + (i == 1 ? ncol-1 : 0)];
z1 = (mjtNum)data[mju_round(y0+1)*nrow + (i == 1 ? ncol-1 : 0)];
}
// side normal to y-axis
else {
y = (lpnt[0] + all[i]*lvec[0] + size[0]) / dx;
y0 = mjMAX(0, mjMIN(ncol-2, mju_floor(y)));
z0 = (mjtNum)data[mju_round(y0) + (i==3 ? (nrow-1)*ncol : 0)];
z1 = (mjtNum)data[mju_round(y0+1) + (i==3 ? (nrow-1)*ncol : 0)];
z0 = (mjtNum)data[mju_round(y0) + (i == 3 ? (nrow-1)*ncol : 0)];
z1 = (mjtNum)data[mju_round(y0+1) + (i == 3 ? (nrow-1)*ncol : 0)];
}
// check if point is below line segment
@@ -631,7 +631,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
if (meshid==-1) {
if (meshid == -1) {
mju_error("mju_rayTree: mesh id of geom %d is -1", meshid); // SHOULD NOT OCCUR
}
@@ -647,9 +647,9 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
if (mju_abs(lvec[0])>=mju_abs(lvec[1]) && mju_abs(lvec[0])>=mju_abs(lvec[2])) {
if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) && mju_abs(lvec[0]) >= mju_abs(lvec[2])) {
b0[0] = 0;
} else if (mju_abs(lvec[1])>=mju_abs(lvec[2])) {
} else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) {
b0[1] = 0;
} else {
b0[2] = 0;
@@ -687,8 +687,8 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
// convert to mjtNum
mjtNum v[3][3];
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++) {
v[i][j] = (mjtNum)vf[i][j];
}
}
@@ -697,7 +697,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
sol = ray_triangle(v, lpnt, lvec, b0, b1);
// update
if (sol>=0 && (x<0 || sol<x)) {
if (sol >= 0 && (x < 0 || sol < x)) {
x = sol;
}
continue;
@@ -707,7 +707,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
d->bvh_active[node + bvhadr] = 1;
// add children to the stack
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
if (child[2*node+i] != -1) {
if (nstack >= mjMAXTREEDEPTH) mju_error("BVH stack depth exceeded in geom %d.", id);
stack[nstack] = child[2*node+i];
@@ -723,12 +723,12 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
const mjtNum* pnt, const mjtNum* vec) {
// check geom type
if (m->geom_type[id]!=mjGEOM_MESH) {
if (m->geom_type[id] != mjGEOM_MESH) {
mju_error("mj_rayMesh: geom with mesh type expected");
}
// bounding box test
if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL)<0) {
if (ray_box(d->geom_xpos+3*id, d->geom_xmat+9*id, m->geom_size+3*id, pnt, vec, NULL) < 0) {
return -1;
}
@@ -772,15 +772,15 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]) {
// compute bounding box
mjtNum box[3][2] = {{0, 0}, {0, 0}, {0, 0}};
for (int i=0; i<nvert; i++) {
for (int j=0; j<3; j++) {
for (int i=0; i < nvert; 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];
}
}
@@ -788,21 +788,21 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
// 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;
@@ -816,7 +816,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
mjtNum x = -1, sol;
// process all faces
for (int i=0; i<nface; i++) {
for (int i=0; i < nface; i++) {
// get float vertices
const float* vf[3];
vf[0] = vert + 3*(face[3*i]);
@@ -825,8 +825,8 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
// convert to mjtNum
mjtNum v[3][3];
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
for (int j=0; j < 3; j++) {
for (int k=0; k < 3; k++) {
v[j][k] = (mjtNum)vf[j][k];
}
}
@@ -835,7 +835,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
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
@@ -845,9 +845,9 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
// find nearest vertex
mjtNum dist = mju_dist3(intersect, v[0]);
*vertid = face[3*i];
for (int j=1; j<3; j++) {
for (int j=1; j < 3; j++) {
mjtNum newdist = mju_dist3(intersect, v[j]);
if (newdist<dist) {
if (newdist < dist) {
dist = newdist;
*vertid = face[3*i+j];
}
@@ -871,7 +871,7 @@ static int point_in_box(const mjtNum aabb[6], const mjtNum xpos[3],
mju_subFrom3(point, aabb);
// check intersections
for (int j=0; j<3; j++) { // directions
for (int j=0; j < 3; j++) { // directions
if (mju_abs(point[j]) > aabb[3+j]) {
return 0;
}
@@ -893,7 +893,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
mjtNum dist, newdist;
// check vector length
if (mju_norm3(vec)<mjMINVAL) {
if (mju_norm3(vec) < mjMINVAL) {
mju_error("mj_ray: vector length is too small");
}
@@ -902,12 +902,12 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
*geomid = -1;
// loop over geoms not eliminated by mask and bodyexclude
for (int i=0; i<m->ngeom; i++) {
for (int i=0; i < m->ngeom; i++) {
if (!ray_eliminate(m, d, i, geomgroup, flg_static, bodyexclude)) {
// handle mesh and hfield separately
if (m->geom_type[i]==mjGEOM_MESH) {
if (m->geom_type[i] == mjGEOM_MESH) {
newdist = mj_rayMesh(m, d, i, pnt, vec);
} else if (m->geom_type[i]==mjGEOM_HFIELD) {
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
newdist = mj_rayHfield(m, d, i, pnt, vec);
}
@@ -918,7 +918,7 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
}
// update if closer intersection found
if (newdist>=0 && (newdist<dist || dist<0)) {
if (newdist >= 0 && (newdist < dist || dist < 0)) {
dist = newdist;
*geomid = i;
}
@@ -938,17 +938,17 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
}
// compute eliminate flag for all geoms
for (int geomid=0; geomid<m->ngeom; geomid++)
for (int geomid=0; geomid < m->ngeom; geomid++)
geom_eliminate[geomid] = ray_eliminate(m, d, geomid, geomgroup, flg_static, bodyexclude);
for (int b=0; b<m->nbody; b++) {
for (int b=0; b < m->nbody; b++) {
// skip precomputation if no bounding volume is available
if (m->body_bvhadr[b] == -1) {
continue;
}
// loop over child geoms, compute bounding angles
for (int i=0; i<m->body_geomnum[b]; i++) {
for (int i=0; i < m->body_geomnum[b]; i++) {
int g = i + m->body_geomadr[b];
mjtNum AABB[4] = {mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum* aabb = m->geom_aabb + 6*g;
@@ -961,7 +961,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
}
// add to geom_eliminate if distance of bounding sphere is above cutoff
if (mju_dist3(d->geom_xpos+3*g, pnt)>cutoff+m->geom_rbound[g]) {
if (mju_dist3(d->geom_xpos+3*g, pnt) > cutoff+m->geom_rbound[g]) {
geom_eliminate[g] = 1;
continue;
}
@@ -975,7 +975,7 @@ void mju_multiRayPrepare(const mjModel* m, const mjData* d, const mjtNum pnt[3],
}
// loop over box vertices, compute spherical aperture
for (int v=0; v<8; v++) {
for (int v=0; v < 8; v++) {
mjtNum vert[3], box[3];
vert[0] = (v&1 ? aabb[0]+aabb[3] : aabb[0]-aabb[3]);
vert[1] = (v&2 ? aabb[1]+aabb[4] : aabb[1]-aabb[4]);
@@ -1020,7 +1020,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
mjtNum dist, newdist;
// check vector length
if (mju_norm3(vec)<mjMINVAL) {
if (mju_norm3(vec) < mjMINVAL) {
mju_error("mj_ray: vector length is too small");
}
@@ -1033,7 +1033,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
mjtNum elevation = latitude(vec);
// loop over bodies not eliminated by bodyexclude
for (int b=0; b<m->nbody; b++) {
for (int b=0; b < m->nbody; b++) {
// exclude body using bounding sphere test
if (m->body_bvhadr[b] != -1) {
mjtNum* pos = m->bvh_aabb + 6*m->body_bvhadr[b];
@@ -1041,13 +1041,13 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
mjtNum* size = pos + 3;
mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2];
mju_add3(center, pos, d->xipos+3*b);
if (ray_sphere(center, NULL, ssz, pnt, vec)<0) {
if (ray_sphere(center, NULL, ssz, pnt, vec) < 0) {
continue;
}
}
// loop over geoms if bounding sphere test fails
for (int g=0; g<m->body_geomnum[b]; g++) {
for (int g=0; g < m->body_geomnum[b]; g++) {
int i = m->body_geomadr[b] + g;
if (ray_eliminate[i]) {
continue;
@@ -1055,16 +1055,16 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
// exclude geom using bounding angles
if (m->body_bvhadr[b] != -1) {
if (azimuth<(geom_ba+4*i)[0] || elevation<(geom_ba+4*i)[1] ||
azimuth>(geom_ba+4*i)[2] || elevation>(geom_ba+4*i)[3]) {
if (azimuth < (geom_ba+4*i)[0] || elevation < (geom_ba+4*i)[1] ||
azimuth > (geom_ba+4*i)[2] || elevation > (geom_ba+4*i)[3]) {
continue;
}
}
// handle mesh and hfield separately
if (m->geom_type[i]==mjGEOM_MESH) {
if (m->geom_type[i] == mjGEOM_MESH) {
newdist = mj_rayMesh(m, d, i, pnt, vec);
} else if (m->geom_type[i]==mjGEOM_HFIELD) {
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
newdist = mj_rayHfield(m, d, i, pnt, vec);
}
@@ -1075,7 +1075,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
}
// update if closer intersection found
if (newdist>=0 && (newdist<dist || dist<0)) {
if (newdist >= 0 && (newdist < dist || dist < 0)) {
dist = newdist;
*geomid = i;
}
@@ -1101,7 +1101,7 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum*
cutoff, geom_ba, geom_eliminate);
// loop over rays
for (int i=0; i<nray; i++) {
for (int i=0; i < nray; i++) {
dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i);
}
src/engine/engine_sensor.c
+74 -74
View File
@@ -41,24 +41,24 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
mjtNum rnd[4], noise, quat[4], res[4];
// process sensors matching stage and having positive noise
for (int i=0; i<m->nsensor; i++) {
if (m->sensor_needstage[i]==stage && m->sensor_noise[i]>0) {
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_needstage[i] == stage && m->sensor_noise[i] > 0) {
// get sensor info
adr = m->sensor_adr[i];
dim = m->sensor_dim[i];
noise = m->sensor_noise[i];
// real or positive: add noise directly, with clamp for positive
if (m->sensor_datatype[i]==mjDATATYPE_REAL ||
m->sensor_datatype[i]==mjDATATYPE_POSITIVE) {
for (int j=0; j<dim; j++) {
if (m->sensor_datatype[i] == mjDATATYPE_REAL ||
m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
for (int j=0; j < dim; j++) {
// get random numbers; use only the first one
rnd[0] = mju_standardNormal(rnd+1);
// positive
if (m->sensor_datatype[i]==mjDATATYPE_POSITIVE) {
if (m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
// add noise only if positive, keep it positive
if (d->sensordata[adr+j]>0) {
if (d->sensordata[adr+j] > 0) {
d->sensordata[adr+j] = mjMAX(0, d->sensordata[adr+j]+rnd[0]*noise);
}
}
@@ -82,14 +82,14 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
mju_axisAngle2Quat(quat, rnd+1, rnd[0]);
// axis
if (m->sensor_datatype[i]==mjDATATYPE_AXIS) {
if (m->sensor_datatype[i] == mjDATATYPE_AXIS) {
// apply quaternion rotation to axis, assign
mju_rotVecQuat(res, d->sensordata+adr, quat);
mju_copy3(d->sensordata+adr, res);
}
// quaternion
else if (m->sensor_datatype[i]==mjDATATYPE_QUATERNION) {
else if (m->sensor_datatype[i] == mjDATATYPE_QUATERNION) {
// apply quaternion rotation to quaternion, assign
mju_mulQuat(d->sensordata+adr, d->sensordata+adr, quat);
}
@@ -108,22 +108,22 @@ static void add_noise(const mjModel* m, mjData* d, mjtStage stage) {
// apply cutoff after each stage
static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// process sensors matching stage and having positive cutoff
for (int i=0; i<m->nsensor; i++) {
if (m->sensor_needstage[i]==stage && m->sensor_cutoff[i]>0) {
for (int i=0; i < m->nsensor; i++) {
if (m->sensor_needstage[i] == stage && m->sensor_cutoff[i] > 0) {
// get sensor info
int adr = m->sensor_adr[i];
int dim = m->sensor_dim[i];
mjtNum cutoff = m->sensor_cutoff[i];
// process all dimensions
for (int j=0; j<dim; j++) {
for (int j=0; j < dim; j++) {
// real: apply on both sides
if (m->sensor_datatype[i]==mjDATATYPE_REAL) {
if (m->sensor_datatype[i] == mjDATATYPE_REAL) {
d->sensordata[adr+j] = mju_clip(d->sensordata[adr+j], -cutoff, cutoff);
}
// positive: apply on positive side only
else if (m->sensor_datatype[i]==mjDATATYPE_POSITIVE) {
else if (m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
d->sensordata[adr+j] = mju_min(cutoff, d->sensordata[adr+j]);
}
}
@@ -201,13 +201,13 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
}
// process sensors matching stage
for (int i=0; i<m->nsensor; i++) {
for (int i=0; i < m->nsensor; i++) {
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i]==mjSTAGE_POS) {
if (m->sensor_needstage[i] == mjSTAGE_POS) {
// get sensor info
objtype = m->sensor_objtype[i];
objid = m->sensor_objid[i];
@@ -247,8 +247,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
case mjSENS_JOINTLIMITPOS: // jointlimitpos
d->sensordata[adr] = 0;
for (int j=ne+nf; j<nefc; j++) {
if (d->efc_type[j]==mjCNSTR_LIMIT_JOINT && d->efc_id[j]==objid) {
for (int j=ne+nf; j < nefc; j++) {
if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) {
d->sensordata[adr] = d->efc_pos[j] - d->efc_margin[j];
break;
}
@@ -257,8 +257,8 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
case mjSENS_TENDONLIMITPOS: // tendonlimitpos
d->sensordata[adr] = 0;
for (int j=ne+nf; j<nefc; j++) {
if (d->efc_type[j]==mjCNSTR_LIMIT_TENDON && d->efc_id[j]==objid) {
for (int j=ne+nf; j < nefc; j++) {
if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) {
d->sensordata[adr] = d->efc_pos[j] - d->efc_margin[j];
break;
}
@@ -274,7 +274,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// reference frame unspecified: global frame
if (refid == -1) {
if (m->sensor_type[i]==mjSENS_FRAMEPOS) {
if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
mju_copy3(d->sensordata+adr, xpos);
} else {
// offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
@@ -288,7 +288,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// reference frame specified
else {
get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
if (m->sensor_type[i]==mjSENS_FRAMEPOS) {
if (m->sensor_type[i] == mjSENS_FRAMEPOS) {
mju_sub3(rvec, xpos, xpos_ref);
mju_rotVecMatT(d->sensordata+adr, rvec, xmat_ref);
} else {
@@ -301,25 +301,25 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
break;
case mjSENS_FRAMEQUAT: // framequat
{
// get global object quaternion
mjtNum objquat[4];
get_xquat(m, d, objtype, objid, i, objquat);
{
// get global object quaternion
mjtNum objquat[4];
get_xquat(m, d, objtype, objid, i, objquat);
// reference frame unspecified: copy object quaternion
if (refid == -1) {
mju_copy4(d->sensordata+adr, objquat);
} else {
// reference frame specified, get global reference quaternion
mjtNum refquat[4];
get_xquat(m, d, reftype, refid, i, refquat);
// reference frame unspecified: copy object quaternion
if (refid == -1) {
mju_copy4(d->sensordata+adr, objquat);
} else {
// reference frame specified, get global reference quaternion
mjtNum refquat[4];
get_xquat(m, d, reftype, refid, i, refquat);
// relative quaternion
mju_negQuat(refquat, refquat);
mju_mulQuat(d->sensordata+adr, refquat, objquat);
}
// relative quaternion
mju_negQuat(refquat, refquat);
mju_mulQuat(d->sensordata+adr, refquat, objquat);
}
break;
}
break;
case mjSENS_SUBTREECOM: // subtreecom
mju_copy3(d->sensordata+adr, d->subtree_com+3*objid);
@@ -352,14 +352,14 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// compute plugin sensor values
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
for (int i=0; i < m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error("invalid plugin slot: %d", slot);
}
if ((plugin->capabilityflags & mjPLUGIN_SENSOR) &&
(plugin->needstage==mjSTAGE_POS || plugin->needstage==mjSTAGE_NONE)) {
(plugin->needstage == mjSTAGE_POS || plugin->needstage == mjSTAGE_NONE)) {
if (!plugin->compute) {
mju_error("`compute` is a null function pointer for plugin at slot %d", slot);
}
@@ -387,13 +387,13 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
// process sensors matching stage
int subtreeVel = 0;
for (int i=0; i<m->nsensor; i++) {
for (int i=0; i < m->nsensor; i++) {
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i]==mjSTAGE_VEL) {
if (m->sensor_needstage[i] == mjSTAGE_VEL) {
// get sensor info
type = m->sensor_type[i];
objtype = m->sensor_objtype[i];
@@ -403,10 +403,10 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
adr = m->sensor_adr[i];
// call mj_subtreeVel when first relevant sensor is encountered
if (subtreeVel==0 &&
(type==mjSENS_SUBTREELINVEL ||
type==mjSENS_SUBTREEANGMOM ||
type==mjSENS_USER)) {
if (subtreeVel == 0 &&
(type == mjSENS_SUBTREELINVEL ||
type == mjSENS_SUBTREEANGMOM ||
type == mjSENS_USER)) {
// compute subtree_linvel, subtree_angmom
mj_subtreeVel(m, d);
@@ -450,8 +450,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
case mjSENS_JOINTLIMITVEL: // jointlimitvel
d->sensordata[adr] = 0;
for (int j=ne+nf; j<nefc; j++) {
if (d->efc_type[j]==mjCNSTR_LIMIT_JOINT && d->efc_id[j]==objid) {
for (int j=ne+nf; j < nefc; j++) {
if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) {
d->sensordata[adr] = d->efc_vel[j];
break;
}
@@ -460,8 +460,8 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
case mjSENS_TENDONLIMITVEL: // tendonlimitvel
d->sensordata[adr] = 0;
for (int j=ne+nf; j<nefc; j++) {
if (d->efc_type[j]==mjCNSTR_LIMIT_TENDON && d->efc_id[j]==objid) {
for (int j=ne+nf; j < nefc; j++) {
if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) {
d->sensordata[adr] = d->efc_vel[j];
break;
}
@@ -495,7 +495,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
}
// copy linear or angular component
if (m->sensor_type[i]==mjSENS_FRAMELINVEL) {
if (m->sensor_type[i] == mjSENS_FRAMELINVEL) {
mju_copy3(d->sensordata+adr, xvel+3);
} else {
mju_copy3(d->sensordata+adr, xvel);
@@ -533,13 +533,13 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
// trigger computation of plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
for (int i=0; i < m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error("invalid plugin slot: %d", slot);
}
if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage==mjSTAGE_VEL) {
if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_VEL) {
if (!plugin->compute) {
mju_error("`compute` is null for plugin at slot %d", slot);
}
@@ -576,13 +576,13 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// process sensors matching stage
int rnePost = 0;
for (int i=0; i<m->nsensor; i++) {
for (int i=0; i < m->nsensor; i++) {
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i]==mjSTAGE_ACC) {
if (m->sensor_needstage[i] == mjSTAGE_ACC) {
// get sensor info
type = m->sensor_type[i];
objtype = m->sensor_objtype[i];
@@ -590,11 +590,11 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
adr = m->sensor_adr[i];
// call mj_rnePostConstraint when first relevant sensor is encountered
if (rnePost==0 &&
type!=mjSENS_TOUCH &&
type!=mjSENS_ACTUATORFRC &&
type!=mjSENS_JOINTLIMITFRC &&
type!=mjSENS_TENDONLIMITFRC) {
if (rnePost == 0 &&
type != mjSENS_TOUCH &&
type != mjSENS_ACTUATORFRC &&
type != mjSENS_JOINTLIMITFRC &&
type != mjSENS_TENDONLIMITFRC) {
// compute cacc, cfrc_int, cfrc_ext
mj_rnePostConstraint(m, d);
@@ -613,19 +613,19 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
d->sensordata[adr] = 0;
// find contacts in sensor zone, add normal forces
for (int j=0; j<d->ncon; j++) {
for (int j=0; j < d->ncon; j++) {
// contact pointer, contacting bodies
con = d->contact + j;
body1 = m->geom_bodyid[con->geom1];
body2 = m->geom_bodyid[con->geom2];
// select contacts involving sensorized body
if (con->efc_address>=0 && (bodyid==body1 || bodyid==body2)) {
if (con->efc_address >= 0 && (bodyid == body1 || bodyid == body2)) {
// get contact force:torque in contact frame
mj_contactForce(m, d, j, conforce);
// nothing to do if normal is zero
if (conforce[0]<=0) {
if (conforce[0] <= 0) {
continue;
}
@@ -634,7 +634,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
mju_normalize3(conray);
// flip ray direction if sensor is on body2
if (bodyid==body2) {
if (bodyid == body2) {
mju_scl3(conray, conray, -1);
}
@@ -688,8 +688,8 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
case mjSENS_JOINTLIMITFRC: // jointlimitfrc
d->sensordata[adr] = 0;
for (int j=ne+nf; j<nefc; j++) {
if (d->efc_type[j]==mjCNSTR_LIMIT_JOINT && d->efc_id[j]==objid) {
for (int j=ne+nf; j < nefc; j++) {
if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) {
d->sensordata[adr] = d->efc_force[j];
break;
}
@@ -698,8 +698,8 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
case mjSENS_TENDONLIMITFRC: // tendonlimitfrc
d->sensordata[adr] = 0;
for (int j=ne+nf; j<nefc; j++) {
if (d->efc_type[j]==mjCNSTR_LIMIT_TENDON && d->efc_id[j]==objid) {
for (int j=ne+nf; j < nefc; j++) {
if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) {
d->sensordata[adr] = d->efc_force[j];
break;
}
@@ -712,7 +712,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
mj_objectAcceleration(m, d, objtype, objid, tmp, 0);
// copy linear or angular component
if (m->sensor_type[i]==mjSENS_FRAMELINACC) {
if (m->sensor_type[i] == mjSENS_FRAMELINACC) {
mju_copy3(d->sensordata+adr, tmp+3);
} else {
mju_copy3(d->sensordata+adr, tmp);
@@ -742,13 +742,13 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// trigger computation of plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
for (int i=0; i < m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error("invalid plugin slot: %d", slot);
}
if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage==mjSTAGE_ACC) {
if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_ACC) {
if (!plugin->compute) {
mju_error("`compute` is null for plugin at slot %d", slot);
}
@@ -787,14 +787,14 @@ void mj_energyPos(const mjModel* m, mjData* d) {
// init potential energy: -sum_i body(i).mass * mju_dot(body(i).pos, gravity)
d->energy[0] = 0;
if (!mjDISABLED(mjDSBL_GRAVITY)) {
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
d->energy[0] -= m->body_mass[i] * mju_dot3(m->opt.gravity, d->xipos+3*i);
}
}
// add joint-level springs
if (!mjDISABLED(mjDSBL_PASSIVE)) {
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
stiffness = m->jnt_stiffness[i];
padr = m->jnt_qposadr[i];
@@ -825,7 +825,7 @@ void mj_energyPos(const mjModel* m, mjData* d) {
// add tendon-level springs
if (!mjDISABLED(mjDSBL_PASSIVE)) {
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
stiffness = m->tendon_stiffness[i];
mjtNum length = d->ten_length[i];
mjtNum displacement = 0;
src/engine/engine_setconst.c
+66 -66
View File
@@ -42,14 +42,14 @@ static void set0(mjModel* m, mjData* d) {
// save camera and light mode, set to fixed
if (m->ncam) {
cammode = (int*) mj_stackAlloc(d, m->ncam);
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
cammode[i] = m->cam_mode[i];
m->cam_mode[i] = mjCAMLIGHT_FIXED;
}
}
if (m->nlight) {
lightmode = (int*) mj_stackAlloc(d, m->nlight);
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
lightmode[i] = m->light_mode[i];
m->light_mode[i] = mjCAMLIGHT_FIXED;
}
@@ -63,7 +63,7 @@ static void set0(mjModel* m, mjData* d) {
mj_crbSkip(m, d, 0);
// save dof_M0
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
m->dof_M0[i] = d->qM[m->dof_Madr[i]];
}
@@ -73,10 +73,10 @@ static void set0(mjModel* m, mjData* d) {
mj_transmission(m, d);
// restore camera and light mode
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
m->cam_mode[i] = cammode[i];
}
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
m->light_mode[i] = lightmode[i];
}
@@ -86,7 +86,7 @@ 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++) {
if (nv) {
// inverse spatial inertia: A = J*inv(M)*J'
mj_jacBodyCom(m, d, jac, jac+3*nv, i);
@@ -100,13 +100,13 @@ 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++) {
id = m->jnt_dofadr[i];
// get number of components
if (m->jnt_type[i]==mjJNT_FREE) {
if (m->jnt_type[i] == mjJNT_FREE) {
dnum = 6;
} else if (m->jnt_type[i]==mjJNT_BALL) {
} else if (m->jnt_type[i] == mjJNT_BALL) {
dnum = 3;
} else {
dnum = 1;
@@ -115,7 +115,7 @@ static void set0(mjModel* m, mjData* d) {
// inverse joint inertia: A = J*inv(M)*J'
if (nv) {
mju_zero(jac, dnum*nv);
for (int j=0; j<dnum; j++) {
for (int j=0; j < dnum; j++) {
jac[j*(nv+1) + id] = 1;
}
mj_solveM(m, d, tmp, jac, dnum);
@@ -123,14 +123,14 @@ static void set0(mjModel* m, mjData* d) {
}
// average diagonal and assign
if (dnum==6) {
if (dnum == 6) {
m->dof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] =
(A[0] + A[7] + A[14])/3;
(A[0] + A[7] + A[14])/3;
m->dof_invweight0[id+3] = m->dof_invweight0[id+4] = m->dof_invweight0[id+5] =
(A[21] + A[28] + A[35])/3;
} else if (dnum==3)
(A[21] + A[28] + A[35])/3;
} else if (dnum == 3)
m->dof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] =
(A[0] + A[4] + A[8])/3;
(A[0] + A[4] + A[8])/3;
else {
m->dof_invweight0[id] = A[0];
}
@@ -138,12 +138,12 @@ static void set0(mjModel* m, mjData* d) {
// compute tendon_invweight0
if (nv) {
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);
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
for (int j=d->ten_J_rowadr[i]; j<end; j++) {
for (int j=d->ten_J_rowadr[i]; j < end; j++) {
tmp[d->ten_J_colind[j]] = d->ten_J[j];
}
} else {
@@ -156,24 +156,24 @@ static void set0(mjModel* m, mjData* d) {
}
// compute actuator_acc0
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
mj_solveM(m, d, tmp, d->actuator_moment+i*nv, 1);
m->actuator_acc0[i] = mju_norm(tmp, nv);
}
} else {
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
m->actuator_acc0[i] = 0;
}
}
// compute missing eq_data for body constraints
for (int i=0; i<m->neq; i++) {
for (int i=0; i < m->neq; i++) {
// get ids
id1 = m->eq_obj1id[i];
id2 = m->eq_obj2id[i];
// connect constraint
if (m->eq_type[i]==mjEQ_CONNECT) {
if (m->eq_type[i] == mjEQ_CONNECT) {
// pos = anchor position in global frame
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0);
@@ -183,7 +183,7 @@ static void set0(mjModel* m, mjData* d) {
}
// weld constraint
else if (m->eq_type[i]==mjEQ_WELD) {
else if (m->eq_type[i] == mjEQ_WELD) {
// skip if user has set any quaternion data
if (m->eq_data[mjNEQDATA*i+6] ||
m->eq_data[mjNEQDATA*i+7] ||
@@ -208,28 +208,28 @@ static void set0(mjModel* m, mjData* d) {
}
// camera compos0, pos0, mat0
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
// get body ids
id = m->cam_bodyid[i]; // camera body
id1 = m->cam_targetbodyid[i]; // target body
// compute positional offsets
mju_sub3(m->cam_pos0+3*i, d->cam_xpos+3*i, d->xpos+3*id);
mju_sub3(m->cam_poscom0+3*i, d->cam_xpos+3*i, d->subtree_com+ (id1>=0 ? 3*id1 : 3*id));
mju_sub3(m->cam_poscom0+3*i, d->cam_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
// copy mat
mju_copy(m->cam_mat0+9*i, d->cam_xmat+9*i, 9);
}
// light compos0, pos0, dir0
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
// get body ids
id = m->light_bodyid[i]; // light body
id1 = m->light_targetbodyid[i]; // target body
// compute positional offsets
mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id);
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1>=0 ? 3*id1 : 3*id));
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
// copy dir
mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
@@ -242,7 +242,7 @@ static void set0(mjModel* m, mjData* d) {
// accumulate bounding box
static void updateBox(mjtNum* xmin, mjtNum* xmax, mjtNum* pos, mjtNum radius) {
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
xmin[i] = mjMIN(xmin[i], pos[i] - radius);
xmax[i] = mjMAX(xmax[i], pos[i] + radius);
}
@@ -258,22 +258,22 @@ static void setStat(mjModel* m, mjData* d) {
mjtNum* body = mj_stackAlloc(d, m->nbody);
// compute bounding box of bodies, joint centers, geoms and sites
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
updateBox(xmin, xmax, d->xpos+3*i, 0);
updateBox(xmin, xmax, d->xipos+3*i, 0);
}
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
updateBox(xmin, xmax, d->xanchor+3*i, 0);
}
for (int i=0; i<m->nsite; i++) {
for (int i=0; i < m->nsite; i++) {
updateBox(xmin, xmax, d->site_xpos+3*i, 0);
}
for (int i=0; i<m->ngeom; i++) {
for (int i=0; i < m->ngeom; i++) {
// set rbound: regular geom rbound, or 0.1 of plane or hfield max size
rbound = 0;
if (m->geom_rbound[i] > 0) {
rbound = m->geom_rbound[i];
} else if (m->geom_type[i]==mjGEOM_PLANE) {
} else if (m->geom_type[i] == mjGEOM_PLANE) {
// finite in at least one direction
if (m->geom_size[3*i] || m->geom_size[3*i+1]) {
rbound = mjMAX(m->geom_size[3*i], m->geom_size[3*i+1]) * 0.1;
@@ -283,7 +283,7 @@ static void setStat(mjModel* m, mjData* d) {
else {
rbound = 1;
}
} else if (m->geom_type[i]==mjGEOM_HFIELD) {
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
int j = m->geom_dataid[i];
rbound = mjMAX(m->hfield_size[4*j],
mjMAX(m->hfield_size[4*j+1],
@@ -298,13 +298,13 @@ static void setStat(mjModel* m, mjData* d) {
mju_scl3(m->stat.center, m->stat.center, 0.5);
// compute bounding box size
if (xmax[0]>xmin[0])
if (xmax[0] > xmin[0])
m->stat.extent = mju_max(1E-5,
mju_max(xmax[0]-xmin[0], mju_max(xmax[1]-xmin[1], xmax[2]-xmin[2])));
// set body size to max com-joint distance
mju_zero(body, m->nbody);
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
// handle this body
int id = m->jnt_bodyid[i];
body[id] = mju_max(body[id], mju_dist3(d->xipos+3*id, d->xanchor+3*i));
@@ -316,18 +316,18 @@ static void setStat(mjModel* m, mjData* d) {
body[0] = 0;
// set body size to max of old value, and geom rbound + com-geom dist
for (int i=1; i<m->nbody; i++) {
for (int id=m->body_geomadr[i]; id<m->body_geomadr[i]+m->body_geomnum[i]; id++) {
if (m->geom_rbound[id]>0) {
for (int i=1; i < m->nbody; i++) {
for (int id=m->body_geomadr[i]; id < m->body_geomadr[i]+m->body_geomnum[i]; id++) {
if (m->geom_rbound[id] > 0) {
body[i] = mju_max(body[i], m->geom_rbound[id] + mju_dist3(d->xipos+3*i, d->geom_xpos+3*id));
}
}
}
// compute meansize, make sure all sizes are above min
if (m->nbody>1) {
if (m->nbody > 1) {
m->stat.meansize = 0;
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
body[i] = mju_max(body[i], 1E-5);
m->stat.meansize += body[i]/(m->nbody-1);
}
@@ -337,9 +337,9 @@ static void setStat(mjModel* m, mjData* d) {
m->stat.extent = mju_max(m->stat.extent, 2 * m->stat.meansize);
// compute meanmass
if (m->nbody>1) {
if (m->nbody > 1) {
m->stat.meanmass = 0;
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
m->stat.meanmass += m->body_mass[i];
}
m->stat.meanmass /= (m->nbody-1);
@@ -348,7 +348,7 @@ static void setStat(mjModel* m, mjData* d) {
// compute meaninertia
if (m->nv) {
m->stat.meaninertia = 0;
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
m->stat.meaninertia += d->qM[m->dof_Madr[i]];
}
m->stat.meaninertia /= m->nv;
@@ -369,7 +369,7 @@ static void setSpring(mjModel* m, mjData* d) {
mj_transmission(m, d);
// copy if model spring length is -1
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_lengthspring[2*i] == -1 && m->tendon_lengthspring[2*i+1] == -1) {
// explicit springlength unused, set equal to ten_length
m->tendon_lengthspring[2*i] = m->tendon_lengthspring[2*i+1] = d->ten_length[i];
@@ -382,10 +382,10 @@ static void setSpring(mjModel* m, mjData* d) {
// entry point: set all constant fields of mjModel, except for lengthrange
void mj_setConst(mjModel* m, mjData* d) {
// compute subtreemass
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
m->body_subtreemass[i] = m->body_mass[i];
}
for (int i=m->nbody-1; i>0; i--) {
for (int i=m->nbody-1; i > 0; i--) {
m->body_subtreemass[m->body_parentid[i]] += m->body_subtreemass[i];
}
@@ -418,7 +418,7 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
// impose maxforce
nrm = mju_norm(d->qfrc_applied, nv);
if (opt->maxforce>0 && nrm>opt->maxforce) {
if (opt->maxforce > 0 && nrm > opt->maxforce) {
mju_scl(d->qfrc_applied, d->qfrc_applied, opt->maxforce/mjMAX(mjMINVAL, nrm), nv);
}
@@ -435,18 +435,18 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
int mj_setLengthRange(mjModel* m, mjData* d, int index,
const mjLROpt* opt, char* error, int error_sz) {
// check index
if (index<0 || index>=m->nu) {
if (index < 0 || index >= m->nu) {
mju_error("Invalid actuator index in mj_setLengthRange");
}
// skip depending on mode and type
int ismuscle = (m->actuator_gaintype[index]==mjGAIN_MUSCLE ||
m->actuator_biastype[index]==mjBIAS_MUSCLE);
int isuser = (m->actuator_gaintype[index]==mjGAIN_USER ||
m->actuator_biastype[index]==mjBIAS_USER);
if ((opt->mode==mjLRMODE_NONE) ||
(opt->mode==mjLRMODE_MUSCLE && !ismuscle) ||
(opt->mode==mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) {
int ismuscle = (m->actuator_gaintype[index] == mjGAIN_MUSCLE ||
m->actuator_biastype[index] == mjBIAS_MUSCLE);
int isuser = (m->actuator_gaintype[index] == mjGAIN_USER ||
m->actuator_biastype[index] == mjBIAS_USER);
if ((opt->mode == mjLRMODE_NONE) ||
(opt->mode == mjLRMODE_MUSCLE && !ismuscle) ||
(opt->mode == mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) {
return 1;
}
@@ -461,8 +461,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
// use joint and tendon limits if available
if (opt->uselimit) {
// joint or jointinparent
if (m->actuator_trntype[index]==mjTRN_JOINT ||
m->actuator_trntype[index]==mjTRN_JOINTINPARENT) {
if (m->actuator_trntype[index] == mjTRN_JOINT ||
m->actuator_trntype[index] == mjTRN_JOINTINPARENT) {
// make sure joint is limited
if (m->jnt_limited[threadid]) {
// copy range
@@ -475,7 +475,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// tendon
if (m->actuator_trntype[index]==mjTRN_TENDON) {
if (m->actuator_trntype[index] == mjTRN_TENDON) {
// make sure tendon is limited
if (m->tendon_limited[threadid]) {
// copy range
@@ -491,7 +491,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
// optimize in both directions
mjtNum lmin[2] = {0, 0}, lmax[2] = {0, 0};
int side;
for (side=0; side<2; side++) {
for (side=0; side < 2; side++) {
// init at qpos0
mj_resetData(m, d);
@@ -502,17 +502,17 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
mjtNum len = evalAct(m, d, index, side, opt);
// reset: cannot proceed
if (d->time==0) {
if (d->time == 0) {
snprintf(error, error_sz, "Unstable lengthrange simulation in actuator %d", index);
return 0;
}
// update limits
if (d->time > opt->inttotal-opt->interval) {
if (len<lmin[side] || !updated) {
if (len < lmin[side] || !updated) {
lmin[side] = len;
}
if (len>lmax[side] || !updated) {
if (len > lmax[side] || !updated) {
lmax[side] = len;
}
@@ -521,12 +521,12 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// assign
m->actuator_lengthrange[2*index+side] = (side==0 ? lmin[side] : lmax[side]);
m->actuator_lengthrange[2*index+side] = (side == 0 ? lmin[side] : lmax[side]);
}
// check range
mjtNum dif = m->actuator_lengthrange[2*index+1] - m->actuator_lengthrange[2*index];
if (dif<=0) {
if (dif <= 0) {
snprintf(error, error_sz,
"Invalid lengthrange (%g, %g) in actuator %d",
m->actuator_lengthrange[2*index],
@@ -535,7 +535,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// check convergence, side 0
if (lmax[0]-lmin[0]>opt->tolrange*dif) {
if (lmax[0]-lmin[0] > opt->tolrange*dif) {
snprintf(error, error_sz,
"Lengthrange computation did not converge in actuator %d:\n"
" eval (%g, %g)\n range (%g, %g)",
@@ -546,7 +546,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// check convergence, side 1
if (lmax[1]-lmin[1]>opt->tolrange*dif) {
if (lmax[1]-lmin[1] > opt->tolrange*dif) {
snprintf(error, error_sz,
"Lengthrange computation did not converge in actuator %d:\n"
" eval (%g, %g)\n range (%g, %g)",
src/engine/engine_solver.c
+165 -163
View File
@@ -48,7 +48,7 @@ static void saveStats(const mjModel* m, mjData* d, int* piter,
(*piter)++;
// save if within range
if (i<mjNSOLVER) {
if (i < mjNSOLVER) {
d->solver[i].improvement = improvement;
d->solver[i].gradient = gradient;
d->solver[i].lineslope = lineslope;
@@ -80,9 +80,9 @@ static void ARdiaginv(const mjModel* m, mjData* d, mjtNum* res, int flg_subR) {
// sparse
if (mj_isSparse(m)) {
for (int i=0; i<nefc; i++) {
for (int j=0; j<d->efc_AR_rownnz[i]; j++) {
if (i==d->efc_AR_colind[rowadr[i]+j]) {
for (int i=0; i < nefc; i++) {
for (int j=0; j < d->efc_AR_rownnz[i]; j++) {
if (i == d->efc_AR_colind[rowadr[i]+j]) {
res[i] = 1/(flg_subR ? mju_max(mjMINVAL, d->efc_AR[rowadr[i]+j]-d->efc_R[i])
: d->efc_AR[rowadr[i]+j]);
break;
@@ -93,7 +93,7 @@ static void ARdiaginv(const mjModel* m, mjData* d, mjtNum* res, int flg_subR) {
// dense
else {
for (int i=0; i<nefc; i++) {
for (int i=0; i < nefc; i++) {
res[i] = 1/(flg_subR ? mju_max(mjMINVAL, d->efc_AR[i*(nefc+1)]-d->efc_R[i])
: d->efc_AR[i*(nefc+1)]);
}
@@ -121,36 +121,36 @@ static void extractBlock(const mjModel* m, mjData* d, mjtNum* Ac,
if( col>=start && col<start+n )
Ac[j*n+col-start] = AR[rowadr[start+j]+k];
}
*/
*/
// assume full sub-matrix, find starting k: same for all rows
int k;
for (k=0; k<rownnz[start]; k++) {
if (colind[rowadr[start]+k]==start) {
for (k=0; k < rownnz[start]; k++) {
if (colind[rowadr[start]+k] == start) {
break;
}
}
// sanity check; SHOULD NOT OCCUR
if (k>=rownnz[start]) {
if (k >= rownnz[start]) {
mju_error("Internal error in extractComponent");
}
// copy rows
for (int j=0; j<n; j++) {
for (int j=0; j < n; j++) {
mju_copy(Ac+j*n, AR+rowadr[start+j]+k, n);
}
}
// dense
else {
for (int j=0; j<n; j++) {
for (int j=0; j < n; j++) {
mju_copy(Ac+j*n, AR+start+(start+j)*nefc, n);
}
}
// subtract R from diagonal, clamp to 1e-10 from below
if (flg_subR) {
for (int j=0; j<n; j++) {
for (int j=0; j < n; j++) {
Ac[j*(n+1)] -= d->efc_R[start+j];
Ac[j*(n+1)] = mjMAX(1e-10, Ac[j*(n+1)]);
}
@@ -165,7 +165,7 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i
// sparse
if (mj_isSparse(m)) {
for (int j=0; j<dim; j++) {
for (int j=0; j < dim; j++) {
res[j] = d->efc_b[i+j] + mju_dotSparse(d->efc_AR + d->efc_AR_rowadr[i+j],
d->efc_force, d->efc_AR_rownnz[i+j],
d->efc_AR_colind + d->efc_AR_rowadr[i+j]);
@@ -174,13 +174,13 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i
// dense
else {
for (int j=0; j<dim; j++) {
for (int j=0; j < dim; j++) {
res[j] = d->efc_b[i+j] + mju_dot(d->efc_AR+(i+j)*nefc, d->efc_force, nefc);
}
}
if (flg_subR) {
for (int j=0; j<dim; j++) {
for (int j=0; j < dim; j++) {
res[j] -= d->efc_R[i+j]*d->efc_force[i+j];
}
}
@@ -194,7 +194,7 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce,
mjtNum delta[6], change;
// compute change
if (dim==1) {
if (dim == 1) {
delta[0] = force[0] - oldforce[0];
change = 0.5*delta[0]*delta[0]*A[0] + delta[0]*res[0];
} else {
@@ -203,7 +203,7 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce,
}
// positive change: restore
if (change>1e-10) {
if (change > 1e-10) {
mju_copy(force, oldforce, dim);
change = 0;
}
@@ -223,15 +223,15 @@ static int dualState(const mjModel* m, mjData* d) {
nactive = ne + nf;
// equality
for (int i=0; i<ne; i++) {
for (int i=0; i < ne; i++) {
state[i] = mjCNSTRSTATE_QUADRATIC;
}
// friction
for (int i=ne; i<ne+nf; i++) {
if (force[i]<=-floss[i]) {
for (int i=ne; i < ne+nf; i++) {
if (force[i] <= -floss[i]) {
state[i] = mjCNSTRSTATE_LINEARPOS; // opposite of primal
} else if (force[i]>=floss[i]) {
} else if (force[i] >= floss[i]) {
state[i] = mjCNSTRSTATE_LINEARNEG;
} else {
state[i] = mjCNSTRSTATE_QUADRATIC;
@@ -239,10 +239,10 @@ static int dualState(const mjModel* m, mjData* d) {
}
// limit and contact
for (int i=ne+nf; i<nefc; i++) {
for (int i=ne+nf; i < nefc; i++) {
// non-negative
if (d->efc_type[i]!=mjCNSTR_CONTACT_ELLIPTIC) {
if (force[i]<=0) {
if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) {
if (force[i] <= 0) {
state[i] = mjCNSTRSTATE_SATISFIED;
} else {
state[i] = mjCNSTRSTATE_QUADRATIC;
@@ -259,7 +259,7 @@ static int dualState(const mjModel* m, mjData* d) {
// f = map force to regular-cone space
f[0] = force[i]/mu;
for (int j=1; j<dim; j++) {
for (int j=1; j < dim; j++) {
f[j] = force[i+j]/con->friction[j-1];
}
@@ -268,12 +268,12 @@ static int dualState(const mjModel* m, mjData* d) {
mjtNum T = mju_norm(f+1, dim-1);
// top zone
if (mu*N>=T) {
if (mu*N >= T) {
result = mjCNSTRSTATE_SATISFIED;
}
// bottom zone
else if (N+mu*T<=0) {
else if (N+mu*T <= 0) {
result = mjCNSTRSTATE_QUADRATIC;
nactive += dim;
}
@@ -285,7 +285,7 @@ static int dualState(const mjModel* m, mjData* d) {
}
// replicate state in all cone dimensions
for (int j=0; j<dim; j++) {
for (int j=0; j < dim; j++) {
state[i+j] = result;
}
@@ -319,14 +319,14 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
dualState(m, d);
// main iteration
while (iter<maxiter) {
while (iter < maxiter) {
// clear improvement
improvement = 0;
// perform one sweep
for (int i=0; i<nefc; i++) {
for (int i=0; i < nefc; i++) {
// get constraint dimensionality
if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) {
if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) {
dim = d->contact[d->efc_id[i]].dim;
} else {
dim = 1;
@@ -337,19 +337,19 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
mju_copy(oldforce, force+i, dim);
// simple constraint
if (d->efc_type[i]!=mjCNSTR_CONTACT_ELLIPTIC) {
if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) {
// unconstrained minimum
force[i] -= res[0]*ARinv[i];
// impose interval and inequality constraints
if (i>=ne && i<ne+nf) {
if (force[i]<-floss[i]) {
if (i >= ne && i < ne+nf) {
if (force[i] < -floss[i]) {
force[i] = -floss[i];
} else if (force[i]>floss[i]) {
} else if (force[i] > floss[i]) {
force[i] = floss[i];
}
} else if (i>=ne+nf) {
if (force[i]<0) {
} else if (i >= ne+nf) {
if (force[i] < 0) {
force[i] = 0;
}
}
@@ -368,12 +368,12 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
extractBlock(m, d, Athis, i, dim, 0);
// normal force too small: normal update
if (force[i]<mjMINVAL) {
if (force[i] < mjMINVAL) {
// unconstrained minimum
force[i] -= res[0]*ARinv[i];
// clamp
if (force[i]<0) {
if (force[i] < 0) {
force[i] = 0;
}
@@ -391,17 +391,17 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
denom = mju_dot(v, v1, dim);
// avoid division by 0
if (denom>=mjMINVAL) {
if (denom >= mjMINVAL) {
// x = v' * res / denom
x = -mju_dot(v, res, dim) / denom;
// make sure normal is non-negative
if (force[i]+x*v[0]<0) {
if (force[i]+x*v[0] < 0) {
x = -v[0]/force[i];
}
// add x*v to f
for (int j=0; j<dim; j++) {
for (int j=0; j < dim; j++) {
force[i+j] += x*v[j];
}
}
@@ -411,14 +411,14 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
// Ac = AR-submatrix; bc = b-subvector + Ac,rest * f_rest
mju_copy(bc, res+1, dim-1);
for (int j=0; j<dim-1; j++) {
for (int j=0; j < dim-1; j++) {
mju_copy(Ac+j*(dim-1), Athis+(j+1)*dim+1, dim-1);
bc[j] -= mju_dot(Ac+j*(dim-1), oldforce+1, dim-1);
bc[j] += Athis[(j+1)*dim]*(force[i]-oldforce[0]);
}
// guard for f_normal==0
if (force[i]<mjMINVAL) {
if (force[i] < mjMINVAL) {
mju_zero(force+i+1, dim-1);
}
@@ -427,9 +427,9 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
int flg_active;
// solve
if (dim==3) {
if (dim == 3) {
flg_active = mju_QCQP2(v, Ac, bc, mu, force[i]);
} else if (dim==4) {
} else if (dim == 4) {
flg_active = mju_QCQP3(v, Ac, bc, mu, force[i]);
} else {
flg_active = mju_QCQP(v, Ac, bc, mu, force[i], dim-1);
@@ -438,11 +438,11 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
// on constraint: put v on ellipsoid, in case QCQP is approximate
if (flg_active) {
mjtNum s = 0;
for (int j=0; j<dim-1; j++) {
for (int j=0; j < dim-1; j++) {
s += v[j]*v[j] / (mu[j]*mu[j]);
}
s = mju_sqrt(force[i]*force[i] / mju_max(mjMINVAL, s));
for (int j=0; j<dim-1; j++) {
for (int j=0; j < dim-1; j++) {
v[j] *= s;
}
}
@@ -453,7 +453,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
}
// accumulate improvement
if (dim==1) {
if (dim == 1) {
Athis[0] = 1/ARinv[i];
}
improvement -= costChange(Athis, force+i, oldforce, res, dim);
@@ -466,8 +466,8 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
memcpy(oldstate, d->efc_state, nefc*sizeof(int));
int nactive = dualState(m, d);
int nchange = 0;
for (int i=0; i<nefc; i++) {
nchange += (oldstate[i]!=d->efc_state[i]);
for (int i=0; i < nefc; i++) {
nchange += (oldstate[i] != d->efc_state[i]);
}
// scale improvement, save stats, count
@@ -475,7 +475,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0);
// terminate
if (improvement<m->opt.tolerance) {
if (improvement < m->opt.tolerance) {
break;
}
}
@@ -486,7 +486,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
// set nnz
if (mj_isSparse(m)) {
d->solver_nnz = 0;
for (int i=0; i<nefc; i++) {
for (int i=0; i < nefc; i++) {
d->solver_nnz += d->efc_AR_rownnz[i];
}
} else {
@@ -521,19 +521,19 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
dualState(m, d);
// main iteration
while (iter<maxiter) {
while (iter < maxiter) {
// clear improvement
improvement = 0;
// correct for cost change at iter 0
if (iter==0) {
for (int i=0; i<nefc; i++) {
if (iter == 0) {
for (int i=0; i < nefc; i++) {
improvement += 0.5*force[i]*force[i]*d->efc_R[i];
}
}
// perform one sweep: dry friction
for (int i=ne; i<ne+nf; i++) {
for (int i=ne; i < ne+nf; i++) {
// compute residual, save old
residual(m, d, res, i, 1, 1);
oldforce[0] = force[i];
@@ -542,9 +542,9 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
force[i] -= res[0]*ARinv[i];
// impose interval constraints
if (force[i]<-floss[i]) {
if (force[i] < -floss[i]) {
force[i] = -floss[i];
} else if (force[i]>floss[i]) {
} else if (force[i] > floss[i]) {
force[i] = floss[i];
}
@@ -554,16 +554,16 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
}
// perform one sweep: contact friction
for (int i=ne+nf; i<nefc; i++) {
for (int i=ne+nf; i < nefc; i++) {
// pyramidal contact
if (d->efc_type[i]==mjCNSTR_CONTACT_PYRAMIDAL) {
if (d->efc_type[i] == mjCNSTR_CONTACT_PYRAMIDAL) {
// get contact info
con = d->contact + d->efc_id[i];
dim = con->dim;
mu = con->friction;
// loop over pairs of opposing pyramid edges
for (int j=i; j<i+2*(dim-1); j+=2) {
for (int j=i; j < i+2*(dim-1); j+=2) {
// compute residual, save old
residual(m, d, res, j, 2, 1);
mju_copy(oldforce, force+j, 2);
@@ -573,7 +573,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
// bc = b-subvector + Ac,rest * f_rest
mju_copy(bc, res, 2);
for (int k=0; k<2; k++) {
for (int k=0; k < 2; k++) {
bc[k] -= mju_dot(Ac+k*2, oldforce, 2);
}
@@ -586,7 +586,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
K0 = mid*(Ac[0] - Ac[3]) + bc[0] - bc[1];
// guard against Ac==0
if (K1<mjMINVAL) {
if (K1 < mjMINVAL) {
force[j] = force[j+1] = mid;
}
@@ -596,10 +596,10 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
y = -K0/K1;
// clamp and assign
if (y<-mid) {
if (y < -mid) {
force[j] = 0;
force[j+1] = 2*mid;
} else if (y>mid) {
} else if (y > mid) {
force[j] = 2*mid;
force[j+1] = 0;
} else {
@@ -617,7 +617,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
}
// elliptic contact
else if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) {
else if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) {
// get contact info
con = d->contact + d->efc_id[i];
dim = con->dim;
@@ -632,12 +632,12 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
// bc = b-subvector + Ac,rest * f_rest
mju_copy(bc, res, dim-1);
for (int j=0; j<dim-1; j++) {
for (int j=0; j < dim-1; j++) {
bc[j] -= mju_dot(Ac+j*(dim-1), oldforce, dim-1);
}
// guard for f_normal==0
if (force[i]<mjMINVAL) {
if (force[i] < mjMINVAL) {
mju_zero(force+i+1, dim-1);
}
@@ -646,9 +646,9 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
int flg_active = 0;
// solve
if (dim==3) {
if (dim == 3) {
flg_active = mju_QCQP2(v, Ac, bc, mu, force[i]);
} else if (dim==4) {
} else if (dim == 4) {
flg_active = mju_QCQP3(v, Ac, bc, mu, force[i]);
} else {
flg_active = mju_QCQP(v, Ac, bc, mu, force[i], dim-1);
@@ -657,11 +657,11 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
// on constraint: put v on ellipsoid, in case QCQP is approximate
if (flg_active) {
mjtNum s = 0;
for (int j=0; j<dim-1; j++) {
for (int j=0; j < dim-1; j++) {
s += v[j]*v[j]/(mu[j]*mu[j]);
}
s = mju_sqrt(force[i]*force[i] / mju_max(mjMINVAL, s));
for (int j=0; j<dim-1; j++) {
for (int j=0; j < dim-1; j++) {
v[j] *= s;
}
}
@@ -682,8 +682,8 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
memcpy(oldstate, d->efc_state, nefc*sizeof(int));
int nactive = dualState(m, d);
int nchange = 0;
for (int i=0; i<nefc; i++) {
nchange += (oldstate[i]!=d->efc_state[i]);
for (int i=0; i < nefc; i++) {
nchange += (oldstate[i] != d->efc_state[i]);
}
// scale improvement, save stats, count
@@ -691,7 +691,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0);
// terminate
if (improvement<m->opt.noslip_tolerance) {
if (improvement < m->opt.noslip_tolerance) {
break;
}
}
@@ -722,8 +722,8 @@ struct _mjCGContext {
mjtNum* quad; // quadratic polynomials for constraint costs (nefc x 3)
// Hessian (Newton only)
int flg_Newton; // 1: Newton, 0: CG (const)
int nnz; // total number of non-zeros
int flg_Newton; // 1: Newton, 0: CG (const)
int nnz; // total number of non-zeros
mjtNum* H; // Cholesky factorization of Hessian (nv x nv)
mjtNum* Hcone; // with cone contributions if present (nv x nv)
int* rownnz; // non-zeros in row (nv X 1)
@@ -731,16 +731,16 @@ struct _mjCGContext {
int* colind; // column indices (nv x nv)
// globals
mjtNum cost; // constraint + Gauss cost
mjtNum quadGauss[3]; // quadratic polynomial for Gauss cost
int nactive; // number of active constraints
int ncone; // number of contacts in cone state
int nupdate; // number of Cholesky updates
mjtNum cost; // constraint + Gauss cost
mjtNum quadGauss[3]; // quadratic polynomial for Gauss cost
int nactive; // number of active constraints
int ncone; // number of contacts in cone state
int nupdate; // number of Cholesky updates
// linesearch diagnostics
int LSiter; // number of linesearch iterations
int LSresult; // linesearch result
mjtNum LSslope; // linesearch slope at solution
int LSiter; // number of linesearch iterations
int LSresult; // linesearch result
mjtNum LSslope; // linesearch slope at solution
};
typedef struct _mjCGContext mjCGContext;
@@ -787,14 +787,14 @@ static void CGupdateConstraint(const mjModel* m, mjData* d, mjCGContext* ctx) {
// count active and cone
ctx->nactive = 0;
ctx->ncone = 0;
for (int i=0; i<nefc; i++) {
ctx->nactive += (d->efc_state[i]!=mjCNSTRSTATE_SATISFIED);
ctx->ncone += (d->efc_state[i]==mjCNSTRSTATE_CONE);
for (int i=0; i < nefc; i++) {
ctx->nactive += (d->efc_state[i] != mjCNSTRSTATE_SATISFIED);
ctx->ncone += (d->efc_state[i] == mjCNSTRSTATE_CONE);
}
// add Gauss cost, set in quadratic[0]
mjtNum Gauss = 0;
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
Gauss += 0.5*(ctx->Ma[i]-d->qfrc_smooth[i])*(d->qacc[i]-d->qacc_smooth[i]);
}
ctx->quadGauss[0] = Gauss;
@@ -808,7 +808,7 @@ static void CGupdateGradient(const mjModel* m, mjData* d, mjCGContext* ctx) {
int nv = m->nv;
// grad = M*qacc - qfrc_smooth - qfrc_constraint
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
ctx->grad[i] = ctx->Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i];
}
@@ -841,7 +841,7 @@ static void CGprepare(const mjModel* m, const mjData* d, mjCGContext* ctx) {
ctx->quadGauss[2] = 0.5*mju_dot(v, ctx->Mv, nv);
// process constraints
for (int i=0; i<nefc; i++) {
for (int i=0; i < nefc; i++) {
// pointers to numeric data
mjtNum* Jv = ctx->Jv + i;
mjtNum* Jaref = ctx->Jaref + i;
@@ -857,7 +857,7 @@ static void CGprepare(const mjModel* m, const mjData* d, mjCGContext* ctx) {
quad[2] = Jv[0]*D[0]*Jv[0];
// elliptic cone: extra processing
if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) {
if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) {
// extract contact info
mjContact* con = d->contact + d->efc_id[i];
int dim = con->dim;
@@ -865,7 +865,7 @@ static void CGprepare(const mjModel* m, const mjData* d, mjCGContext* ctx) {
mjtNum* friction = con->friction;
// complete vector quadratic (for bottom zone)
for (int j=1; j<dim; j++) {
for (int j=1; j < dim; j++) {
mjtNum DJj = D[j]*Jaref[j];
quad[0] += Jaref[j]*DJj;
quad[1] += Jv[j]*DJj;
@@ -875,13 +875,13 @@ static void CGprepare(const mjModel* m, const mjData* d, mjCGContext* ctx) {
// rescale to make primal cone circular
U[0] = Jaref[0]*mu;
V[0] = Jv[0]*mu;
for (int j=1; j<dim; j++) {
for (int j=1; j < dim; j++) {
U[j] = Jaref[j]*friction[j-1];
V[j] = Jv[j]*friction[j-1];
}
// accumulate sums of squares
for (int j=1; j<dim; j++) {
for (int j=1; j < dim; j++) {
UU += U[j]*U[j];
UV += U[j]*V[j];
VV += V[j]*V[j];
@@ -930,12 +930,12 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
mju_copy3(quadTotal, ctx->quadGauss);
// equality
for (int i=0; i<ne; i++) {
for (int i=0; i < ne; i++) {
mju_addTo3(quadTotal, ctx->quad+3*i);
}
// friction
for (int i=ne; i<ne+nf; i++) {
for (int i=ne; i < ne+nf; i++) {
// search point, friction loss, bound (Rf)
mjtNum start = ctx->Jaref[i], dir = ctx->Jv[i];
mjtNum x = start + alpha*dir;
@@ -943,12 +943,12 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
mjtNum Rf = d->efc_R[i]*f;
// -bound < x < bound : quadratic
if (-Rf<x && x<Rf) {
if (-Rf < x && x < Rf) {
mju_addTo3(quadTotal, ctx->quad+3*i);
}
// x < -bound : linear negative
else if (x<=-Rf) {
else if (x <= -Rf) {
mjtNum qf[3] = {f*(-0.5*Rf-start), -f*dir, 0};
mju_addTo3(quadTotal, qf);
}
@@ -961,8 +961,8 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
}
// limit and contact
for (int i=ne+nf; i<nefc; i++) {
if (d->efc_type[i]==mjCNSTR_CONTACT_ELLIPTIC) { // elliptic cone
for (int i=ne+nf; i < nefc; i++) {
if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { // elliptic cone
// extract contact info
mjContact* con = d->contact + d->efc_id[i];
mjtNum* quad = ctx->quad + 3*i;
@@ -982,9 +982,9 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
mjtNum Tsqr = UU + alpha*(2*UV + alpha*VV);
// no tangential force : top or bottom zone
if (Tsqr<=0) {
if (Tsqr <= 0) {
// bottom zone: quadratic cost
if (N<0) {
if (N < 0) {
mju_addTo3(quadTotal, quad);
}
@@ -997,12 +997,12 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
mjtNum T = mju_sqrt(Tsqr);
// N>=mu*T : top zone
if (N>=mu*T) {
if (N >= mu*T) {
// nothing to do
}
// mu*N+T<=0 : bottom zone
else if (mu*N+T<=0) {
else if (mu*N+T <= 0) {
mju_addTo3(quadTotal, quad);
}
@@ -1027,7 +1027,7 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
mjtNum x = ctx->Jaref[i] + alpha*ctx->Jv[i];
// active
if (x<0) {
if (x < 0) {
mju_addTo3(quadTotal, ctx->quad+3*i);
}
}
@@ -1039,7 +1039,7 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
deriv[1] += 2*quadTotal[2];
// check for convexity; SHOULD NOT OCCUR
if (deriv[1]<=0) {
if (deriv[1] <= 0) {
mju_warning("Linesearch objective is not convex");
deriv[1] = mjMINVAL;
}
@@ -1057,15 +1057,17 @@ static void CGeval(const mjModel* m, mjData* d, mjCGContext* ctx, mjCGPnt* p) {
static int updateBracket(const mjModel* m, mjData* d, mjCGContext* ctx,
mjCGPnt* p, mjCGPnt candidates[3], mjCGPnt* pnext) {
int flag = 0;
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
// negative deriv
if (p->deriv[0]<0 && candidates[i].deriv[0]<0 && p->deriv[0]<candidates[i].deriv[0]) {
if (p->deriv[0] < 0 && candidates[i].deriv[0] < 0 && p->deriv[0] < candidates[i].deriv[0]) {
*p = candidates[i];
flag = 1;
}
// positive deriv
else if (p->deriv[0]>0 && candidates[i].deriv[0]>0 && p->deriv[0]>candidates[i].deriv[0]) {
else if (p->deriv[0] > 0 &&
candidates[i].deriv[0] > 0 &&
p->deriv[0] > candidates[i].deriv[0]) {
*p = candidates[i];
flag = 2;
}
@@ -1096,7 +1098,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
// save search vector length, check
mjtNum snorm = mju_norm(ctx->search, m->nv);
if (snorm<mjMINVAL) {
if (snorm < mjMINVAL) {
ctx->LSresult = 1; // search vector too small
return 0;
}
@@ -1119,13 +1121,13 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
// always attempt one Newton step
p1.alpha = p0.alpha - p0.deriv[0]/p0.deriv[1];
CGeval(m, d, ctx, &p1);
if (p0.cost<p1.cost) {
if (p0.cost < p1.cost) {
p1 = p0;
}
// check for initial convergence
if (mju_abs(p1.deriv[0])<gtol) {
if (p1.alpha==0) {
if (mju_abs(p1.deriv[0]) < gtol) {
if (p1.alpha == 0) {
ctx->LSresult = 2; // no improvement, initial convergence
} else {
ctx->LSresult = 0; // SUCCESS
@@ -1135,32 +1137,32 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
}
// save direction
int dir = (p1.deriv[0]<0 ? +1 : -1);
int dir = (p1.deriv[0] < 0 ? +1 : -1);
// SANITY CHECKS
/*
// descent direction
if( mju_dot(ctx->grad, ctx->search, m->nv)>=0 )
// descent direction
if( mju_dot(ctx->grad, ctx->search, m->nv)>=0 )
printf("NOT A DESCENT: grad %g search %g dot %g\n",
mju_norm(ctx->grad, m->nv),
mju_norm(ctx->search, m->nv),
mju_dot(ctx->grad, ctx->search, m->nv));
// 2nd derivative for Newton cone
if( ctx->flg_Newton && ctx->ncone )
{
// 2nd derivative for Newton cone
if( ctx->flg_Newton && ctx->ncone )
{
mjtNum dd = -p0.deriv[0]/p0.deriv[1];
if( mju_abs(dd-1)>1e-6 )
printf("2nd DERIVATIVE FAIL: d0 %g d1 %g alpha %g\n",
p0.deriv[0], p0.deriv[1], dd);
}
}
// cost and gradient at 0: full-space vs. linesearch
mjtNum grd = mju_dot(ctx->grad, ctx->search, m->nv);
if( mju_abs(p0.cost-ctx->cost)/mjMAX(mjMINVAL,mju_abs(p0.cost+ctx->cost)) > 1e-6 ||
// cost and gradient at 0: full-space vs. linesearch
mjtNum grd = mju_dot(ctx->grad, ctx->search, m->nv);
if( mju_abs(p0.cost-ctx->cost)/mjMAX(mjMINVAL,mju_abs(p0.cost+ctx->cost)) > 1e-6 ||
mju_abs(p0.deriv[0]-grd)/mjMAX(mjMINVAL,mju_abs(p0.deriv[0]+grd)) > 1e-6 )
{
{
printf("LSiter = %d:\n", ctx->LSiter);
printf("COST: %g %g %g\n",
p0.cost, ctx->cost,
@@ -1168,12 +1170,12 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
printf("GRAD: %g %g %g\n",
p0.deriv[0], grd,
mju_abs(p0.deriv[0]-grd)/mjMAX(mjMINVAL,mju_abs(p0.deriv[0]+grd)));
}
*/
}
*/
// one-sided search
int p2update = 0;
while (p1.deriv[0]*dir<=-gtol && ctx->LSiter<LSmaxiter) {
while (p1.deriv[0]*dir <= -gtol && ctx->LSiter < LSmaxiter) {
// save current
p2 = p1;
p2update = 1;
@@ -1183,14 +1185,14 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
CGeval(m, d, ctx, &p1);
// check for convergence
if (mju_abs(p1.deriv[0])<gtol) {
if (mju_abs(p1.deriv[0]) < gtol) {
ctx->LSslope = mju_abs(p1.deriv[0])*slopescl;
return p1.alpha; // SUCCESS
}
}
// check for failure to bracket
if (ctx->LSiter>=LSmaxiter) {
if (ctx->LSiter >= LSmaxiter) {
ctx->LSresult = 3; // could not bracket
ctx->LSslope = mju_abs(p1.deriv[0])*slopescl;
return p1.alpha;
@@ -1209,7 +1211,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
CGeval(m, d, ctx, &p1next);
// bracketed search
while (ctx->LSiter<LSmaxiter) {
while (ctx->LSiter < LSmaxiter) {
// evaluate at midpoint
pmid.alpha = 0.5*(p1.alpha + p2.alpha);
CGeval(m, d, ctx, &pmid);
@@ -1220,14 +1222,14 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
// check candidates for convergence
mjtNum bestcost = 0;
int bestind = -1;
for (int i=0; i<3; i++) {
if (mju_abs(candidates[i].deriv[0])<gtol &&
(bestind==-1 || candidates[i].cost<bestcost)) {
for (int i=0; i < 3; i++) {
if (mju_abs(candidates[i].deriv[0]) < gtol &&
(bestind == -1 || candidates[i].cost < bestcost)) {
bestcost = candidates[i].cost;
bestind = i;
}
}
if (bestind>=0) {
if (bestind >= 0) {
ctx->LSslope = mju_abs(candidates[bestind].deriv[0])*slopescl;
return candidates[bestind].alpha; // SUCCESS
}
@@ -1238,7 +1240,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
// no update possible: numerical accuracy reached, use midpoint
if (!b1 && !b2) {
if (pmid.cost<p0.cost) {
if (pmid.cost < p0.cost) {
ctx->LSresult = 0; // SUCCESS
} else {
ctx->LSresult = 7; // no improvement, could not bracket
@@ -1250,11 +1252,11 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
}
// choose bracket with best cost
if (p1.cost<=p2.cost && p1.cost<p0.cost) {
if (p1.cost <= p2.cost && p1.cost < p0.cost) {
ctx->LSresult = 4; // improvement but no convergence
ctx->LSslope = mju_abs(p1.deriv[0])*slopescl;
return p1.alpha;
} else if (p2.cost<=p1.cost && p2.cost<p0.cost) {
} else if (p2.cost <= p1.cost && p2.cost < p0.cost) {
ctx->LSresult = 4; // improvement but no convergence
ctx->LSslope = mju_abs(p2.deriv[0])*slopescl;
return p2.alpha;
@@ -1281,8 +1283,8 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
mju_copy(ctx->Hcone, ctx->H, ctx->nnz);
// add contributions
for (int i=0; i<nefc; i++) {
if (d->efc_state[i]==mjCNSTRSTATE_CONE) {
for (int i=0; i < nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_CONE) {
mjContact* con = d->contact + d->efc_id[i];
int dim = con->dim;
@@ -1297,14 +1299,14 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
// compute LTJ = L'*J for this contact
mju_zero(LTJ, dim*nnz);
for (int r=0; r<dim; r++) {
for (int c=0; c<=r; c++) {
for (int r=0; r < dim; r++) {
for (int c=0; c <= r; c++) {
mju_addToScl(LTJ+c*nnz, d->efc_J+d->efc_J_rowadr[i+r], local[r*dim+c], nnz);
}
}
// update
for (int r=0; r<dim; r++) {
for (int r=0; r < dim; r++) {
// copy data for this row
mju_copy(LTJ_row, LTJ+r*nnz, nnz);
memcpy(LTJ_ind, d->efc_J_colind+d->efc_J_rowadr[i+r], nnz*sizeof(int));
@@ -1320,14 +1322,14 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
else {
// compute LTJ = L'*J for this contact row
mju_zero(LTJ, dim*nv);
for (int r=0; r<dim; r++) {
for (int c=0; c<=r; c++) {
for (int r=0; r < dim; r++) {
for (int c=0; c <= r; c++) {
mju_addToScl(LTJ+c*nv, d->efc_J+(i+r)*nv, local[r*dim+c], nv);
}
}
// update
for (int r=0; r<dim; r++) {
for (int r=0; r < dim; r++) {
mju_cholUpdate(ctx->Hcone, LTJ+r*nv, nv, 1);
}
}
@@ -1352,8 +1354,8 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
// compute D corresponding to quad states
mjtNum* D = mj_stackAlloc(d, nefc);
for (int i=0; i<nefc; i++) {
if (d->efc_state[i]==mjCNSTRSTATE_QUADRATIC) {
for (int i=0; i < nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i];
} else {
D[i] = 0;
@@ -1381,7 +1383,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
d);
// rank-defficient, SHOULD NOT OCCUR
if (rank!=nv) {
if (rank != nv) {
mju_error("Rank-defficient Hessian in HessianDirect");
}
@@ -1390,7 +1392,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
// count nnz
ctx->nnz = 0;
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
ctx->nnz += ctx->rownnz[i];
}
if (ctx->nnz > nv*nv) { // SHOULD NOT OCCUR
@@ -1438,21 +1440,21 @@ static void HessianIncremental(const mjModel* m, mjData* d,
ctx->nupdate = 0;
// update H factorization
for (int i=0; i<nefc; i++) {
for (int i=0; i < nefc; i++) {
int flag_update = -1;
// add quad
if (oldstate[i]!=mjCNSTRSTATE_QUADRATIC && d->efc_state[i]==mjCNSTRSTATE_QUADRATIC) {
if (oldstate[i] != mjCNSTRSTATE_QUADRATIC && d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
flag_update = 1;
}
// subtract quad
else if (oldstate[i]==mjCNSTRSTATE_QUADRATIC && d->efc_state[i]!=mjCNSTRSTATE_QUADRATIC) {
else if (oldstate[i] == mjCNSTRSTATE_QUADRATIC && d->efc_state[i] != mjCNSTRSTATE_QUADRATIC) {
flag_update = 0;
}
// perform update if flagged
if (flag_update!=-1) {
if (flag_update != -1) {
// update with vec = J(i,:)*sqrt(D[i]))
if (mj_isSparse(m)) {
// get nnz and adr of row i
@@ -1473,7 +1475,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
ctx->nupdate++;
// recompute H directly if accuracy lost
if (rank<nv) {
if (rank < nv) {
mjFREESTACK;
HessianDirect(m, d, ctx);
@@ -1528,12 +1530,12 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
mju_scl(ctx.search, ctx.Mgrad, -1, nv);
// main loop
while (iter<maxiter) {
while (iter < maxiter) {
// perform linesearch
alpha = CGsearch(m, d, &ctx);
// no improvement: done
if (alpha==0) {
if (alpha == 0) {
break;
}
@@ -1559,8 +1561,8 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
// count state changes
int nchange = 0;
for (int i=0; i<nefc; i++) {
nchange += (d->efc_state[i]!=oldstate[i]);
for (int i=0; i < nefc; i++) {
nchange += (d->efc_state[i] != oldstate[i]);
}
// scale improvement, save stats, count
@@ -1570,7 +1572,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
ctx.nactive, nchange, ctx.LSiter, ctx.nupdate);
// termination
if (improvement<m->opt.tolerance || gradient<m->opt.tolerance) {
if (improvement < m->opt.tolerance || gradient < m->opt.tolerance) {
break;
}
@@ -1584,12 +1586,12 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
mju_max(mjMINVAL, mju_dot(gradold, Mgradold, nv));
// reset if negative
if (beta<0) {
if (beta < 0) {
beta = 0;
}
// update
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
ctx.search[i] = -ctx.Mgrad[i] + beta*ctx.search[i];
}
}
src/engine/engine_support.c
+221 -221
View File
@@ -36,7 +36,7 @@
#endif
#endif
#define mjVERSION 236
#define mjVERSION 236
#define mjVERSIONSTRING "2.3.6"
// names of disable flags
@@ -120,7 +120,7 @@ void mj_jac(const mjModel* m, const mjData* d,
da = m->body_dofadr[body] + m->body_dofnum[body] - 1;
// backward pass over dof ancestor chain
while (da>=0) {
while (da >= 0) {
// construct rotation jacobian
if (jacr) {
jacr[da] = cdof[6*da];
@@ -167,7 +167,7 @@ void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) {
mju_zero(jacp, 3*nv);
// forward pass starting from body
for (int b=body; b<m->nbody; b++) {
for (int b=body; b < m->nbody; b++) {
// end of body subtree, break from the loop
if (b > body && m->body_parentid[b] < body) {
break;
@@ -213,7 +213,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
// jacAxis_col = cross(jacr_col, axis)
if (jacAxis) {
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
jacAxis[ i] = jacr[ nv+i]*axis[2] - jacr[2*nv+i]*axis[1];
jacAxis[ nv+i] = jacr[2*nv+i]*axis[0] - jacr[ i]*axis[2];
jacAxis[2*nv+i] = jacr[ i]*axis[1] - jacr[ nv+i]*axis[0];
@@ -260,14 +260,14 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
ci = NV-1;
// backward pass over dof ancestor chain
while (da>=0) {
while (da >= 0) {
// find chain index for this dof
while (ci>=0 && chain[ci]>da) {
while (ci >= 0 && chain[ci] > da) {
ci--;
}
// make sure we found it; SHOULD NOT OCCUR
if (chain[ci]!=da) {
if (chain[ci] != da) {
mju_error("dof index %d not found in chain", da);
}
@@ -311,7 +311,7 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d,
// process dofs
int ci = start;
int end = m->body_dofadr[body] + m->body_dofnum[body];
for (int da=m->body_dofadr[body]; da<end; da++) {
for (int da=m->body_dofadr[body]; da < end; da++) {
// construct rotation jacobian
if (jacdifr) {
// plus sign
@@ -389,11 +389,11 @@ int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
if (issimple) {
// first body
mj_jacSparseSimple(m, d, jacdifp, jacdifr, pos1, b1, 0, NV,
b1<b2 ? 0 : m->body_dofnum[b2]);
b1 < b2 ? 0 : m->body_dofnum[b2]);
// second body
mj_jacSparseSimple(m, d, jacdifp, jacdifr, pos2, b2, 1, NV,
b2<b1 ? 0 : m->body_dofnum[b1]);
b2 < b1 ? 0 : m->body_dofnum[b1]);
}
// regular processing
@@ -442,186 +442,186 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) {
// get address list and size for object type
switch (type) {
case mjOBJ_BODY:
case mjOBJ_XBODY:
*mapadr -= mjLOAD_MULTIPLE*m->nbody;
*padr = m->name_bodyadr;
num = m->nbody;
mjFALLTHROUGH;
case mjOBJ_BODY:
case mjOBJ_XBODY:
*mapadr -= mjLOAD_MULTIPLE*m->nbody;
*padr = m->name_bodyadr;
num = m->nbody;
mjFALLTHROUGH;
case mjOBJ_JOINT:
*mapadr -= mjLOAD_MULTIPLE*m->njnt;
if (num < 0) {
*padr = m->name_jntadr;
num = m->njnt;
}
mjFALLTHROUGH;
case mjOBJ_JOINT:
*mapadr -= mjLOAD_MULTIPLE*m->njnt;
if (num < 0) {
*padr = m->name_jntadr;
num = m->njnt;
}
mjFALLTHROUGH;
case mjOBJ_GEOM:
*mapadr -= mjLOAD_MULTIPLE*m->ngeom;
if (num < 0) {
*padr = m->name_geomadr;
num = m->ngeom;
}
mjFALLTHROUGH;
case mjOBJ_GEOM:
*mapadr -= mjLOAD_MULTIPLE*m->ngeom;
if (num < 0) {
*padr = m->name_geomadr;
num = m->ngeom;
}
mjFALLTHROUGH;
case mjOBJ_SITE:
*mapadr -= mjLOAD_MULTIPLE*m->nsite;
if (num < 0) {
*padr = m->name_siteadr;
num = m->nsite;
}
mjFALLTHROUGH;
case mjOBJ_SITE:
*mapadr -= mjLOAD_MULTIPLE*m->nsite;
if (num < 0) {
*padr = m->name_siteadr;
num = m->nsite;
}
mjFALLTHROUGH;
case mjOBJ_CAMERA:
*mapadr -= mjLOAD_MULTIPLE*m->ncam;
if (num < 0) {
*padr = m->name_camadr;
num = m->ncam;
}
mjFALLTHROUGH;
case mjOBJ_CAMERA:
*mapadr -= mjLOAD_MULTIPLE*m->ncam;
if (num < 0) {
*padr = m->name_camadr;
num = m->ncam;
}
mjFALLTHROUGH;
case mjOBJ_LIGHT:
*mapadr -= mjLOAD_MULTIPLE*m->nlight;
if (num < 0) {
*padr = m->name_lightadr;
num = m->nlight;
}
mjFALLTHROUGH;
case mjOBJ_LIGHT:
*mapadr -= mjLOAD_MULTIPLE*m->nlight;
if (num < 0) {
*padr = m->name_lightadr;
num = m->nlight;
}
mjFALLTHROUGH;
case mjOBJ_MESH:
*mapadr -= mjLOAD_MULTIPLE*m->nmesh;
if (num < 0) {
*padr = m->name_meshadr;
num = m->nmesh;
}
mjFALLTHROUGH;
case mjOBJ_MESH:
*mapadr -= mjLOAD_MULTIPLE*m->nmesh;
if (num < 0) {
*padr = m->name_meshadr;
num = m->nmesh;
}
mjFALLTHROUGH;
case mjOBJ_SKIN:
*mapadr -= mjLOAD_MULTIPLE*m->nskin;
if (num < 0) {
*padr = m->name_skinadr;
num = m->nskin;
}
mjFALLTHROUGH;
case mjOBJ_SKIN:
*mapadr -= mjLOAD_MULTIPLE*m->nskin;
if (num < 0) {
*padr = m->name_skinadr;
num = m->nskin;
}
mjFALLTHROUGH;
case mjOBJ_HFIELD:
*mapadr -= mjLOAD_MULTIPLE*m->nhfield;
if (num < 0) {
*padr = m->name_hfieldadr;
num = m->nhfield;
}
mjFALLTHROUGH;
case mjOBJ_HFIELD:
*mapadr -= mjLOAD_MULTIPLE*m->nhfield;
if (num < 0) {
*padr = m->name_hfieldadr;
num = m->nhfield;
}
mjFALLTHROUGH;
case mjOBJ_TEXTURE:
*mapadr -= mjLOAD_MULTIPLE*m->ntex;
if (num < 0) {
*padr = m->name_texadr;
num = m->ntex;
}
mjFALLTHROUGH;
case mjOBJ_TEXTURE:
*mapadr -= mjLOAD_MULTIPLE*m->ntex;
if (num < 0) {
*padr = m->name_texadr;
num = m->ntex;
}
mjFALLTHROUGH;
case mjOBJ_MATERIAL:
*mapadr -= mjLOAD_MULTIPLE*m->nmat;
if (num < 0) {
*padr = m->name_matadr;
num = m->nmat;
}
mjFALLTHROUGH;
case mjOBJ_MATERIAL:
*mapadr -= mjLOAD_MULTIPLE*m->nmat;
if (num < 0) {
*padr = m->name_matadr;
num = m->nmat;
}
mjFALLTHROUGH;
case mjOBJ_PAIR:
*mapadr -= mjLOAD_MULTIPLE*m->npair;
if (num < 0) {
*padr = m->name_pairadr;
num = m->npair;
}
mjFALLTHROUGH;
case mjOBJ_PAIR:
*mapadr -= mjLOAD_MULTIPLE*m->npair;
if (num < 0) {
*padr = m->name_pairadr;
num = m->npair;
}
mjFALLTHROUGH;
case mjOBJ_EXCLUDE:
*mapadr -= mjLOAD_MULTIPLE*m->nexclude;
if (num < 0) {
*padr = m->name_excludeadr;
num = m->nexclude;
}
mjFALLTHROUGH;
case mjOBJ_EXCLUDE:
*mapadr -= mjLOAD_MULTIPLE*m->nexclude;
if (num < 0) {
*padr = m->name_excludeadr;
num = m->nexclude;
}
mjFALLTHROUGH;
case mjOBJ_EQUALITY:
*mapadr -= mjLOAD_MULTIPLE*m->neq;
if (num < 0) {
*padr = m->name_eqadr;
num = m->neq;
}
mjFALLTHROUGH;
case mjOBJ_EQUALITY:
*mapadr -= mjLOAD_MULTIPLE*m->neq;
if (num < 0) {
*padr = m->name_eqadr;
num = m->neq;
}
mjFALLTHROUGH;
case mjOBJ_TENDON:
*mapadr -= mjLOAD_MULTIPLE*m->ntendon;
if (num < 0) {
*padr = m->name_tendonadr;
num = m->ntendon;
}
mjFALLTHROUGH;
case mjOBJ_TENDON:
*mapadr -= mjLOAD_MULTIPLE*m->ntendon;
if (num < 0) {
*padr = m->name_tendonadr;
num = m->ntendon;
}
mjFALLTHROUGH;
case mjOBJ_ACTUATOR:
*mapadr -= mjLOAD_MULTIPLE*m->nu;
if (num < 0) {
*padr = m->name_actuatoradr;
num = m->nu;
}
mjFALLTHROUGH;
case mjOBJ_ACTUATOR:
*mapadr -= mjLOAD_MULTIPLE*m->nu;
if (num < 0) {
*padr = m->name_actuatoradr;
num = m->nu;
}
mjFALLTHROUGH;
case mjOBJ_SENSOR:
*mapadr -= mjLOAD_MULTIPLE*m->nsensor;
if (num < 0) {
*padr = m->name_sensoradr;
num = m->nsensor;
}
mjFALLTHROUGH;
case mjOBJ_SENSOR:
*mapadr -= mjLOAD_MULTIPLE*m->nsensor;
if (num < 0) {
*padr = m->name_sensoradr;
num = m->nsensor;
}
mjFALLTHROUGH;
case mjOBJ_NUMERIC:
*mapadr -= mjLOAD_MULTIPLE*m->nnumeric;
if (num < 0) {
*padr = m->name_numericadr;
num = m->nnumeric;
}
mjFALLTHROUGH;
case mjOBJ_NUMERIC:
*mapadr -= mjLOAD_MULTIPLE*m->nnumeric;
if (num < 0) {
*padr = m->name_numericadr;
num = m->nnumeric;
}
mjFALLTHROUGH;
case mjOBJ_TEXT:
*mapadr -= mjLOAD_MULTIPLE*m->ntext;
if (num < 0) {
*padr = m->name_textadr;
num = m->ntext;
}
mjFALLTHROUGH;
case mjOBJ_TEXT:
*mapadr -= mjLOAD_MULTIPLE*m->ntext;
if (num < 0) {
*padr = m->name_textadr;
num = m->ntext;
}
mjFALLTHROUGH;
case mjOBJ_TUPLE:
*mapadr -= mjLOAD_MULTIPLE*m->ntuple;
if (num < 0) {
*padr = m->name_tupleadr;
num = m->ntuple;
}
mjFALLTHROUGH;
case mjOBJ_TUPLE:
*mapadr -= mjLOAD_MULTIPLE*m->ntuple;
if (num < 0) {
*padr = m->name_tupleadr;
num = m->ntuple;
}
mjFALLTHROUGH;
case mjOBJ_KEY:
*mapadr -= mjLOAD_MULTIPLE*m->nkey;
if (num < 0) {
*padr = m->name_keyadr;
num = m->nkey;
}
mjFALLTHROUGH;
case mjOBJ_KEY:
*mapadr -= mjLOAD_MULTIPLE*m->nkey;
if (num < 0) {
*padr = m->name_keyadr;
num = m->nkey;
}
mjFALLTHROUGH;
case mjOBJ_PLUGIN:
*mapadr -= mjLOAD_MULTIPLE*m->nplugin;
if (num < 0) {
*padr = m->name_pluginadr;
num = m->nplugin;
}
mjFALLTHROUGH;
case mjOBJ_PLUGIN:
*mapadr -= mjLOAD_MULTIPLE*m->nplugin;
if (num < 0) {
*padr = m->name_pluginadr;
num = m->nplugin;
}
mjFALLTHROUGH;
default:
if (num < 0) {
*padr = 0;
num = 0;
}
default:
if (num < 0) {
*padr = 0;
num = 0;
}
}
return num;
@@ -653,15 +653,15 @@ int mj_name2id(const mjModel* m, int type, const char* name) {
do {
int j = m->names_map[mapadr + i];
if (j<0) {
if (j < 0) {
return -1;
}
if (!strncmp(name, m->names+adr[j], m->nnames-adr[j])) {
return j;
}
if ((++i)==num) i = 0;
} while (i!=hash);
if ((++i) == num)i = 0;
} while (i != hash);
}
return -1;
}
@@ -678,7 +678,7 @@ const char* mj_id2name(const mjModel* m, int type, int id) {
int num = _getnumadr(m, type, &adr, &mapadr);
// id is in [0, num) and the found name is not the empty string "\0"
if (id>=0 && id<num && m->names[adr[id]]) {
if (id >= 0 && id < num && m->names[adr[id]]) {
return m->names+adr[id];
}
@@ -694,9 +694,9 @@ void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M) {
int adr = 0, nv = m->nv;
mju_zero(dst, nv*nv);
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
int j = i;
while (j>=0) {
while (j >= 0) {
dst[i*nv+j] = M[adr];
dst[j*nv+i] = M[adr];
j = m->dof_parentid[j];
@@ -715,10 +715,10 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
mju_zero(res, nv);
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
#ifdef mjUSEAVX
// simple: diagonal division, AVX
if (m->dof_simplenum[i]>=4) {
if (m->dof_simplenum[i] >= 4) {
// init
__m256d result, val1, val2;
@@ -753,7 +753,7 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
// off-diagonal
int j = m->dof_parentid[i];
adr++;
while (j>=0) {
while (j >= 0) {
res[i] += M[adr]*vec[j];
res[j] += M[adr]*vec[i];
@@ -776,10 +776,10 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
mju_zero(res, nv);
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
#ifdef mjUSEAVX
// simple: diagonal division, AVX
if (m->dof_simplenum[i]>=4) {
if (m->dof_simplenum[i] >= 4) {
// init
__m256d result, val1, val2;
@@ -814,7 +814,7 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
// off-diagonal
int j = m->dof_parentid[i];
adr++;
while (j>=0) {
while (j >= 0) {
res[i] += qLD[adr]*vec[j];
// advance to next element
@@ -837,7 +837,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
if (rownnz && rowadr && colind) {
// special processing of simple dofs
int simplecnt = 0;
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
if (m->dof_simplenum[i]) {
// count simple
simplecnt++;
@@ -854,8 +854,8 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
// find dof in row, add
adr = rowadr[i];
int end = adr + rownnz[i];
while (adr<end)
if (colind[adr]==i) {
while (adr < end)
if (colind[adr] == i) {
dst[adr] += d->qM[m->dof_Madr[i]];
break;
} else {
@@ -863,7 +863,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
}
// not found: error
if (adr>=end) {
if (adr >= end) {
mju_error("mj_addM sparse: dst row expected to be empty");
}
}
@@ -871,7 +871,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
}
// done if all simple
if (simplecnt==nv) {
if (simplecnt == nv) {
return;
}
@@ -885,13 +885,13 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
mjtNum* sparse_buf = mj_stackAlloc(d, nv);
// convert M into sparse format, lower-triangular
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
if (!m->dof_simplenum[i]) {
// backward pass over dofs: construct M_row(i) in reverse order
adr = m->dof_Madr[i];
int j = i;
adr1 = 0;
while (j>=0) {
while (j >= 0) {
// assign
M[i*nv+adr1] = d->qM[adr];
M_colind[i*nv+adr1] = j;
@@ -909,7 +909,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
M_rowadr[i] = i*nv;
// reverse order
for (int k=0; k<adr1/2; k++) {
for (int k=0; k < adr1/2; k++) {
mjtNum tmp = M[i*nv+k];
M[i*nv+k] = M[i*nv+adr1-1-k];
M[i*nv+adr1-1-k] = tmp;
@@ -922,9 +922,9 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
}
// make symmetric
for (int i=1; i<nv; i++) {
for (int i=1; i < nv; i++) {
if (!m->dof_simplenum[i]) {
for (int k=nv*i; k<nv*i+M_rownnz[i]-1; k++) {
for (int k=nv*i; k < nv*i+M_rownnz[i]-1; k++) {
// add to row given by column index
adr1 = nv*M_colind[k] + M_rownnz[M_colind[k]]++;
M[adr1] = M[k];
@@ -934,7 +934,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
}
// add to destination
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
if (!m->dof_simplenum[i]) {
int new_nnz =
mju_combineSparse(dst + rowadr[i], M + M_rowadr[i], nv, 1, 1,
@@ -951,13 +951,13 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
// dense
else {
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
adr = m->dof_Madr[i];
int j = i;
while (j>=0) {
while (j >= 0) {
// add
dst[i*nv+j] += d->qM[adr];
if (j<i) {
if (j < i) {
dst[j*nv+i] += d->qM[adr];
}
@@ -1052,7 +1052,7 @@ void mj_applyFT(const mjModel* m, mjData* d,
mjtNum* qforce = mj_stackAlloc(d, nv);
// make sure body is in range
if (body<0 || body>=m->nbody) {
if (body < 0 || body >= m->nbody) {
mju_error("Invalid body %d in applyFT", body);
}
@@ -1076,7 +1076,7 @@ void mj_applyFT(const mjModel* m, mjData* d,
// accumulate xfrc_applied in qfrc
void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) {
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
if (!mju_isZero(d->xfrc_applied+6*i, 6)) {
mj_applyFT(m, d, d->xfrc_applied+6*i, d->xfrc_applied+6*i+3, d->xipos+3*i, i, qfrc);
}
@@ -1092,35 +1092,35 @@ void mj_objectVelocity(const mjModel* m, const mjData* d,
const mjtNum *pos = 0, *rot = 0;
// body-inertial
if (objtype==mjOBJ_BODY) {
if (objtype == mjOBJ_BODY) {
bodyid = objid;
pos = d->xipos+3*objid;
rot = (flg_local ? d->ximat+9*objid : 0);
}
// body-regular
else if (objtype==mjOBJ_XBODY) {
else if (objtype == mjOBJ_XBODY) {
bodyid = objid;
pos = d->xpos+3*objid;
rot = (flg_local ? d->xmat+9*objid : 0);
}
// geom
else if (objtype==mjOBJ_GEOM) {
else if (objtype == mjOBJ_GEOM) {
bodyid = m->geom_bodyid[objid];
pos = d->geom_xpos+3*objid;
rot = (flg_local ? d->geom_xmat+9*objid : 0);
}
// site
else if (objtype==mjOBJ_SITE) {
else if (objtype == mjOBJ_SITE) {
bodyid = m->site_bodyid[objid];
pos = d->site_xpos+3*objid;
rot = (flg_local ? d->site_xmat+9*objid : 0);
}
// camera
else if (objtype==mjOBJ_CAMERA) {
else if (objtype == mjOBJ_CAMERA) {
bodyid = m->cam_bodyid[objid];
pos = d->cam_xpos+3*objid;
rot = (flg_local ? d->cam_xmat+9*objid : 0);
@@ -1145,35 +1145,35 @@ void mj_objectAcceleration(const mjModel* m, const mjData* d,
mjtNum correction[3], vel[6];
// body-inertial
if (objtype==mjOBJ_BODY) {
if (objtype == mjOBJ_BODY) {
bodyid = objid;
pos = d->xipos+3*objid;
rot = (flg_local ? d->ximat+9*objid : 0);
}
// body-regular
else if (objtype==mjOBJ_XBODY) {
else if (objtype == mjOBJ_XBODY) {
bodyid = objid;
pos = d->xpos+3*objid;
rot = (flg_local ? d->xmat+9*objid : 0);
}
// geom
else if (objtype==mjOBJ_GEOM) {
else if (objtype == mjOBJ_GEOM) {
bodyid = m->geom_bodyid[objid];
pos = d->geom_xpos+3*objid;
rot = (flg_local ? d->geom_xmat+9*objid : 0);
}
// site
else if (objtype==mjOBJ_SITE) {
else if (objtype == mjOBJ_SITE) {
bodyid = m->site_bodyid[objid];
pos = d->site_xpos+3*objid;
rot = (flg_local ? d->site_xmat+9*objid : 0);
}
// camera
else if (objtype==mjOBJ_CAMERA) {
else if (objtype == mjOBJ_CAMERA) {
bodyid = m->cam_bodyid[objid];
pos = d->cam_xpos+3*objid;
rot = (flg_local ? d->cam_xmat+9*objid : 0);
@@ -1207,7 +1207,7 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]
mju_zero(result, 6);
// make sure contact is valid
if (id>=0 && id<d->ncon && d->contact[id].efc_address>=0) {
if (id >= 0 && id < d->ncon && d->contact[id].efc_address >= 0) {
// get contact pointer
con = d->contact + id;
@@ -1225,14 +1225,14 @@ void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]
void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
const mjtNum* qpos1, const mjtNum* qpos2) {
// loop over joints
for (int j=0; j<m->njnt; j++) {
for (int j=0; j < m->njnt; j++) {
// get addresses in qpos and qvel
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
switch (m->jnt_type[j]) {
case mjJNT_FREE:
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
qvel[vadr+i] = (qpos2[padr+i] - qpos1[padr+i]) / dt;
}
vadr += 3;
@@ -1259,7 +1259,7 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
// integrate qpos with given qvel
void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt) {
// loop over joints
for (int j=0; j<m->njnt; j++) {
for (int j=0; j < m->njnt; j++) {
// get addresses in qpos and qvel
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
@@ -1267,7 +1267,7 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum
switch (m->jnt_type[j]) {
case mjJNT_FREE:
// position update
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
qpos[padr+i] += dt * qvel[vadr+i];
}
padr += 3;
@@ -1294,9 +1294,9 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum
// normalize all quaternions in qpos-type vector
void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) {
// find quaternion fields and normalize
for (int i=0; i<m->njnt; i++) {
if (m->jnt_type[i]==mjJNT_BALL || m->jnt_type[i]==mjJNT_FREE) {
mju_normalize4(qpos+m->jnt_qposadr[i]+3*(m->jnt_type[i]==mjJNT_FREE));
for (int i=0; i < m->njnt; i++) {
if (m->jnt_type[i] == mjJNT_BALL || m->jnt_type[i] == mjJNT_FREE) {
mju_normalize4(qpos+m->jnt_qposadr[i]+3*(m->jnt_type[i] == mjJNT_FREE));
}
}
}
@@ -1312,13 +1312,13 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
// position
if (xpos && pos) {
// compute
if (sameframe==0) {
if (sameframe == 0) {
mju_rotVecMat(xpos, pos, d->xmat+9*body);
mju_addTo3(xpos, d->xpos+3*body);
}
// copy body position
else if (sameframe==1) {
else if (sameframe == 1) {
mju_copy3(xpos, d->xpos+3*body);
}
@@ -1331,13 +1331,13 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
// orientation
if (xmat && quat) {
// compute
if (sameframe==0) {
if (sameframe == 0) {
mju_mulQuat(tmp, d->xquat+4*body, quat);
mju_quat2Mat(xmat, tmp);
}
// copy body orientation
else if (sameframe==1) {
else if (sameframe == 1) {
mju_copy(xmat, d->xmat+9*body, 9);
}
@@ -1354,7 +1354,7 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
mjtNum mj_getTotalmass(const mjModel* m) {
mjtNum res = 0;
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
res += m->body_mass[i];
}
@@ -1369,7 +1369,7 @@ void mj_setTotalmass(mjModel* m, mjtNum newmass) {
mjtNum scale = mjMAX(mjMINVAL, newmass / mjMAX(mjMINVAL, mj_getTotalmass(m)));
// scale all masses and inertias
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
m->body_mass[i] *= scale;
m->body_inertia[3*i] *= scale;
m->body_inertia[3*i+1] *= scale;
@@ -1385,7 +1385,7 @@ void mj_setTotalmass(mjModel* m, mjtNum newmass) {
void mj_warning(mjData* d, int warning, int info) {
// check type
if (warning<0 || warning>=mjNWARNING) {
if (warning < 0 || warning >= mjNWARNING) {
mju_error("Invalid warning type %d", warning);
}
src/engine/engine_util_blas.c
+78 -78
View File
@@ -115,7 +115,7 @@ void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtN
mjtNum mju_normalize3(mjtNum vec[3]) {
mjtNum norm = mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
if (norm<mjMINVAL) {
if (norm < mjMINVAL) {
vec[0] = 1;
vec[1] = 0;
vec[2] = 0;
@@ -216,7 +216,7 @@ void mju_copy4(mjtNum res[4], const mjtNum data[4]) {
mjtNum mju_normalize4(mjtNum vec[4]) {
mjtNum norm = mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2] + vec[3]*vec[3]);
if (norm<mjMINVAL) {
if (norm < mjMINVAL) {
vec[0] = 1;
vec[1] = 0;
vec[2] = 0;
@@ -238,7 +238,7 @@ mjtNum mju_normalize4(mjtNum vec[4]) {
// res = 0
void mju_zero(mjtNum* res, int n) {
if (n>0) {
if (n > 0) {
memset(res, 0, n*sizeof(mjtNum));
}
}
@@ -247,7 +247,7 @@ void mju_zero(mjtNum* res, int n) {
// res = val
void mju_fill(mjtNum* res, mjtNum val, int n) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = val;
}
}
@@ -256,7 +256,7 @@ void mju_fill(mjtNum* res, mjtNum val, int n) {
// res = vec
void mju_copy(mjtNum* res, const mjtNum* vec, int n) {
if (n>0) {
if (n > 0) {
memcpy(res, vec, n*sizeof(mjtNum));
}
}
@@ -267,7 +267,7 @@ void mju_copy(mjtNum* res, const mjtNum* vec, int n) {
mjtNum mju_sum(const mjtNum* vec, int n) {
mjtNum res = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res += vec[i];
}
@@ -280,7 +280,7 @@ mjtNum mju_sum(const mjtNum* vec, int n) {
mjtNum mju_L1(const mjtNum* vec, int n) {
mjtNum res = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res += mju_abs(vec[i]);
}
@@ -297,14 +297,14 @@ void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) {
int n_4 = n - 4;
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d sclpar, val1, val1scl;
// init
sclpar = _mm256_set1_pd(scl);
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(vec+i);
val1scl = _mm256_mul_pd(val1, sclpar);
_mm256_storeu_pd(res+i, val1scl);
@@ -314,19 +314,19 @@ void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) {
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res[i] = vec[i]*scl;
res[i+1] = vec[i+1]*scl;
res[i+2] = vec[i+2]*scl;
} else if (n_i==2) {
} else if (n_i == 2) {
res[i] = vec[i]*scl;
res[i+1] = vec[i+1]*scl;
} else if (n_i==1) {
} else if (n_i == 1) {
res[i] = vec[i]*scl;
}
#else
for (; i<n; i++) {
for (; i < n; i++) {
res[i] = vec[i]*scl;
}
#endif
@@ -342,11 +342,11 @@ void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) {
int n_4 = n - 4;
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d sum, val1, val2;
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(vec1+i);
val2 = _mm256_loadu_pd(vec2+i);
sum = _mm256_add_pd(val1, val2);
@@ -357,19 +357,19 @@ void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) {
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res[i] = vec1[i] + vec2[i];
res[i+1] = vec1[i+1] + vec2[i+1];
res[i+2] = vec1[i+2] + vec2[i+2];
} else if (n_i==2) {
} else if (n_i == 2) {
res[i] = vec1[i] + vec2[i];
res[i+1] = vec1[i+1] + vec2[i+1];
} else if (n_i==1) {
} else if (n_i == 1) {
res[i] = vec1[i] + vec2[i];
}
#else
for (; i<n; i++) {
for (; i < n; i++) {
res[i] = vec1[i] + vec2[i];
}
#endif
@@ -385,11 +385,11 @@ void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) {
int n_4 = n - 4;
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d dif, val1, val2;
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(vec1+i);
val2 = _mm256_loadu_pd(vec2+i);
dif = _mm256_sub_pd(val1, val2);
@@ -400,19 +400,19 @@ void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) {
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res[i] = vec1[i] - vec2[i];
res[i+1] = vec1[i+1] - vec2[i+1];
res[i+2] = vec1[i+2] - vec2[i+2];
} else if (n_i==2) {
} else if (n_i == 2) {
res[i] = vec1[i] - vec2[i];
res[i+1] = vec1[i+1] - vec2[i+1];
} else if (n_i==1) {
} else if (n_i == 1) {
res[i] = vec1[i] - vec2[i];
}
#else
for (; i<n; i++) {
for (; i < n; i++) {
res[i] = vec1[i] - vec2[i];
}
#endif
@@ -428,11 +428,11 @@ void mju_addTo(mjtNum* res, const mjtNum* vec, int n) {
int n_4 = n - 4;
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d sum, val1, val2;
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(res+i);
val2 = _mm256_loadu_pd(vec+i);
sum = _mm256_add_pd(val1, val2);
@@ -443,19 +443,19 @@ void mju_addTo(mjtNum* res, const mjtNum* vec, int n) {
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res[i] += vec[i];
res[i+1] += vec[i+1];
res[i+2] += vec[i+2];
} else if (n_i==2) {
} else if (n_i == 2) {
res[i] += vec[i];
res[i+1] += vec[i+1];
} else if (n_i==1) {
} else if (n_i == 1) {
res[i] += vec[i];
}
#else
for (; i<n; i++) {
for (; i < n; i++) {
res[i] += vec[i];
}
#endif
@@ -471,11 +471,11 @@ void mju_subFrom(mjtNum* res, const mjtNum* vec, int n) {
int n_4 = n - 4;
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d dif, val1, val2;
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(res+i);
val2 = _mm256_loadu_pd(vec+i);
dif = _mm256_sub_pd(val1, val2);
@@ -486,19 +486,19 @@ void mju_subFrom(mjtNum* res, const mjtNum* vec, int n) {
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res[i] -= vec[i];
res[i+1] -= vec[i+1];
res[i+2] -= vec[i+2];
} else if (n_i==2) {
} else if (n_i == 2) {
res[i] -= vec[i];
res[i+1] -= vec[i+1];
} else if (n_i==1) {
} else if (n_i == 1) {
res[i] -= vec[i];
}
#else
for (; i<n; i++) {
for (; i < n; i++) {
res[i] -= vec[i];
}
#endif
@@ -514,14 +514,14 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) {
int n_4 = n - 4;
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d sclpar, sum, val1, val2, val2scl;
// init
sclpar = _mm256_set1_pd(scl);
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(res+i);
val2 = _mm256_loadu_pd(vec+i);
val2scl = _mm256_mul_pd(val2, sclpar);
@@ -533,19 +533,19 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) {
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res[i] += vec[i]*scl;
res[i+1] += vec[i+1]*scl;
res[i+2] += vec[i+2]*scl;
} else if (n_i==2) {
} else if (n_i == 2) {
res[i] += vec[i]*scl;
res[i+1] += vec[i+1]*scl;
} else if (n_i==1) {
} else if (n_i == 1) {
res[i] += vec[i]*scl;
}
#else
for (; i<n; i++) {
for (; i < n; i++) {
res[i] += vec[i]*scl;
}
#endif
@@ -559,14 +559,14 @@ void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl,
int n_4 = n - 4;
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d sclpar, sum, val1, val2, val2scl;
// init
sclpar = _mm256_set1_pd(scl);
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(vec1+i);
val2 = _mm256_loadu_pd(vec2+i);
val2scl = _mm256_mul_pd(val2, sclpar);
@@ -578,19 +578,19 @@ void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl,
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res[i] = vec1[i] + vec2[i]*scl;
res[i+1] = vec1[i+1] + vec2[i+1]*scl;
res[i+2] = vec1[i+2] + vec2[i+2]*scl;
} else if (n_i==2) {
} else if (n_i == 2) {
res[i] = vec1[i] + vec2[i]*scl;
res[i+1] = vec1[i+1] + vec2[i+1]*scl;
} else if (n_i==1) {
} else if (n_i == 1) {
res[i] = vec1[i] + vec2[i]*scl;
}
#else
for (; i<n; i++) {
for (; i < n; i++) {
res[i] = vec1[i] + vec2[i]*scl;
}
#endif
@@ -603,14 +603,14 @@ mjtNum mju_normalize(mjtNum* res, int n) {
mjtNum norm = (mjtNum)mju_sqrt(mju_dot(res, res, n));
mjtNum normInv;
if (norm<mjMINVAL) {
if (norm < mjMINVAL) {
res[0] = 1;
for (int i=1; i<n; i++) {
for (int i=1; i < n; i++) {
res[i] = 0;
}
} else {
normInv = 1/norm;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] *= normInv;
}
}
@@ -635,7 +635,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) {
#ifdef mjUSEAVX
// vector part
if (n_4>=0) {
if (n_4 >= 0) {
__m256d sum, prod, val1, val2;
__m128d vlow, vhigh, high64;
@@ -646,7 +646,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) {
i = 4;
// parallel computation
while (i<=n_4) {
while (i <= n_4) {
val1 = _mm256_loadu_pd(vec1+i);
val2 = _mm256_loadu_pd(vec2+i);
prod = _mm256_mul_pd(val1, val2);
@@ -671,7 +671,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) {
mjtNum res2 = 0;
mjtNum res3 = 0;
for (; i<=n_4; i+=4) {
for (; i <= n_4; i+=4) {
res0 += vec1[i] * vec2[i];
res1 += vec1[i+1] * vec2[i+1];
res2 += vec1[i+2] * vec2[i+2];
@@ -682,11 +682,11 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) {
// process remaining
int n_i = n - i;
if (n_i==3) {
if (n_i == 3) {
res += vec1[i]*vec2[i] + vec1[i+1]*vec2[i+1] + vec1[i+2]*vec2[i+2];
} else if (n_i==2) {
} else if (n_i == 2) {
res += vec1[i]*vec2[i] + vec1[i+1]*vec2[i+1];
} else if (n_i==1) {
} else if (n_i == 1) {
res += vec1[i]*vec2[i];
}
return res;
@@ -696,7 +696,7 @@ mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) {
// multiply matrix and vector
void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc) {
for (int r=0; r<nr; r++) {
for (int r=0; r < nr; r++) {
res[r] = mju_dot(mat + r*nc, vec, nc);
}
}
@@ -708,7 +708,7 @@ void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, i
mjtNum tmp;
mju_zero(res, nc);
for (int r=0; r<nr; r++) {
for (int r=0; r < nr; r++) {
if ((tmp = vec[r])) {
mju_addToScl(res, mat+r*nc, tmp, nc);
}
@@ -720,7 +720,7 @@ void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, i
// multiply square matrix with vectors on both sides: return vec1'*mat*vec2
mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n) {
mjtNum res = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res += vec1[i] * mju_dot(mat + i*n, vec2, n);
}
return res;
@@ -732,8 +732,8 @@ mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec
// transpose matrix
void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc) {
for (int i=0; i<nr; i++) {
for (int j=0; j<nc; j++) {
for (int i=0; i < nr; i++) {
for (int j=0; j < nc; j++) {
res[j*nr+i] = mat[i*nc+j];
}
}
@@ -743,9 +743,9 @@ void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc) {
// symmetrize square matrix res = (mat + mat')/2
void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i*(n+1)] = mat[i*(n+1)];
for (int j=0; j<i; j++) {
for (int j=0; j < i; j++) {
res[i*n+j] = res[j*n+i] = 0.5 * (mat[i*n+j] + mat[j*n+i]);
}
}
@@ -756,7 +756,7 @@ void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n) {
// identity matrix
void mju_eye(mjtNum* mat, int n) {
mju_zero(mat, n*n);
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
mat[i*(n + 1)] = 1;
}
}
@@ -772,8 +772,8 @@ void mju_mulMatMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2,
mju_zero(res, r1*c2);
for (int i=0; i<r1; i++) {
for (int k=0; k<c1; k++) {
for (int i=0; i < r1; i++) {
for (int k=0; k < c1; k++) {
if ((tmp = mat1[i*c1+k])) {
mju_addToScl(res+i*c2, mat2+k*c2, tmp, c2);
}
@@ -786,8 +786,8 @@ void mju_mulMatMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2,
// multiply matrices, second argument transposed
void mju_mulMatMatT(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2,
int r1, int c1, int r2) {
for (int i=0; i<r1; i++) {
for (int j=0; j<r2; j++) {
for (int i=0; i < r1; i++) {
for (int j=0; j < r2; j++) {
res[i*r2+j] = mju_dot(mat1+i*c1, mat2+j*c1, c1);
}
}
@@ -802,9 +802,9 @@ void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, in
// half of MatMat routine: only lower triangle
mju_zero(res, nc*nc);
if (diag) {
for (int j=0; j<nr; j++) {
for (int j=0; j < nr; j++) {
if (diag[j]) {
for (int i=0; i<nc; i++) {
for (int i=0; i < nc; i++) {
if ((tmp = mat[j*nc+i])) {
mju_addToScl(res+i*nc, mat+j*nc, tmp*diag[j], i+1);
}
@@ -812,8 +812,8 @@ void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, in
}
}
} else {
for (int i=0; i<nc; i++) {
for (int j=0; j<nr; j++) {
for (int i=0; i < nc; i++) {
for (int j=0; j < nr; j++) {
if ((tmp = mat[j*nc+i])) {
mju_addToScl(res+i*nc, mat+j*nc, tmp, i+1);
}
@@ -822,8 +822,8 @@ void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, in
}
// make symmetric
for (int i=0; i<nc; i++) {
for (int j=i+1; j<nc; j++) {
for (int i=0; i < nc; i++) {
for (int j=i+1; j < nc; j++) {
res[i*nc+j] = res[j*nc+i];
}
}
@@ -838,8 +838,8 @@ void mju_mulMatTMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2,
mju_zero(res, c1*c2);
for (int i=0; i<r1; i++) {
for (int j=0; j<c1; j++) {
for (int i=0; i < r1; i++) {
for (int j=0; j < c1; j++) {
if ((tmp = mat1[i*c1+j])) {
mju_addToScl(res+j*c2, mat2+i*c2, tmp, c2);
}
src/engine/engine_util_errmem.c
+1 -1
View File
@@ -139,7 +139,7 @@ void mju_error(const char* msg, ...) {
} else if (mju_user_error) {
mju_user_error(errmsg);
} else {
// write to log and console
// write to log and console
mju_writeLog("ERROR", errmsg);
printf("ERROR: %s\n\nPress Enter to exit ...", errmsg);
src/engine/engine_util_misc.c
+78 -78
View File
@@ -37,7 +37,7 @@ static mjtByte is_intersect(const mjtNum* p1, const mjtNum* p2,
// compute determinant, check
mjtNum det = (p4[1]-p3[1])*(p2[0]-p1[0]) - (p4[0]-p3[0])*(p2[1]-p1[1]);
if (fabs(det)<mjMINVAL) {
if (fabs(det) < mjMINVAL) {
return 0;
}
@@ -45,7 +45,7 @@ static mjtByte is_intersect(const mjtNum* p1, const mjtNum* p2,
a = ((p4[0]-p3[0])*(p1[1]-p3[1]) - (p4[1]-p3[1])*(p1[0]-p3[0])) / det;
b = ((p2[0]-p1[0])*(p1[1]-p3[1]) - (p2[1]-p1[1])*(p1[0]-p3[0])) / det;
return ((a>=0 && a<=1 && b>=0 && b<=1) ? 1 : 0);
return ((a >= 0 && a <= 1 && b >= 0 && b <= 1) ? 1 : 0);
}
@@ -63,7 +63,7 @@ static mjtNum length_circle(const mjtNum* p0, const mjtNum* p1, int ind, mjtNum
// flip if necessary
cross = p0[1]*p1[0]-p0[0]*p1[1];
if ((cross>0 && ind) || (cross<0 && !ind)) {
if ((cross > 0 && ind) || (cross < 0 && !ind)) {
angle = 2*mjPI - angle;
}
@@ -85,37 +85,37 @@ static mjtNum wrap_circle(mjtNum* pnt, const mjtNum* d, const mjtNum* sd, mjtNum
int sgn;
// either point inside circle or circle too small: no wrap
if (sqlen0<sqrad || sqlen1<sqrad || rad<mjMINVAL) {
if (sqlen0 < sqrad || sqlen1 < sqrad || rad < mjMINVAL) {
return -1;
}
// points too close: no wrap
dd = dif[0]*dif[0] + dif[1]*dif[1];
if (dd<mjMINVAL) {
if (dd < mjMINVAL) {
return -1;
}
// find nearest point on line segment to origin: a*dif + d0
a = -(dif[0]*d[0]+dif[1]*d[1])/dd;
if (a<0) {
if (a < 0) {
a = 0;
} else if (a>1) {
} else if (a > 1) {
a = 1;
}
tmp[0] = a*dif[0] + d[0];
tmp[1] = a*dif[1] + d[1];
// check for intersection and side
if (tmp[0]*tmp[0]+tmp[1]*tmp[1]>sqrad && (!sd || mju_dot(sd, tmp, 2)>=0)) {
if (tmp[0]*tmp[0]+tmp[1]*tmp[1] > sqrad && (!sd || mju_dot(sd, tmp, 2) >= 0)) {
return -1;
}
// construct the two solutions, compute goodness
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
sqrt0 = mju_sqrt(sqlen0 - sqrad);
sqrt1 = mju_sqrt(sqlen1 - sqrad);
sgn = (i==0 ? 1 : -1);
sgn = (i == 0 ? 1 : -1);
sol[i][0][0] = (d[0]*sqrad + sgn*rad*d[1]*sqrt0)/sqlen0;
sol[i][0][1] = (d[1]*sqrad - sgn*rad*d[0]*sqrt0)/sqlen0;
@@ -139,7 +139,7 @@ static mjtNum wrap_circle(mjtNum* pnt, const mjtNum* d, const mjtNum* sd, mjtNum
}
// select the better solution
int i = (good[0]>good[1] ? 0 : 1);
int i = (good[0] > good[1] ? 0 : 1);
pnt[0] = sol[i][0][0];
pnt[1] = sol[i][0][1];
pnt[2] = sol[i][1][0];
@@ -173,20 +173,20 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
mjtNum dd = dif[0]*dif[0] + dif[1]*dif[1];
// either point inside circle or circle too small: no wrap
if (len0<=rad || len1<=rad || rad<mjMINVAL || len0<mjMINVAL || len1<mjMINVAL) {
if (len0 <= rad || len1 <= rad || rad < mjMINVAL || len0 < mjMINVAL || len1 < mjMINVAL) {
return -1;
}
// segment-circle intersection: no wrap
if (dd>mjMINVAL) {
if (dd > mjMINVAL) {
// find nearest point on line segment to origin: d0 + a*dif
mjtNum a = -(dif[0]*d[0]+dif[1]*d[1])/dd;
// in segment
if (a>0 && a<1) {
if (a > 0 && a < 1) {
mjtNum tmp[2];
mju_addScl(tmp, d, dif, a, 2);
if (mju_norm(tmp, 2)<=rad) {
if (mju_norm(tmp, 2) <= rad) {
return -1;
}
}
@@ -204,9 +204,9 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
mjtNum A = rad/len0;
mjtNum B = rad/len1;
mjtNum cosG = (len0*len0 + len1*len1 - dd) / (2*len0*len1);
if (cosG<-1+mjMINVAL) {
if (cosG < -1+mjMINVAL) {
return -1;
} else if (cosG>1-mjMINVAL) {
} else if (cosG > 1-mjMINVAL) {
return 0;
}
mjtNum G = mju_acos(cosG);
@@ -216,20 +216,20 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
mjtNum f = mju_asin(A*z) + mju_asin(B*z) - 2*mju_asin(z) + G;
// make sure init is not on the other side
if (f>0) {
if (f > 0) {
return 0;
}
// Newton method
int iter;
for (iter=0; iter<maxiter && mju_abs(f)>tolerance; iter++) {
for (iter=0; iter < maxiter && mju_abs(f) > tolerance; iter++) {
// derivative
mjtNum df = A/mju_max(mjMINVAL, mju_sqrt(1-z*z*A*A)) +
B/mju_max(mjMINVAL, mju_sqrt(1-z*z*B*B)) -
2/mju_max(mjMINVAL, mju_sqrt(1-z*z));
// check sign; SHOULD NOT OCCUR
if (df>-mjMINVAL) {
if (df > -mjMINVAL) {
return 0;
}
@@ -237,7 +237,7 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
mjtNum z1 = z - f/df;
// make sure we are moving to the left; SHOULD NOT OCCUR
if (z1>z) {
if (z1 > z) {
return 0;
}
@@ -246,13 +246,13 @@ static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) {
f = mju_asin(A*z) + mju_asin(B*z) - 2*mju_asin(z) + G;
// exit if positive; SHOULD NOT OCCUR
if (f>tolerance) {
if (f > tolerance) {
return 0;
}
}
// check convergence
if (iter>=maxiter) {
if (iter >= maxiter) {
return 0;
}
@@ -286,7 +286,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
mjtNum L0, L1;
// check object type; SHOULD NOT OCCUR
if (type!=mjWRAP_SPHERE && type!=mjWRAP_CYLINDER) {
if (type != mjWRAP_SPHERE && type != mjWRAP_CYLINDER) {
mju_error("mju_wrap: unknown wrapping object type %d", type);
}
@@ -297,12 +297,12 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
mju_mulMatTVec(p[1], xmat, tmp, 3, 3);
// too close to origin: return
if (mju_norm3(p[0])<mjMINVAL || mju_norm3(p[1])<mjMINVAL) {
if (mju_norm3(p[0]) < mjMINVAL || mju_norm3(p[1]) < mjMINVAL) {
return -1;
}
// construct 2D frame for circle wrap
if (type==mjWRAP_SPHERE) {
if (type == mjWRAP_SPHERE) {
// 1st axis = p0
mju_copy3(axis[0], p[0]);
mju_normalize3(axis[0]);
@@ -312,15 +312,15 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
mjtNum nrm = mju_normalize3(normal);
// if (p0, p1) parallel: different normal
if (nrm<mjMINVAL) {
if (nrm < mjMINVAL) {
// find max component of axis0
int i = 0;
if (mju_abs(axis[0][1])>mju_abs(axis[0][0]) &&
mju_abs(axis[0][1])>mju_abs(axis[0][2])) {
if (mju_abs(axis[0][1]) > mju_abs(axis[0][0]) &&
mju_abs(axis[0][1]) > mju_abs(axis[0][2])) {
i = 1;
}
if (mju_abs(axis[0][2])>mju_abs(axis[0][0]) &&
mju_abs(axis[0][2])>mju_abs(axis[0][1])) {
if (mju_abs(axis[0][2]) > mju_abs(axis[0][0]) &&
mju_abs(axis[0][2]) > mju_abs(axis[0][1])) {
i = 2;
}
@@ -374,7 +374,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
}
// apply inside wrap
if (side && sd[0]==0 && sd[1]==0) {
if (side && sd[0] == 0 && sd[1] == 0) {
wlen = wrap_inside(pnt, d, size[0]);
}
@@ -384,12 +384,12 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
}
// no wrap
if (wlen<0) {
if (wlen < 0) {
return -1;
}
// reconstruct 3D points in local frame: res
for (int i=0; i<2; i++) {
for (int i=0; i < 2; i++) {
// res = axis0*d0 + axis1*d1
mju_scl3(res+3*i, axis[0], pnt[2*i]);
mju_scl3(tmp, axis[1], pnt[2*i+1]);
@@ -397,7 +397,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
}
// cylinder: correct along z
if (type==mjWRAP_CYLINDER) {
if (type == mjWRAP_CYLINDER) {
// set vertical coordinates
L0 = mju_sqrt((p[0][0]-res[0])*(p[0][0]-res[0]) + (p[0][1]-res[1])*(p[0][1]-res[1]));
L1 = mju_sqrt((p[1][0]-res[3])*(p[1][0]-res[3]) + (p[1][1]-res[4])*(p[1][1]-res[4]));
@@ -466,7 +466,7 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
mjtNum fvmax = prm[8];
// scale force if negative
if (force<0) {
if (force < 0) {
force = scale / mjMAX(mjMINVAL, acc0);
}
@@ -484,16 +484,16 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
// length curve
mjtNum FL = 0;
if (L>=lmin && L<=a) {
if (L >= lmin && L <= a) {
x = (L-lmin) / mjMAX(mjMINVAL, a-lmin);
FL = 0.5*x*x;
} else if (L<=1) {
} else if (L <= 1) {
x = (1-L) / mjMAX(mjMINVAL, 1-a);
FL = 1 - 0.5*x*x;
} else if (L<=b) {
} else if (L <= b) {
x = (L-1) / mjMAX(mjMINVAL, b-1);
FL = 1 - 0.5*x*x;
} else if (L<=lmax) {
} else if (L <= lmax) {
x = (lmax-L) / mjMAX(mjMINVAL, lmax-b);
FL = 0.5*x*x;
}
@@ -501,11 +501,11 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
// velocity curve
mjtNum FV;
mjtNum y = fvmax-1;
if (V<=-1) {
if (V <= -1) {
FV = 0;
} else if (V<=0) {
} else if (V <= 0) {
FV = (V+1)*(V+1);
} else if (V<=y) {
} else if (V <= y) {
FV = fvmax - (y-V)*(y-V) / mjMAX(mjMINVAL, y);
} else {
FV = fvmax;
@@ -528,7 +528,7 @@ mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
mjtNum fpmax = prm[7];
// scale force if negative
if (force<0) {
if (force < 0) {
force = scale / mjMAX(mjMINVAL, acc0);
}
@@ -540,9 +540,9 @@ mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
// half-quadratic to (L0+lmax)/2, linear beyond
mjtNum b = 0.5*(1+lmax);
if (L<=1) {
if (L <= 1) {
return 0;
} else if (L<=b) {
} else if (L <= b) {
mjtNum x = (L-1) / mjMAX(mjMINVAL, b-1);
return -force*fpmax*0.5*x*x;
} else {
@@ -606,7 +606,7 @@ void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, i
// arbitary redundancy resolution:
// pyramid0_i + pyramid1_i = force_normal/(dim-1) = a
// pyramid0_i - pyramid1_i = force_tangent_i/mu_i = b
for (int i=0; i<dim-1; i++) {
for (int i=0; i < dim-1; i++) {
b = mju_min(a, force[i+1]/mu[i]);
pyramid[2*i] = 0.5*(a+b);
pyramid[2*i+1] = 0.5*(a-b);
@@ -618,19 +618,19 @@ void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, i
// convert pyramid representation to contact force
void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, int dim) {
// special handling of frictionless contacts
if (dim==1) {
if (dim == 1) {
force[0] = pyramid[0];
return;
}
// force_normal = sum(pyramid0_i + pyramid1_i)
force[0] = 0;
for (int i=0; i<2*(dim-1); i++) {
for (int i=0; i < 2*(dim-1); i++) {
force[0] += pyramid[i];
}
// force_tangent_i = (pyramid0_i - pyramid1_i) * mu_i
for (int i=0; i<dim-1; i++) {
for (int i=0; i < dim-1; i++) {
force[i+1] = (pyramid[2*i] - pyramid[2*i+1]) * mu[i];
}
}
@@ -646,7 +646,7 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t)
// overdamping
// pos(t) = c1*exp(r1*t) + c2*exp(r2*t); r12 = (-b +- sqrt(det))/2
if (det>mjMINVAL) {
if (det > mjMINVAL) {
// compute w = sqrt(det)/2
w = mju_sqrt(det)/2;
@@ -664,7 +664,7 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t)
// critical damping
// pos(t) = exp(-b*t/2) * (c1 + c2*t)
else if (det<=mjMINVAL && det>=-mjMINVAL) {
else if (det <= mjMINVAL && det >= -mjMINVAL) {
// compute coefficients
c1 = pos0;
c2 = vel0 + b*c1/2;
@@ -692,8 +692,8 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t)
// print matrix to screen
void mju_printMat(const mjtNum* mat, int nr, int nc) {
for (int r=0; r<nr; r++) {
for (int c=0; c<nc; c++) {
for (int r=0; r < nr; r++) {
for (int c=0; c < nc; c++) {
printf("%.8f ", mat[r*nc+c]);
}
printf("\n");
@@ -707,8 +707,8 @@ void mju_printMat(const mjtNum* mat, int nr, int nc) {
void mju_printMatSparse(const mjtNum* mat, int nr,
const int* rownnz, const int* rowadr,
const int* colind) {
for (int r=0; r<nr; r++) {
for (int adr=rowadr[r]; adr<rowadr[r]+rownnz[r]; adr++) {
for (int r=0; r < nr; r++) {
for (int adr=rowadr[r]; adr < rowadr[r]+rownnz[r]; adr++) {
printf("(%d %d): %9.6f ", r, colind[adr], mat[adr]);
}
printf("\n");
@@ -742,9 +742,9 @@ mjtNum mju_max(mjtNum a, mjtNum b) {
// clip x to the range [min, max]
mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) {
if (x<min) {
if (x < min) {
return min;
} else if (x>max) {
} else if (x > max) {
return max;
} else {
return x;
@@ -755,9 +755,9 @@ mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) {
// sign function
mjtNum mju_sign(mjtNum x) {
if (x<0) {
if (x < 0) {
return -1;
} else if (x>0) {
} else if (x > 0) {
return 1;
} else {
return 0;
@@ -972,13 +972,13 @@ const char* mju_writeNumBytes(size_t nbytes) {
int i;
static mjTHREADLOCAL char message[20];
static const char suffix[] = " KMGTPE";
for (i=0; i<6; i++) {
for (i=0; i < 6; i++) {
const size_t bits = (size_t)(1) << (10*(6-i));
if (nbytes >= bits && !(nbytes & (bits - 1))) {
break;
}
}
if (i<6) {
if (i < 6) {
mjSNPRINTF(message, "%zu%c", nbytes >> (10*(6-i)), suffix[6-i]);
} else {
mjSNPRINTF(message, "%zu", nbytes >> (10*(6-i)));
@@ -1042,15 +1042,15 @@ const char* mju_warningText(int warning, size_t info) {
// return 1 if nan or abs(x)>mjMAXVAL, 0 otherwise
int mju_isBad(mjtNum x) {
return (x!=x || x>mjMAXVAL || x<-mjMAXVAL);
return (x != x || x > mjMAXVAL || x < -mjMAXVAL);
}
// return 1 if all elements are 0
int mju_isZero(mjtNum* vec, int n) {
for (int i=0; i<n; i++) {
if (vec[i]!=0) {
for (int i=0; i < n; i++) {
if (vec[i] != 0) {
return 0;
}
}
@@ -1069,7 +1069,7 @@ mjtNum mju_standardNormal(mjtNum* num2) {
x1 = scale * (mjtNum)rand() - 1.0;
x2 = scale * (mjtNum)rand() - 1.0;
w = x1 * x1 + x2 * x2;
} while (w>=1.0 || w==0);
} while (w >= 1.0 || w == 0);
w = mju_sqrt((-2.0 * mju_log(w)) / w);
if (num2) {
@@ -1083,7 +1083,7 @@ mjtNum mju_standardNormal(mjtNum* num2) {
// convert from float to mjtNum
void mju_f2n(mjtNum* res, const float* vec, int n) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = (mjtNum) vec[i];
}
}
@@ -1092,7 +1092,7 @@ void mju_f2n(mjtNum* res, const float* vec, int n) {
// convert from mjtNum to float
void mju_n2f(float* res, const mjtNum* vec, int n) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = (float) vec[i];
}
}
@@ -1100,7 +1100,7 @@ void mju_n2f(float* res, const mjtNum* vec, int n) {
// convert from double to mjtNum
void mju_d2n(mjtNum* res, const double* vec, int n) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = (mjtNum) vec[i];
}
}
@@ -1109,7 +1109,7 @@ void mju_d2n(mjtNum* res, const double* vec, int n) {
// convert from mjtNum to double
void mju_n2d(double* res, const mjtNum* vec, int n) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = (double) vec[i];
}
}
@@ -1118,10 +1118,10 @@ void mju_n2d(double* res, const mjtNum* vec, int n) {
// insertion sort, increasing order
void mju_insertionSort(mjtNum* list, int n) {
for (int i=1; i<n; i++) {
for (int i=1; i < n; i++) {
mjtNum x = list[i];
int j = i-1;
while (j>=0 && list[j]>x) {
while (j >= 0 && list[j] > x) {
list[j+1] = list[j];
j--;
}
@@ -1133,10 +1133,10 @@ void mju_insertionSort(mjtNum* list, int n) {
// integer insertion sort, increasing order
void mju_insertionSortInt(int* list, int n) {
for (int i=1; i<n; i++) {
for (int i=1; i < n; i++) {
int x = list[i];
int j = i-1;
while (j>=0 && list[j]>x) {
while (j >= 0 && list[j] > x) {
list[j+1] = list[j];
j--;
}
@@ -1152,7 +1152,7 @@ mjtNum mju_Halton(int index, int base) {
mjtNum b = (mjtNum)base;
mjtNum f = 1/b, hn = 0;
while (n0>0) {
while (n0 > 0) {
int n1 = n0/base;
int r = n0 - n1*base;
hn += f*r;
@@ -1167,7 +1167,7 @@ mjtNum mju_Halton(int index, int base) {
// Call strncpy, then set dst[n-1] = 0.
char* mju_strncpy(char *dst, const char *src, int n) {
if (dst && src && n>0) {
if (dst && src && n > 0) {
strncpy(dst, src, n);
dst[n-1] = 0;
}
@@ -1180,10 +1180,10 @@ char* mju_strncpy(char *dst, const char *src, int n) {
// sigmoid function over 0<=x<=1 using quintic polynomial
mjtNum mju_sigmoid(mjtNum x) {
// fast return
if (x<=0) {
if (x <= 0) {
return 0;
}
if (x>=1) {
if (x >= 1) {
return 1;
}
src/engine/engine_util_solve.c
+124 -124
View File
@@ -35,7 +35,7 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
mjtNum tmp;
// in-place Cholesky factorization
for (int j=0; j<n; j++) {
for (int j=0; j < n; j++) {
// compute new diagonal
tmp = mat[j*(n+1)];
if (j) {
@@ -43,7 +43,7 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
}
// correct diagonal values below threshold
if (tmp<mindiag) {
if (tmp < mindiag) {
tmp = mindiag;
rank--;
}
@@ -53,7 +53,7 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
// process off-diagonal entries
tmp = 1/mat[j*(n+1)];
for (int i=j+1; i<n; i++) {
for (int i=j+1; i < n; i++) {
mat[i*n+j] = (mat[i*n+j] - mju_dot(mat+i*n, mat+j*n, j)) * tmp;
}
}
@@ -66,12 +66,12 @@ int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag) {
// Cholesky solve
void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n) {
// copy if source and destination are different
if (res!=vec) {
if (res != vec) {
mju_copy(res, vec, n);
}
// forward substitution: solve L*res = vec
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (i) {
res[i] -= mju_dot(mat+i*n, res, i);
}
@@ -81,9 +81,9 @@ void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n) {
}
// backward substitution: solve L'*res = res
for (int i=n-1; i>=0; i--) {
if (i<n-1) {
for (int j=i+1; j<n; j++) {
for (int i=n-1; i >= 0; i--) {
if (i < n-1) {
for (int j=i+1; j < n; j++) {
res[i] -= mat[j*n+i] * res[j];
}
}
@@ -99,12 +99,12 @@ int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) {
int rank = n;
mjtNum r, c, cinv, s, Lkk, tmp;
for (int k=0; k<n; k++) {
for (int k=0; k < n; k++) {
if (x[k]) {
// prepare constants
Lkk = mat[k*(n+1)];
tmp = Lkk*Lkk + (flg_plus ? x[k]*x[k] : -x[k]*x[k]);
if (tmp<mjMINVAL) {
if (tmp < mjMINVAL) {
tmp = mjMINVAL;
rank--;
}
@@ -118,17 +118,17 @@ int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus) {
// update mat
if (flg_plus) {
for (int i=k+1; i<n; i++) {
for (int i=k+1; i < n; i++) {
mat[i*n+k] = (mat[i*n+k] + s*x[i])*cinv;
}
} else {
for (int i=k+1; i<n; i++) {
for (int i=k+1; i < n; i++) {
mat[i*n+k] = (mat[i*n+k] - s*x[i])*cinv;
}
}
// update x
for (int i=k+1; i<n; i++) {
for (int i=k+1; i < n; i++) {
x[i] = c*x[i] - s*mat[i*n+k];
}
}
@@ -153,26 +153,26 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
mjtNum* sparse_buf = mj_stackAlloc(d, n);
// shrink rows so that rownnz ends at diagonal
for (int r=0; r<n; r++) {
for (int r=0; r < n; r++) {
// shrink
while (rownnz[r]>0 && colind[rowadr[r]+rownnz[r]-1]>r) {
while (rownnz[r] > 0 && colind[rowadr[r]+rownnz[r]-1] > r) {
rownnz[r]--;
}
// check
if (rownnz[r]==0 || colind[rowadr[r]+rownnz[r]-1]!=r) {
if (rownnz[r] == 0 || colind[rowadr[r]+rownnz[r]-1] != r) {
mju_error("Matrix must have non-zero diagonal in mju_cholFactorSparse");
}
}
// backpass over rows
for (int r=n-1; r>=0; r--) {
for (int r=n-1; r >= 0; r--) {
// get rownnz and rowadr for row r
int nnz = rownnz[r], adr = rowadr[r];
// update row r diagonal
mjtNum tmp = mat[adr+nnz-1];
if (tmp<mindiag) {
if (tmp < mindiag) {
tmp = mindiag;
rank--;
}
@@ -180,12 +180,12 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
tmp = 1/mat[adr+nnz-1];
// update row r before diagonal
for (int i=0; i<nnz-1; i++) {
for (int i=0; i < nnz-1; i++) {
mat[adr+i] *= tmp;
}
// update row c<r where mat(r,c)!=0
for (int i=0; i<nnz-1; i++) {
for (int i=0; i < nnz-1; i++) {
// get column index
int c = colind[adr+i];
@@ -212,7 +212,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
mju_copy(res, vec, n);
// vec <- L^-T vec
for (int i=n-1; i>=0; i--) {
for (int i=n-1; i >= 0; i--) {
if (res[i]) {
// get rowadr[i], rownnz[i]
const int adr = rowadr[i], nnz = rownnz[i];
@@ -222,19 +222,19 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
mjtNum tmp = res[i];
// x(j) -= L(i,j)*x(i), j=0:i-1
for (int j=0; j<nnz-1; j++) {
for (int j=0; j < nnz-1; j++) {
res[colind[adr+j]] -= mat[adr+j]*tmp;
}
}
}
// vec <- L^-1 vec
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
// get rowadr[i], rownnz[i]
const int adr = rowadr[i], nnz = rownnz[i];
// x(i) -= sum_j L(i,j)*x(j), j=0:i-1
if (nnz>1) {
if (nnz > 1) {
res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr);
// modulo AVX, the above line does
// for (int j=0; j<nnz-1; j++)
@@ -260,13 +260,13 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
// backpass over rows corresponding to non-zero x(r)
int rank = n, i = x_nnz - 1;
while (i>=0) {
while (i >= 0) {
// get rownnz and rowadr for this row
int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]];
// compute quantities
mjtNum tmp = mat[adr+nnz-1]*mat[adr+nnz-1] + (flg_plus ? x[i]*x[i] : -x[i]*x[i]);
if (tmp<mjMINVAL) {
if (tmp < mjMINVAL) {
tmp = mjMINVAL;
rank--;
}
@@ -283,7 +283,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
sparse_buf, buf_ind);
// check for size change
if (new_nnz!=nnz-1) {
if (new_nnz != nnz-1) {
mju_error("Varying sparsity pattern in mju_cholUpdateSparse");
}
@@ -314,7 +314,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mjtNum mindiag = -1;
// sparse part, including sparse-sparse and sparse-dense
for (int j=0; j<nsparse; j++) {
for (int j=0; j < nsparse; j++) {
// number of non-zeros left of (j,j)
int width_jj = mjMIN(j, nband-1);
@@ -325,16 +325,16 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
int adr_jj = (j+1)*nband-1;
// compute L(j,j), before sqrt
mjtNum left_ij = width_jj>0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_jj-width_jj, width_jj) : 0;
mjtNum left_ij = width_jj > 0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_jj-width_jj, width_jj) : 0;
mjtNum Ljj = diagadd + diagmul*mat[adr_jj] + mat[adr_jj] - left_ij;
// update mindiag
if (Ljj<mindiag || mindiag<0) {
if (Ljj < mindiag || mindiag < 0) {
mindiag = Ljj;
}
// stop if rank-deficient
if (Ljj<mjMINVAL) {
if (Ljj < mjMINVAL) {
return 0;
}
@@ -343,7 +343,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mjtNum scale = 1/Ljj;
// compute L(i,j) for i>j, sparse part
for (int i=j+1; i<=j+height; i++) {
for (int i=j+1; i <= j+height; i++) {
// number of non-zeros left of (i,j)
int width_ij = mjMIN(j, nband-1-i+j);
@@ -351,18 +351,18 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
int adr_ij = (i+1)*nband-1-i+j;
// in-place computation of L(i,j)
left_ij = width_ij>0 ? mju_dot(mat+adr_jj-width_ij, mat+adr_ij-width_ij, width_ij) : 0;
left_ij = width_ij > 0 ? mju_dot(mat+adr_jj-width_ij, mat+adr_ij-width_ij, width_ij) : 0;
mat[adr_ij] = scale * (mat[adr_ij] - left_ij);
}
// compute L(i,j) for i>j, dense part
for (int i=nsparse; i<ntotal; i++) {
for (int i=nsparse; i < ntotal; i++) {
// address of (i,j)
int adr_ij = nsparse*nband + (i-nsparse)*ntotal + j;
// in-place computation of L(i,j)
// number of non-zeros left of (i,j) now equals width_jj
left_ij = width_jj>0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_ij-width_jj, width_jj) : 0;
left_ij = width_jj > 0 ? mju_dot(mat+adr_jj-width_jj, mat+adr_ij-width_jj, width_jj) : 0;
mat[adr_ij] = scale * (mat[adr_ij] - left_ij);
}
@@ -371,7 +371,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
}
// dense part
for (int j=nsparse; j<ntotal; j++) {
for (int j=nsparse; j < ntotal; j++) {
// address of (j,j)
int adr_jj = nsparse*nband + (j-nsparse)*ntotal + j;
@@ -380,12 +380,12 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mju_dot(mat+adr_jj-j, mat+adr_jj-j, j);
// update mindiag
if (Ljj<mindiag || mindiag<0) {
if (Ljj < mindiag || mindiag < 0) {
mindiag = Ljj;
}
// stop if rank-deficient
if (Ljj<mjMINVAL) {
if (Ljj < mjMINVAL) {
return 0;
}
@@ -394,7 +394,7 @@ mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense,
mjtNum scale = 1/Ljj;
// compute L(i,j) for i>j
for (int i=j+1; i<ntotal; i++) {
for (int i=j+1; i < ntotal; i++) {
// address of off-diagonal element
int adr_ij = adr_jj + ntotal*(i-j);
@@ -417,14 +417,14 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int width, height, nsparse = ntotal - ndense;
// copy into result if different
if (res!=vec) {
if (res != vec) {
mju_copy(res, vec, ntotal);
}
//------- forward substitution: solve L*res = vec
// sparse part
for (int i=0; i<nsparse; i++) {
for (int i=0; i < nsparse; i++) {
// number of non-zeros left of (i,i)
width = mjMIN(i, nband-1);
@@ -437,7 +437,7 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
}
// dense part
for (int i=nsparse; i<ntotal; i++) {
for (int i=nsparse; i < ntotal; i++) {
res[i] -= mju_dot(mat+nsparse*nband+(i-nsparse)*ntotal, res, i);
// diagonal
@@ -447,8 +447,8 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
//------- backward substitution: solve L'*res = res
// dense part
for (int i=ntotal-1; i>=nsparse; i--) {
for (int j=i+1; j<ntotal; j++) {
for (int i=ntotal-1; i >= nsparse; i--) {
for (int j=i+1; j < ntotal; j++) {
res[i] -= mat[nsparse*nband+(j-nsparse)*ntotal+i] * res[j];
}
@@ -457,16 +457,16 @@ void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
}
// sparse part
for (int i=nsparse-1; i>=0; i--) {
for (int i=nsparse-1; i >= 0; i--) {
// number of non-zeros below (i,i), sparse part
height = mjMIN(nsparse-1-i, nband-1);
// sparse rows
for (int j=i+1; j<=i+height; j++)
for (int j=i+1; j <= i+height; j++)
res[i] -= mat[(j+1)*nband-1-(j-i)] * res[j];
// dense rows
for (int j=nsparse; j<ntotal; j++)
for (int j=nsparse; j < ntotal; j++)
res[i] -= mat[nsparse*nband+(j-nsparse)*ntotal+i] * res[j];
// diagonal
@@ -481,7 +481,7 @@ int mju_bandDiag(int i, int ntotal, int nband, int ndense) {
int nsparse = ntotal-ndense;
// sparse part
if (i<nsparse) {
if (i < nsparse) {
return i*nband + nband-1;
}
@@ -502,7 +502,7 @@ void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
mju_zero(res, ntotal*ntotal);
// sparse part
for(int i=0; i<nsparse; i++) {
for(int i=0; i < nsparse; i++) {
// number of non-zeros left of (i,i)
int width = mjMIN(i, nband-1);
@@ -511,14 +511,14 @@ void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
}
// dense part
for(int i=nsparse; i<ntotal; i++) {
for(int i=nsparse; i < ntotal; i++) {
mju_copy(res + i*ntotal, mat + nsparse*nband + (i-nsparse)*ntotal, i+1);
}
// make symmetric
if (flg_sym) {
for(int i=0; i<ntotal; i++) {
for (int j=i+1; j<ntotal; j++) {
for(int i=0; i < ntotal; i++) {
for (int j=i+1; j < ntotal; j++) {
res[i*ntotal + j] = res[j*ntotal + i];
}
}
@@ -532,7 +532,7 @@ void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
int nsparse = ntotal-ndense;
// sparse part
for(int i=0; i<nsparse; i++) {
for(int i=0; i < nsparse; i++) {
// number of non-zeros left of (i,i)
int width = mjMIN(i, nband-1);
@@ -541,7 +541,7 @@ void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int n
}
// dense part
for(int i=nsparse; i<ntotal; i++) {
for(int i=nsparse; i < ntotal; i++) {
mju_copy(res + nsparse*nband + (i-nsparse)*ntotal, mat + i*ntotal, i+1);
}
}
@@ -554,13 +554,13 @@ void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
int nsparse = ntotal-ndense;
// handle multiple vectors
for(int j=0; j<nvec; j++ ) {
for(int j=0; j < nvec; j++ ) {
// precompute pointer to corresponding vector in vec and res
const mjtNum* vec_j = vec + ntotal*j;
mjtNum* res_j = res + ntotal*j;
// sparse part
for(int i=0; i<nsparse; i++) {
for(int i=0; i < nsparse; i++) {
int width = mjMIN(i+1, nband);
int adr = i*nband + nband - width;
int offset = mjMAX(0, i-nband+1);
@@ -572,7 +572,7 @@ void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
}
// dense part
for(int i=nsparse; i<ntotal; i++) {
for(int i=nsparse; i < ntotal; i++) {
int adr = nsparse*nband + (i-nsparse)*ntotal;
res_j[i] = mju_dot(mat+adr, vec_j, i+1);
if (flg_sym) {
@@ -596,28 +596,28 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
memcpy(remaining, rownnz, n*sizeof(int));
// diagonal elements (i,i)
for (int i=n-1; i>=0; i--) {
for (int i=n-1; i >= 0; i--) {
// get address of last remaining element of row i, adjust remaining counter
int ii = rowadr[i] + remaining[i] - 1;
remaining[i]--;
// make sure ii is on diagonal
if (colind[ii]!=i) {
if (colind[ii] != i) {
mju_error("missing diagonal element in mju_factorLUSparse");
}
// make sure diagonal is not too small
if (mju_abs(LU[ii])<mjMINVAL) {
if (mju_abs(LU[ii]) < mjMINVAL) {
mju_error("diagonal element too small in mju_factorLUSparse");
}
// rows j above i
for (int j=i-1; j>=0; j--) {
for (int j=i-1; j >= 0; j--) {
// get address of last remaining element of row j
int ji = rowadr[j] + remaining[j] - 1;
// process row j if (j,i) is non-zero
if (colind[ji]==i) {
if (colind[ji] == i) {
// adjust remaining counter
remaining[j]--;
@@ -627,15 +627,15 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
// (j,k) = (j,k) - (i,k) * (j,i) for k<i; handle incompatible sparsity
int icnt = rowadr[i], jcnt = rowadr[j];
while (jcnt<rowadr[j]+remaining[j]) {
while (jcnt < rowadr[j]+remaining[j]) {
// both non-zero
if (colind[icnt]==colind[jcnt]) {
if (colind[icnt] == colind[jcnt]) {
// update LU, advance counters
LU[jcnt++] -= LU[icnt++] * LUji;
}
// only (j,k) non-zero
else if (colind[icnt]>colind[jcnt]) {
else if (colind[icnt] > colind[jcnt]) {
// advance j counter
jcnt++;
}
@@ -647,7 +647,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
}
// make sure both rows fully processed
if (icnt!=rowadr[i]+remaining[i] || jcnt!=rowadr[j]+remaining[j]) {
if (icnt != rowadr[i]+remaining[i] || jcnt != rowadr[j]+remaining[j]) {
mju_error("row processing incomplete in mju_factorLUSparse");
}
}
@@ -655,8 +655,8 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
}
// make sure remaining points to diagonal
for (int i=0; i<n; i++) {
if (remaining[i]<0 || colind[rowadr[i]+remaining[i]]!=i) {
for (int i=0; i < n; i++) {
if (remaining[i] < 0 || colind[rowadr[i]+remaining[i]] != i) {
mju_error("unexpected sparse matrix structure in mju_factorLUSparse");
}
}
@@ -668,28 +668,28 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
const int* rownnz, const int* rowadr, const int* colind) {
//------------------ solve (U+I)*res = vec
for (int i=n-1; i>=0; i--) {
for (int i=n-1; i >= 0; i--) {
// init: diagonal of (U+I) is 1
res[i] = vec[i];
// res[i] -= sum_k>i res[k]*LU(i,k)
int j = rownnz[i] - 1;
while (colind[rowadr[i]+j]>i) {
while (colind[rowadr[i]+j] > i) {
res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j];
j--;
}
// make sure j points to diagonal
if (colind[rowadr[i]+j]!=i) {
if (colind[rowadr[i]+j] != i) {
mju_error("diagonal of U not reached in mju_factorLUSparse");
}
}
//------------------ solve L*res(new) = res
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
// res[i] -= sum_k<i res[k]*LU(i,k)
int j = 0;
while (colind[rowadr[i]+j]<i) {
while (colind[rowadr[i]+j] < i) {
res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j];
j++;
}
@@ -698,7 +698,7 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
res[i] /= LU[rowadr[i]+j];
// make sure j points to diagonal
if (colind[rowadr[i]+j]!=i) {
if (colind[rowadr[i]+j] != i) {
mju_error("diagonal of L not reached in mju_factorLUSparse");
}
}
@@ -720,7 +720,7 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
quat[1] = quat[2] = quat[3] = 0;
// Jacobi iteration
for (iter=0; iter<500; iter++) {
for (iter=0; iter < 500; iter++) {
// make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec
mju_quat2Mat(eigvec, quat);
mju_mulMatTMat(tmp, eigvec, mat, 3, 3, 3);
@@ -732,11 +732,11 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
eigval[2] = D[8];
// find max off-diagonal element, set indices
if (fabs(D[1])>fabs(D[2]) && fabs(D[1])>fabs(D[5])) {
if (fabs(D[1]) > fabs(D[2]) && fabs(D[1]) > fabs(D[5])) {
rk = 0; // row
ck = 1; // column
rotk = 2; // rotation axis
} else if (fabs(D[2])>fabs(D[5])) {
} else if (fabs(D[2]) > fabs(D[5])) {
rk = 0;
ck = 2;
rotk = 1;
@@ -747,13 +747,13 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
}
// terminate if max off-diagonal element too small
if (fabs(D[3*rk+ck])<eigEPS) {
if (fabs(D[3*rk+ck]) < eigEPS) {
break;
}
// 2x2 symmetric Schur decomposition
tau = (D[4*ck]-D[4*rk])/(2*D[3*rk+ck]);
if (tau>=0) {
if (tau >= 0) {
t = 1.0/(tau + mju_sqrt(1 + tau*tau));
} else {
t = -1.0/(-tau + mju_sqrt(1 + tau*tau));
@@ -761,14 +761,14 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
c = 1.0/mju_sqrt(1 + t*t);
// terminate if cosine too close to 1
if (c>1.0-eigEPS) {
if (c > 1.0-eigEPS) {
break;
}
// express rotation as quaternion
tmp[1] = tmp[2] = tmp[3] = 0;
tmp[rotk+1] = (tau>=0 ? -mju_sqrt(0.5-0.5*c) : mju_sqrt(0.5-0.5*c));
if (rotk==1) {
tmp[rotk+1] = (tau >= 0 ? -mju_sqrt(0.5-0.5*c) : mju_sqrt(0.5-0.5*c));
if (rotk == 1) {
tmp[rotk+1] = -tmp[rotk+1];
}
tmp[0] = mju_sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]);
@@ -780,7 +780,7 @@ int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]
}
// sort eigenvalues in decreasing order (bubblesort: 0, 1, 0)
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
int j1 = j%2; // lead index
if (eigval[j1] < eigval[j1+1]) {
@@ -825,12 +825,12 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// Newton iteration
la = 0;
for (int iter=0; iter<20; iter++) {
for (int iter=0; iter < 20; iter++) {
// det(A+la)
det = (A11+la)*(A22+la) - A12*A12;
// check SPD, with 1e-10 threshold
if (det<1e-10) {
if (det < 1e-10) {
res[0] = 0;
res[1] = 0;
return 0;
@@ -850,7 +850,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
val = v1*v1 + v2*v2 - r*r;
// check for convergence, or initial solution inside constraint set
if (val<1e-10) {
if (val < 1e-10) {
break;
}
@@ -859,7 +859,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// compute update, exit if too small
mjtNum delta = -val/deriv;
if (delta<1e-10) {
if (delta < 1e-10) {
break;
}
@@ -871,7 +871,7 @@ int mju_QCQP2(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
res[0] = v1*d[0];
res[1] = v2*d[1];
return (la!=0);
return (la != 0);
}
@@ -897,7 +897,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// Newton iteration
la = 0;
for (int iter=0; iter<20; iter++) {
for (int iter=0; iter < 20; iter++) {
// unscaled P
P11 = (A22+la)*(A33+la) - A23*A23;
P22 = (A11+la)*(A33+la) - A13*A13;
@@ -910,7 +910,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
det = (A11+la)*P11 + A12*P12 + A13*P13;
// check SPD, with 1e-10 threshold
if (det<1e-10) {
if (det < 1e-10) {
res[0] = 0;
res[1] = 0;
res[2] = 0;
@@ -937,7 +937,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
val = v1*v1 + v2*v2 + v3*v3 - r*r;
// check for convergence, or initial solution inside constraint set
if (val<1e-10) {
if (val < 1e-10) {
break;
}
@@ -947,7 +947,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// compute update, exit if too small
mjtNum delta = -val/deriv;
if (delta<1e-10) {
if (delta < 1e-10) {
break;
}
@@ -960,7 +960,7 @@ int mju_QCQP3(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
res[1] = v2*d[1];
res[2] = v3*d[2];
return (la!=0);
return (la != 0);
}
@@ -974,25 +974,25 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
mjtNum la, val, deriv, tmp[5];
// check size
if (n>5) {
if (n > 5) {
mju_error("mju_QCQP supports n up to 5");
}
// scale A,b so that constraint becomes x'*x <= r*r
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
b[i] = bin[i] * d[i];
for (int j=0; j<n; j++) {
for (int j=0; j < n; j++) {
A[j+i*n] = Ain[j+i*n] * d[i] * d[j];
}
}
// Newton iteration
la = 0;
for (int iter=0; iter<20; iter++) {
for (int iter=0; iter < 20; iter++) {
// make A+la
mju_copy(Ala, A, n*n);
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
Ala[i*(n+1)] += la;
}
@@ -1010,7 +1010,7 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
val = mju_dot(res, res, n) - r*r;
// check for convergence, or initial solution inside constraint set
if (val<1e-10) {
if (val < 1e-10) {
break;
}
@@ -1020,7 +1020,7 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
// compute update, exit if too small
mjtNum delta = -val/deriv;
if (delta<1e-10) {
if (delta < 1e-10) {
break;
}
@@ -1029,11 +1029,11 @@ int mju_QCQP(mjtNum* res, const mjtNum* Ain, const mjtNum* bin,
}
// undo scaling
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = res[i] * d[i];
}
return (la!=0);
return (la != 0);
}
@@ -1119,7 +1119,7 @@ enum mjtStatusBoxQP {
// assumes symmetry of mat, ignores upper triangle
static mjtNum mulVecMatVecSym(const mjtNum* vec, const mjtNum* mat, int n) {
mjtNum res = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res += vec[i] * mat[n*i+i] * vec[i]; // diagonal
res += 2 * vec[i] * mju_dot(mat+n*i, vec, i); // off-diagonal
}
@@ -1150,11 +1150,11 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
mjtNum sdotg, improvement=0, value=0, norm2=0;
// basic checks
if (n<=0) {
if (n <= 0) {
mju_error("mju_boxQP: problem size n must be positive");
}
if (upper && lower) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (lower[i] >= upper[i]) {
mju_error("mju_boxQP: upper bounds must be stricly larger than lower bounds");
}
@@ -1200,14 +1200,14 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
}
// full index set (no clamping)
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
index[i] = i;
}
}
// have bounds: clamp res
else {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (lower) {
res[i] = mju_max(res[i], lower[i]);
}
@@ -1220,7 +1220,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// ------ main loop
int iter, logptr = 0;
mjtNum oldvalue;
for (iter=0; iter<maxiter; iter++) {
for (iter=0; iter < maxiter; iter++) {
if (status != mjBOXQP_NO_DESCENT) {
break;
}
@@ -1236,14 +1236,14 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
mju_addTo(grad, g, n);
// find clamped dimensions
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
clamped[i] = ( lower && res[i] == lower[i] && grad[i] > 0 ) ||
( upper && res[i] == upper[i] && grad[i] < 0 );
}
// build index of free dimensions, count them
nfree = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (!clamped[i]) {
index[nfree++] = i;
}
@@ -1258,7 +1258,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// re-factorize if clamped dimensions have changed
if (iter) {
factorize = 0;
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
if (clamped[i] != oldclamped[i]) {
factorize = 1;
break;
@@ -1267,26 +1267,26 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
}
// save last clamped
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
oldclamped[i] = clamped[i];
}
// get search direction: search = g + H_all,clamped * res_clamped
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
temp[i] = clamped[i] ? res[i] : 0;
}
mju_mulMatVec(search, H, temp, n, n);
mju_addTo(search, g, n);
// search = compress_free(search)
for (int i=0; i<nfree; i++) {
for (int i=0; i < nfree; i++) {
search[i] = search[index[i]];
}
// R = compress_free(H)
if (factorize) {
for (int i=0; i<nfree; i++) {
for (int j=0; j<i+1; j++) {
for (int i=0; i < nfree; i++) {
for (int j=0; j < i+1; j++) {
R[i*nfree+j] = H[index[i]*n+index[j]];
}
}
@@ -1307,7 +1307,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// search_free = expand_free(-temp) - x_free
mju_zero(search, n);
for (int i=0; i<nfree; i++) {
for (int i=0; i < nfree; i++) {
search[index[i]] = -temp[i] -res[index[i]];
}
@@ -1315,13 +1315,13 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// squared norm of free gradient
norm2 = 0;
for (int i=0; i<nfree; i++) {
for (int i=0; i < nfree; i++) {
mjtNum grad_i = grad[index[i]];
norm2 += grad_i*grad_i;
}
// small gradient: minimum found
if (norm2<mingrad) {
if (norm2 < mingrad) {
status = nfree == n ? mjBOXQP_UNBOUNDED : mjBOXQP_TOL_GRAD;
break;
}
@@ -1338,10 +1338,10 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// candidate = clamp(x + step*search)
mju_scl(candidate, search, step, n);
mju_addTo(candidate, res, n);
for (int i=0; i<n; i++) {
if (lower && candidate[i]<lower[i]) {
for (int i=0; i < n; i++) {
if (lower && candidate[i] < lower[i]) {
candidate[i] = lower[i];
} else if (upper && candidate[i]>upper[i]) {
} else if (upper && candidate[i] > upper[i]) {
candidate[i] = upper[i];
}
}
@@ -1352,7 +1352,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
// increment and break if step is too small
nstep++;
step = step*backtrack;
if (step<minstep) {
if (step < minstep) {
status = mjBOXQP_MAX_LS_ITER;
break;
}
@@ -1376,7 +1376,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
}
// max iterations exceeded
if (iter==maxiter) {
if (iter == maxiter) {
status = mjBOXQP_MAX_ITER;
}
src/engine/engine_util_sparse.c
+68 -68
View File
@@ -40,7 +40,7 @@ mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
mjtNum res2 = 0;
mjtNum res3 = 0;
for (; i<=n_4; i+=4) {
for (; i <= n_4; i+=4) {
res0 += vec1[i+0] * vec2[ind1[i+0]];
res1 += vec1[i+1] * vec2[ind1[i+1]];
res2 += vec1[i+2] * vec2[ind1[i+2]];
@@ -49,7 +49,7 @@ mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
res = (res0 + res2) + (res1 + res3);
// scalar part
for (; i<nnz1; i++) {
for (; i < nnz1; i++) {
res += vec1[i] * vec2[ind1[i]];
}
@@ -73,7 +73,7 @@ void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2,
mjtNum RES1 = 0;
mjtNum RES2 = 0;
for (; i<nnz1; i++) {
for (; i < nnz1; i++) {
mjtNum v2 = vec2[ind1[i]];
RES0 += vec10[i] * v2;
@@ -102,17 +102,17 @@ mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
return 0;
}
while (i1<nnz1 && i2<nnz2) {
while (i1 < nnz1 && i2 < nnz2) {
// get current indices
int adr1 = ind1[i1], adr2 = ind2[i2];
// match: accumulate result, advance both
if (adr1==adr2) {
if (adr1 == adr2) {
res += vec1[i1++] * vec2[i2++];
}
// otherwise advance smaller
else if (adr1<adr2) {
else if (adr1 < adr2) {
i1++;
} else {
i2++;
@@ -130,13 +130,13 @@ void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
int adr = 0;
// find non-zeros and construct sparse
for (int r=0; r<nr; r++) {
for (int r=0; r < nr; r++) {
// init row
rownnz[r] = 0;
rowadr[r] = adr;
// find non-zeros
for (int c=0; c<nc; c++) {
for (int c=0; c < nc; c++) {
if (mat[r*nc+c]) {
// record index and count
colind[adr] = c;
@@ -158,8 +158,8 @@ void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc,
mju_zero(res, nr*nc);
// copy non-zeros
for (int r=0; r<nr; r++) {
for (int i=0; i<rownnz[r]; i++) {
for (int r=0; r < nr; r++) {
for (int i=0; i < rownnz[r]; i++) {
res[r*nc + colind[rowadr[r]+i]] = mat[rowadr[r]+i];
}
}
@@ -175,7 +175,7 @@ void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
mju_mulMatVecSparse_avx(res, mat, vec, nr, rownnz, rowadr, colind, rowsuper);
#else
// regular sparse dot-product
for (int r=0; r<nr; r++) {
for (int r=0; r < nr; r++) {
res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
}
#endif // mjUSEAVX
@@ -188,7 +188,7 @@ static void mju_addToSclScl(mjtNum* res, const mjtNum* vec, mjtNum scl1, mjtNum
#ifdef mjUSEAVX
mju_addToSclScl_avx(res, vec, scl1, scl2, n);
#else
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
res[i] = res[i]*scl1 + vec[i]*scl2;
}
#endif // mjUSEAVX
@@ -212,7 +212,7 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b,
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 == src_nnz) {
if (mju_compare(dst_ind, src_ind, dst_nnz)) {
// combine mjtNum data directly
mju_addToSclScl(dst, src, a, b, dst_nnz);
@@ -229,26 +229,26 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b,
// prepare to merge buf and scr into dst
int bi = 0, si = 0, nnz = 0;
int buf_nnz = dst_nnz;
int badr = bi<buf_nnz ? buf_ind[bi] : n+1;
int sadr = si<src_nnz ? src_ind[si] : n+1;
int badr = bi < buf_nnz ? buf_ind[bi] : n+1;
int sadr = si < src_nnz ? src_ind[si] : n+1;
// merge vectors
while (bi<buf_nnz || si<src_nnz) {
while (bi < buf_nnz || si < src_nnz) {
// both
if (badr==sadr) {
if (badr == sadr) {
dst[nnz] = a*buf[bi++] + b*src[si++];
dst_ind[nnz++] = badr;
badr = bi<buf_nnz ? buf_ind[bi] : n+1;
sadr = si<src_nnz ? src_ind[si] : n+1;
badr = bi < buf_nnz ? buf_ind[bi] : n+1;
sadr = si < src_nnz ? src_ind[si] : n+1;
}
// dst only
else if (badr<sadr) {
else if (badr < sadr) {
dst[nnz] = a*buf[bi++];
dst_ind[nnz++] = badr;
badr = bi<buf_nnz ? buf_ind[bi] : n+1;
badr = bi < buf_nnz ? buf_ind[bi] : n+1;
}
// src only
@@ -256,7 +256,7 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b,
dst[nnz] = b*src[si++];
dst_ind[nnz++] = sadr;
sadr = si<src_nnz ? src_ind[si] : n+1;
sadr = si < src_nnz ? src_ind[si] : n+1;
}
}
@@ -269,7 +269,7 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b,
void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b,
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind) {
// check for identical pattern
if (dst_nnz==src_nnz) {
if (dst_nnz == src_nnz) {
if (mju_compare(dst_ind, src_ind, dst_nnz)) {
// combine mjtNum data directly
mju_addToSclScl(dst, src, a, b, dst_nnz);
@@ -278,35 +278,35 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu
}
// scale dst by a
if (a!=1) {
if (a != 1) {
mju_scl(dst, dst, a, dst_nnz);
}
// prepare to merge
int di = 0, si = 0;
int dadr = di<dst_nnz ? dst_ind[di] : n+1;
int sadr = si<src_nnz ? src_ind[si] : n+1;
int dadr = di < dst_nnz ? dst_ind[di] : n+1;
int sadr = si < src_nnz ? src_ind[si] : n+1;
// add src*b at common indices
while (di<dst_nnz) {
while (di < dst_nnz) {
// both
if (dadr==sadr) {
if (dadr == sadr) {
dst[di++] += b*src[si++];
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;
sadr = si < src_nnz ? src_ind[si] : n+1;
}
// dst only
else if (dadr<sadr) {
else if (dadr < sadr) {
di++;
dadr = di<dst_nnz ? dst_ind[di] : n+1;
dadr = di < dst_nnz ? dst_ind[di] : n+1;
}
// src only
else {
si++;
sadr = si<src_nnz ? src_ind[si] : n+1;
sadr = si < src_nnz ? src_ind[si] : n+1;
}
}
}
@@ -317,13 +317,13 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu
void mju_compressSparse(mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind) {
rowadr[0] = 0;
int adr = rownnz[0];
for (int r=1; r<nr; r++) {
for (int r=1; r < nr; r++) {
// save old rowadr, record new
int rowadr1 = rowadr[r];
rowadr[r] = adr;
// shift mat and mat_colind
for (int adr1=rowadr1; adr1<rowadr1+rownnz[r]; adr1++) {
for (int adr1=rowadr1; adr1 < rowadr1+rownnz[r]; adr1++) {
mat[adr] = mat[adr1];
colind[adr] = colind[adr1];
adr++;
@@ -344,13 +344,13 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
int nnz = rowadr[nr-1] + rownnz[nr-1];
// count the number of non-zeros for each row of the transposed matrix
for (int i = 0; i<nnz; i++) {
for (int i = 0; i < nnz; i++) {
res_rownnz[colind[i]]++;
}
// compute the row addresses for the transposed matrix
res_rowadr[0] = 0;
for (int i = 1; i<nc; i++) {
for (int i = 1; i < nc; i++) {
res_rowadr[i] = res_rowadr[i-1] + res_rownnz[i-1];
}
@@ -358,7 +358,7 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
int r = 0;
// iterate through each non-zero entry of mat
for (int i = 0; i<nnz; i++) {
for (int i = 0; i < nnz; i++) {
// iterate to get to the current row (skipping rows with all zeros)
while ((i-rowadr[r]) >= rownnz[r]) r++;
@@ -369,7 +369,7 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
}
// shift back row addresses
for (int i = nc-1; i>0; i--) {
for (int i = nc-1; i > 0; i--) {
res_rowadr[i] = res_rowadr[i-1];
}
@@ -387,9 +387,9 @@ void mju_superSparse(int nr, int* rowsuper,
}
// find match to child
for (int r=0; r<nr-1; r++) {
for (int r=0; r < nr-1; r++) {
// different number of nonzeros: cannot be a match
if (rownnz[r]!=rownnz[r+1]) {
if (rownnz[r] != rownnz[r+1]) {
rowsuper[r] = 0;
}
@@ -403,7 +403,7 @@ void mju_superSparse(int nr, int* rowsuper,
rowsuper[nr-1] = 0;
// accumulate in reverse
for (int r=nr-2; r>=0; r--) {
for (int r=nr-2; r >= 0; r--) {
if (rowsuper[r]) {
rowsuper[r] += rowsuper[r+1];
}
@@ -424,14 +424,14 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
int nchain = 0;
int* res_colind = NULL;
for (int r=0; r<nc; r++) {
for (int r=0; r < nc; r++) {
// supernode; copy everything to next row
if (rowsuperT && r>0 && rowsuperT[r-1]>0) {
if (rowsuperT && r > 0 && rowsuperT[r-1] > 0) {
res_rownnz[r] = res_rownnz[r - 1];
// fill in upper triangle
for (int j=0; j <nchain; j++) {
for (int j=0; j < nchain; j++) {
res_rownnz[res_colind[j]]++;
}
@@ -444,7 +444,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
int inew = 0, iold = nc;
nchain = 0;
for (int i=0; i<rownnzT[r]; i++) {
for (int i=0; i < rownnzT[r]; i++) {
int c = colindT[rowadrT[r] + i];
int adr = inew;
@@ -454,15 +454,15 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
int nnewchain = 0;
adr = 0;
int end = rowadr[c] + rownnz[c];
for (int adr1=rowadr[c]; adr1<end; adr1++) {
for (int adr1=rowadr[c]; adr1 < end; adr1++) {
int col_mat = colind[adr1];
while (adr<nchain && chain[iold + adr] < col_mat &&
chain[iold + adr]<=r) {
while (adr < nchain && chain[iold + adr] < col_mat &&
chain[iold + adr] <= r) {
chain[inew + nnewchain++] = chain[iold + adr++];
}
// skip upper triangle
if (col_mat>r) {
if (col_mat > r) {
break;
}
@@ -472,7 +472,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
chain[inew + nnewchain++] = col_mat;
}
while (adr<nchain && chain[iold + adr]<=r) {
while (adr < nchain && chain[iold + adr] <= r) {
chain[inew + nnewchain++] = chain[iold + adr++];
}
nchain = nnewchain;
@@ -486,11 +486,11 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
int nchain_end = nchain;
// avoid double counting.
if (nchain>0 && res_colind[nchain-1]==r) {
if (nchain > 0 && res_colind[nchain-1] == r) {
nchain_end = nchain - 1;
}
for (int j=0; j<nchain_end; j++) {
for (int j=0; j < nchain_end; j++) {
res_rownnz[res_colind[j]]++;
}
}
@@ -507,7 +507,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr,
// precompute res_rowadr for mju_sqrMatTDSparse using uncompressed memory
void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc) {
for (int r=0; r<nc; r++) {
for (int r=0; r < nc; r++) {
res_rowadr[r] = r*nc;
}
}
@@ -534,7 +534,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
// used for when creating the resulting sparse row
int* markers = mj_stackAllocInt(d, nc);
for (int i=0; i<nc; i++) {
for (int i=0; i < nc; i++) {
int* cols = res_colind+res_rowadr[i];
res_rownnz[i] = 0;
@@ -542,20 +542,20 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
markers[i] = 0;
// if rowsuper, use the previous row sparsity structure
if (rowsuperT && i>0 && rowsuperT[i-1]) {
if (rowsuperT && i > 0 && rowsuperT[i-1]) {
res_rownnz[i] = res_rownnz[i-1];
memcpy(cols, res_colind+res_rowadr[i-1], res_rownnz[i]*sizeof(int));
}
// iterate through each row of M'
int end = rowadrT[i] + rownnzT[i];
for (int r = rowadrT[i]; r<end; r++) {
for (int r = rowadrT[i]; r < end; r++) {
int t = colindT[r];
mjtNum v = diag ? matT[r] * diag[t] : matT[r];
for (int c=rowadr[t]; c<rowadr[t]+rownnz[t]; c++) {
for (int c=rowadr[t]; c < rowadr[t]+rownnz[t]; c++) {
int cc = colind[c];
// ignore upper triangle
if (cc>i) {
if (cc > i) {
break;
}
@@ -566,16 +566,16 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
markers[cc] = 1;
// since i is the rightmost column, it can be inserted at the end
if (cc==i) {
if (cc == i) {
cols[res_rownnz[i]++] = cc;
continue;
}
// insert col in order via binary search
int l = 0, h = res_rownnz[i];
while (l<h) {
while (l < h) {
int m = (l + h) >> 1;
if (cols[m]<cc) {
if (cols[m] < cc) {
l = m + 1;
} else {
h = m;
@@ -583,7 +583,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
}
// cc is the rightmost column so far, it can be inserted at the end
if (l==res_rownnz[i]) {
if (l == res_rownnz[i]) {
cols[l] = cc;
res_rownnz[i]++;
continue;
@@ -591,7 +591,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
// move the cols to the right
h = res_rownnz[i];
while (l<h) {
while (l < h) {
cols[h] = cols[h-1];
h--;
}
@@ -607,13 +607,13 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
// rowsuperT: reuse sparsity, copy into res
if (rowsuperT && rowsuperT[i]) {
for (int r=0; r<end; r++) {
for (int r=0; r < end; r++) {
res[res_rowadr[i] + r] = buffer[cols[r]];
buffer[cols[r]] = 0;
}
} else {
// clear out buffers since sparsity cannot be reused
for (int r=0; r<end; r++) {
for (int r=0; r < end; r++) {
int cc = cols[r];
res[res_rowadr[i] + r] = buffer[cc];
res_colind[res_rowadr[i] + r] = cc;
@@ -625,9 +625,9 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
// fill upper triangle
for (int i=0; i<nc; i++) {
for (int i=0; i < nc; i++) {
int end = res_rowadr[i] + res_rownnz[i] - 1;
for (int j=res_rowadr[i]; j<end; j++) {
for (int j=res_rowadr[i]; j < end; j++) {
int adr = res_rowadr[res_colind[j]] + res_rownnz[res_colind[j]]++;
res[adr] = res[j];
res_colind[adr] = i;
src/engine/engine_util_spatial.c
+14 -15
View File
@@ -26,7 +26,7 @@
// rotate vector by quaternion
void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
// null quat: copy vec
if (quat[0]==1 && quat[1]==0 && quat[2]==0 && quat[3]==0) {
if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
mju_copy3(res, vec);
}
@@ -75,7 +75,7 @@ void mju_mulQuat(mjtNum res[4], const mjtNum qa[4], const mjtNum qb[4]) {
// multiply quaternion and axis
void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]) {
mjtNum tmp[4] = {
-quat[1]*axis[0] - quat[2]*axis[1] - quat[3]*axis[2],
-quat[1]*axis[0] - quat[2]*axis[1] - quat[3]*axis[2],
quat[0]*axis[0] + quat[2]*axis[2] - quat[3]*axis[1],
quat[0]*axis[1] + quat[3]*axis[0] - quat[1]*axis[2],
quat[0]*axis[2] + quat[1]*axis[1] - quat[2]*axis[0]
@@ -91,7 +91,7 @@ void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3])
// convert axisAngle to quaternion
void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle) {
// zero angle: null quat
if (angle==0) {
if (angle == 0) {
res[0] = 1;
res[1] = 0;
res[2] = 0;
@@ -117,7 +117,7 @@ void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt) {
mjtNum speed = 2 * mju_atan2(sin_a_2, quat[0]);
// when axis-angle is larger than pi, rotation is in the opposite direction
if (speed>mjPI) {
if (speed > mjPI) {
speed -= 2*mjPI;
}
speed /= dt;
@@ -143,7 +143,7 @@ void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]) {
// convert quaternion to 3D rotation matrix
void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) {
// null quat: identity
if (quat[0]==1 && quat[1]==0 && quat[2]==0 && quat[3]==0) {
if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
res[0] = 1;
res[1] = 0;
res[2] = 0;
@@ -186,7 +186,7 @@ void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]) {
// convert 3D rotation matrix to quaternion
void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) {
// q0 largest
if (mat[0]+mat[4]+mat[8]>0) {
if (mat[0]+mat[4]+mat[8] > 0) {
quat[0] = 0.5 * mju_sqrt(1 + mat[0] + mat[4] + mat[8]);
quat[1] = 0.25 * (mat[7] - mat[5]) / quat[0];
quat[2] = 0.25 * (mat[2] - mat[6]) / quat[0];
@@ -194,7 +194,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) {
}
// q1 largest
else if (mat[0]>mat[4] && mat[0]>mat[8]) {
else if (mat[0] > mat[4] && mat[0] > mat[8]) {
quat[1] = 0.5 * mju_sqrt(1 + mat[0] - mat[4] - mat[8]);
quat[0] = 0.25 * (mat[7] - mat[5]) / quat[1];
quat[2] = 0.25 * (mat[1] + mat[3]) / quat[1];
@@ -202,7 +202,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]) {
}
// q2 largest
else if (mat[4]>mat[8]) {
else if (mat[4] > mat[8]) {
quat[2] = 0.5 * mju_sqrt(1 - mat[0] + mat[4] - mat[8]);
quat[0] = 0.25 * (mat[2] - mat[6]) / quat[2];
quat[1] = 0.25 * (mat[1] + mat[3]) / quat[2];
@@ -255,7 +255,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
mju_zero3(quat+1);
// normalize vector; if too small, no rotation
if (mju_normalize3(vn)<mjMINVAL) {
if (mju_normalize3(vn) < mjMINVAL) {
return;
}
@@ -264,7 +264,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
a = mju_normalize3(axis);
// almost parallel
if (fabs(a)<mjMINVAL) {
if (fabs(a) < mjMINVAL) {
// opposite: 180 deg rotation around x axis
if (mju_dot3(vn, z) < 0) {
quat[0] = 0;
@@ -368,8 +368,7 @@ void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9],
// tmp = diag(inert) * mat' (mat is local-to-global rotation)
mjtNum tmp[9] = {mat[0]*inert[0], mat[3]*inert[0], mat[6]*inert[0],
mat[1]*inert[1], mat[4]*inert[1], mat[7]*inert[1],
mat[2]*inert[2], mat[5]*inert[2], mat[8]*inert[2]
};
mat[2]*inert[2], mat[5]*inert[2], mat[8]*inert[2]};
// res_rot = mat * diag(inert) * mat'
res[0] = mat[0]*tmp[0] + mat[1]*tmp[3] + mat[2]*tmp[6];
@@ -429,9 +428,9 @@ void mju_dofCom(mjtNum res[6], const mjtNum axis[3], const mjtNum offset[3]) {
// multiply dof matrix (6-by-n, transposed) by vector (n-by-1)
void mju_mulDofVec(mjtNum* res, const mjtNum* dof, const mjtNum* vec, int n) {
if (n==1) {
if (n == 1) {
mju_scl(res, dof, vec[0], 6);
} else if (n<=0) {
} else if (n <= 0) {
mju_zero(res, 6);
} else {
mju_mulMatTVec(res, dof, vec, n, 6);
@@ -485,7 +484,7 @@ void mju_makeFrame(mjtNum frame[9]) {
if (mju_norm3(frame+3) < 0.5) {
mju_zero3(frame+3);
if (frame[1]<0.5 && frame[1]>-0.5) {
if (frame[1] < 0.5 && frame[1] > -0.5) {
frame[4] = 1;
} else {
frame[5] = 1;
src/engine/engine_vfs.c
+19 -19
View File
@@ -29,17 +29,17 @@ static void vfs_strippath(char* newname, const char* oldname) {
// find last delimiter
int i;
for (i=sz-1; i>=0; i--) {
if (oldname[i]=='\\' || oldname[i]=='/') {
for (i=sz-1; i >= 0; i--) {
if (oldname[i] == '\\' || oldname[i] == '/') {
break;
}
}
// check resulting length
if (sz-(i+1)>=mjMAXVFSNAME) {
if (sz-(i+1) >= mjMAXVFSNAME) {
mju_error("Filename too long in VFS");
}
if (sz-(i+1)<=0) {
if (sz-(i+1) <= 0) {
mju_error("Empty filename in VFS");
}
@@ -47,8 +47,8 @@ static void vfs_strippath(char* newname, const char* oldname) {
mju_strncpy(newname, oldname+i+1, mjMAXVFSNAME);
// make lowercase
for (int j=strlen(newname)-1; j>=0; j--) {
if (newname[j]>='A' && newname[j]<='Z') {
for (int j=strlen(newname)-1; j >= 0; j--) {
if (newname[j] >= 'A' && newname[j] <= 'Z') {
newname[j] = (char)(((int)newname[j]) +'a' - 'A');
}
}
@@ -66,7 +66,7 @@ void mj_defaultVFS(mjVFS* vfs) {
// add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load
int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) {
// check vfs size
if (vfs->nfile>=mjMAXVFS-1) {
if (vfs->nfile >= mjMAXVFS-1) {
return 1;
}
@@ -84,8 +84,8 @@ int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) {
vfs_strippath(newname, filename);
// check for repeated name
for (int i=0; i<vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) {
for (int i=0; i < vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) {
return 2;
}
}
@@ -111,12 +111,12 @@ int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) {
// make empty file in VFS, return 0: success, 1: full, 2: repeated name
int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) {
// check vfs size
if (vfs->nfile>=mjMAXVFS-1) {
if (vfs->nfile >= mjMAXVFS-1) {
return 1;
}
// check filesize
if (filesize<=0) {
if (filesize <= 0) {
mju_error("mj_makeEmptyFileVFS expects positive filesize");
}
@@ -125,8 +125,8 @@ int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) {
vfs_strippath(newname, filename);
// check for repeated name
for (int i=0; i<vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) {
for (int i=0; i < vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) {
return 2;
}
}
@@ -156,8 +156,8 @@ int mj_findFileVFS(const mjVFS* vfs, const char* filename) {
char newname[mjMAXVFSNAME];
vfs_strippath(newname, filename);
// find specific file
for (int i=0; i<vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) {
for (int i=0; i < vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) {
return i;
}
}
@@ -174,13 +174,13 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) {
vfs_strippath(newname, filename);
// find specified file
for (int i=0; i<vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME)==0) {
for (int i=0; i < vfs->nfile; i++) {
if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) {
// free buffer
mju_free(vfs->filedata[i]);
// scroll remaining files forward
for (int j=i; j<vfs->nfile-1; j++) {
for (int j=i; j < vfs->nfile-1; j++) {
mjSTRNCPY(vfs->filename[j], vfs->filename[j+1]);
vfs->filesize[j] = vfs->filesize[j+1];
vfs->filedata[j] = vfs->filedata[j+1];
@@ -204,7 +204,7 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) {
// delete all files from VFS
void mj_deleteVFS(mjVFS* vfs) {
for (int i=0; i<vfs->nfile; i++) {
for (int i=0; i < vfs->nfile; i++) {
mju_free(vfs->filedata[i]);
}
src/engine/engine_vis_init.c
+12 -12
View File
@@ -119,7 +119,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
mjv_freeScene(scn);
// allocate geom buffers
if (maxgeom>0) {
if (maxgeom > 0) {
// allocate
scn->maxgeom = maxgeom;
scn->geoms = (mjvGeom*) mju_malloc(maxgeom*sizeof(mjvGeom));
@@ -132,8 +132,8 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
}
// set default OpenGL options
for (int i=0; i<mjNRNDFLAG; i++) {
scn->flags[i] = (mjRNDSTRING[i][1][0]=='1');
for (int i=0; i < mjNRNDFLAG; i++) {
scn->flags[i] = (mjRNDSTRING[i][1][0] == '1');
}
// set default model transformation
@@ -152,7 +152,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
// compute number of vertices in all skins
int nskin = m->nskin;
int totvert = 0;
for (int i=0; i<nskin; i++) {
for (int i=0; i < nskin; i++) {
totvert += m->skin_vertnum[i];
}
@@ -173,7 +173,7 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
}
// copy constant data
for (int i=0; i<nskin; i++) {
for (int i=0; i < nskin; i++) {
scn->skinfacenum[i] = m->skin_facenum[i];
scn->skinvertadr[i] = m->skin_vertadr[i];
scn->skinvertnum[i] = m->skin_vertnum[i];
@@ -214,8 +214,8 @@ void mjv_defaultOption(mjvOption* vopt) {
vopt->label = mjLABEL_NONE;
vopt->frame = mjFRAME_NONE;
for (int i=0; i<mjNGROUP; i++) {
int state = (i<3 ? 1 : 0);
for (int i=0; i < mjNGROUP; i++) {
int state = (i < 3 ? 1 : 0);
vopt->geomgroup[i] = state;
vopt->sitegroup[i] = state;
vopt->jointgroup[i] = state;
@@ -224,8 +224,8 @@ void mjv_defaultOption(mjvOption* vopt) {
vopt->skingroup[i] = state;
}
for (int i=0; i<mjNVISFLAG; i++) {
vopt->flags[i] = (mjVISSTRING[i][1][0]=='1');
for (int i=0; i < mjNVISFLAG; i++) {
vopt->flags[i] = (mjVISSTRING[i][1][0] == '1');
}
vopt->bvh_depth = 1;
@@ -326,9 +326,9 @@ void mjv_defaultFigure(mjvFigure* fig) {
mjSTRNCPY(fig->minwidth, "XXX");
// set line colors
for (int n=0; n<mjMAXLINE; n++) {
for (int n=0; n < mjMAXLINE; n++) {
// predefined colors
if (n<8) {
if (n < 8) {
fig->linergb[n][0] = _linergb[n][0];
fig->linergb[n][1] = _linergb[n][1];
fig->linergb[n][2] = _linergb[n][2];
@@ -348,7 +348,7 @@ void mjv_defaultFigure(mjvFigure* fig) {
// compute rbound for mjvGeom
float mjv_rbound(const mjvGeom* geom) {
// model geom: return
if (geom->objtype==mjOBJ_GEOM) {
if (geom->objtype == mjOBJ_GEOM) {
return geom->modelrbound;
}
src/engine/engine_vis_interact.c
+32 -32
View File
@@ -37,7 +37,7 @@ void mjv_room2model(mjtNum* modelpos, mjtNum* modelquat, const mjtNum* roompos,
mjtNum translate[3], rotate[4], invpos[3], invquat[4];
// check scale
if (scn->scale<mjMINVAL) {
if (scn->scale < mjMINVAL) {
mju_error("mjvScene scale too small in mjv_room2model");
}
@@ -72,7 +72,7 @@ void mjv_model2room(mjtNum* roompos, mjtNum* roomquat, const mjtNum* modelpos,
mjtNum translate[3], rotate[4];
// check scale
if (scn->scale<mjMINVAL) {
if (scn->scale < mjMINVAL) {
mju_error("mjvScene scale too small in mjv_model2room");
}
@@ -104,7 +104,7 @@ void mjv_cameraInModel(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSc
mjtNum modelpos[3], modelquat[4], modelmat[9];
// check znear
if (scn->camera[0].frustum_near<mjMINVAL || scn->camera[1].frustum_near<mjMINVAL) {
if (scn->camera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) {
mju_error("mjvScene frustum_near too small in mjv_cameraInModel");
}
@@ -120,7 +120,7 @@ void mjv_cameraInModel(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSc
}
// average over cameras
for (int n=0; n<2; n++) {
for (int n=0; n < 2; n++) {
// convert pos, fwd, u
mju_f2n(pos, scn->camera[n].pos, 3);
mju_f2n(fwd, scn->camera[n].forward, 3);
@@ -177,7 +177,7 @@ void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSce
mjtNum pos[3], fwd[3], u[3];
// check znear
if (scn->camera[0].frustum_near<mjMINVAL || scn->camera[1].frustum_near<mjMINVAL) {
if (scn->camera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) {
mju_error("mjvScene frustum_near too small in mjv_cameraInRoom");
}
@@ -193,7 +193,7 @@ void mjv_cameraInRoom(mjtNum* headpos, mjtNum* forward, mjtNum* up, const mjvSce
}
// average over cameras
for (int n=0; n<2; n++) {
for (int n=0; n < 2; n++) {
// convert pos, fwd, u
mju_f2n(pos, scn->camera[n].pos, 3);
mju_f2n(fwd, scn->camera[n].forward, 3);
@@ -227,7 +227,7 @@ mjtNum mjv_frustumHeight(const mjvScene* scn) {
mjtNum height;
// check znear
if (scn->camera[0].frustum_near<mjMINVAL || scn->camera[1].frustum_near<mjMINVAL) {
if (scn->camera[0].frustum_near < mjMINVAL || scn->camera[1].frustum_near < mjMINVAL) {
mju_error("mjvScene frustum_near too small in mjv_frustumHeight");
}
@@ -311,7 +311,7 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
mjtNum vec[3], dif[3], scl;
// fixed camera: nothing to do
if (cam->type==mjCAMERA_FIXED) {
if (cam->type == mjCAMERA_FIXED) {
return;
}
@@ -326,7 +326,7 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
case mjMOUSE_MOVE_V:
case mjMOUSE_MOVE_H:
// do not move lookat point of tracking camera
if (cam->type==mjCAMERA_TRACKING) {
if (cam->type == mjCAMERA_TRACKING) {
return;
}
@@ -405,7 +405,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx
mju_mulQuat(xiquat, d->xquat+4*sel, m->body_iquat+4*sel);
// limit rotation relative to selected body
if (sel>0 && sel<m->nbody) {
if (sel > 0 && sel < m->nbody) {
// q2 = neg(selbody) * refquat
mjtNum q2[4];
mju_negQuat(q1, xiquat);
@@ -417,7 +417,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx
scl = mju_normalize3(dif);
// check limit: +/- 90 deg allowed
if (scl<-mjPI*0.5 || scl>mjPI*0.5) {
if (scl < -mjPI*0.5 || scl > mjPI*0.5) {
// clamp angle
scl = mju_max(-mjPI*0.5, mju_min(mjPI*0.5, scl));
@@ -467,8 +467,8 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
case mjMOUSE_ROTATE_V:
case mjMOUSE_ROTATE_H:
// construct rotation vector
for (int i=0; i<3; i++) {
if (action==mjMOUSE_ROTATE_V) {
for (int i=0; i < 3; i++) {
if (action == mjMOUSE_ROTATE_V) {
vec[i] = roomup[i]*reldx + roomright[i]*reldy;
} else {
vec[i] = roomforward[i]*reldx + roomright[i]*reldy;
@@ -489,22 +489,22 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
break;
case mjMOUSE_MOVE_V:
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
scn->translate[i] += (float)(roomright[i]*reldx - roomup[i]*reldy) * m->stat.extent;
}
break;
case mjMOUSE_MOVE_H:
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
scn->translate[i] += (float)(roomright[i]*reldx - roomforward[i]*reldy) * m->stat.extent;
}
break;
case mjMOUSE_ZOOM:
scn->scale += (float)(mju_log(1 + scn->scale/3) * reldy * 3);
if (scn->scale<0.01f) {
if (scn->scale < 0.01f) {
scn->scale = 0.01f;
} else if (scn->scale>100.0f) {
} else if (scn->scale > 100.0f) {
scn->scale = 100.0f;
}
break;
@@ -528,7 +528,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
mjtNum* jacM2 = mj_stackAlloc(d, 3*nv);
// invalid selected body: return
if (sel<=0 || sel>=m->nbody) {
if (sel <= 0 || sel >= m->nbody) {
return;
}
@@ -570,7 +570,7 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, i
mjtNum *Rpos, *Rquat, *Cpos, *Cquat;
// exit if nothing to do
if (sel<=0 || sel>=m->nbody || !(pert->active | pert->active2)) {
if (sel <= 0 || sel >= m->nbody || !(pert->active | pert->active2)) {
return;
}
@@ -582,23 +582,23 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, i
mju_mulPose(refpos, refquat, pert->refpos, pert->refquat, pos1, quat1);
// mocap body
if (m->body_mocapid[sel]>=0) {
if (m->body_mocapid[sel] >= 0) {
// copy ref pose into mocap pose
mju_copy3(d->mocap_pos + 3*m->body_mocapid[sel], refpos);
mju_copy4(d->mocap_quat + 4*m->body_mocapid[sel], refquat);
}
// floating body, paused
else if (flg_paused && m->body_jntnum[sel]==1 &&
m->jnt_type[m->body_jntadr[sel]]==mjJNT_FREE) {
else if (flg_paused && m->body_jntnum[sel] == 1 &&
m->jnt_type[m->body_jntadr[sel]] == mjJNT_FREE) {
// copy ref pose into qpos
mju_copy3(d->qpos + m->jnt_qposadr[m->body_jntadr[sel]], refpos);
mju_copy4(d->qpos + m->jnt_qposadr[m->body_jntadr[sel]] + 3, refquat);
}
// child of floating body, paused
else if (flg_paused && m->body_jntnum[rootid]==1 &&
m->jnt_type[m->body_jntadr[rootid]]==mjJNT_FREE) {
else if (flg_paused && m->body_jntnum[rootid] == 1 &&
m->jnt_type[m->body_jntadr[rootid]] == mjJNT_FREE) {
// get pointers to root
Rpos = d->qpos + m->jnt_qposadr[m->body_jntadr[rootid]];
Rquat = Rpos + 3;
@@ -621,7 +621,7 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert)
int sel = pert->select;
// exit if nothing to do
if (sel<0 || sel>=m->nbody || !(pert->active | pert->active2)) {
if (sel < 0 || sel >= m->nbody || !(pert->active | pert->active2)) {
return;
}
@@ -763,7 +763,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum skindist = -1;
*skinid = -1;
if (vopt->flags[mjVIS_SKIN]) {
for (int i=0; i<m->nskin; i++) {
for (int i=0; i < m->nskin; i++) {
// process one skin
int vertid;
mjtNum newdist = mju_raySkin(m->skin_facenum[i], m->skin_vertnum[i],
@@ -772,24 +772,24 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
pos, ray, &vertid);
// update if closer intersection found
if (newdist>=0 && (newdist<skindist || skindist<0)) {
if (newdist >= 0 && (newdist < skindist || skindist < 0)) {
// assign result
skindist = newdist;
// find body with largest weight for this vertex
float bestweight = -1;
for (int j=m->skin_boneadr[i];
j<m->skin_boneadr[i]+m->skin_bonenum[i];
j < m->skin_boneadr[i]+m->skin_bonenum[i];
j++) {
for (int k=m->skin_bonevertadr[j];
k<m->skin_bonevertadr[j]+m->skin_bonevertnum[j];
k < m->skin_bonevertadr[j]+m->skin_bonevertnum[j];
k++) {
// get vertex id and weight
int vid = m->skin_bonevertid[k];
float vweight = m->skin_bonevertweight[k];
// update if matching id and bigger weight
if (vid==vertid && vweight>bestweight) {
if (vid == vertid && vweight > bestweight) {
bestweight = vweight;
bodyid = m->skin_bonebodyid[j];
*skinid = i;
@@ -801,12 +801,12 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
}
// no intersection
if (geomdist<0 && skindist<0) {
if (geomdist < 0 && skindist < 0) {
return -1;
}
// geom only, or geom closer than skin
else if (geomdist>=0 && (skindist<0 || skindist>geomdist)) {
else if (geomdist >= 0 && (skindist < 0 || skindist > geomdist)) {
mju_addScl3(selpnt, pos, ray, geomdist);
*skinid = -1;
return m->geom_bodyid[*geomid];
src/engine/engine_vis_state.c
+5 -5
View File
@@ -141,8 +141,8 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData*
memset(m, 0, sizeof(mjModel));
#ifdef MEMORY_SANITIZER
// Tell msan to treat the entire buffer as uninitialized
__msan_allocated_memory(m, sizeof(mjModel));
// Tell msan to treat the entire buffer as uninitialized
__msan_allocated_memory(m, sizeof(mjModel));
#endif
#define X(var)
@@ -166,8 +166,8 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData*
memset(d, 0, sizeof(mjData));
#ifdef MEMORY_SANITIZER
// Tell msan to treat the entire buffer as uninitialized
__msan_allocated_memory(d, sizeof(mjData));
// Tell msan to treat the entire buffer as uninitialized
__msan_allocated_memory(d, sizeof(mjData));
#endif
memcpy(d->warning, scnstate->data.warning, sizeof(d->warning));
@@ -247,7 +247,7 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
if (m->nplugin) {
const int nslot = mjp_pluginCount();
// iterate over plugins, call visualize if defined
for (int i=0; i<m->nplugin; i++) {
for (int i=0; i < m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
src/engine/engine_vis_visualize.c
+269 -268
View File
File diff suppressed because it is too large Load Diff