From fd17b2144ac29fae196d943ca8839e3d55919ea5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 16 Sep 2024 03:39:11 -0700 Subject: [PATCH] Replace double- or single-precision-denominated function calls in the engine with the equivalent mjtNum-denominated calls. This avoids unnecessary casting or precision loss when MuJoCo is compiled for 32-bit float precision. Accordingly, remove `#include ` from all engine source files. Also, minor type-related fixes in `user_mesh.cc`. PiperOrigin-RevId: 675082329 Change-Id: Icc5eb9f9eb4fbf09b08d7d4b9b2929f414e4a75c --- src/engine/engine_collision_box.c | 46 ++++++++++++------------- src/engine/engine_collision_convex.c | 1 - src/engine/engine_collision_driver.c | 15 ++++---- src/engine/engine_collision_gjk.c | 2 +- src/engine/engine_collision_primitive.c | 4 +-- src/engine/engine_collision_sdf.c | 1 - src/engine/engine_ray.c | 1 - src/engine/engine_util_misc.c | 3 +- src/engine/engine_util_solve.c | 1 - src/engine/engine_util_spatial.c | 4 +-- src/engine/engine_vis_interact.c | 7 ++-- src/user/user_mesh.cc | 31 +++++++++-------- 12 files changed, 52 insertions(+), 64 deletions(-) diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index 17bbecbe..bb42ce89 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -12,11 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include -#include #include "engine/engine_collision_primitive.h" #include "engine/engine_util_blas.h" @@ -65,9 +63,9 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin, closest = (size2[0] + size2[1] + size2[2]) * 2; for (i = 0; i < 6; i++) - if (closest > fabs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2])) + if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2])) { - closest = fabs((i % 2 ? 1 : -1) * size2[i / 2] - center[i / 2]); + closest = mju_abs((i % 2 ? 1 : -1) * size2[i / 2] - center[i / 2]); k = i; } @@ -240,7 +238,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, v = mju_dot3(halfaxis, dif); det = ma * mc - mb * mb; - if (fabs(det) < mjMINVAL) + if (mju_abs(det) < mjMINVAL) continue; idet = 1 / det; @@ -374,7 +372,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, // c.x = s.x * ((c1 / 2) ? -1 : 1); // c.y = s.y * ((c1 % 2) ? -1 : 1); - ee1 = fabs(w) / l; + ee1 = mju_abs(w) / l; // e2 = best / l; // printf("%g %g %g %g %g %g\n",c.x,c.y,d.x,d.y,e1,e2); @@ -453,7 +451,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, if (axis[ax]*axis[ax] > 0.5) { // second point along the edge of the box secondpos = de; // initial position from the - e1 = 2 * size2[ax] / fabs(halfaxis[ax]); + e1 = 2 * size2[ax] / mju_abs(halfaxis[ax]); if (e1 < secondpos) { secondpos = e1; // we overshoot, move back to the other corner of the edge @@ -464,11 +462,11 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, // check for overshoot again - e1 = 2 * size2[ax1] / fabs(halfaxis[ax1]); + e1 = 2 * size2[ax1] / mju_abs(halfaxis[ax1]); if (e1 < secondpos) secondpos = e1; - e1 = 2 * size2[ax2] / fabs(halfaxis[ax2]); + e1 = 2 * size2[ax2] / mju_abs(halfaxis[ax2]); if (e1 < secondpos) secondpos = e1; @@ -502,7 +500,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, // Then it finds with which face the capsule has a lower angle and switches the axis names - if (fabs(axis[ax1]) > fabs(axis[ax2])) + if (mju_abs(axis[ax1]) > mju_abs(axis[ax2])) ax1 = ax2; ax2 = 3 - ax - ax1; @@ -522,7 +520,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, // now we have to find out whether we point towards the opposite side or towards one of the // sides and also find the farthest point along the capsule that is above the box - e1 = 2 * size2[ax2] / fabs(halfaxis[ax2]); + e1 = 2 * size2[ax2] / mju_abs(halfaxis[ax2]); if (e1 < secondpos) secondpos = e1; @@ -531,7 +529,7 @@ int mjraw_CapsuleBox(mjContact* con, mjtNum margin, else e2 = 1 + bestboxpos; - e1 = size2[ax] * e2 / fabs(halfaxis[ax]); + e1 = size2[ax] * e2 / mju_abs(halfaxis[ax]); if (e1 < secondpos) secondpos = e1; @@ -642,9 +640,9 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 mju_transpose(rott, rot, 3, 3); for (i = 0; i < 9; i++) - rotabs[i] = fabs(rot[i]); + rotabs[i] = mju_abs(rot[i]); for (i = 0; i < 9; i++) - rottabs[i] = fabs(rott[i]); + rottabs[i] = mju_abs(rott[i]); mju_mulMatVec3(plen2, rotabs, size2); mju_mulMatTVec3(plen1, rotabs, size1); @@ -653,8 +651,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 penetration += size1[i] * 3 + size2[i] * 3; for (i = 0; i < 3; i++) { - c1 = -fabs(pos21[i]) + size1[i] + plen2[i]; - c2 = -fabs(pos12[i]) + size2[i] + plen1[i]; + c1 = -mju_abs(pos21[i]) + size1[i] + plen2[i]; + c2 = -mju_abs(pos12[i]) + size2[i] + plen1[i]; if (c1 < -margin || c2 < -margin) return 0; @@ -697,12 +695,12 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 for (k = 0; k < 3; k++) if (k != i) - c3 += size1[k] * fabs(tmp2[k]); + c3 += size1[k] * mju_abs(tmp2[k]); for (k = 0; k < 3; k++) if (k != j) c3 += size2[k] * rotabs[3 * i + 3 - k - j] / c1; - c3 -= fabs(c2); + c3 -= mju_abs(c2); if (c3 < -margin) return 0; @@ -851,7 +849,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 mju_copy3(pts[m++], lp); for (i = 0; i < 3; i++) - if (fabs(r[6 + i]) < 0.5) + if (mju_abs(r[6 + i]) < 0.5) mju_scl3(pts[m++], rt + 3 * i, s[i] * ((clcorner & (1 << i)) ? -2 : 2)); mju_add3(pts[3], pts[0], pts[1]); @@ -880,14 +878,14 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 c = lines[i][1 - q]; d = lines[i][4 - q]; - if (fabs(b) > mjMINVAL) { + if (mju_abs(b) > mjMINVAL) { for (j = -1; j <= 1; j += 2) { l = ss[q] * j; c1 = (l - a) * (1 / b); if (c1 < 0 || c1 > 1) continue; c2 = c + d * c1; - if (fabs(c2) > ss[1 - q]) + if (mju_abs(c2) > ss[1 - q]) continue; mju_copy3(points[n], lines[i]); @@ -1123,7 +1121,7 @@ edgeedge: mju_sub3(axi[2], points[2], points[0]); - if (fabs(rnorm[2]) < mjMINVAL) + if (mju_abs(rnorm[2]) < mjMINVAL) return 0; // shouldn't happen innorm = (1 / rnorm[2]) * (in ? -1 : 1); @@ -1183,7 +1181,7 @@ edgeedge: c = lines[i][1 - q]; d = lines[i][4 - q]; - if (fabs(b) > mjMINVAL) { + if (mju_abs(b) > mjMINVAL) { for (j = -1; j <= 1; j += 2) { if (n < mjMAXCONPAIR) { l = s[q] * j; @@ -1191,7 +1189,7 @@ edgeedge: if (c1 < 0 || c1 > 1) continue; c2 = c + d * c1; - if (fabs(c2) > s[1 - q]) + if (mju_abs(c2) > s[1 - q]) continue; if ((linesu[i][2] + linesu[i][5]*c1)*innorm > margin) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 7a495f59..00ff5515 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -14,7 +14,6 @@ #include "engine/engine_collision_convex.h" -#include #include #include diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 3f5e548b..b49d54ec 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -14,7 +14,6 @@ #include "engine/engine_collision_driver.h" -#include #include #include @@ -603,22 +602,22 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], 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])) + - fabs(aabb[i][5]*mju_dot3(normal[i][2], normal[j][k])); + radius[i] = mju_abs(aabb[i][3]*mju_dot3(normal[i][0], normal[j][k])) + + mju_abs(aabb[i][4]*mju_dot3(normal[i][1], normal[j][k])) + + mju_abs(aabb[i][5]*mju_dot3(normal[i][2], normal[j][k])); } else { int adr = 18*i + 9*j + 3*k; proj[i] = aabb[i][0] * product[adr + 0] + aabb[i][1] * product[adr + 1] + aabb[i][2] * product[adr + 2] + offset[6*i + 3*j + k]; - radius[i] = fabs(aabb[i][3]*product[adr + 0]) + - fabs(aabb[i][4]*product[adr + 1]) + - fabs(aabb[i][5]*product[adr + 2]); + radius[i] = mju_abs(aabb[i][3]*product[adr + 0]) + + mju_abs(aabb[i][4]*product[adr + 1]) + + mju_abs(aabb[i][5]*product[adr + 2]); } } - if (radius[0]+radius[1]+margin < fabs(proj[1]-proj[0])) { + if (radius[0]+radius[1]+margin < mju_abs(proj[1]-proj[0])) { return 0; } } diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 298a81c5..101e490a 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -19,8 +19,8 @@ #include #include -#include #include "engine/engine_collision_convex.h" +#include "engine/engine_io.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_spatial.h" diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index 262548a6..db6a6c14 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -14,8 +14,6 @@ #include "engine/engine_collision_primitive.h" -#include - #include #include #include @@ -414,7 +412,7 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin, mjtNum det = ma*mc - mb*mb; // general configuration (non-parallel axes) - if (fabs(det) >= mjMINVAL) { + if (mju_abs(det) >= mjMINVAL) { // find projections, clip to segments mjtNum x1 = (mc*u - mb*v) / det; mjtNum x2 = (ma*v - mb*u) / det; diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 8396b61e..4174d34b 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -14,7 +14,6 @@ #include "engine/engine_collision_sdf.h" -#include #include #include diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 13e26389..9c5c0f18 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -15,7 +15,6 @@ #include "engine/engine_ray.h" -#include #include #include diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 2b5d1fe4..5a3e82d3 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -15,7 +15,6 @@ #include "engine/engine_util_misc.h" #include -#include #include #include #include @@ -39,7 +38,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 (mju_abs(det) < mjMINVAL) { return 0; } diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 03c0bebd..f51928b7 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -14,7 +14,6 @@ #include "engine/engine_util_solve.h" -#include #include #include diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 39cc30b9..57720e28 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -14,8 +14,6 @@ #include "engine/engine_util_spatial.h" -#include - #include #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" @@ -271,7 +269,7 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { a = mju_normalize3(axis); // almost parallel - if (fabs(a) < mjMINVAL) { + if (mju_abs(a) < mjMINVAL) { // opposite: 180 deg rotation around x axis if (mju_dot3(vn, z) < 0) { quat[0] = 0; diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 4803c525..7786d405 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -14,7 +14,6 @@ #include "engine/engine_vis_interact.h" -#include #include #include @@ -690,7 +689,7 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) mju_addTo3(svel, body_linvel); // add critical damping force of selection point - mju_addToScl3(force, svel, -sqrtf(stiffness)*pert->localmass); + mju_addToScl3(force, svel, -mju_sqrt(stiffness)*pert->localmass); // torque on body com due to force mju_cross(torque, moment_arm, force); @@ -698,7 +697,7 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) // add critically damped torsional torque along displacement axis stiffness = m->vis.map.stiffnessrot; mju_normalize3(diff); - mju_addToScl3(torque, diff, -sqrtf(stiffness)*inertia*mju_dot3(diff, body_rotvel)); + mju_addToScl3(torque, diff, -mju_sqrt(stiffness)*inertia*mju_dot3(diff, body_rotvel)); } if (((pert->active | pert->active2) & mjPERT_ROTATE)) { @@ -709,7 +708,7 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) mju_negQuat(xiquat, xiquat); mju_mulQuat(difquat, pert->refquat, xiquat); mju_quat2Vel(torque, difquat, 1.0/(stiffness*inertia)); - mju_addToScl3(torque, body_rotvel, -sqrtf(stiffness)*inertia); + mju_addToScl3(torque, body_rotvel, -mju_sqrt(stiffness)*inertia); } } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 75c3abfd..16b30961 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -28,7 +28,6 @@ #include #include -#include #include "user/user_api.h" #ifdef MUJOCO_TINYOBJLOADER_IMPL @@ -73,6 +72,8 @@ extern "C" { namespace { using mujoco::user::VectorToString; using mujoco::user::FilePath; + using std::max; + using std::min; } // namespace // compute triangle area, surface normal, center @@ -730,7 +731,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { case mjGEOM_CAPSULE: geom->size[0] = (boxsz[0] + boxsz[1])/2; - geom->size[1] = mju_max(0, boxsz[2] - geom->size[0]/2); + geom->size[1] = max(0.0, boxsz[2] - geom->size[0]/2); break; case mjGEOM_CYLINDER: @@ -768,7 +769,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { for (int i=0; i < nvert(); i++) { double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; double dst = mjuu_dist3(v, cen); - geom->size[0] = mju_max(geom->size[0], dst); + geom->size[0] = max(geom->size[0], dst); } break; @@ -781,11 +782,11 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) + (v[1]-cen[1])*(v[1]-cen[1])); - geom->size[0] = mju_max(geom->size[0], dst); + geom->size[0] = max(geom->size[0], dst); // proceed with z: valid for cylinder - double dst2 = fabs(v[2]-cen[2]); - geom->size[1] = mju_max(geom->size[1], dst2); + double dst2 = abs(v[2]-cen[2]); + geom->size[1] = max(geom->size[1], dst2); } // special handling of capsule: consider curved cap @@ -796,11 +797,11 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) + (v[1]-cen[1])*(v[1]-cen[1])); - double dst2 = fabs(v[2]-cen[2]); + double dst2 = abs(v[2]-cen[2]); // get spherical elevation at horizontal distance dst double h = geom->size[0] * sin(acos(dst/geom->size[0])); - geom->size[1] = mju_max(geom->size[1], dst2-h); + geom->size[1] = max(geom->size[1], dst2-h); } } break; @@ -1120,7 +1121,7 @@ void mjCMesh::LoadSTL(mjResource* resource) { resource->name); } // check if vertex coordinates can be cast to an int safely - if (fabs(v[k])>pow(2, 30)) { + if (fabs(v[k]) > pow(2, 30)) { throw mjCError(this, "vertex coordinates in STL file '%s' exceed maximum bounds", resource->name); @@ -1249,7 +1250,7 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type, // if legacy computation requested, then always positive if (!exactmeshinertia && type==mjINERTIA_VOLUME) { - vol = fabs(vol); + vol = abs(vol); } // add pyramid com @@ -1441,7 +1442,7 @@ void mjCMesh::Process() { // if legacy computation requested, then always positive if (!exactmeshinertia && type==mjINERTIA_VOLUME) { - vol = fabs(vol); + vol = abs(vol); } // apply formula, accumulate @@ -1515,8 +1516,8 @@ void mjCMesh::Process() { vert_[3*i+j] = (float) res[j]; // axis-aligned bounding box - aamm_[j+0] = mju_min(aamm_[j+0], res[j]); - aamm_[j+3] = mju_max(aamm_[j+3], res[j]); + aamm_[j+0] = min(aamm_[j+0], res[j]); + aamm_[j+3] = max(aamm_[j+3], res[j]); } } for (int i=0; i < nnormal(); i++) { @@ -2638,8 +2639,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { int* v = elem_.data() + f*(dim+1); for (int e = 0; e < kNumEdges[dim-1]; e++) { auto pair = std::pair( - std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]), - std::max(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]) + min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]), + max(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]) ); // if edge is already present in the vector only store its index