// Copyright 2024 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. #include "engine/engine_collision_gjk.h" #include #include #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" // subdistance algorithm for GJK that computes the barycentric coordinates of the point in a // simplex closest to the origin // implementation adapted from Montanari et al, ToG 2017 static void subdistance(mjtNum lambda[4], const mjtNum simplex[12], int n); // these internal functions compute the barycentric coordinates of the closest point // to the origin in the n-simplex, where n = 3, 2, 1 respectively static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3], const mjtNum s4[3]); static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]); static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]); // helper function to compute the support point for EPA static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3], mjtNum dnorm); // support function tweaked for GJK by taking kth iteration point as input and setting both // support points to recover witness points static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum x_k[3]); // linear combination of n 3D vectors static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n); // one face in a polytope typedef struct { int verts[3]; // indices of the three vertices of the face in the polytope int adj[3]; // adjacent faces (one for each edge: [v1,v2], [v2,v3], [v3,v1]) mjtNum v[3]; // the projection of the origin on the face (can be used as face normal) mjtNum dist; // norm of v; negative if deleted int index; // index in map } Face; // polytope used in the Expanding Polytope Algorithm (EPA) typedef struct { mjtNum* verts1; // vertices of polytope in obj1 mjtNum* verts2; // vertices of polytope in obj2 mjtNum* verts; // v1 - v2; vertices in Minkowski sum making up polytope int nverts; // number of vertices Face* faces; // list of faces that make up the polytope int nfaces; // number of faces int maxfaces; // max number of faces that can be stored in polytope Face** map; // linear map storing faces int nmap; // number of faces in map } Polytope; // copies a vertex into the polytope and returns its index static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]); // attaches a face to the polytope with the given vertex indices; returns non-zero on error static void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3); // returns 1 if objects are in contact; 0 if not; -1 if inconclusive // status must have initial tetrahedrons static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); // returns the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); // -------------------------------- inlined 3D vector utils -------------------------------------- // v1 == v2 static inline int equal3(const mjtNum v1[3], const mjtNum v2[3]) { return mju_abs(v1[0] - v2[0]) < mjMINVAL && mju_abs(v1[1] - v2[1]) < mjMINVAL && mju_abs(v1[2] - v2[2]) < mjMINVAL; } // res = v1 - v2 static inline void sub3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { res[0] = v1[0] - v2[0], res[1] = v1[1] - v2[1], res[2] = v1[2] - v2[2]; } // dot product static inline mjtNum dot3(const mjtNum v1[3], const mjtNum v2[3]) { return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; } // res = v static inline void copy3(mjtNum res[3], const mjtNum v[3]) { res[0] = v[0], res[1] = v[1], res[2] = v[2]; } // scalar product: res = s*v static inline void scl3(mjtNum res[3], const mjtNum v[3], mjtNum s) { res[0] = s*v[0], res[1] = s*v[1], res[2] = s*v[2]; } // cross product: res = v1 x v2 static inline void cross3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { res[0] = v1[1]*v2[2] - v1[2]*v2[1]; res[1] = v1[2]*v2[0] - v1[0]*v2[2]; res[2] = v1[0]*v2[1] - v1[1]*v2[0]; } // returns determinant of the 3x3 matrix with columns v1, v2, v3 static inline mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { // v1 * (v2 x v3) return v1[0]*(v2[1]*v3[2] - v2[2]*v3[1]) + v1[1]*(v2[2]*v3[0] - v2[0]*v3[2]) + v1[2]*(v2[0]*v3[1] - v2[1]*v3[0]); } // ---------------------------------------- GJK --------------------------------------------------- // returns true if both geoms are discrete shapes (i.e. meshes or boxes with no margin) static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // non-zero margin makes geoms smooth if (obj1->margin != 0 || obj2->margin != 0) return 0; int g1 = obj1->geom_type; int g2 = obj2->geom_type; return (g1 == mjGEOM_MESH || g1 == mjGEOM_BOX || g1 == mjGEOM_HFIELD) && (g2 == mjGEOM_MESH || g2 == mjGEOM_BOX || g2 == mjGEOM_HFIELD); } // GJK algorithm static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact int backup_gjk = !get_dist; // use gjkIntersect if no geom distances needed mjtNum *simplex1 = status->simplex1; // simplex for obj1 mjtNum *simplex2 = status->simplex2; // simplex for obj2 mjtNum *simplex = status->simplex; // simplex in Minkowski difference int n = 0; // number of vertices in the simplex int k = 0; // current iteration int kmax = status->max_iterations; // max number of iterations mjtNum* x1_k = status->x1; // the kth approximation point for obj1 mjtNum* x2_k = status->x2; // the kth approximation point for obj2 mjtNum x_k[3]; // the kth approximation point in Minkowski difference mjtNum lambda[4]; // barycentric coordinates for x_k mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff; // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0 mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : status->tolerance * status->tolerance; // set initial guess sub3(x_k, x1_k, x2_k); for (; k < kmax; k++) { mjtNum *s1_k = simplex1 + 3*n; // the kth support point in obj1 mjtNum *s2_k = simplex2 + 3*n; // the kth support point in obj2 mjtNum *s_k = simplex + 3*n; // the kth support point of Minkowski difference // compute the kth support point gjkSupport(s1_k, s2_k, obj1, obj2, x_k); sub3(s_k, s1_k, s2_k); // stopping criteria using the Frank-Wolfe duality gap given by // |f(x_k) - f(x_min)|^2 <= < grad f(x_k), (x_k - s_k) > mjtNum diff[3]; sub3(diff, x_k, s_k); if (2*dot3(x_k, diff) < epsilon) { break; } // if the hyperplane separates the Minkowski difference and origin, the objects don't collide // if geom distance isn't requested, return early if (!get_dist) { if (dot3(x_k, s_k) > 0) { status->gjk_iterations = k; status->nsimplex = 0; status->nx = 0; return mjMAXVAL; } } else if (status->dist_cutoff < mjMAXVAL) { mjtNum vs = mju_dot3(x_k, s_k), vv = mju_dot3(x_k, x_k); if (mju_dot3(x_k, s_k) > 0 && (vs*vs / vv) >= cutoff2) { status->gjk_iterations = k; status->nsimplex = 0; status->nx = 0; return mjMAXVAL; } } // tetrahedron is generated and only need contact info; fallback to gjkIntersect to // determine contact if (n == 3 && backup_gjk) { status->gjk_iterations = k; int ret = gjkIntersect(status, obj1, obj2); if (ret != -1) { status->nx = 0; return ret > 0 ? 0 : mjMAXVAL; } k = status->gjk_iterations; backup_gjk = 0; } // run the distance subalgorithm to compute the barycentric coordinates // of the closest point to the origin in the simplex subdistance(lambda, simplex, n + 1); // remove vertices from the simplex no longer needed n = 0; for (int i = 0; i < 4; i++) { if (lambda[i] == 0) continue; copy3(simplex1 + 3*n, simplex1 + 3*i); copy3(simplex2 + 3*n, simplex2 + 3*i); copy3(simplex + 3*n, simplex + 3*i); lambda[n++] = lambda[i]; } // get the next iteration of x_k mjtNum x_next[3]; lincomb(x_next, lambda, simplex, n); // x_k has converged to minimum if (equal3(x_next, x_k)) { break; } // copy next iteration into x_k copy3(x_k, x_next); // we have a tetrahedron containing the origin so return early if (n == 4) { break; } } // compute the approximate witness points lincomb(x1_k, lambda, simplex1, n); lincomb(x2_k, lambda, simplex2, n); status->nx = 1; status->gjk_iterations = k; status->nsimplex = n; status->dist = mju_norm3(x_k); return status->dist; } // computes the support point in obj1 and obj2 for Minkowski difference static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3], const mjtNum dir_neg[3]) { // obj1 obj1->support(s1, obj1, dir); if (obj1->margin > 0 && obj1->geom >= 0) { mjtNum margin = 0.5 * obj1->margin; s1[0] += dir[0] * margin; s1[1] += dir[1] * margin; s1[2] += dir[2] * margin; } // obj2 obj2->support(s2, obj2, dir_neg); if (obj2->margin > 0 && obj2->geom >= 0) { mjtNum margin = 0.5 * obj2->margin; s2[0] += dir_neg[0] * margin; s2[1] += dir_neg[1] * margin; s2[2] += dir_neg[2] * margin; } } // computes the support points in obj1 and obj2 for the kth approximation point static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum x_k[3]) { mjtNum dir[3], dir_neg[3]; copy3(dir_neg, x_k); mju_normalize3(dir_neg); // mjc_support assumes a normalized direction scl3(dir, dir_neg, -1); // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) support(s1, s2, obj1, obj2, dir, dir_neg); } // helper function to compute the support point in the Minkowski difference static void epaSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3], mjtNum dnorm) { mjtNum dir[3], dir_neg[3]; // mjc_support assumes a normalized direction if (dnorm < mjMINVAL) { dir[0] = 1, dir_neg[0] = -1; dir[1] = 0, dir_neg[1] = 0; dir[2] = 0, dir_neg[2] = 0; } else { dir[0] = d[0] / dnorm; dir[1] = d[1] / dnorm; dir[2] = d[2] / dnorm; dir_neg[0] = -dir[0]; dir_neg[1] = -dir[1]; dir_neg[2] = -dir[2]; } // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) support(s1, s2, obj1, obj2, dir, dir_neg); } // helper function to compute the support point in the Minkowski difference (without normalization) static void gjkIntersectSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3]) { mjtNum dir_neg[3] = {-dir[0], -dir[1], -dir[2]}; // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) support(s1, s2, obj1, obj2, dir, dir_neg); } // compute the signed distance of a face along with the normal static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { mjtNum diff1[3], diff2[3]; sub3(diff1, v3, v1); sub3(diff2, v2, v1); cross3(normal, diff1, diff2); mjtNum norm = mju_norm3(normal); if (norm > mjMINVAL && norm < mjMAXVAL) { mjtNum invnorm = 1/norm; normal[0] *= invnorm; normal[1] *= invnorm; normal[2] *= invnorm; return dot3(normal, v1); } return mjMAXVAL; // cannot recover normal (ignore face) } // returns 1 if objects are in contact; 0 if not; -1 if inconclusive static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum simplex1[12], simplex2[12], simplex[12]; memcpy(simplex1, status->simplex1, sizeof(mjtNum) * 12); memcpy(simplex2, status->simplex2, sizeof(mjtNum) * 12); memcpy(simplex, status->simplex, sizeof(mjtNum) * 12); int s[4] = {0, 3, 6, 9}; int k = status->gjk_iterations, kmax = status->max_iterations; for (; k < kmax; k++) { // compute the signed distance to each face in the simplex along with normals mjtNum dist[4], normals[12]; dist[0] = signedDistance(&normals[0], simplex + s[2], simplex + s[1], simplex + s[3]); dist[1] = signedDistance(&normals[3], simplex + s[0], simplex + s[2], simplex + s[3]); dist[2] = signedDistance(&normals[6], simplex + s[1], simplex + s[0], simplex + s[3]); dist[3] = signedDistance(&normals[9], simplex + s[0], simplex + s[1], simplex + s[2]); // if origin is on any affine hull, convergence will fail if (!dist[3] || !dist[2] || !dist[1] || !dist[0]) { status->gjk_iterations = k; return -1; } // find the face with the smallest distance to the origin int i = (dist[0] < dist[1]) ? 0 : 1; int j = (dist[2] < dist[3]) ? 2 : 3; int index = (dist[i] < dist[j]) ? i : j; // origin inside of simplex (run EPA for contact information) if (dist[index] > 0) { status->nsimplex = 4; for (int n = 0; n < 4; n++) { copy3(status->simplex + 3*n, simplex + s[n]); copy3(status->simplex1 + 3*n, simplex1 + s[n]); copy3(status->simplex2 + 3*n, simplex2 + s[n]); } status->gjk_iterations = k; return 1; } // replace worst vertex (farthest from origin) with new candidate gjkIntersectSupport(simplex1 + s[index], simplex2 + s[index], obj1, obj2, normals + 3*index); sub3(simplex + s[index], simplex1 + s[index], simplex2 + s[index]); // found origin outside the Minkowski difference (return no collision) if (dot3(&normals[3*index], simplex + s[index]) < 0) { status->gjk_iterations = k; return 0; } // swap vertices in the simplex to retain orientation i = (index + 1) & 3; j = (index + 2) & 3; int swap = s[i]; s[i] = s[j]; s[j] = swap; } status->gjk_iterations = k; return -1; // never found origin } // linear combination of n 3D vectors static inline void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n) { res[0] = res[1] = res[2] = 0; for (int i = 0; i < n; i++) { res[0] += coef[i] * v[3*i + 0]; res[1] += coef[i] * v[3*i + 1]; res[2] += coef[i] * v[3*i + 2]; } } // linear combination of 2 3D vectors static inline void lincomb2(mjtNum res[3], const mjtNum coef[2], const mjtNum v1[3], const mjtNum v2[3]) { res[0] = coef[0]*v1[0] + coef[1]*v2[0]; res[1] = coef[0]*v1[1] + coef[1]*v2[1]; res[2] = coef[0]*v1[2] + coef[1]*v2[2]; } // linear combination of 3 3D vectors static inline void lincomb3(mjtNum res[3], const mjtNum coef[3], const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { res[0] = coef[0]*v1[0] + coef[1]*v2[0] + coef[2]*v3[0]; res[1] = coef[0]*v1[1] + coef[1]*v2[1] + coef[2]*v3[1]; res[2] = coef[0]*v1[2] + coef[1]*v2[2] + coef[2]*v3[2]; } // res = origin projected onto plane defined by v1, v2, v3 static inline void projectOriginPlane(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { mjtNum diff21[3], diff31[3], diff32[3], n[3], nv, nn; sub3(diff21, v2, v1); sub3(diff31, v3, v1); sub3(diff32, v3, v2); // n = (v1 - v2) x (v3 - v2) cross3(n, diff32, diff21); nv = dot3(n, v2); nn = dot3(n, n); if (nv != 0 && nn > mjMINVAL) { scl3(res, n, nv / nn); return; } // n = (v2 - v1) x (v3 - v1) cross3(n, diff21, diff31); nv = dot3(n, v1); nn = dot3(n, n); if (nv != 0 && nn > mjMINVAL) { scl3(res, n, nv / nn); return; } // n = (v1 - v3) x (v2 - v3) cross3(n, diff31, diff32); nv = dot3(n, v3); nn = dot3(n, n); scl3(res, n, nv / nn); } // res = origin projected onto line defined by v1, v2 static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { // res = v2 - / * (v2 - v1) mjtNum diff[3]; sub3(diff, v2, v1); mjtNum scl = -(dot3(v2, diff) / dot3(diff, diff)); res[0] = v2[0] + scl*diff[0]; res[1] = v2[1] + scl*diff[1]; res[2] = v2[2] + scl*diff[2]; } // returns true only when a and b are both strictly positive or both strictly negative static inline int sameSign(mjtNum a, mjtNum b) { if (a > 0 && b > 0) return 1; if (a < 0 && b < 0) return 1; return 0; } // subdistance algorithm for GJK that computes the barycentric coordinates of the point in a // simplex closest to the origin // implementation adapted from Montanari et al, ToG 2017 static inline void subdistance(mjtNum lambda[4], const mjtNum simplex[12], int n) { lambda[0] = lambda[1] = lambda[2] = lambda[3] = 0; const mjtNum* s1 = simplex; const mjtNum* s2 = simplex + 3; const mjtNum* s3 = simplex + 6; const mjtNum* s4 = simplex + 9; if (n == 4) { S3D(lambda, s1, s2, s3, s4); } else if (n == 3) { S2D(lambda, s1, s2, s3); } else if (n == 2) { S1D(lambda, s1, s2); } else { lambda[0] = 1; } } static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3], const mjtNum s4[3]) { // the matrix M is 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 ]] // we want to solve M*lambda = P, where P = [p_x, p_y, p_z, 1] with [p_x, p_y, p_z] is the // origin projected onto the simplex // compute cofactors to find det(M) mjtNum C41 = -det3(s2, s3, s4); mjtNum C42 = det3(s1, s3, s4); mjtNum C43 = -det3(s1, s2, s4); mjtNum C44 = det3(s1, s2, s3); // note that m_det = 6*SignVol(simplex) with C4i corresponding to the volume of the 3-simplex // with vertices {s1, s2, s3, 0} - si mjtNum m_det = C41 + C42 + C43 + C44; int comp1 = sameSign(m_det, C41), comp2 = sameSign(m_det, C42), comp3 = sameSign(m_det, C43), comp4 = sameSign(m_det, C44); // if all signs are the same then the origin is inside the simplex if (comp1 && comp2 && comp3 && comp4) { lambda[0] = C41 / m_det; lambda[1] = C42 / m_det; lambda[2] = C43 / m_det; lambda[3] = C44 / m_det; return; } // find the smallest distance, and use the corresponding barycentric coordinates mjtNum dmin = mjMAXVAL; if (!comp1) { mjtNum lambda_2d[3], x[3]; S2D(lambda_2d, s2, s3, s4); lincomb3(x, lambda_2d, s2, s3, s4); mjtNum d = dot3(x, x); lambda[0] = 0; lambda[1] = lambda_2d[0]; lambda[2] = lambda_2d[1]; lambda[3] = lambda_2d[2]; dmin = d; } if (!comp2) { mjtNum lambda_2d[3], x[3]; S2D(lambda_2d, s1, s3, s4); lincomb3(x, lambda_2d, s1, s3, s4); mjtNum d = dot3(x, x); if (d < dmin) { lambda[0] = lambda_2d[0]; lambda[1] = 0; lambda[2] = lambda_2d[1]; lambda[3] = lambda_2d[2]; dmin = d; } } if (!comp3) { mjtNum lambda_2d[3], x[3]; S2D(lambda_2d, s1, s2, s4); lincomb3(x, lambda_2d, s1, s2, s4); mjtNum d = dot3(x, x); if (d < dmin) { lambda[0] = lambda_2d[0]; lambda[1] = lambda_2d[1]; lambda[2] = 0; lambda[3] = lambda_2d[2]; dmin = d; } } if (!comp4) { mjtNum lambda_2d[3], x[3]; S2D(lambda_2d, s1, s2, s3); lincomb3(x, lambda_2d, s1, s2, s3); mjtNum d = dot3(x, x); if (d < dmin) { lambda[0] = lambda_2d[0]; lambda[1] = lambda_2d[1]; lambda[2] = lambda_2d[2]; lambda[3] = 0; dmin = d; } } } static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum s3[3]) { // project origin onto affine hull of the simplex mjtNum p_o[3]; projectOriginPlane(p_o, s1, s2, s3); // 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 the axis with the largest projection of the simplex using the computed minors mjtNum M_max = 0; mjtNum s1_2D[2], s2_2D[2], s3_2D[2], p_o_2D[2]; mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34); if (mu1 >= mu2 && mu1 >= mu3) { M_max = M_14; s1_2D[0] = s1[1]; s1_2D[1] = s1[2]; s2_2D[0] = s2[1]; s2_2D[1] = s2[2]; s3_2D[0] = s3[1]; s3_2D[1] = s3[2]; p_o_2D[0] = p_o[1]; p_o_2D[1] = p_o[2]; } else if (mu2 >= mu3) { M_max = M_24; s1_2D[0] = s1[0]; s1_2D[1] = s1[2]; s2_2D[0] = s2[0]; s2_2D[1] = s2[2]; s3_2D[0] = s3[0]; s3_2D[1] = s3[2]; p_o_2D[0] = p_o[0]; p_o_2D[1] = p_o[2]; } else { M_max = M_34; s1_2D[0] = s1[0]; s1_2D[1] = s1[1]; s2_2D[0] = s2[0]; s2_2D[1] = s2[1]; s3_2D[0] = s3[0]; s3_2D[1] = s3[1]; p_o_2D[0] = p_o[0]; p_o_2D[1] = p_o[1]; } // compute the cofactors C3i of the following matrix: // [[ s1_2D[0] - p_o_2D[0], s2_2D[0] - p_o_2D[0], s3_2D[0] - p_o_2D[0] ], // [ s1_2D[1] - p_o_2D[1], s2_2D[1] - p_o_2D[1], s3_2D[1] - p_o_2D[1] ], // [ 1, 1, 1 ]] // C31 corresponds to the signed area of 2-simplex: (p_o_2D, s2_2D, s3_2D) mjtNum C31 = p_o_2D[0]*s2_2D[1] + p_o_2D[1]*s3_2D[0] + s2_2D[0]*s3_2D[1] - p_o_2D[0]*s3_2D[1] - p_o_2D[1]*s2_2D[0] - s3_2D[0]*s2_2D[1]; // C32 corresponds to the signed area of 2-simplex: (_po_2D, s1_2D, s3_2D) mjtNum C32 = p_o_2D[0]*s3_2D[1] + p_o_2D[1]*s1_2D[0] + s3_2D[0]*s1_2D[1] - p_o_2D[0]*s1_2D[1] - p_o_2D[1]*s3_2D[0] - s1_2D[0]*s3_2D[1]; // C33 corresponds to the signed area of 2-simplex: (p_o_2D, s1_2D, s2_2D) mjtNum C33 = p_o_2D[0]*s1_2D[1] + p_o_2D[1]*s2_2D[0] + s1_2D[0]*s2_2D[1] - p_o_2D[0]*s2_2D[1] - p_o_2D[1]*s1_2D[0] - s2_2D[0]*s1_2D[1]; int comp1 = sameSign(M_max, C31), comp2 = sameSign(M_max, C32), comp3 = sameSign(M_max, C33); // all the same sign, p_o is inside the 2-simplex if (comp1 && comp2 && comp3) { lambda[0] = C31 / M_max; lambda[1] = C32 / M_max; lambda[2] = C33 / M_max; return; } // find the smallest distance, and use the corresponding barycentric coordinates mjtNum dmin = mjMAXVAL; if (!comp1) { mjtNum lambda_1d[2], x[3]; S1D(lambda_1d, s2, s3); lincomb2(x, lambda_1d, s2, s3); mjtNum d = dot3(x, x); lambda[0] = 0; lambda[1] = lambda_1d[0]; lambda[2] = lambda_1d[1]; dmin = d; } if (!comp2) { mjtNum lambda_1d[2], x[3]; S1D(lambda_1d, s1, s3); lincomb2(x, lambda_1d, s1, s3); mjtNum d = dot3(x, x); if (d < dmin) { lambda[0] = lambda_1d[0]; lambda[1] = 0; lambda[2] = lambda_1d[1]; dmin = d; } } if (!comp3) { mjtNum lambda_1d[2], x[3]; S1D(lambda_1d, s1, s2); lincomb2(x, lambda_1d, s1, s2); mjtNum d = dot3(x, x); if (d < dmin) { lambda[0] = lambda_1d[0]; lambda[1] = lambda_1d[1]; lambda[2] = 0; dmin = d; } } } static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) { // find projection of origin onto the 1-simplex: mjtNum p_o[3]; projectOriginLine(p_o, s1, s2); // find the axis with the largest projection "shadow" of the simplex mjtNum mu_max = 0; int index; for (int i = 0; i < 3; i++) { mjtNum mu = s1[i] - s2[i]; if (mju_abs(mu) >= mju_abs(mu_max)) { mu_max = mu; index = i; } } mjtNum C1 = p_o[index] - s2[index]; mjtNum C2 = s1[index] - p_o[index]; // inside the simplex if (sameSign(mu_max, C1) && sameSign(mu_max, C2)) { lambda[0] = C1 / mu_max; lambda[1] = C2 / mu_max; } else { lambda[0] = 0; lambda[1] = 1; } } // ---------------------------------------- EPA --------------------------------------------------- // returns 1 if the origin and p3 are on the same side of the plane defined by p0, p1, p2 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]; sub3(diff1, p1, p0); sub3(diff2, p2, p0); cross3(n, diff1, diff2); sub3(diff3, p3, p0); mjtNum dot1 = dot3(n, diff3); scl3(diff4, p0, -1); mjtNum dot2 = dot3(n, diff4); if (dot1 > 0 && dot2 > 0) return 1; if (dot1 < 0 && dot2 < 0) return 1; return 0; } // returns 1 if the origin is contained in the tetrahedron, 0 otherwise 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); } // matrix for 120 degrees rotation around given 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) const mjtNum cos = -0.5; // cos(120 deg) 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 (returns 0 if polytope can be created) static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum v1[3], v2[3]; sub3(v1, status->simplex1 + 0, status->simplex2 + 0); sub3(v2, status->simplex1 + 3, status->simplex2 + 3); mjtNum diff[3]; sub3(diff, v2, v1); // find component with smallest 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]; cross3(d1, e, diff); // rotate around the line segment to get three more points spaced 120 degrees apart mjtNum R[9]; rotmat(R, diff); mju_mulMatVec3(d2, R, d1); mju_mulMatVec3(d3, R, d2); mjtNum v3a[3], v3b[3], v3[3]; epaSupport(v3a, v3b, obj1, obj2, d1, mju_norm3(d1)); sub3(v3, v3a, v3b); mjtNum v4a[3], v4b[3], v4[3]; epaSupport(v4a, v4b, obj1, obj2, d2, mju_norm3(d2)); sub3(v4, v4a, v4b); mjtNum v5a[3], v5b[3], v5[3]; epaSupport(v5a, v5b, obj1, obj2, d3, mju_norm3(d3)); sub3(v5, v5a, v5b); // check that all six faces are valid triangles (not collinear) if (mju_abs(det3(v1, v3, v4)) < mjMINVAL || mju_abs(det3(v1, v3, v5)) < mjMINVAL || mju_abs(det3(v1, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v3, v4)) < mjMINVAL || mju_abs(det3(v2, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v4, v5)) < mjMINVAL) { return 2; } // save vertices and get indices for each one int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); int v3i = newVertex(pt, v3a, v3b); int v4i = newVertex(pt, v4a, v4b); int v5i = newVertex(pt, v5a, v5b); // build hexahedron attachFace(pt, v1i, v3i, v4i, 1, 3, 2); attachFace(pt, v1i, v5i, v3i, 2, 4, 0); attachFace(pt, v1i, v4i, v5i, 0, 5, 1); attachFace(pt, v2i, v4i, v3i, 5, 0, 4); attachFace(pt, v2i, v3i, v5i, 3, 1, 5); attachFace(pt, v2i, v5i, v4i, 4, 2, 3); // if the origin is on the affine hull of any of the faces then the origin is not in the // hexahedron or the hexahedron is degenerate for (int i = 0; i < 6; i++) { if (pt->faces[i].dist < mjMINVAL) { return 3; } } // valid hexahedron for EPA return 0; } // computes the affine coordinates of p on the triangle v1v2v3 static void triAffineCoord(mjtNum lambda[3], const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum p[3]) { // compute minors as in S2D mjtNum M_14 = v2[1]*v3[2] - v2[2]*v3[1] - v1[1]*v3[2] + v1[2]*v3[1] + v1[1]*v2[2] - v1[2]*v2[1]; mjtNum M_24 = v2[0]*v3[2] - v2[2]*v3[0] - v1[0]*v3[2] + v1[2]*v3[0] + v1[0]*v2[2] - v1[2]*v2[0]; mjtNum M_34 = v2[0]*v3[1] - v2[1]*v3[0] - v1[0]*v3[1] + v1[1]*v3[0] + v1[0]*v2[1] - v1[1]*v2[0]; // exclude one of the axes with the largest projection of the simplex using the computed minors mjtNum M_max = 0; int x, y; mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34); if (mu1 >= mu2 && mu1 >= mu3) { M_max = M_14; x = 1; y = 2; } else if (mu2 >= mu3) { M_max = M_24; x = 0; y = 2; } else { M_max = M_34; x = 0; y = 1; } // C31 corresponds to the signed area of 2-simplex: (v, s2, s3) mjtNum C31 = p[x]*v2[y] + p[y]*v3[x] + v2[x]*v3[y] - p[x]*v3[y] - p[y]*v2[x] - v3[x]*v2[y]; // C32 corresponds to the signed area of 2-simplex: (v, s1, s3) mjtNum C32 = p[x]*v3[y] + p[y]*v1[x] + v3[x]*v1[y] - p[x]*v1[y] - p[y]*v3[x] - v1[x]*v3[y]; // C33 corresponds to the signed area of 2-simplex: (v, s1, s2) mjtNum C33 = p[x]*v1[y] + p[y]*v2[x] + v1[x]*v2[y] - p[x]*v2[y] - p[y]*v1[x] - v2[x]*v1[y]; // compute affine coordinates lambda[0] = C31 / M_max; lambda[1] = C32 / M_max; lambda[2] = C33 / M_max; } // returns true if point p and triangle v1v2v3 intersect static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], const mjtNum p[3]) { mjtNum lambda[3]; triAffineCoord(lambda, v1, v2, v3, p); if (lambda[0] < 0 || lambda[1] < 0 || lambda[2] < 0) { return 0; } mjtNum pr[3], diff[3]; pr[0] = v1[0]*lambda[0] + v2[0]*lambda[1] + v3[0]*lambda[2]; pr[1] = v1[1]*lambda[0] + v2[1]*lambda[1] + v3[1]*lambda[2]; pr[2] = v1[2]*lambda[0] + v2[2]*lambda[1] + v3[2]*lambda[2]; sub3(diff, pr, p); return mju_norm3(diff) < mjMINVAL; } // creates a polytope from a 2-simplex (returns 0 if polytope can be created) static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // get vertices of simplex from GJK const mjtNum *v1 = status->simplex, *v2 = status->simplex + 3, *v3 = status->simplex + 6; // get normals in both directions mjtNum diff1[3], diff2[3], n[3], n_neg[3]; sub3(diff1, v2, v1); sub3(diff2, v3, v1); cross3(n, diff1, diff2); mjtNum n_norm = mju_norm3(n); if (n_norm < mjMINVAL) { return 4; } // negative of triangle normal n scl3(n_neg, n, -1); // get 4th vertex in n direction mjtNum v4a[3], v4b[3], v4[3]; epaSupport(v4a, v4b, obj1, obj2, n, n_norm); sub3(v4, v4a, v4b); // check that v4 is not contained in the 2-simplex if (triPointIntersect(v1, v2, v3, v4)) { return 5; } // get 5th vertex in -n direction mjtNum v5a[3], v5b[3], v5[3]; epaSupport(v5a, v5b, obj1, obj2, n_neg, n_norm); sub3(v5, v5a, v5b); // check that v5 is not contained in the 2-simplex if (triPointIntersect(v1, v2, v3, v5)) { return 6; } // if origin does not lie on simplex then we need to check that the hexahedron contains the // origin // // TODO(kylebayes): It's possible for GJK to return a 2-simplex with the origin not contained in // it but within tolerance from it. In that case the hexahedron could possibly be constructed // that doesn't contain the origin, but nonetheless there is penetration depth. if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { return 7; } // save vertices and get indices for each one int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); int v3i = newVertex(pt, status->simplex1 + 6, status->simplex2 + 6); int v5i = newVertex(pt, v5a, v5b); int v4i = newVertex(pt, v4a, v4b); // create hexahedron for EPA attachFace(pt, v4i, v1i, v2i, 1, 3, 2); attachFace(pt, v4i, v3i, v1i, 2, 4, 0); attachFace(pt, v4i, v2i, v3i, 0, 5, 1); attachFace(pt, v5i, v2i, v1i, 5, 0, 4); attachFace(pt, v5i, v1i, v3i, 3, 1, 5); attachFace(pt, v5i, v3i, v2i, 4, 2, 3); // if the origin is on the affine hull of any of the faces then the origin is not in the // hexahedron or the hexahedron is degenerate for (int i = 0; i < 6; i++) { if (pt->faces[i].dist < mjMINVAL) { return 8; } } return 0; } // creates a polytope from a 3-simplex (returns 0 if polytope can be created) static int polytope4(Polytope* pt, const mjCCDStatus* status) { int v1 = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); int v2 = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); int v3 = newVertex(pt, status->simplex1 + 6, status->simplex2 + 6); int v4 = newVertex(pt, status->simplex1 + 9, status->simplex2 + 9); attachFace(pt, v1, v2, v3, 1, 3, 2); attachFace(pt, v1, v4, v2, 2, 3, 0); attachFace(pt, v1, v3, v4, 0, 3, 1); attachFace(pt, v4, v3, v2, 2, 0, 1); return 0; } // copies a vertex into the polytope and returns its index static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]) { int n = 3*pt->nverts++; copy3(pt->verts1 + n, v1); copy3(pt->verts2 + n, v2); sub3(pt->verts + n, v1, v2); return n; } // delete face from map (return non-zero on error) static int deleteFace(Polytope* pt, Face* face) { // SHOULD NOT OCCUR if (pt->nmap < 2) { pt->nmap = 0; return 1; } face->dist = -1; pt->map[face->index] = pt->map[--pt->nmap]; pt->map[face->index]->index = face->index; return 0; } // returns max number of faces that can be stored in polytope static inline int maxFaces(Polytope* pt) { return pt->maxfaces - pt->nfaces; } // attaches a face to the polytope with the given vertex indices; returns non-zero on error static inline void attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3) { Face* face = &pt->faces[pt->nfaces++]; face->verts[0] = v1; face->verts[1] = v2; face->verts[2] = v3; // adjacent faces face->adj[0] = adj1; face->adj[1] = adj2; face->adj[2] = adj3; // compute witness point v projectOriginPlane(face->v, pt->verts + v1, pt->verts + v2, pt->verts + v3); face->dist = mju_norm3(face->v); // store face in map int i = pt->nmap++; face->index = i; pt->map[i] = face; } // horizon: polytope boundary edges that can be seen from w typedef struct { Polytope* pt; // polytope for which the horizon is defined int* indices; // indices of faces on horizon int* edges; // corresponding edge of each face on the horizon int nedges; // number of edges in horizon mjtNum* w; // point where horizon is created } Horizon; // adds an edge to the horizon static inline void addEdge(Horizon* h, int index, int edge) { h->edges[h->nedges] = edge; h->indices[h->nedges++] = index; } // get edge index where vertex lies static inline int getEdge(Face* face, int vertex) { if (face->verts[0] == vertex) return 0; if (face->verts[1] == vertex) return 1; return 2; } // recursive call to build horizon; return 1 if face is visible from w otherwise 0 static int horizonRec(Horizon* h, Face* face, int e) { mjtNum dist2 = face->dist * face->dist; // v is visible from w so it is deleted and adjacent faces are checked if (dot3(face->v, h->w) >= dist2) { if (deleteFace(h->pt, face)) return 1; // escape recursion on error // recursively search the adjacent faces on the next two edges for (int k = 1; k < 3; k++) { int i = (e + k) % 3; Face* adjFace = &h->pt->faces[face->adj[i]]; if (adjFace->dist > 0) { int adjEdge = getEdge(adjFace, face->verts[(i + 1) % 3]); if (!horizonRec(h, adjFace, adjEdge)) { addEdge(h, face->adj[i], adjEdge); } } } return 1; } return 0; } // creates horizon given the face as starting point static void horizon(Horizon* h, Face* face) { if (deleteFace(h->pt, face)) return; // first edge Face* adjFace = &h->pt->faces[face->adj[0]]; int adjEdge = getEdge(adjFace, face->verts[1]); if (!horizonRec(h, adjFace, adjEdge)) { addEdge(h, face->adj[0], adjEdge); } // second edge adjFace = &h->pt->faces[face->adj[1]]; adjEdge = getEdge(adjFace, face->verts[2]); if (adjFace->dist > 0 && !horizonRec(h, adjFace, adjEdge)) { addEdge(h, face->adj[1], adjEdge); } // third edge adjFace = &h->pt->faces[face->adj[2]]; adjEdge = getEdge(adjFace, face->verts[0]); if (adjFace->dist > 0 && !horizonRec(h, adjFace, adjEdge)) { addEdge(h, face->adj[2], adjEdge); } } // recover witness points from EPA polytope static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNum x2[3]) { // compute affine coordinates for witness points on plane defined by face mjtNum lambda[3]; mjtNum* v1 = pt->verts + face->verts[0]; mjtNum* v2 = pt->verts + face->verts[1]; mjtNum* v3 = pt->verts + face->verts[2]; triAffineCoord(lambda, v1, v2, v3, face->v); // face on geom 1 v1 = pt->verts1 + face->verts[0]; v2 = pt->verts1 + face->verts[1]; v3 = pt->verts1 + face->verts[2]; x1[0] = v1[0]*lambda[0] + v2[0]*lambda[1] + v3[0]*lambda[2]; x1[1] = v1[1]*lambda[0] + v2[1]*lambda[1] + v3[1]*lambda[2]; x1[2] = v1[2]*lambda[0] + v2[2]*lambda[1] + v3[2]*lambda[2]; // face on geom 2 v1 = pt->verts2 + face->verts[0]; v2 = pt->verts2 + face->verts[1]; v3 = pt->verts2 + face->verts[2]; x2[0] = v1[0]*lambda[0] + v2[0]*lambda[1] + v3[0]*lambda[2]; x2[1] = v1[1]*lambda[0] + v2[1]*lambda[1] + v3[1]*lambda[2]; x2[2] = v1[2]*lambda[0] + v2[2]*lambda[1] + v3[2]*lambda[2]; } // returns the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum dist, tolerance = status->tolerance; int k, kmax = status->max_iterations; mjData* d = (mjData*) obj1->data; Face* face; // face closest to origin // initialize horizon Horizon h; mj_markStack(d); h.indices = mj_stackAllocInt(d, 6 + status->max_iterations); h.edges = mj_stackAllocInt(d, 6 + status->max_iterations); h.nedges = 0; h.pt = pt; for (k = 0; k < kmax; k++) { // find the face closest to the origin if (!pt->nmap) { mju_warning("EPA: empty polytope"); mj_freeStack(d); return 0; // assume 0 depth } dist = mjMAXVAL; for (int i = 0; i < pt->nmap; i++) { if (pt->map[i]->dist < dist) { face = pt->map[i]; dist = face->dist; } } // check if dist is 0 if (dist <= 0) { mju_warning("EPA: origin lies on affine hull of face"); } // compute support point w from the closest face's normal mjtNum w1[3], w2[3], w[3]; epaSupport(w1, w2, obj1, obj2, face->v, dist); sub3(w, w1, w2); mjtNum next_dist = dot3(face->v, w) / dist; if (next_dist - dist < tolerance) { break; } h.w = w; horizon(&h, face); if (!pt->nmap) { h.nedges = 0; // next iteration will clean up and error out continue; } // insert w as new vertex and attach faces along the horizon int wi = newVertex(pt, w1, w2), nfaces = pt->nfaces, nedges = h.nedges; // check if there's enough memory to store new faces if (nedges > maxFaces(pt)) { mju_warning("EPA: out of memory for faces on expanding polytope"); break; } // attach first face int horIndex = h.indices[0], horEdge = h.edges[0]; Face* horFace = &pt->faces[horIndex]; int v1 = horFace->verts[horEdge], v2 = horFace->verts[(horEdge + 1) % 3]; horFace->adj[horEdge] = nfaces; attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1); // attach remaining faces for (int i = 1; i < nedges; i++) { int cur = nfaces + i; // index of attached face int next = nfaces + (i + 1) % nedges; // index of next face horIndex = h.indices[i], horEdge = h.edges[i]; horFace = &pt->faces[horIndex]; v1 = horFace->verts[horEdge]; v2 = horFace->verts[(horEdge + 1) % 3]; horFace->adj[horEdge] = cur; attachFace(pt, wi, v2, v1, cur - 1, horIndex, next); } h.nedges = 0; // clear horizon } mj_freeStack(d); epaWitness(pt, face, status->x1, status->x2); status->epa_iterations = k; status->nx = 1; return dist; } // general convex collision detection mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // set up obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); status->gjk_iterations = 0; status->epa_iterations = -1; status->tolerance = config->tolerance; status->max_iterations = config->max_iterations; status->max_contacts = config->max_contacts; status->dist_cutoff = config->dist_cutoff; mjtNum dist = gjk(status, obj1, obj2); // penetration recovery for contacts not needed if (!config->max_contacts) { return dist; } if (dist <= config->tolerance && status->nsimplex > 1) { int N = status->max_iterations; mjData* d = (mjData*) obj1->data; mj_markStack((mjData*) obj1->data); Polytope pt; pt.nfaces = pt.nmap = pt.nverts = 0; // allocate memory for vertices pt.verts = mj_stackAllocNum(d, 3*(5 + N)); pt.verts1 = mj_stackAllocNum(d, 3*(5 + N)); pt.verts2 = mj_stackAllocNum(d, 3*(5 + N)); // allocate memory for faces pt.maxfaces = (6*N > 1000) ? 6*N : 1000; // use 1000 faces as lower bound size_t size1 = sizeof(Face) * pt.maxfaces; size_t size2 = sizeof(Face*) * pt.maxfaces; // since a generous upper bound is used, we need to rescale stack use if not enough // memory is available size_t max_size = mj_stackBytesAvailable(d) - 12*(N * sizeof(int)); if (size1 + size2 > max_size) { pt.maxfaces = max_size / (sizeof(Face) + sizeof(Face*)); size1 = sizeof(Face) * pt.maxfaces; size2 = sizeof(Face*) * pt.maxfaces; } pt.faces = mj_stackAllocByte(d, size1, _Alignof(Face)); pt.map = mj_stackAllocByte(d, size2, _Alignof(Face*)); int ret; if (status->nsimplex == 2) { ret = polytope2(&pt, status, obj1, obj2); } else if (status->nsimplex == 3) { ret = polytope3(&pt, status, obj1, obj2); } else { ret = polytope4(&pt, status); } // simplex not on boundary (objects are penetrating) if (!ret) { dist = -epa(status, &pt, obj1, obj2); } else { status->epa_iterations = -ret; dist = 0; } mj_freeStack(d); } return dist; }