diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 381b6a36..a8f8d256 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -15,12 +15,14 @@ #include "engine/engine_collision_gjk.h" #include +#include #include #include -#include "engine/engine_util_blas.h" -#include "engine/engine_util_spatial.h" #include "engine/engine_collision_convex.h" +#include "engine/engine_util_blas.h" +#include "engine/engine_util_errmem.h" +#include "engine/engine_util_spatial.h" // Computes the shortest distance between the origin and an n-simplex (n <= 3) and returns the // barycentric coordinates of the closest point in the simplex. This is the so called distance @@ -38,16 +40,60 @@ 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 s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, - const mjtNum x_k[3]); +static void support(mjtNum s[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3]); + +// support function tweaked for GJK by taking kth iteration point as input and setting both +// support points to recover witness points +static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum x_k[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) { +typedef struct { + int ignored; + int verts[3]; // indices of the three vertices of the face in the polytope + mjtNum v[3]; + mjtNum dist; + mjtNum n[3]; +} Face; + +typedef struct { + mjtNum* verts; + int nverts; + int vcap; + Face* faces; + int nfaces; + int fcap; +} Polytope; + +// generates a polytope from a 1-simplex, 2-simplex, or 3-simplex respectively +// returns true if the polytope can be generated, false otherwise +static int polytope2(Polytope* pt, const mjtNum simplex[6], mjCCDObj* obj1, mjCCDObj* obj2); +static int polytope3(Polytope* pt, const mjtNum simplex[9]); +static int polytope4(Polytope* pt, const mjtNum simplex[12]); + +// initializes the polytope (faces and vertices must be freed by caller) +static void initPolytope(Polytope* pt); + +// copies a vertex into the polytope and return its index +static int newVertex(Polytope* pt, const mjtNum v1[3]); + +// attaches a face to the polytope with the given vertex indices in the polytope +static void attachFace(Polytope* pt, int v1, int v2, int v3); + +// returns the penetration depth (negative distance) of the convex objects +static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); + +// internal data structure for the returning simplex from GJK +typedef struct { + mjtNum verts[12]; + int nverts; +} Simplex; + +// internal GJK with returned data for EPA +static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, Simplex* ret) { 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 @@ -65,7 +111,7 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum lambda[4]; // barycentric coordinates for x_k // compute the kth support point - support(s1, s2, obj1, obj2, x_k); + gjk_support(s1, s2, obj1, obj2, x_k); mju_sub3(s_k, s1, s2); // the stopping criteria relies on the Frank-Wolfe duality gap given by @@ -73,7 +119,7 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum diff[3]; mju_sub3(diff, x_k, s_k); if (2*mju_dot3(x_k, diff) < config->tolerance) { - return mju_norm3(x_k); + break; } // TODO(kylebayes): signedVolume has been written to assume the first vertex is the latest @@ -113,14 +159,56 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { mju_copy3(simplex + 3*n++, simplex + 3*i); } } + if (ret) { + ret->nverts = n; + for (int i = 0; i < n; i++) { + mju_copy3(ret->verts + 3*i, simplex + 3*i); + } + } return mju_norm3(x_k); } +// returns the distance between the two objects. The witness points are +// recoverable from the x_0 field in obj1 and obj2. +mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { + return _gjk(config, obj1, obj2, NULL); +} + + + +// Same as mj_gjk, but returns the penetration depth (negative distance) if the objects intersect. +mjtNum mj_gjkPenetration(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { + Simplex simplex; + mjtNum dist = _gjk(config, obj1, obj2, &simplex); + + if (dist <= config->tolerance && simplex.nverts > 1) { + Polytope pt; + int ret; + if (simplex.nverts == 2) { + ret = polytope2(&pt, simplex.verts, obj1, obj2); + } else if (simplex.nverts == 3) { + ret = polytope3(&pt, simplex.verts); + } else { + ret = polytope4(&pt, simplex.verts); + } + + // simplex not on boundary (objects are penetrating) + if (ret) { + dist = -epa(config, &pt, obj1, obj2); + } + mju_free(pt.faces); + mju_free(pt.verts); + } + return dist; +} + + + // computes the support points in obj1 and obj2 for the kth approximation point -static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, - const mjtNum x_k[3]) { +static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum x_k[3]) { mjtNum dir[3], dir_neg[3]; mju_copy3(dir_neg, x_k); mju_normalize3(dir_neg); // mjc_support assumes a normalized direction @@ -133,6 +221,22 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, +// helper function to compute the support point in the Minkowski difference +static void support(mjtNum s[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum d[3]) { + mjtNum dir[3], dir_neg[3], s1[3], s2[3]; + mju_copy3(dir, d); + mju_normalize3(dir); // mjc_support assumes a normalized direction + 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(s, s1, s2); +} + + + // linear combination of n 3D vectors: // res = coef[0]*v[0] + ... + coef[n-1]*v[3*(n-1)] static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n) { @@ -473,3 +577,354 @@ static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) { lambda[1] = 0; } } + + + +// helper function to test if the origin is in the same side of the plane formed by P0P1P2 as P3. +static int sameSide(const mjtNum p0[3], const mjtNum p1[3], + const mjtNum p2[3], const mjtNum p3[3]) { + mjtNum diff1[3], diff2[3], diff3[3], diff4[3], n[3]; + mju_sub3(diff1, p1, p0); + mju_sub3(diff2, p2, p0); + mju_cross(n, diff1, diff2); + + mju_sub3(diff3, p3, p0); + mjtNum dot1 = mju_dot3(n, diff3); + + mju_scl3(diff4, p0, -1); + mjtNum dot2 = mju_dot3(n, diff4); + if (dot1 > 0 && dot2 > 0) return 1; + if (dot1 < 0 && dot2 < 0) return 1; + return 0; +} + + + +// determines if the origin is contained in the tetrahedron. +static int testTetra(const mjtNum p0[3], const mjtNum p1[3], + const mjtNum p2[3], const mjtNum p3[3]) { + return sameSide(p0, p1, p2, p3) + && sameSide(p1, p2, p3, p0) + && sameSide(p2, p3, p0, p1) + && sameSide(p3, p0, p1, p2); +} + + + +// sets rotation matrix for 120 degrees along axis +static void rotmat(mjtNum R[9], const mjtNum axis[3]) { + mjtNum n = mju_norm3(axis); + mjtNum u1 = axis[0] / n, u2 = axis[1] / n, u3 = axis[2] / n; + const mjtNum sin = 0.86602540378; // sin(120 deg) = sqrt(3)/2 ~ 0.86602540378 + const mjtNum cos = -0.5; // cos(120 deg) = -1/2 + R[0] = cos + u1*u1*(1 - cos); + R[1] = u1*u2*(1 - cos) - u3*sin; + R[2] = u1*u3*(1 - cos) + u2*sin; + R[3] = u2*u1*(1 - cos) + u3*sin; + R[4] = cos + u2*u2*(1 - cos); + R[5] = u2*u3*(1 - cos) - u1*sin; + R[6] = u1*u3*(1 - cos) - u2*sin; + R[7] = u2*u3*(1 - cos) + u1*sin; + R[8] = cos + u3*u3*(1 - cos); +} + + + +// creates a polytope from a 1-simplex (2 points i.e. line segment) +static int polytope2(Polytope* pt, const mjtNum simplex[6], + mjCCDObj* obj1, mjCCDObj* obj2) { + initPolytope(pt); + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + mjtNum diff[3]; + mju_sub3(diff, s2, s1); + + // find component with largest magnitude (so cross product is largest) + mjtNum value = mjMAXVAL; + int index = 0; + for (int i = 0; i < 3; i++) { + if (mju_abs(diff[i]) < value) { + value = mju_abs(diff[i]); + index = i; + } + } + + // cross product with best coordinate axis + mjtNum e[3] = {0, 0, 0}; + e[index] = 1; + mjtNum d1[3], d2[3], d3[3]; + mju_cross(d1, e, diff); + + // rotate around the line segment to get three more points spaced 120 degrees apart + mjtNum R[9]; + rotmat(R, diff); + + mju_mulMatVec(d2, R, d1, 3, 3); + mju_mulMatVec(d3, R, d2, 3, 3); + + + mjtNum v1[3], v2[3], v3[3]; + support(v1, obj1, obj2, d1); + support(v2, obj1, obj2, d2); + support(v3, obj1, obj2, d3); + + // points of a hexahedron (we test to see what half the origin is contained in) + int s1i = newVertex(pt, s1); + int v1i = newVertex(pt, v1); + int v2i = newVertex(pt, v2); + int v3i = newVertex(pt, v3); + int s2i = newVertex(pt, s2); + + if (testTetra(s1, v1, v2, v3)) { + attachFace(pt, s1i, v2i, v1i); + attachFace(pt, s1i, v3i, v1i); + attachFace(pt, s1i, v3i, v2i); + attachFace(pt, v1i, v2i, v3i); + return 1; + } + + if (testTetra(s2, v1, v2, v3)) { + attachFace(pt, s2i, v1i, v2i); + attachFace(pt, s2i, v1i, v3i); + attachFace(pt, s2i, v2i, v3i); + attachFace(pt, v1i, v2i, v3i); + return 1; + } + return 0; +} + + + +// creates a polytope from a 2-simplex (3 points i.e. triangle) +static int polytope3(Polytope* pt, const mjtNum simplex[9]) { + initPolytope(pt); + + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + const mjtNum* s3 = simplex + 6; + + // form hexahedron from triangle and two face normals + + mjtNum diff1[3], diff2[3], n[3], neg_n[3]; + mju_sub3(diff1, s2, s1); + mju_sub3(diff2, s3, s1); + mju_cross(n, diff1, diff2); + mju_scl3(neg_n, n, -1); + + int ni = newVertex(pt, n); + int s1i = newVertex(pt, s1); + int s2i = newVertex(pt, s2); + int s3i = newVertex(pt, s3); + int nni = newVertex(pt, neg_n); + + attachFace(pt, s1i, s2i, ni); + attachFace(pt, s3i, s1i, ni); + attachFace(pt, s2i, s3i, ni); + + attachFace(pt, s1i, s2i, nni); + attachFace(pt, s3i, s1i, nni); + attachFace(pt, s2i, s3i, nni); + + // TODO(kylebayes): check what side of the hexahedron the origin is on + return 1; +} + + + +// creates a polytope from a 3-simplex (4 points i.e. tetrahedron) +static int polytope4(Polytope* pt, const mjtNum simplex[12]) { + initPolytope(pt); + + int v1 = newVertex(pt, simplex); + int v2 = newVertex(pt, simplex + 3); + int v3 = newVertex(pt, simplex + 6); + int v4 = newVertex(pt, simplex + 9); + + attachFace(pt, v1, v2, v3); + attachFace(pt, v1, v2, v4); + attachFace(pt, v1, v4, v3); + attachFace(pt, v4, v2, v3); + + // TODO(kylebayes): check if contains origin + return 1; +} + +#define mjMINCAP 100 // starting capacity for dynamic buffers + +// an edge in the horizon +typedef struct { + int v1; + int v2; + int ignore; // deleted +} Edge; + +// the horizon of the polytope +typedef struct { + Edge* edges; // edges in horizon + int n; + int capacity; +} Horizon; + + + +// initializes the polytope (faces and vertices must be freed by caller) +static void initPolytope(Polytope* pt) { + // vertices + pt->nverts = 0; + pt->vcap = mjMINCAP; + pt->verts = (mjtNum*) mju_malloc(pt->vcap * 3 * sizeof(mjtNum)); + + // faces + pt->nfaces = 0; + pt->fcap = mjMINCAP; + pt->faces = (Face*) mju_malloc(pt->fcap * sizeof(Face)); +} + + + +// copies a vertex into the polytope and return its index +static int newVertex(Polytope* pt, const mjtNum v[3]) { + int capacity = pt->vcap; + int n = pt->nverts++; + if (n == capacity) { + capacity *= 2; + pt->verts = (mjtNum*) realloc(pt->verts, capacity * 3 * sizeof(mjtNum)); + pt->vcap = capacity; + } + + + mju_copy3(pt->verts + 3*n, v); + return n; +} + + + +// attaches a face to the polytope with the given vertex indices in the polytope +static void attachFace(Polytope* pt, int v1, int v2, int v3) { + int capacity = pt->fcap; + if (pt->nfaces == capacity) { + capacity *= 2; + pt->faces = (Face*) realloc(pt->faces, capacity * sizeof(Face)); + pt->fcap = capacity; + } + + Face* face = &pt->faces[pt->nfaces]; + face->ignored = 0; + face->verts[0] = v1; + face->verts[1] = v2; + face->verts[2] = v3; + + // compute normal n + mjtNum* pv1 = pt->verts + (v1 * 3); + mjtNum* pv2 = pt->verts + (v2 * 3); + mjtNum* pv3 = pt->verts + (v3 * 3); + mjtNum diff1[3], diff2[3]; + mju_sub3(diff1, pv2, pv1); + mju_sub3(diff2, pv3, pv1); + mju_cross(face->n, diff1, diff2); + mju_normalize3(face->n); + + // compute witness point v + mju_scl3(face->v, face->n, mju_dot3(face->n, pv1)); + face->dist = mju_norm3(face->v); + + // orientation check + if (mju_dot3(face->n, pv1) < 0) mju_scl3(face->n, face->n, -1); + pt->nfaces++; +} + + + +// initializes the horizon (edges must be freed by caller) +static void initHorizon(Horizon* h) { + h->n = 0; + h->capacity = mjMINCAP; + h->edges = (Edge*) mju_malloc(h->capacity * sizeof(Edge)); +} + + + +// adds an edge to the horizon, if the edge is already in the horizon, it is +// deleted (marked ignored) +static void addEdgeIfUnique(Horizon* h, int v1, int v2) { + int capacity = h->capacity; + int n = h->n; + + for (int i = 0; i < n; i++) { + if (h->edges[i].ignore) continue; + int old_v1 = h->edges[i].v1; + int old_v2 = h->edges[i].v2; + if ((old_v1 == v1 && old_v2 == v2) || (old_v1 == v2 && old_v2 == v1)) { + h->edges[i].ignore = 1; + return; + } + } + if (n == capacity) { + capacity *= 2; + h->edges = (Edge*) realloc(h->edges, capacity * sizeof(Edge)); + h->capacity = capacity; + } + h->edges[n].v1 = v1; + h->edges[n].v2 = v2; + h->edges[n].ignore = 0; + h->n++; +} + +#undef mjMINCAP + + +// returns the penetration depth (negative distance) of the convex objects +static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { + mjtNum dist = mjMAXVAL; + Horizon h; + initHorizon(&h); + int N = config->max_iterations; + mjtNum tolerance = config->tolerance; + + for (int j = 0; j < N; j++) { + dist = mjMAXVAL; + int index = -1; + + // find the closest face to the origin + for (int i = 0; i < pt->nfaces; i++) { + if (pt->faces[i].ignored) continue; + if (pt->faces[i].dist < dist) { + dist = pt->faces[i].dist; + index = i; + } + } + + // compute support point w from the closest face's normal + mjtNum w[3]; + support(w, obj1, obj2, pt->faces[index].v); + mjtNum next_dist = mju_dot3(pt->faces[index].v, w) / dist; + if (next_dist - dist < tolerance) { + break; + } + + // compute horizon for w + for (int i = 0; i < pt->nfaces; i++) { + Face* face = &pt->faces[i]; + if (face->ignored) continue; + mjtNum dist2 = face->dist; + mjtNum* v = face->v; // use witness point as normal + if (mju_dot3(v, w) >= dist2*dist2) { + face->ignored = 1; + addEdgeIfUnique(&h, face->verts[0], face->verts[1]); + addEdgeIfUnique(&h, face->verts[1], face->verts[2]); + addEdgeIfUnique(&h, face->verts[2], face->verts[0]); + } + } + + // insert w as new vertex and attach faces along the horizon + int wi = newVertex(pt, w); + for (int i = 0; i < h.n; i++) { + if (h.edges[i].ignore) continue; + attachFace(pt, wi, h.edges[i].v1, h.edges[i].v2); + } + + h.n = 0; // clear horizon + } + mju_free(h.edges); + return dist; +} diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index ead02eba..3cd8998e 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -34,6 +34,9 @@ typedef struct _mjCCDConfig mjCCDConfig; // recoverable from x_0 in obj1 and obj2. MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2); +// Same as mj_gjk, but returns the penetration depth (negative distance) if the objects intersect. +MJAPI mjtNum mj_gjkPenetration(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2); + #ifdef __cplusplus } #endif diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 22cc7297..ffc8161c 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -41,7 +41,7 @@ static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], 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); + mjtNum dist = mj_gjkPenetration(&config, &obj1, &obj2); if (x1 != nullptr) mju_copy3(x1, obj1.x0); if (x2 != nullptr) mju_copy3(x2, obj2.x0); return dist; @@ -49,40 +49,9 @@ static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], using MjGjkTest = MujocoTest; -TEST_F(MjGjkTest, SphereSphereIntersect) { - static constexpr char xml[] = R"( - - - - - - - )"; - - std::array 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_EQ(dist, 0); - mj_deleteData(data); - mj_deleteModel(model); -} - TEST_F(MjGjkTest, SphereSphere) { static constexpr char xml[] = R"( - @@ -117,9 +86,6 @@ TEST_F(MjGjkTest, SphereSphere) { TEST_F(MjGjkTest, BoxBox) { static constexpr char xml[] = R"( - @@ -142,12 +108,34 @@ TEST_F(MjGjkTest, BoxBox) { mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBoxIntersect) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array 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, -1, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, EllipsoidEllipsoid) { static constexpr char xml[] = R"( - @@ -173,9 +161,6 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) { TEST_F(MjGjkTest, CapsuleCapsule) { static constexpr char xml[] = R"( -