diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 64fdab40..06da687f 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -898,16 +898,16 @@ int mjc_Convex(const mjModel* m, const mjData* d, mjc_initCCDObj(&obj2, m, d, g2, margin); int max_contacts = 1; if (mjENABLED(mjENBL_MULTICCD)) { - // TODO(kylebayes): Support contact pruning. - max_contacts = 8; + max_contacts = 4; } // find initial contact int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin); // nativeccd supports multi Box-Box collision directly - if (mjENABLED(mjENBL_NATIVECCD) && m->geom_type[g1] == mjGEOM_BOX - && m->geom_type[g2] == mjGEOM_BOX) { + if (mjENABLED(mjENBL_NATIVECCD) && + (m->geom_type[g1] == mjGEOM_BOX || m->geom_type[g1] == mjGEOM_MESH) && + (m->geom_type[g2] == mjGEOM_BOX || m->geom_type[g2] == mjGEOM_MESH)) { return ncon; } diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index ae37f3ec..0846b6d0 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -113,6 +113,11 @@ 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]; } +// norm of v +static inline mjtNum norm3(const mjtNum v[3]) { + return mju_sqrt(dot3(v, v)); +} + // 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]; @@ -1190,7 +1195,7 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, // compute witness point v int ret = projectOriginPlane(face->v, pt->verts[v3].vert, pt->verts[v2].vert, pt->verts[v1].vert); if (ret) return 0; - face->dist = mju_sqrt(dot3(face->v, face->v)); + face->dist = norm3(face->v); face->index = -1; return face->dist; @@ -1449,6 +1454,80 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob // ------------------------------------- MultiCCD ------------------------------------------------- +// compute area of a quadrilateral +static inline mjtNum area4(const mjtNum a[3], const mjtNum b[3], + const mjtNum c[3], const mjtNum d[3]) { + mjtNum ad[3] = {d[0] - a[0], d[1] - a[1], d[2] - a[2]}; + mjtNum db[3] = {b[0] - d[0], b[1] - d[1], b[2] - d[2]}; + mjtNum bc[3] = {c[0] - b[0], c[1] - b[1], c[2] - b[2]}; + mjtNum ca[3] = {a[0] - c[0], a[1] - c[1], a[2] - c[2]}; + mjtNum e[3], f[3], g[3]; + cross3(e, ad, db); + cross3(f, bc, ca); + add3(g, e, f); + return 0.5 * norm3(g); +} + + + +// return pointer to next vertex in a polygon +static inline mjtNum* next(mjtNum* polygon, int nvert, mjtNum* curr) { + if (curr == polygon + 3*(nvert - 1)) { + return polygon; + } + return curr + 3; +} + + + +// prune a polygon to a maximum area convex quadrilateral +static inline void polygonQuad(mjtNum* res[4], mjtNum* polygon, int nvert) { + mjtNum* a = polygon, *b = polygon + 3, *c = polygon + 6, *d = polygon + 9; + res[0] = a, res[1] = b, res[2] = c, res[3] = d; + mjtNum m = area4(a, b, c, d), m_next; + mjtNum* end = polygon + 3 * nvert; + for (; a < end; a += 3) { + while (1) { + m_next = area4(a, b, c, next(polygon, nvert, d)); + if (m_next <= m) { + break; + } + m = m_next; + d = next(polygon, nvert, d); + res[0] = a, res[1] = b, res[2] = c, res[3] = d; + while (1) { + m_next = area4(a, b, next(polygon, nvert, c), d); + if (m_next <= m) { + break; + } + m = m_next; + c = next(polygon, nvert, c); + res[0] = a, res[1] = b, res[2] = c, res[3] = d; + } + while (1) { + m_next = area4(a, next(polygon, nvert, b), c, d); + if (m_next <= m) { + break; + } + m = m_next; + b = next(polygon, nvert, b); + res[0] = a, res[1] = b, res[2] = c, res[3] = d; + } + } + if (b == a) { + b = next(polygon, nvert, b); + if (c == b) { + c = next(polygon, nvert, c); + if (d == c) { + d = next(polygon, nvert, d); + } + } + } + } +} + + + // find the normal of a plane perpendicular to the face (given by its normal n) and intersecting the // face edge (v1, v2) static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], @@ -1490,18 +1569,18 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd, // clip a polygon against another polygon -static void polygonClip(mjCCDStatus* status, const mjtNum face1[3 * mjMAX_SIDES], int nface1, - const mjtNum face2[3 * mjMAX_SIDES], int nface2, const mjtNum n[3], +static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, + const mjtNum* face2, int nface2, const mjtNum n[3], const mjtNum dir[3]) { // compute plane normal and distance to plane for each vertex - mjtNum pn[3 * mjMAX_SIDES], pd[mjMAX_SIDES]; + mjtNum pn[3 * mjMAX_POLYVERT], pd[mjMAX_POLYVERT]; for (int i = 0; i < nface1 - 1; i++) { pd[i] = planeNormal(&pn[3*i], &face1[3*i], &face1[3*i + 3], n); } pd[nface1 - 1] = planeNormal(&pn[3*(nface1 - 1)], &face1[3*(nface1 - 1)], &face1[0], n); // reserve 2 * max_sides as max sides for a clipped polygon - mjtNum polygon1[6 * mjMAX_SIDES], polygon2[6 * mjMAX_SIDES], *polygon, *clipped; + mjtNum polygon1[6 * mjMAX_POLYVERT], polygon2[6 * mjMAX_POLYVERT], *polygon, *clipped; int npolygon = nface2, nclipped = 0; polygon = polygon1; clipped = polygon2; @@ -1552,13 +1631,37 @@ static void polygonClip(mjCCDStatus* status, const mjtNum face1[3 * mjMAX_SIDES] nclipped = 0; } + if (npolygon < 1) { + return; + } + // copy final clipped polygon to status - if (npolygon > 0) { - status->nx = npolygon; - for (int i = 0; i < 3*npolygon; i += 3) { + if (status->max_contacts < 5 && npolygon > 4) { + status->nx = 4; + mjtNum* rect[4]; + polygonQuad(rect, polygon, npolygon); + for (int i = 0; i < 4; i++) { + copy3(status->x2 + 3*i, rect[i]); + sub3(status->x1 + 3*i, status->x2 + 3*i, dir); + } + return; + } + + // TODO(kylebayes): Consider using a heristic to prune the polygon. + if (npolygon > mjMAXCONPAIR) { + status->nx = mjMAXCONPAIR; + for (int i = 0; i < 3*mjMAXCONPAIR; i += 3) { copy3(status->x2 + i, polygon + i); sub3(status->x1 + i, status->x2 + i, dir); } + return; + } + + // no pruning needed + status->nx = npolygon; + for (int i = 0; i < 3*npolygon; i += 3) { + copy3(status->x2 + i, polygon + i); + sub3(status->x1 + i, status->x2 + i, dir); } } @@ -1580,6 +1683,91 @@ static inline void globalcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum +// find up to n <= 2 common integers of two arrays, return n +static int intersect(int res[2], const int* arr1, const int* arr2, int n, int m) { + int count = 0; + for (int i = 0; i < n; i++) { + for (int j = 0; j < m; j++) { + if (arr1[i] == arr2[j]) { + res[count++] = arr1[i]; + if (count == 2) return 2; + } + } + } + return count; +} + + + +// compute possible polygon normals of a mesh given up to 3 vertices +static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj, + int v1, int v2, int v3) { + const mjModel* m = obj->model; + const mjData* d = obj->data; + int g = obj->geom; + int polyadr = m->mesh_polyadr[m->geom_dataid[g]]; + int vertadr = m->mesh_vertadr[m->geom_dataid[g]]; + const mjtNum* mat = d->geom_xmat + 9*g; + + if (dim == 3) { + int v1_adr = m->mesh_polymapadr[vertadr + v1]; + int v1_num = m->mesh_polymapnum[vertadr + v1]; + + int v2_adr = m->mesh_polymapadr[vertadr + v2]; + int v2_num = m->mesh_polymapnum[vertadr + v2]; + + int v3_adr = m->mesh_polymapadr[vertadr + v3]; + int v3_num = m->mesh_polymapnum[vertadr + v3]; + + int edgeset[2], faceset[1]; + int n = intersect(edgeset, m->mesh_polymap + v1_adr, m->mesh_polymap + v2_adr, v1_num, v2_num); + if (n == 0) return 0; + n = intersect(faceset, edgeset, m->mesh_polymap + v3_adr, n, v3_num); + if (n == 0) return 0; + + // three vertices defined an unique face + mjtNum* normal = m->mesh_polynormal + 3*(polyadr + faceset[0]); + globalcoord(res, mat, NULL, normal[0], normal[1], normal[2]); + resind[0] = faceset[0]; + return 1; + } + + if (dim == 2) { + int v1_adr = m->mesh_polymapadr[vertadr + v1]; + int v1_num = m->mesh_polymapnum[vertadr + v1]; + + int v2_adr = m->mesh_polymapadr[vertadr + v2]; + int v2_num = m->mesh_polymapnum[vertadr + v2]; + + // up to two faces if vertices defined an edge + int edgeset[2]; + int n = intersect(edgeset, m->mesh_polymap + v1_adr, m->mesh_polymap + v2_adr, v1_num, v2_num); + if (n == 0) return 0; + for (int i = 0; i < n; i++) { + mjtNum* normal = m->mesh_polynormal + 3*(polyadr + edgeset[i]); + globalcoord(res + 3*i, mat, NULL, normal[0], normal[1], normal[2]); + resind[i] = edgeset[i]; + } + return n; + } + + if (dim == 1) { + int v1_adr = m->mesh_polymapadr[vertadr + v1]; + int v1_num = m->mesh_polymapnum[vertadr + v1]; + if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT; + for (int i = 0; i < v1_num; i++) { + int index = m->mesh_polymap[v1_adr + i]; + mjtNum* normal = m->mesh_polynormal + 3*(polyadr + index); + globalcoord(res + 3*i, mat, NULL, normal[0], normal[1], normal[2]); + resind[i] = index; + } + return v1_num; + } + return 0; +} + + + // compute possible face normals of a box given up to 3 vertices static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj, int v1, int v2, int v3) { @@ -1689,11 +1877,37 @@ static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) { -static inline int compareNorms(int res[2], const mjtNum* v, int nv, - const mjtNum* w, int nw) { +// recover mesh polygon from its index, return number of edges +static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) { + const mjModel* m = obj->model; + + // mesh data + int g = 3*obj->geom; + const mjtNum* mat = obj->data->geom_xmat + 3*g; + const mjtNum* pos = obj->data->geom_xpos + g; + int polyadr = m->mesh_polyadr[m->geom_dataid[obj->geom]]; + int vertadr = m->mesh_vertadr[m->geom_dataid[obj->geom]]; + + int adr = m->mesh_polyvertadr[polyadr + idx], j = 0; + int nvert = m->mesh_polyvertnum[polyadr + idx]; + if (nvert > mjMAX_POLYVERT) nvert = mjMAX_POLYVERT; + for (int i = nvert - 1; i >= 0; i--) { + float* verts = m->mesh_vert + 3*vertadr; + int v = m->mesh_polyvert[adr + i]; + float* vert = verts + 3*v; + globalcoord(res + 3*j++, mat, pos, vert[0], vert[1], vert[2]); + } + return nvert; +} + + + +// find two normals that are facing each other within a tolerance, return 1 if found +static inline int alignedNormals(int res[2], const mjtNum* v, int nv, + const mjtNum* w, int nw) { for (int i = 0; i < nv; i++) { for (int j = 0; j < nw; j++) { - if (dot3(v + 3*i, w + 3*j) < -0.99999872) { + if (dot3(v + 3*i, w + 3*j) < -mjCOSINE_TOL) { res[0] = i; res[1] = j; return 1; @@ -1723,11 +1937,10 @@ static inline int simplexDim(int* v1, int* v2, int* v3) { - // recover multiple contacts from EPA polytope static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { - mjtNum face1[mjMAX_SIDES * 3], face2[mjMAX_SIDES * 3]; + mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3]; // get vertices of faces from EPA int v11 = pt->verts[face->verts[0]].index1; @@ -1741,20 +1954,24 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, int nface1 = simplexDim(&v11, &v12, &v13); int nface2 = simplexDim(&v21, &v22, &v23); int nnorms1 = 0, nnorms2 = 0; - mjtNum n1[9], n2[9]; // normals of possible face collisions - int idx1[3], idx2[3]; // indices of faces, so they can be recovered later + mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT]; // normals of possible face collisions + int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT]; // indices of faces // get all possible face normals for each geom if (obj1->geom_type == mjGEOM_BOX) { nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11, v12, v13); + } else if (obj1->geom_type == mjGEOM_MESH) { + nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11, v12, v13); } if (obj2->geom_type == mjGEOM_BOX) { nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21, v22, v23); + } else if (obj2->geom_type == mjGEOM_MESH) { + nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21, v22, v23); } // determine if any two normals match int res[2]; - if (!compareNorms(res, n1, nnorms1, n2, nnorms2)) { + if (!alignedNormals(res, n1, nnorms1, n2, nnorms2)) { return; } int i = res[0], j = res[1]; @@ -1762,9 +1979,13 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, // recover matching faces if (obj1->geom_type == mjGEOM_BOX) { nface1 = boxFace(face1, obj1, idx1[i]); + } else if (obj1->geom_type == mjGEOM_MESH) { + nface1 = meshFace(face1, obj1, idx1[i]); } if (obj2->geom_type == mjGEOM_BOX) { nface2 = boxFace(face2, obj2, idx2[j]); + } else if (obj2->geom_type == mjGEOM_MESH) { + nface2 = meshFace(face2, obj2, idx2[j]); } if (nface1 >= 3 && nface2 >= 3) { @@ -1773,7 +1994,7 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, // this for each contact point. mjtNum diff[3], approx_dir[3]; sub3(diff, status->x2, status->x1); - scl3(approx_dir, n2 + 3*j, mju_sqrt(dot3(diff, diff))); + scl3(approx_dir, n2 + 3*j, norm3(diff)); // clip the faces and store the results in status polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir); diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index dfcfc906..cca220cb 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -25,8 +25,11 @@ extern "C" { #endif -// max sides of a face of mesh supported for multiple contacts -#define mjMAX_SIDES 10 +// tolerance for considering two normals to be aligned +#define mjCOSINE_TOL 0.99999872 + +// max number of supported vertices in a polygon face of a mesh +#define mjMAX_POLYVERT 150 // Status of an EPA run typedef enum { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index ba2b3046..6d67191c 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -2215,11 +2215,8 @@ const char* mj_validateReferences(const mjModel* m) { X(mesh_bvhadr, nmesh, nbvh , m->mesh_bvhnum ) \ X(mesh_graphadr, nmesh, nmeshgraph , 0 ) \ X(mesh_polyadr, nmesh, nmeshpoly , m->mesh_polynum ) \ - X(mesh_polynormal, nmeshpoly*3, nmeshpoly*3 , 0 ) \ X(mesh_polyvertadr, nmeshpoly, nmeshpolyvert , m->mesh_polyvertnum ) \ - X(mesh_polyvert, nmeshpolyvert, nmeshpolyvert , 0 ) \ X(mesh_polymapadr, nmeshvert, nmeshpolymap , m->mesh_polymapnum ) \ - X(mesh_polymap, nmeshpolymap, nmeshpolymap , 0 ) \ X(flex_vertadr, nflex, nflexvert , m->flex_vertnum ) \ X(flex_edgeadr, nflex, nflexedge , m->flex_edgenum ) \ X(flex_elemadr, nflex, nflexelem , m->flex_elemnum ) \ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index a46df9e0..83b8fb7b 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -593,6 +594,8 @@ void mjCMesh::Compile(const mjVFS* vfs) { memcpy(facenormal_.data(), face_.data(), 3*nface()*sizeof(int)); } + MakePolygons(); + // scale, center, orient, compute mass and inertia Process(); processed_ = true; @@ -602,6 +605,8 @@ void mjCMesh::Compile(const mjVFS* vfs) { MakeCenter(); } + MakePolygonNormals(); + // make bounding volume hierarchy if (tree_.Bvh().empty()) { face_aabb_.assign(6*nface(), 0); @@ -707,6 +712,44 @@ void mjCMesh::CopyGraph(int* arr) const { +void mjCMesh::CopyPolygons(int* verts, int* adr, int* num, int poly_adr) const { + int n = polygons_.size(), count = 0; + for (int i = 0; i < n; ++i) { + int m = num[i] = polygons_[i].size(); + adr[i] = poly_adr + count; + count += m; + for (int j = 0; j < m; ++j) { + verts[adr[i] + j - poly_adr] = polygons_[i][j]; + } + } +} + + + +void mjCMesh::CopyPolygonMap(int* faces, int* adr, int* num, int poly_adr) const { + int n = polygon_map_.size(), count = 0; + for (int i = 0; i < n; ++i) { + int m = num[i] = polygon_map_[i].size(); + adr[i] = poly_adr + count; + count += m; + for (int j = 0; j < m; ++j) { + faces[adr[i] + j - poly_adr] = polygon_map_[i][j]; + } + } +} + + + +void mjCMesh::CopyPolygonNormals(mjtNum* arr) { + for (int i = 0; i < polygon_normals_.size(); i += 3) { + arr[i + 0] = (mjtNum)polygon_normals_[i + 0]; + arr[i + 1] = (mjtNum)polygon_normals_[i + 1]; + arr[i + 2] = (mjtNum)polygon_normals_[i + 2]; + } +} + + + void mjCMesh::DelTexcoord() { texcoord_.clear(); } @@ -1997,6 +2040,295 @@ void mjCMesh::MakeCenter(void) { +// compute the normals of the polygons +void mjCMesh::MakePolygonNormals() { + for (int i = 0; i < polygons_.size(); ++i) { + double n[3]; + mjuu_makenormal(n, &vert_[3*polygons_[i][0]], &vert_[3*polygons_[i][1]], + &vert_[3*polygons_[i][2]]); + polygon_normals_[3*i + 0] = n[0]; + polygon_normals_[3*i + 1] = n[1]; + polygon_normals_[3*i + 2] = n[2]; + } +} + + + +// helper class to compute the polygons of a mesh +class MeshPolygon { + public: + // constructors (need starting face) + MeshPolygon(const float v1[3], const float v2[3], const float v3[3], + int v1i, int v2i, int v3i); + MeshPolygon() = delete; + MeshPolygon(const MeshPolygon&) = delete; + MeshPolygon& operator=(const MeshPolygon&) = delete; + + void InsertFace(int v1, int v2, int v3); // insert a face into the polygon + std::vector> Paths() const; // return trace of the polygons + const double* Normal() const { return normal_; } // return the normal of the polygon + + // return the ith component of the normal of the polygon + double Normal(int i) const { return normal_[i]; } + + private: + std::vector> edges_; + + // inserted faces do not necessarily share edges with the current polygon, so they're grouped as + // islands until they can be combined with later face insertions + std::vector islands_; + int nisland_ = 0; + double normal_[3] = {0.0, 0.0, 0.0}; + void CombineIslands(int& island1, int& island2); +}; + + + +MeshPolygon::MeshPolygon(const float v1[3], const float v2[3], const float v3[3], + int v1i, int v2i, int v3i) { + mjuu_makenormal(normal_, v1, v2, v3); + edges_ = {{v1i, v2i}, {v2i, v3i}, {v3i, v1i}}; + nisland_ = 1; + islands_ = {0, 0, 0}; +} + + + +// comparison operator for std::set +bool PolygonCmp(const MeshPolygon& p1, const MeshPolygon& p2) { + const double* n1 = p1.Normal(); + const double* n2 = p2.Normal(); + double dot3 = n1[0] * n2[0] + n1[1] * n2[1] + n1[2] * n2[2]; + + // TODO(kylebayes): The tolerance should be a parameter set the user, as it should be optimized + // from mesh to mesh. + if (dot3 > 0.99999872) { + return false; + } + + if (std::abs(n1[0] - n2[0]) > mjMINVAL) { + return n1[0] > n2[0]; + } + if (std::abs(n1[1] - n2[1]) > mjMINVAL) { + return n1[1] > n2[1]; + } + if (std::abs(n1[2] - n2[2]) > mjMINVAL) { + return n1[2] > n2[2]; + } + return false; +} + + + +// combine two islands when a newly inserted face connects them +void MeshPolygon::CombineIslands(int& island1, int& island2) { + // pick the smaller island + if (island2 < island1) { + int tmp = island1; + island1 = island2; + island2 = tmp; + } + + // renumber the islands + for (int k = 0; k < islands_.size(); ++k) { + if (islands_[k] == island2) { + islands_[k] = island1; + } else if (islands_[k] > island2) { + islands_[k]--; + } + } +} + + + +// insert a triangular face into the polygon +void MeshPolygon::InsertFace(int v1, int v2, int v3) { + int add1 = 1, add2 = 1, add3 = 1; + int island = -1; + + // check if face can be attached via edge v1v2 + for (int i = 0; i < edges_.size(); ++i) { + if (edges_[i].first == v2 && edges_[i].second == v1) { + add1 = 0; + island = islands_[i]; + edges_.erase(edges_.begin() + i); + islands_.erase(islands_.begin() + i); + break; + } + } + + // check if face can be attached via edge v2v3 + for (int i = 0; i < edges_.size(); ++i) { + if (edges_[i].first == v3 && edges_[i].second == v2) { + int island2 = islands_[i]; + if (island == -1) { + island = island2; + } else if (island2 != island) { + nisland_--; + CombineIslands(island, island2); + } + add2 = 0; + edges_.erase(edges_.begin() + i); + islands_.erase(islands_.begin() + i); + break; + } + } + + // check if face can be attached via edge v3v1 + for (int i = 0; i < edges_.size(); ++i) { + if (edges_[i].first == v1 && edges_[i].second == v3) { + int island3 = islands_[i]; + if (island == -1) { + island = island3; + } else if (island3 != island) { + nisland_--; + CombineIslands(island, island3); + } + add3 = 0; + edges_.erase(edges_.begin() + i); + islands_.erase(islands_.begin() + i); + break; + } + } + + if (island == -1) { + island = nisland_++; + } + + // add only new edges to the polygon + + if (add1) { + edges_.push_back({v1, v2}); + islands_.push_back(island); + } + if (add2) { + edges_.push_back({v2, v3}); + islands_.push_back(island); + } + if (add3) { + edges_.push_back({v3, v1}); + islands_.push_back(island); + } +} + + + +// return the traverse vertices of the polygon; there may be multiple paths if the polygon is +// not connected +std::vector> MeshPolygon::Paths() const { + std::vector> paths; + // shortcut if polygon is just a triangular face + if (edges_.size() == 3) { + return {{edges_[0].first, edges_[1].first, edges_[2].first}}; + } + + // go through each connected component of the polygon + for (int i = 0; i < nisland_; ++i) { + std::vector path; + + // find starting vertex + for (int j = 0; j < edges_.size(); ++j) { + if (islands_[j] == i) { + path.push_back(edges_[j].first); + path.push_back(edges_[j].second); + break; + } + } + + // SHOULD NOT OCCUR (See logic in MeshPolygon::CombineIslands) + if (path.empty()) { + continue; + } + + // visit the next vertex given the current edge + int next = path.back(); + for (int l = 0; l < edges_.size(); ++l) { + int finished = 0; + for (int k = 1; k < edges_.size(); ++k) { + if (islands_[k] == i && edges_[k].first == next) { + next = edges_[k].second; + if (next == path[0]) { + paths.push_back(path); + finished = 1; + break; + } + path.push_back(next); + break; + } + } + + // back at start + if (finished) { + break; + } + } + } + return paths; +} + + + +// merge coplanar mesh triangular faces into polygonal sides to represent the geometry of the mesh +void mjCMesh::MakePolygons() { + std::set polygons(PolygonCmp); + polygons_.clear(); + polygon_normals_.clear(); + polygon_map_.clear(); + + // initialize polygon map + for (int i = 0; i < nvert(); i++) { + polygon_map_.push_back(std::vector()); + } + + // use graph data if available + int *faces, nfaces; + if (graph_) { + int nvert = graph_[0]; + nfaces = graph_[1]; + faces = graph_ + 2 + 3*nvert + 3*nfaces; + } else { + nfaces = nface(); + faces = face_.data(); + } + + // process each face + for (int i = 0; i < nfaces; i++) { + float* v1 = &vert_[3*faces[3*i + 0]]; + float* v2 = &vert_[3*faces[3*i + 1]]; + float* v3 = &vert_[3*faces[3*i + 2]]; + + MeshPolygon face(v1, v2, v3, faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]); + auto it = polygons.find(face); + if (it == polygons.end()) { + polygons.emplace(v1, v2, v3, faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]); + } else { + MeshPolygon& p = const_cast(*it); + p.InsertFace(faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]); + } + } + + for (const auto& polygon : polygons) { + std::vector> paths = polygon.Paths(); + + // separate the polygons if they were grouped together + for (const auto& path : paths) { + if (path.size() < 3) continue; + polygons_.push_back(path); + polygon_normals_.push_back(polygon.Normal(0)); + polygon_normals_.push_back(polygon.Normal(1)); + polygon_normals_.push_back(polygon.Normal(2)); + } + } + + // populate the polygon map + for (int i = 0; i < polygons_.size(); i++) { + for (int j = 0; j < polygons_[i].size(); ++j) { + polygon_map_[polygons_[i][j]].push_back(i); + } + } +} + + //------------------ class mjCSkin implementation -------------------------------------------------- // constructor diff --git a/src/user/user_model.cc b/src/user/user_model.cc index c59c07f3..d5382a93 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -837,6 +837,9 @@ void mjCModel::Clear() { nmeshtexcoord = 0; nmeshface = 0; nmeshgraph = 0; + nmeshpoly = 0; + nmeshpolyvert = 0; + nmeshpolymap = 0; nskinvert = 0; nskintexvert = 0; nskinface = 0; @@ -1752,6 +1755,9 @@ void mjCModel::SetSizes() { nmeshface += meshes_[i]->nface(); nmeshtexcoord += (meshes_[i]->HasTexcoord() ? meshes_[i]->ntexcoord() : 0); nmeshgraph += meshes_[i]->szgraph(); + nmeshpoly += meshes_[i]->npolygon(); + nmeshpolyvert += meshes_[i]->npolygonvert(); + nmeshpolymap += meshes_[i]->npolygonmap(); } // skin counts @@ -2755,6 +2761,7 @@ void mjCModel::CopyObjects(mjModel* m) { int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr; int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr; int bonevert_adr, graph_adr, data_adr, bvh_adr; + int poly_adr, polymap_adr, polyvert_adr; // sizes outside call to mj_makeModel m->nemax = nemax; @@ -2776,19 +2783,16 @@ void mjCModel::CopyObjects(mjModel* m) { texcoord_adr = 0; face_adr = 0; graph_adr = 0; + poly_adr = 0; + polyvert_adr = 0; + polymap_adr = 0; for (int i=0; invert(); j++) { - m->mesh_polymapadr[vert_adr + j] = 0; - m->mesh_polymapnum[vert_adr + j] = 0; - } - m->mesh_polynum[i] = 0; - m->mesh_polyadr[i] = 0; - // set fields + m->mesh_polyadr[i] = poly_adr; + m->mesh_polynum[i] = pme->npolygon(); m->mesh_vertadr[i] = vert_adr; m->mesh_vertnum[i] = pme->nvert(); m->mesh_normaladr[i] = normal_adr; @@ -2818,6 +2822,11 @@ void mjCModel::CopyObjects(mjModel* m) { if (pme->szgraph()) { pme->CopyGraph(m->mesh_graph + graph_adr); } + pme->CopyPolygonNormals(m->mesh_polynormal + 3*poly_adr); + pme->CopyPolygons(m->mesh_polyvert + polyvert_adr, m->mesh_polyvertadr + poly_adr, + m->mesh_polyvertnum + poly_adr, polyvert_adr); + pme->CopyPolygonMap(m->mesh_polymap + polymap_adr, m->mesh_polymapadr + vert_adr, + m->mesh_polymapnum + vert_adr, polymap_adr); // copy bvh data if (pme->tree().Nbvh()) { @@ -2830,6 +2839,9 @@ void mjCModel::CopyObjects(mjModel* m) { } // advance counters + poly_adr += pme->npolygon(); + polyvert_adr += pme->npolygonvert(); + polymap_adr += pme->npolygonmap(); vert_adr += pme->nvert(); normal_adr += pme->nnormal(); texcoord_adr += (pme->HasTexcoord() ? pme->ntexcoord() : 0); @@ -4266,7 +4278,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { SetNuser(); // compile meshes (needed for geom compilation) - if (compiler.usethread && meshes_.size() > 1) { + if (!compiler.usethread && meshes_.size() > 1) { // multi-threaded mesh compile CompileMeshes(vfs); } else { @@ -4340,9 +4352,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite, ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem, nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord, - nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, 0, 0, - 0, nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert, - nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude, + nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmeshpoly, + nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, nskinface, nskinbone, + nskinbonevert, nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude, neq, ntendon, nwrap, nsensor, nnumeric, nnumericdata, ntext, ntextdata, ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr, nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam, diff --git a/src/user/user_model.h b/src/user/user_model.h index 7a99f6d8..ac36f6dc 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -99,7 +99,10 @@ class mjCModel_ : public mjsElement { int nmeshnormal; // number of normals in all meshes int nmeshtexcoord; // number of texture coordinates in all meshes int nmeshface; // number of triangular faces in all meshes - int nmeshgraph; // number of shorts in mesh auxiliary data + int nmeshpoly; // number of polygon faces in all meshes + int nmeshgraph; // number of ints in mesh auxiliary data + int nmeshpolyvert; // number of vertices in all polygon faces + int nmeshpolymap; // number of polygons in vertex map int nskinvert; // number of vertices in all skins int nskintexvert; // number of vertices with texcoord in all skins int nskinface; // number of faces in all skins diff --git a/src/user/user_objects.h b/src/user/user_objects.h index f69b0e6d..2c5af46a 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -942,6 +942,21 @@ class mjCMesh: public mjCMesh_, private mjsMesh { int nnormal() const { return normal_.size()/3; } int ntexcoord() const { return texcoord_.size()/2; } int nface() const { return face_.size()/3; } + int npolygon() const { return polygons_.size(); } + int npolygonvert() const { + int acc = 0; + for (const auto& polygon : polygons_) { + acc += polygon.size(); + } + return acc; + } + int npolygonmap() const { + int acc = 0; + for (const auto& polygon : polygon_map_) { + acc += polygon.size(); + } + return acc; + } // return size of graph data in ints int szgraph() const { return szgraph_; } @@ -968,6 +983,15 @@ class mjCMesh: public mjCMesh_, private mjsMesh { void CopyTexcoord(float* arr) const; // copy texcoord data into array void CopyGraph(int* arr) const; // copy graph data into array + // copy polygon data into array + void CopyPolygons(int* verts, int* adr, int* num, int poly_adr) const; + + // copy polygon map data into array + void CopyPolygonMap(int *faces, int* adr, int* num, int poly_adr) const; + + // copy polygon normal data into array + void CopyPolygonNormals(mjtNum* arr); + // sets properties of a bounding volume given a face id void SetBoundingVolume(int faceid); @@ -996,6 +1020,8 @@ class mjCMesh: public mjCMesh_, private mjsMesh { void CopyPlugin(); void Rotate(double quat[4]); // rotate mesh by quaternion void Transform(double pos[3], double quat[4]); // transform mesh by position and quaternion + void MakePolygons(); // compute the polygon sides of the mesh + void MakePolygonNormals(); // compute the normals of the polygons // computes the inertia matrix of the mesh given the type of inertia void ComputeInertia(double inert[6], double CoM[3]); @@ -1004,6 +1030,11 @@ class mjCMesh: public mjCMesh_, private mjsMesh { double* center_; // face circumcenter data (3*nface) int* graph_; // convex graph data + // mesh data for collision detection + std::vector> polygons_; // polygons of the mesh + std::vector polygon_normals_; // normals of the polygons + std::vector> polygon_map_; // map from vertex to polygon + // for caching purposes std::vector vertex_index_; std::vector normal_index_; diff --git a/src/user/user_util.cc b/src/user/user_util.cc index db022bfb..0833b020 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -360,17 +360,23 @@ void mjuu_crossvec(double* a, const double* b, const double* c) { // compute normal vector to given triangle, return length double mjuu_makenormal(double* normal, const float* a, const float* b, const float* c) { - double v1[3] = {b[0]-a[0], b[1]-a[1], b[2]-a[2]}; - double v2[3] = {c[0]-a[0], c[1]-a[1], c[2]-a[2]}; - double res; + double v1[3] = {a[0], a[1], a[2]}; + double v2[3] = {b[0], b[1], b[2]}; + double v3[3] = {c[0], c[1], c[2]}; + double diffAB[3] = {v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]}; + double diffAC[3] = {v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2]}; - mjuu_crossvec(normal, v1, v2); - if ((res=mjuu_normvec(normal, 3)) < mjEPS) { - normal[0] = normal[1] = 0; - normal[2] = 1; + mjuu_crossvec(normal, diffAB, diffAC); + double nrm = std::sqrt(mjuu_dot3(normal, normal)); + if (nrm < mjEPS) { + normal[0] = 1; + normal[1] = 0; + normal[2] = 0; } - - return res; + normal[0] /= nrm; + normal[1] /= nrm; + normal[2] /= nrm; + return nrm; } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 2b5b94bf..52c63002 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -30,7 +30,7 @@ #include // uncomment to run tests with libccd -// #define TEST_WITH_LIBCCD +//#define TEST_WITH_LIBCCD namespace mujoco { namespace { @@ -235,10 +235,10 @@ TEST_F(MjGjkTest, SphereSphereNoDist) { TEST_F(MjGjkTest, SphereSphereIntersect) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -276,10 +276,10 @@ TEST_F(MjGjkTest, SphereSphereIntersect) { TEST_F(MjGjkTest, BoxBoxDepth) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -310,10 +310,10 @@ TEST_F(MjGjkTest, BoxBoxDepth) { TEST_F(MjGjkTest, BoxBoxDepth2) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -360,10 +360,10 @@ TEST_F(MjGjkTest, BoxBoxDepth2) { TEST_F(MjGjkTest, BoxBoxDepth3) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -426,10 +426,10 @@ TEST_F(MjGjkTest, BoxBoxDepth3) { TEST_F(MjGjkTest, BoxBoxTouching) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -454,10 +454,10 @@ TEST_F(MjGjkTest, BoxBoxTouching) { TEST_F(MjGjkTest, BoxBoxMultiCCD) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -492,10 +492,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) { TEST_F(MjGjkTest, BoxBoxMultiCCD2) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -530,10 +530,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) { TEST_F(MjGjkTest, BoxBoxMultiCCD3) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -575,10 +575,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) { TEST_F(MjGjkTest, BoxBoxMultiCCD4) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -641,10 +641,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) { TEST_F(MjGjkTest, BoxBoxMultiCCD5) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -708,10 +708,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) { TEST_F(MjGjkTest, BoxBoxMultiCCD6) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -757,10 +757,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) { TEST_F(MjGjkTest, BoxBoxMultiCCD7) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -819,10 +819,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) { TEST_F(MjGjkTest, BoxBoxMultiCCD8) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -880,11 +880,11 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) { TEST_F(MjGjkTest, BoxBoxMultiCCD9) { static constexpr char xml[] = R"( - - - - -)"; + + + + + )"; std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); @@ -942,31 +942,31 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) { TEST_F(MjGjkTest, SmallBoxMesh) { static constexpr char xml[] = R"( - - - - + + + + - - - - + + + + )"; std::array error; @@ -998,6 +998,174 @@ TEST_F(MjGjkTest, SmallBoxMesh) { mj_deleteData(data); mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxMesh) { + 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"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 1000); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxMesh2) { + 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"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 1000); + + EXPECT_EQ(ncons, 5); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxMeshPrune) { + 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"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 4); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, MeshMesh) { + 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"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 5); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, MeshMeshPrune) { + 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"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { std::array error; @@ -1023,10 +1191,10 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { TEST_F(MjGjkTest, EllipsoidEllipsoid) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -1048,10 +1216,10 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) { TEST_F(MjGjkTest, BoxBox) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -1071,18 +1239,18 @@ TEST_F(MjGjkTest, BoxBox) { } TEST_F(MjGjkTest, LongBox) { - static constexpr char xml[] = R"( - - - - - - - - -)"; +static constexpr char xml[] = R"( + + + + + + + + + )"; std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); @@ -1108,6 +1276,10 @@ TEST_F(MjGjkTest, LongBox) { EXPECT_NEAR(pos[1], 0, kTolerance); EXPECT_NEAR(pos[2], -0.005, kTolerance); + // multicontact + ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + EXPECT_EQ(ncons, 4); + mj_deleteData(data); mj_deleteModel(model); } @@ -1115,10 +1287,10 @@ TEST_F(MjGjkTest, LongBox) { TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error; @@ -1143,10 +1315,10 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { TEST_F(MjGjkTest, CapsuleCapsule) { static constexpr char xml[] = R"( - - - - + + + + )"; std::array error;