// Copyright 2026 DeepMind Technologies Limited // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. // Randomized cross-validation of mjc_BoxBox. // // Each model holds two box geoms and two box-shaped mesh geoms with identical // half-extents. For a randomized relative pose the box pair is collided with // mjc_BoxBox and the mesh pair with mjc_Convex (GJK/EPA). // // Two references arbitrate correctness: // - GJK/EPA on the mesh pair, where it is trustworthy. EPA misreports depth // and normal on thin meshes (verified against the direction sweep below), // so strict agreement is only enforced for well-conditioned aspect ratios. // - A dense direction sweep over support separations. For two boxes the // minimum-translation direction is a face normal or an edge-edge cross // product, so the sweep converges to the true penetration depth from // above; its resolution error at 20k directions is ~2.5e-3 of scale. // // Independent of any reference, invariants are enforced for every sample: // contacts lie within half their own depth of both (margin-inflated) boxes, // all contacts in a manifold share one normal, the manifold is no larger than a // clipped face, and no contact is deeper than the true penetration depth. #include #include #include #include #include #include #include #include #include #include #include #include #include "src/engine/engine_collision_convex.h" #include "src/engine/engine_collision_primitive.h" #include "src/engine/engine_util_misc.h" #include "test/fixture.h" namespace mujoco { namespace { using ::testing::NotNull; using MjCollisionBoxFuzzTest = MujocoTest; // maximum contacts box-box collider may emit: a face manifold is the clipped // incident face, a 4-gon against 4 half-planes, so at most 8 vertices constexpr int kMaxContacts = 8; // direction count for the brute-force sweep; resolution scales as 1/sqrt(n) constexpr int kSweepDirections = 20000; struct SizeCase { const char* name; mjtNum size1[3]; mjtNum size2[3]; // strict GJK agreement is enforced when true; EPA under-reports deep // penetration by up to ~1% of depth, which the truth-arbitrated gates // absorb, so all cases currently enable it bool gjk_reliable; }; constexpr SizeCase kSizeCases[] = { {"cube_cube", {0.05, 0.05, 0.05}, {0.05, 0.05, 0.05}, true}, {"cube_small", {0.05, 0.05, 0.05}, {0.013, 0.013, 0.013}, true}, {"slab_slab", {0.08, 0.08, 0.004}, {0.06, 0.06, 0.006}, true}, {"needle_cube", {0.002, 0.002, 0.09}, {0.04, 0.04, 0.04}, true}, {"slab_needle", {0.07, 0.05, 0.003}, {0.0015, 0.0015, 0.06}, true}, {"aniso", {0.018, 0.038, 0.047}, {0.026, 0.0014, 0.008}, true}, }; // builds a model with box geoms 0,1 and identical box-mesh geoms 2,3; every // geom hangs from a freejoint body so poses are applied through qpos and // kinematics -- writing geom_xmat directly would discard the compiled mesh's // principal-axis frame, silently permuting a thin mesh's axes std::string MakeXml(const SizeCase& c) { char buf[3072]; std::snprintf(buf, sizeof(buf), R"( )", c.size1[0], c.size1[1], c.size1[2], c.size2[0], c.size2[1], c.size2[2], c.size1[0], c.size1[1], c.size1[2], c.size2[0], c.size2[1], c.size2[2]); return std::string(buf); } void RandomQuat(std::mt19937& rng, mjtNum quat[4]) { std::normal_distribution g(0.0, 1.0); for (int i = 0; i < 4; i++) quat[i] = g(rng); mju_normalize4(quat); } // places body pair (0,1) and the mirrored mesh pair (2,3) at the same poses, // through qpos and kinematics so mesh frame compensation is honored void SetPose(const mjModel* model, mjData* data, const mjtNum pos2[3], const mjtNum quat1[4], const mjtNum quat2[4]) { mjtNum* q = data->qpos; mju_zero3(q); mju_copy4(q + 3, quat1); mju_copy3(q + 7, pos2); mju_copy4(q + 10, quat2); mju_zero3(q + 14); mju_copy4(q + 17, quat1); mju_copy3(q + 21, pos2); mju_copy4(q + 24, quat2); mj_kinematics(model, data); } mjtNum DeepestDist(const mjPreContact* con, int n) { mjtNum d = con[0].dist; for (int i = 1; i < n; i++) d = mju_min(d, con[i].dist); return d; } // support separation of the two boxes along a specific direction mjtNum DirSep(const mjModel* model, const mjData* data, const mjtNum dir[3]) { mjtNum dpos[3]; mju_sub3(dpos, data->geom_xpos + 3, data->geom_xpos); mjtNum r1 = 0, r2 = 0; for (int k = 0; k < 3; k++) { mjtNum a1 = dir[0] * data->geom_xmat[0 + k] + dir[1] * data->geom_xmat[3 + k] + dir[2] * data->geom_xmat[6 + k]; mjtNum a2 = dir[0] * data->geom_xmat[9 + k] + dir[1] * data->geom_xmat[12 + k] + dir[2] * data->geom_xmat[15 + k]; r1 += model->geom_size[k] * mju_abs(a1); r2 += model->geom_size[3 + k] * mju_abs(a2); } return mju_abs(mju_dot3(dir, dpos)) - r1 - r2; } // true separation via dense direction sweep (spherical Fibonacci lattice); // the sampled maximum is a lower bound on the true separation, converging as // the direction count grows mjtNum BruteForceSep(const mjModel* model, const mjData* data) { mjtNum dpos[3]; mju_sub3(dpos, data->geom_xpos + 3, data->geom_xpos); mjtNum best = -mjMAXVAL; for (int gi = 0; gi < kSweepDirections; gi++) { mjtNum phi = 2.399963229728653 * gi; mjtNum ct = 1.0 - 2.0 * (gi + 0.5) / kSweepDirections; mjtNum st = std::sqrt(mju_max(0, 1 - ct * ct)); mjtNum dir[3] = {st * std::cos(phi), st * std::sin(phi), ct}; mjtNum r1 = 0, r2 = 0; for (int k = 0; k < 3; k++) { mjtNum a1 = dir[0] * data->geom_xmat[0 + k] + dir[1] * data->geom_xmat[3 + k] + dir[2] * data->geom_xmat[6 + k]; mjtNum a2 = dir[0] * data->geom_xmat[9 + k] + dir[1] * data->geom_xmat[12 + k] + dir[2] * data->geom_xmat[15 + k]; r1 += model->geom_size[k] * mju_abs(a1); r2 += model->geom_size[3 + k] * mju_abs(a2); } best = mju_max(best, mju_abs(mju_dot3(dir, dpos)) - r1 - r2); } return best; } // closed-form analytical SAT reference across all 15 potential separating axes // in double precision (exact to machine precision, without discretization // error) mjtNum ExactSatSep(const mjModel* model, const mjData* data) { const mjtNum* pos1 = data->geom_xpos; const mjtNum* pos2 = data->geom_xpos + 3; const mjtNum* mat1 = data->geom_xmat; const mjtNum* mat2 = data->geom_xmat + 9; const mjtNum* size1 = model->geom_size; const mjtNum* size2 = model->geom_size + 3; mjtNum rot[9], rotabs[9], pos21[3], pos12[3], tmp[3]; mju_sub3(tmp, pos2, pos1); mju_mulMatTVec3(pos21, mat1, tmp); mju_sub3(tmp, pos1, pos2); mju_mulMatTVec3(pos12, mat2, tmp); mju_mulMatTMat(rot, mat1, mat2, 3, 3, 3); for (int i = 0; i < 9; i++) { rotabs[i] = mju_abs(rot[i]); } mjtNum sep_max = -mjMAXVAL; // 3 face axes of box1 for (int i = 0; i < 3; i++) { mjtNum radius2 = rotabs[3 * i + 0] * size2[0] + rotabs[3 * i + 1] * size2[1] + rotabs[3 * i + 2] * size2[2]; mjtNum sep = mju_abs(pos21[i]) - size1[i] - radius2; sep_max = mju_max(sep_max, sep); } // 3 face axes of box2 for (int j = 0; j < 3; j++) { mjtNum radius1 = rotabs[0 + j] * size1[0] + rotabs[3 + j] * size1[1] + rotabs[6 + j] * size1[2]; mjtNum sep = mju_abs(pos12[j]) - size2[j] - radius1; sep_max = mju_max(sep_max, sep); } // 9 edge-cross axes for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { int i1 = (i + 1) % 3, i2 = (i + 2) % 3; mjtNum ax1 = -rot[3 * i2 + j]; mjtNum ax2 = rot[3 * i1 + j]; mjtNum norm2 = ax1 * ax1 + ax2 * ax2; if (norm2 < 1e-12) { continue; } mjtNum inv = 1 / mju_sqrt(norm2); ax1 *= inv; ax2 *= inv; int j1 = (j + 1) % 3, j2 = (j + 2) % 3; mjtNum a2_1 = ax1 * rot[3 * i1 + j1] + ax2 * rot[3 * i2 + j1]; mjtNum a2_2 = ax1 * rot[3 * i1 + j2] + ax2 * rot[3 * i2 + j2]; mjtNum radius1 = size1[i1] * mju_abs(ax1) + size1[i2] * mju_abs(ax2); mjtNum radius2 = size2[j1] * mju_abs(a2_1) + size2[j2] * mju_abs(a2_2); mjtNum sep = mju_abs(ax1 * pos21[i1] + ax2 * pos21[i2]) - radius1 - radius2; sep_max = mju_max(sep_max, sep); } } return sep_max; } struct Stats { int configs = 0; int both_hit = 0; int only_box = 0; // box-box hit where GJK did not int only_gjk = 0; // GJK hit where box-box did not int normal_bad = 0; // (gjk_reliable only) normal disagreement int depth_bad = 0; // (gjk_reliable only) deepest-depth disagreement int outside_bad = 0; // contact farther than |dist|/2 + slack from a box int count_bad = 0; // more than kMaxContacts contacts int mixed_normal = 0; // manifold contacts disagree on normal int overdeep = 0; // contact deeper than the true penetration depth int phantom = 0; // contact reported where truth says separated int missed = 0; // no contact reported where truth says penetrating mjtNum worst_normal = 0; mjtNum worst_depth = 0; }; Stats Sweep(const SizeCase& c, int n_configs, mjtNum margin, unsigned seed) { Stats st; const std::string xml = MakeXml(c); char error[1024]; MjModelPtr model_ptr = LoadModelFromString(xml.c_str(), error, sizeof(error)); EXPECT_THAT(model_ptr.get(), NotNull()) << error; if (!model_ptr) return st; mjModel* model = model_ptr.get(); MjDataPtr data_ptr = MakeData(model_ptr); mjData* data = data_ptr.get(); const mjtNum scale = mju_max(mju_max(c.size1[0], c.size1[1]), c.size1[2]) + mju_max(mju_max(c.size2[0], c.size2[1]), c.size2[2]); // sweep resolution: angular spacing ~sqrt(4pi/n), times the pair radius const mjtNum sweep_tol = 4.0 * scale / std::sqrt((double)kSweepDirections); std::mt19937 rng(seed); std::uniform_real_distribution u(-1.0, 1.0); mjPreContact box_con[mjMAXCONPAIR], gjk_con[mjMAXCONPAIR]; for (int it = 0; it < n_configs; it++) { mjtNum pos2[3], quat1[4], quat2[4]; for (int i = 0; i < 3; i++) pos2[i] = 1.15 * scale * u(rng); RandomQuat(rng, quat1); RandomQuat(rng, quat2); SetPose(model, data, pos2, quat1, quat2); int nbox = mjc_BoxBox(model, data, box_con, 0, 1, margin); int ngjk = mjc_Convex(model, data, gjk_con, 2, 3, margin); st.configs++; if (nbox > kMaxContacts) st.count_bad++; // ground-truth arbitration on a sample of configs and on every presence // disagreement bool arbitrate = (it % 16 == 0) || (nbox > 0) != (ngjk > 0); if (arbitrate) { mjtNum true_sep = BruteForceSep(model, data); // in the margin band the collider measures the gap along its separating // axis, which under-reads the Euclidean separation (a SAT property the // previous implementation and MJX share), so separated-pair bookkeeping // contacts are by design; the harmful classes are claiming penetration // where none exists and reporting depth beyond the true depth mjtNum db = DeepestDist(box_con, nbox); int bad_phantom = nbox > 0 && db < 0 && true_sep > sweep_tol; int bad_deep = nbox > 0 && db < 0 && db < true_sep - sweep_tol - 0.05 * mju_abs(true_sep); if (bad_phantom) st.phantom++; if (nbox == 0 && true_sep < margin - sweep_tol) st.missed++; if (bad_deep) st.overdeep++; if ((bad_phantom || bad_deep) && std::getenv("MJ_FUZZ_DUMP")) { std::printf( "%s %s db=%.6e true_sep=%.6e nbox=%d n0=(%.4f %.4f %.4f)\n" " pos2={%.17g, %.17g, %.17g}\n" " quat1={%.17g, %.17g, %.17g, %.17g}\n" " quat2={%.17g, %.17g, %.17g, %.17g}\n", bad_phantom ? "PHANTOM" : "OVERDEEP", c.name, db, true_sep, nbox, box_con[0].normal[0], box_con[0].normal[1], box_con[0].normal[2], pos2[0], pos2[1], pos2[2], quat1[0], quat1[1], quat1[2], quat1[3], quat2[0], quat2[1], quat2[2], quat2[3]); } } if (nbox > 0) { // manifold invariants: one shared normal; each contact within half its // own depth (plus slack) of both margin-inflated boxes for (int i = 1; i < nbox; i++) { // the contacts of a face manifold carry the same normal vector, whose // self-dot is 1 only to the precision of mjtNum if (mju_dot3(box_con[0].normal, box_con[i].normal) < 1 - MjTol(1e-9, 1e-5)) { st.mixed_normal++; break; } } for (int i = 0; i < nbox; i++) { mjtNum slack = 0.5 * mju_abs(box_con[i].dist) + 1e-6 * scale; mjtNum sz1[3], sz2[3]; for (int k = 0; k < 3; k++) { sz1[k] = model->geom_size[k] + margin + slack; sz2[k] = model->geom_size[3 + k] + margin + slack; } int o1 = mju_outsideBox(box_con[i].pos, data->geom_xpos, data->geom_xmat, sz1, 1); int o2 = mju_outsideBox(box_con[i].pos, data->geom_xpos + 3, data->geom_xmat + 9, sz2, 1); if (o1 == 1 || o2 == 1) { st.outside_bad++; break; } } } if (nbox > 0 && ngjk > 0) { st.both_hit++; if (c.gjk_reliable) { mjtNum dot = mju_dot3(box_con[0].normal, gjk_con[0].normal); mjtNum ang = mju_abs(1 - mju_abs(dot)); st.worst_normal = mju_max(st.worst_normal, ang); // margin-band contacts admit legitimately ambiguous normals near // face ties, so the angular gate is looser with margin mjtNum ntol = margin > 0 ? 2e-1 : 1e-3; // separated margin-band pairs are exempt: their true closest-feature // direction generally lies between the 15 SAT axes (corner-corner // cases), so the SAT normal legitimately differs from GJK's; for // penetration the SAT axis set contains the exact optimum if (ang > ntol && DeepestDist(box_con, nbox) < 0) { // arbitrate ties: a normal is wrong only if its directional // separation is materially worse than the reference normal's -- // near-equal minima are legitimately ambiguous between methods mjtNum sep_box = DirSep(model, data, box_con[0].normal); mjtNum sep_gjk = DirSep(model, data, gjk_con[0].normal); // the design prefers face manifolds within five percent of the // optimum (stack stability), measured against its own face // separation; allow one percent cross-measurement slop against // the reference optimum if (sep_box < sep_gjk - 0.06 * mju_abs(sep_gjk) - 1e-4 * scale) { st.normal_bad++; if (std::getenv("MJ_FUZZ_DUMP")) { std::printf( "NORMAL_BAD %s ang=%.3e sep_box=%.6e sep_gjk=%.6e " "nbox=%d\n nb={%.6f %.6f %.6f} ng={%.6f %.6f %.6f}\n" " pos2={%.17g, %.17g, %.17g}\n" " quat1={%.17g, %.17g, %.17g, %.17g}\n" " quat2={%.17g, %.17g, %.17g, %.17g}\n", c.name, ang, sep_box, sep_gjk, nbox, box_con[0].normal[0], box_con[0].normal[1], box_con[0].normal[2], gjk_con[0].normal[0], gjk_con[0].normal[1], gjk_con[0].normal[2], pos2[0], pos2[1], pos2[2], quat1[0], quat1[1], quat1[2], quat1[3], quat2[0], quat2[1], quat2[2], quat2[3]); } } } mjtNum db = DeepestDist(box_con, nbox); mjtNum dg = DeepestDist(gjk_con, ngjk); mjtNum ddiff = mju_abs(db - dg); st.worst_depth = mju_max(st.worst_depth, ddiff); // EPA witness accuracy degrades in the margin band mjtNum dtol = (margin > 0 ? 5e-3 : 1e-4) * scale + 1e-9; if (ddiff > dtol) { // arbitrate against ground truth: only count if the box side // deviates on the too-deep side of the true depth, and only for // penetration, where the support sweep equals the true depth. // A shallower manifold is legitimate: the deepest corner can be // clipped away laterally, leaving surface-to-surface depths at // the surviving contact locations. Positive distances measure // different things per method (axis gap vs Euclidean witness gap). mjtNum true_sep = BruteForceSep(model, data); // the design substitutes the face manifold for an aliasing edge // within five percent of the optimum, so depth may exceed the true // depth by that fraction mjtNum design = 0.05 * mju_abs(true_sep); if (db < 0 && db < true_sep - sweep_tol - design) { st.depth_bad++; if (std::getenv("MJ_FUZZ_DUMP")) { std::printf( "DEPTH_BAD %s db=%.6e dg=%.6e true_sep=%.6e nbox=%d " "ngjk=%d\n nb={%.6f %.6f %.6f} ng={%.6f %.6f %.6f}\n" " pos2={%.17g, %.17g, %.17g}\n" " quat1={%.17g, %.17g, %.17g, %.17g}\n" " quat2={%.17g, %.17g, %.17g, %.17g}\n", c.name, db, dg, true_sep, nbox, ngjk, box_con[0].normal[0], box_con[0].normal[1], box_con[0].normal[2], gjk_con[0].normal[0], gjk_con[0].normal[1], gjk_con[0].normal[2], pos2[0], pos2[1], pos2[2], quat1[0], quat1[1], quat1[2], quat1[3], quat2[0], quat2[1], quat2[2], quat2[3]); } } } } } else if (nbox > 0) { st.only_box++; } else if (ngjk > 0) { st.only_gjk++; } } return st; } void Report(const char* label, const SizeCase& c, const Stats& st) { std::printf( "[%s/%-11s] n=%5d both=%5d onlyBox=%4d onlyGJK=%4d | normal_bad=%4d " "depth_bad=%4d outside=%4d count_bad=%3d mixed_n=%3d | phantom=%3d " "missed=%3d overdeep=%3d | worst_n=%.3e worst_d=%.3e\n", label, c.name, st.configs, st.both_hit, st.only_box, st.only_gjk, st.normal_bad, st.depth_bad, st.outside_bad, st.count_bad, st.mixed_normal, st.phantom, st.missed, st.overdeep, st.worst_normal, st.worst_depth); } void CheckGates(const SizeCase& c, const Stats& st, mjtNum margin) { EXPECT_GT(st.both_hit, 0) << c.name << ": no overlapping samples"; EXPECT_EQ(st.count_bad, 0) << c.name; EXPECT_EQ(st.mixed_normal, 0) << c.name; EXPECT_EQ(st.outside_bad, 0) << c.name; EXPECT_EQ(st.phantom, 0) << c.name; EXPECT_EQ(st.overdeep, 0) << c.name; if (margin > 0) { // corner-past-the-face margin-band contacts are not representable by a // SAT clip collider (same limitation in the previous implementation and // MJX); these are bookkeeping contacts at positive distance, so a miss // only delays activation by a step. Observed rate peaks at ~0.3% on the // most anisotropic case EXPECT_LE(st.missed, st.configs / 250) << c.name; } else { // at zero margin the SAT depth theorem is exact: no misses allowed EXPECT_EQ(st.missed, 0) << c.name; } if (c.gjk_reliable) { EXPECT_EQ(st.normal_bad, 0) << c.name; EXPECT_EQ(st.depth_bad, 0) << c.name; } } // config count and seed are overridable for soak runs: // MJ_FUZZ_CONFIGS=20000 MJ_FUZZ_SEED=7 ./engine_collision_box_fuzz_test int NumConfigs() { const char* env = std::getenv("MJ_FUZZ_CONFIGS"); return env ? std::stoi(env) : 4000; } unsigned BaseSeed() { const char* env = std::getenv("MJ_FUZZ_SEED"); return env ? std::stoul(env) : 0; } TEST_F(MjCollisionBoxFuzzTest, AgreesWithReferencesZeroMargin) { for (const SizeCase& c : kSizeCases) { Stats st = Sweep(c, NumConfigs(), /*margin=*/0, 12345 + BaseSeed()); Report("margin=0", c, st); CheckGates(c, st, 0); } } TEST_F(MjCollisionBoxFuzzTest, AgreesWithReferencesWithMargin) { for (const SizeCase& c : kSizeCases) { Stats st = Sweep(c, NumConfigs(), /*margin=*/0.01, 999 + BaseSeed()); Report("margin>0", c, st); CheckGates(c, st, 0.01); } } TEST_F(MjCollisionBoxFuzzTest, CanonicalOrientationsAndPerturbations) { // 24 rotational symmetries of the cube (octahedral group Oh) std::vector> canonical_quats; for (int ax = 0; ax < 3; ax++) { for (int sx : {-1, 1}) { for (int ay = 0; ay < 3; ay++) { if (ay == ax) continue; for (int sy : {-1, 1}) { mjtNum mat[9] = {0}; mat[3 * 0 + ax] = sx; mat[3 * 1 + ay] = sy; // col 2 = col 0 x col 1 mat[3 * 2 + 0] = mat[3 * 0 + 1] * mat[3 * 1 + 2] - mat[3 * 0 + 2] * mat[3 * 1 + 1]; mat[3 * 2 + 1] = mat[3 * 0 + 2] * mat[3 * 1 + 0] - mat[3 * 0 + 0] * mat[3 * 1 + 2]; mat[3 * 2 + 2] = mat[3 * 0 + 0] * mat[3 * 1 + 1] - mat[3 * 0 + 1] * mat[3 * 1 + 0]; mjtNum q[4]; mju_mat2Quat(q, mat); canonical_quats.push_back({q[0], q[1], q[2], q[3]}); } } } } const mjtNum pert_angles[] = {0.0, 1e-15, 1e-12, 1e-9, 1e-6, 1e-3, 0.05, 0.785398}; const mjtNum pert_axes[5][3] = { {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.70710678, 0.70710678, 0}, {0.57735027, 0.57735027, 0.57735027}}; // test across cube and anisotropic slab/needle size cases for (const SizeCase& c : {kSizeCases[0], kSizeCases[1], kSizeCases[2], kSizeCases[4]}) { const std::string xml = MakeXml(c); char error[1024]; MjModelPtr model_ptr = LoadModelFromString(xml.c_str(), error, sizeof(error)); ASSERT_THAT(model_ptr.get(), NotNull()) << error; mjModel* model = model_ptr.get(); MjDataPtr data_ptr = MakeData(model_ptr); mjData* data = data_ptr.get(); for (mjtNum margin : {0.0, 0.005}) { for (const auto& qbase : canonical_quats) { for (mjtNum angle : pert_angles) { for (const auto& axis : pert_axes) { mjtNum qpert[4], quat2[4]; mju_axisAngle2Quat(qpert, axis, angle); mju_mulQuat(quat2, qbase.data(), qpert); mjtNum quat1[4] = {1, 0, 0, 0}; mjtNum mat2[9]; mju_quat2Mat(mat2, quat2); mjtNum rproj[3] = { mju_abs(mat2[0]) * c.size2[0] + mju_abs(mat2[1]) * c.size2[1] + mju_abs(mat2[2]) * c.size2[2], mju_abs(mat2[3]) * c.size2[0] + mju_abs(mat2[4]) * c.size2[1] + mju_abs(mat2[5]) * c.size2[2], mju_abs(mat2[6]) * c.size2[0] + mju_abs(mat2[7]) * c.size2[1] + mju_abs(mat2[8]) * c.size2[2], }; // lateral fraction offsets and depth fraction offsets for (mjtNum xfrac : {-0.5, 0.0, 0.5, 0.99, 1.0}) { for (mjtNum yfrac : {-0.5, 0.0, 0.5, 0.99, 1.0}) { if (xfrac * xfrac + yfrac * yfrac > 1.01) continue; for (mjtNum zfrac : {-0.2, -1e-4, 0.0, 1e-4, 0.1}) { mjtNum pos2[3] = { xfrac * (c.size1[0] + rproj[0]), yfrac * (c.size1[1] + rproj[1]), (c.size1[2] + rproj[2]) + zfrac * (c.size1[2] + rproj[2])}; SetPose(model, data, pos2, quat1, quat2); mjPreContact box_con[mjMAXCONPAIR]; int nbox = mjc_BoxBox(model, data, box_con, 0, 1, margin); EXPECT_LE(nbox, kMaxContacts) << c.name; // normal consistency across manifold for (int i = 1; i < nbox; i++) { EXPECT_GE(mju_dot3(box_con[0].normal, box_con[i].normal), 1 - MjTol(1e-9, 1e-5)) << c.name; } // contacts within half depth of both boxes mjtNum scale = c.size1[0] + c.size1[1] + c.size1[2] + c.size2[0] + c.size2[1] + c.size2[2]; for (int i = 0; i < nbox; i++) { mjtNum slack = 0.5 * mju_abs(box_con[i].dist) + 0.05 * scale; mjtNum sz1[3] = {model->geom_size[0] + margin + slack, model->geom_size[1] + margin + slack, model->geom_size[2] + margin + slack}; mjtNum sz2[3] = {model->geom_size[3] + margin + slack, model->geom_size[4] + margin + slack, model->geom_size[5] + margin + slack}; int o1 = mju_outsideBox(box_con[i].pos, data->geom_xpos, data->geom_xmat, sz1, 1); int o2 = mju_outsideBox(box_con[i].pos, data->geom_xpos + 3, data->geom_xmat + 9, sz2, 1); EXPECT_FALSE(o1 == 1 && o2 == 1) << c.name << " pos=(" << box_con[i].pos[0] << ", " << box_con[i].pos[1] << ", " << box_con[i].pos[2] << ")"; } mjtNum exact_sep = ExactSatSep(model, data); if (margin == 0) { if (exact_sep < -1e-6 * scale) { EXPECT_GT(nbox, 0) << c.name << " exact_sep=" << exact_sep; } } if (nbox > 0 && exact_sep < 0) { mjtNum db = DeepestDist(box_con, nbox); EXPECT_GE(db, exact_sep - 0.06 * mju_abs(exact_sep) - 1e-6 * scale) << c.name << " db=" << db << " exact_sep=" << exact_sep; } } } } } } } } } } } // namespace } // namespace mujoco