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

Improvements to GJK implementation:

Browse Source 
- Fix various bugs
- Improve readability
- Add support for recovering witness points.

PiperOrigin-RevId: 646515019
Change-Id: I295cf90d885e32013e595ea7aad07ed6c1869239
This commit is contained in:
Kyle Bayes
2024-06-25 09:59:52 -07:00
committed by Copybara-Service
parent 5a513a0afb
commit 5e39fc0564
5 changed files with 214 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+12 -12
View File
@@ -34,13 +34,13 @@
// ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) {
mjc_center(center->v, (const mjtCCObj*) obj);
mjc_center(center->v, (const mjCCDObj*) obj);
}
// center function for convex collision algorithms
void mjc_center(mjtNum res[3], const mjtCCObj *obj) {
void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
int g = obj->geom;
int f = obj->flex;
int e = obj->elem;
@@ -66,13 +66,13 @@ void mjc_center(mjtNum res[3], const mjtCCObj *obj) {
// ccd support function
void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjc_support(vec->v, (mjtCCObj*) obj, _dir->v);
mjc_support(vec->v, (mjCCDObj*) obj, _dir->v);
}
// support function for convex collision algorithms
void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]) {
void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
int g = obj->geom;
@@ -289,7 +289,7 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
// find single convex-convex collision, using libccd
static int mjc_MPRIteration(mjtCCObj* obj1, mjtCCObj* obj2, const ccd_t* ccd,
static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
const mjModel* m, const mjData* d,
mjContact* con, mjtNum margin) {
ccd_vec3_t dir, pos;
@@ -360,8 +360,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
ccd_t ccd;
mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
// init ccd structure
mjc_initCCD(&ccd, m);
@@ -489,7 +489,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
mjGETINFO
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
ccd_vec3_t dir, vec;
mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
// get support point in -normal direction
ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
@@ -670,7 +670,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
ccd_t ccd;
// point size1 to hfield size instead of geom1 size
@@ -1103,8 +1103,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
ccd_t ccd;
mjtCCObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}};
mjtCCObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
// init ccd structure
mjc_initCCD(&ccd, m);
@@ -1151,7 +1151,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjtCCObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}};
mjCCDObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
ccd_t ccd;
//------------------------------------- AABB computation, box-box test
src/engine/engine_collision_convex.h
+7 -5
View File
@@ -23,6 +23,7 @@
#include <ccd/vec3.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
@@ -39,8 +40,8 @@
extern "C" {
#endif
// internal object type for convex collision algorithms
struct _mjtCCObj {
// internal object type for convex collision detection
struct _mjCCDObj {
const mjModel* model;
const mjData* data;
int geom;
@@ -50,14 +51,15 @@ struct _mjtCCObj {
int vert;
mjtNum margin;
mjtNum rotate[4];
mjtNum x0[3]; // initial guess of the witness point
};
typedef struct _mjtCCObj mjtCCObj;
typedef struct _mjCCDObj mjCCDObj;
// support function for convex collision algorithms
void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]);
MJAPI void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
// center function for convex collision algorithms
void mjc_center(mjtNum res[3], const mjtCCObj *obj);
MJAPI void mjc_center(mjtNum res[3], const mjCCDObj *obj);
// ccd support function
void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec);
src/engine/engine_collision_gjk.c
+111 -74
View File
@@ -17,6 +17,7 @@
#include <stddef.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_util_blas.h"
#include "engine/engine_util_spatial.h"
#include "engine/engine_collision_convex.h"
@@ -37,34 +38,38 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]);
static void S1D(mjtNum lambda[2], const mjtNum simplex[6]);
// helper function to compute the support point in the Minkowski difference
static void support(mjtNum res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]);
static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3]);
// linear algebra utility functions
static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]);
static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n);
// returns the distance between the two objects. The witness points are
// recoverable from x_0 in obj1 and obj2.
mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions
int n = 0; // number of vertices in the simplex
mjtNum x_k[3]; // the kth approximation point with initial value x_0
// returns the distance between the two given objects given an initial guess x0
mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]) {
// our current simplex with max 4 vertices due to only 3 dimensions
mjtNum simplex[12];
int n = 0; // number of vertices in the simplex
// the kth approximation point with initial value x_0
mjtNum x_k[3];
mju_copy3(x_k, x0);
// segregated simplices and points for the two objects to recover witness points
mjtNum simplex1[12], simplex2[12];
mjtNum* x1_k = obj1->x0;
mjtNum* x2_k = obj2->x0;
mju_sub3(x_k, x1_k, x2_k);
int N = config->max_iterations;
for (size_t k = 0; k < N; k++) {
mjtNum s_k[3]; // the kth support point
mjtNum lambda[4]; // barycentric coordinates for x_k
mjtNum s1[3], s2[3]; // the support points in obj1 and obj2
mjtNum s_k[3]; // the kth support point of Minkowski difference
mjtNum lambda[4]; // barycentric coordinates for x_k
// compute the kth support point in the direction of -(x_k)
mjtNum x_k_neg[3];
mju_scl3(x_k_neg, x_k, -1);
mju_normalize3(x_k_neg);
support(s_k, obj1, obj2, x_k_neg);
support(s1, s2, obj1, obj2, x_k_neg);
mju_sub3(s_k, s1, s2);
// the stopping criteria relies on the Frank-Wolfe duality gap given by
// f(x_k) - f(x_min) <= < grad f(x_k), (x_k - s_k) >
@@ -74,36 +79,57 @@ mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const m
return mju_norm3(x_k);
}
// TODO(kylebayes): signedVolume has been written to assume the first vertex is the latest
// support to be added. Once the logic has been updated, then this hack should be removed.
for (int i = n; i > 0; i--) {
// shift the simplex vertices to the right
mju_copy3(simplex + 3*i, simplex + 3*(i-1));
mju_copy3(simplex1 + 3*i, simplex1 + 3*(i-1));
mju_copy3(simplex2 + 3*i, simplex2 + 3*(i-1));
}
// copy new support point into the simplex
mju_copy3(simplex + 3*n++, s_k);
mju_copy3(simplex, s_k);
// copy new support point into the individual simplexes
mju_copy3(simplex1, s1);
mju_copy3(simplex2, s2);
// run the distance subalgorithm to compute the barycentric coordinates
// of the closest point to the origin in the simplex
signedVolume(lambda, simplex, n);
signedVolume(lambda, simplex, ++n);
lincomb(x_k, lambda, simplex, 4);
// compute the approximate witness points
lincomb(x1_k, lambda, simplex1, 4);
lincomb(x2_k, lambda, simplex2, 4);
// for lambda[i] == 0, remove the ith vertex from the simplex
n = 0;
for (int i = 0; i < 4; i++) {
if (lambda[i] == 0) continue;
// recover simplex for the two objects
mju_copy3(simplex1 + 3*n, simplex1 + 3*i);
mju_copy3(simplex2 + 3*n, simplex2 + 3*i);
// simplex in Minkowski difference
mju_copy3(simplex + 3*n++, simplex + 3*i);
}
}
return mju_norm3(x_k);
}
// helper function to compute the support point in the Minkowski difference
static void support(mjtNum res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]) {
mjtNum s1[3], s2[3], dir_neg[3];
static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3]) {
mjtNum dir_neg[3];
mju_scl3(dir_neg, dir, -1);
// compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
mjc_support(s1, obj1, dir);
mjc_support(s2, obj2, dir_neg);
mju_sub3(res, s1, s2);
}
@@ -198,20 +224,7 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
}
// find the smallest distance, and use the corresponding barycentric coordinates
mjtNum dist;
if (!comp1) {
mjtNum lambda_2d[3], verts[9], x[3];
mju_copy3(verts, s2);
mju_copy3(verts + 3, s3);
mju_copy3(verts + 6, s4);
S2D(lambda_2d, verts);
lincomb(x, lambda_2d, verts, 3);
dist = mju_norm3(x);
lambda[1] = lambda_2d[0];
lambda[2] = lambda_2d[1];
lambda[3] = lambda_2d[2];
}
mjtNum dist = mjMAXVAL;
if (!comp2) {
mjtNum lambda_2d[3], verts[9], x[3];
@@ -221,12 +234,11 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
S2D(lambda_2d, verts);
lincomb(x, lambda_2d, verts, 3);
mjtNum d = mju_norm3(x);
if (d < dist) {
lambda[0] = lambda_2d[0];
lambda[2] = lambda_2d[1];
lambda[3] = lambda_2d[2];
dist = d;
}
lambda[0] = lambda_2d[0];
lambda[1] = 0;
lambda[2] = lambda_2d[1];
lambda[3] = lambda_2d[2];
dist = d;
}
if (!comp3) {
@@ -240,6 +252,7 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
if (d < dist) {
lambda[0] = lambda_2d[0];
lambda[1] = lambda_2d[1];
lambda[2] = 0;
lambda[3] = lambda_2d[2];
dist = d;
}
@@ -257,6 +270,24 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
lambda[0] = lambda_2d[0];
lambda[1] = lambda_2d[1];
lambda[2] = lambda_2d[2];
lambda[3] = 0;
dist = d;
}
}
if (!comp1) {
mjtNum lambda_2d[3], verts[9], x[3];
mju_copy3(verts, s2);
mju_copy3(verts + 3, s3);
mju_copy3(verts + 6, s4);
S2D(lambda_2d, verts);
lincomb(x, lambda_2d, verts, 3);
mjtNum d = mju_norm3(x);
if (d < dist) {
lambda[0] = 0;
lambda[1] = lambda_2d[0];
lambda[2] = lambda_2d[1];
lambda[3] = lambda_2d[2];
dist = d;
}
}
@@ -273,30 +304,29 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
// compute normal
mjtNum diff1[3], diff2[3], n[3];
mju_sub3(diff1, s2, s1);
mju_sub3(diff2, s3, s2);
mju_sub3(diff2, s3, s1);
mju_cross(n, diff1, diff2);
// project origin
mjtNum p_o[3];
mju_scl3(p_o, n, mju_dot3(n, s1) / mju_dot3(n, n));
int index;
mjtNum mu_max = 0;
int k = 1, l = 2;
for (int i = 0; i < 3; i++) {
mjtNum mu = s2[k]*s3[l] + s1[k]*s2[l] + s3[k]*s1[l] - s2[k]*s1[l] - s3[k]*s2[l] - s1[k]*s3[l];
if (mju_abs(mu) >= mju_abs(mu_max)) {
mu_max = mu;
index = i;
}
k = l; l = i;
}
// Below are the minors M_i4 of the matrix M given by
// [[ s1_x, s2_x, s3_x, s4_x ],
// [ s1_y, s2_y, s3_y, s4_y ],
// [ s1_z, s2_z, s3_z, s4_z ],
// [ 1, 1, 1, 1 ]]
mjtNum M_14 = s2[1]*s3[2] - s2[2]*s3[1] - s1[1]*s3[2] + s1[2]*s3[1] + s1[1]*s2[2] - s1[2]*s2[1];
mjtNum M_24 = s2[0]*s3[2] - s2[2]*s3[0] - s1[0]*s3[2] + s1[2]*s3[0] + s1[0]*s2[2] - s1[2]*s2[0];
mjtNum M_34 = s2[0]*s3[1] - s2[1]*s3[0] - s1[0]*s3[1] + s1[1]*s3[0] + s1[0]*s2[1] - s1[1]*s2[0];
// exclude index component
// exclude one of the axes with the largest projection of the simplex using the computed minors
mjtNum s1_2D[2], s2_2D[2], s3_2D[2], p_o_2D[2];
if (index == 0) {
mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34);
if (mu1 >= mu2 && mu1 >= mu3) {
mu_max = mu1;
s1_2D[0] = s1[1];
s1_2D[1] = s1[2];
@@ -308,7 +338,8 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
p_o_2D[0] = p_o[1];
p_o_2D[1] = p_o[2];
} else if (index == 1) {
} else if (mu2 >= mu3) {
mu_max = mu2;
s1_2D[0] = s1[0];
s1_2D[1] = s1[2];
@@ -321,6 +352,7 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
p_o_2D[0] = p_o[0];
p_o_2D[1] = p_o[2];
} else {
mu_max = mu3;
s1_2D[0] = s1[0];
s1_2D[1] = s1[1];
@@ -357,35 +389,23 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
}
// find the smallest distance, and use the corresponding barycentric coordinates
mjtNum dist;
if (!comp1) {
mjtNum lambda_1d[4], verts[6], x[3];
mju_copy3(verts, s2);
mju_copy3(verts + 3, s3);
S1D(lambda_1d, verts);
lincomb(x, lambda_1d, verts, 2);
dist = mju_norm3(x);
lambda[1] = lambda_1d[0];
lambda[2] = lambda_1d[1];
}
mjtNum dist = mjMAXVAL;
if (!comp2) {
mjtNum lambda_1d[4], verts[6], x[3];
mjtNum lambda_1d[2], verts[6], x[3];
mju_copy3(verts, s1);
mju_copy3(verts + 3, s3);
S1D(lambda_1d, verts);
lincomb(x, lambda_1d, verts, 2);
mjtNum d = mju_norm3(x);
if (d < dist) {
lambda[0] = lambda_1d[0];
lambda[2] = lambda_1d[1];
dist = d;
}
lambda[0] = lambda_1d[0];
lambda[1] = 0;
lambda[2] = lambda_1d[1];
dist = d;
}
if (!comp3) {
mjtNum lambda_1d[4], verts[6], x[3];
mjtNum lambda_1d[2], verts[6], x[3];
mju_copy3(verts, s1);
mju_copy3(verts + 3, s2);
S1D(lambda_1d, verts);
@@ -394,6 +414,22 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
if (d < dist) {
lambda[0] = lambda_1d[0];
lambda[1] = lambda_1d[1];
lambda[2] = 0;
dist = d;
}
}
if (!comp1) {
mjtNum lambda_1d[2], verts[6], x[3];
mju_copy3(verts, s2);
mju_copy3(verts + 3, s3);
S1D(lambda_1d, verts);
lincomb(x, lambda_1d, verts, 2);
mjtNum d = mju_norm3(x);
if (d < dist) {
lambda[1] = lambda_1d[0];
lambda[2] = lambda_1d[1];
lambda[0] = 0;
dist = d;
}
}
@@ -435,5 +471,6 @@ static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) {
lambda[1] = C2 / mu_max;
} else {
lambda[0] = 1;
lambda[1] = 0;
}
}
src/engine/engine_collision_gjk.h
+7 -6
View File
@@ -23,15 +23,16 @@
extern "C" {
#endif
// internal struct with settings for GJK
struct _mjGjkConfig {
mjtNum max_iterations;
// internal configuration for convex collision detection
struct _mjCCDConfig {
int max_iterations;
mjtNum tolerance;
};
typedef struct _mjGjkConfig mjGjkConfig;
typedef struct _mjCCDConfig mjCCDConfig;
// Returns the distance between the two objects given an initial guess x0.
MJAPI mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]);
// Returns the distance between the two objects. The witness points are
// recoverable from x_0 in obj1 and obj2.
MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2);
#ifdef __cplusplus
}
test/engine/engine_collision_gjk_test.cc
+77 -11
View File
@@ -29,18 +29,27 @@ namespace mujoco {
namespace {
using ::testing::NotNull;
using ::testing::ElementsAre;
mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum* x_0) {
mjGjkConfig config = {100, 1e-6};
mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
return mj_gjk(&config, &obj1, &obj2, x_0);
constexpr mjtNum kTolerance = 1e-6;
constexpr int kMaxIterations = 1000;
static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
mjtNum x2[3]) {
mjCCDConfig config = {kMaxIterations, kTolerance};
mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
mjc_center(obj1.x0, &obj1);
mjc_center(obj2.x0, &obj2);
mjtNum dist = mj_gjk(&config, &obj1, &obj2);
if (x1 != nullptr) mju_copy3(x1, obj1.x0);
if (x2 != nullptr) mju_copy3(x2, obj2.x0);
return dist;
}
using MjGjkTest = MujocoTest;
TEST_F(MjGjkTest, SphereSphereIntersect) {
mjtNum x_0[3] = {-2, 0, 0};
static constexpr char xml[] = R"(
<mujoco>
<option>
@@ -61,7 +70,7 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, x_0);
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_EQ(dist, 0);
mj_deleteData(data);
@@ -69,7 +78,6 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
}
TEST_F(MjGjkTest, SphereSphere) {
mjtNum x_0[3] = {-2, 0, 0};
static constexpr char xml[] = R"(
<mujoco>
<option>
@@ -96,15 +104,17 @@ TEST_F(MjGjkTest, SphereSphere) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, x_0);
mjtNum x1[3], x2[3];
mjtNum dist = run_gjk(model, data, geom1, geom2, x1, x2);
EXPECT_EQ(dist, 1);
EXPECT_THAT(x1, ElementsAre(-.5, 0, 0));
EXPECT_THAT(x2, ElementsAre(.5, 0, 0));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxBox) {
mjtNum x_0[3] = {-3, .5, 0};
static constexpr char xml[] = R"(
<mujoco>
<option>
@@ -125,12 +135,68 @@ TEST_F(MjGjkTest, BoxBox) {
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, x_0);
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_EQ(dist, 1);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, EllipsoidEllipsoid) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<geom name="geom1" type="ellipsoid" pos="1.5 0 -.5" size=".15 .30 .20"/>
<geom name="geom2" type="ellipsoid" pos="1.5 .5 .5" size=".10 .10 .15"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0.7542, .0001);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, CapsuleCapsule) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag gravity="disable"/>
</option>
<worldbody>
<geom name="geom1" type="capsule" pos="-.3 .2 -.4" size=".15 .30"/>
<geom name="geom2" type="capsule" pos=".3 .2 .4" size=".10 .10"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
EXPECT_NEAR(dist, 0.4765, .0001);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco