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

Support multi-contact with meshes in nativeccd. Contacts are pruned to up to 4 per geom.

Browse Source 
PiperOrigin-RevId: 730457036
Change-Id: Ib2368f3a7def4fdb94673620df3603e372c35745
This commit is contained in:
Kyle Bayes
2025-02-24 08:13:09 -08:00
committed by Copybara-Service
parent cf1784bb79
commit ae1c3b7ec0
10 changed files with 935 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+4 -4
View File
@@ -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;
}
src/engine/engine_collision_gjk.c
+238 -17
View File
@@ -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);
src/engine/engine_collision_gjk.h
+5 -2
View File
@@ -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 {
src/engine/engine_io.c
-3
View File
@@ -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 ) \
src/user/user_mesh.cc
+332
View File
@@ -22,6 +22,7 @@
#include <cstring>
#include <functional>
#include <memory>
#include <set>
#include <string>
#include <string_view>
#include <unordered_map>
@@ -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<std::vector<int>> 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<std::pair<int, int>> 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<int> 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<std::vector<int>> MeshPolygon::Paths() const {
std::vector<std::vector<int>> 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<int> 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<MeshPolygon, decltype(PolygonCmp)*> 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<int>());
}
// 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<MeshPolygon&>(*it);
p.InsertFace(faces[3*i + 0], faces[3*i + 1], faces[3*i + 2]);
}
}
for (const auto& polygon : polygons) {
std::vector<std::vector<int>> 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
src/user/user_model.cc
+24 -12
View File
@@ -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; i<nmesh; i++) {
// get pointer
mjCMesh* pme = meshes_[i];
// zero out multi-ccd fields
for (int j = 0; j < pme->nvert(); 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,
src/user/user_model.h
+4 -1
View File
@@ -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
src/user/user_objects.h
+31
View File
@@ -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<std::vector<int>> polygons_; // polygons of the mesh
std::vector<double> polygon_normals_; // normals of the polygons
std::vector<std::vector<int>> polygon_map_; // map from vertex to polygon
// for caching purposes
std::vector<int> vertex_index_;
std::vector<int> normal_index_;
src/user/user_util.cc
+15 -9
View File
@@ -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;
}
test/engine/engine_collision_gjk_test.cc
+282 -110
View File
@@ -30,7 +30,7 @@
#include <gtest/gtest.h>
// 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"(
<mujoco>
<worldbody>
<geom name="geom1" type="sphere" pos="-1 0 0" size="3"/>
<geom name="geom2" type="sphere" pos="3 0 0" size="3"/>
</worldbody>
<worldbody>
<geom name="geom1" type="sphere" pos="-1 0 0" size="3"/>
<geom name="geom2" type="sphere" pos="3 0 0" size="3"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -276,10 +276,10 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
TEST_F(MjGjkTest, BoxBoxDepth) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="-1 0 0" size="2.5 2.5 2.5"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" pos="-1 0 0" size="2.5 2.5 2.5"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -310,10 +310,10 @@ TEST_F(MjGjkTest, BoxBoxDepth) {
TEST_F(MjGjkTest, BoxBoxDepth2) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="5 5 .1"/>
<geom name="geom2" type="box" pos="0 0 0" size="1 1 1"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="5 5 .1"/>
<geom name="geom2" type="box" pos="0 0 0" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -360,10 +360,10 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
TEST_F(MjGjkTest, BoxBoxDepth3) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
<geom name="geom2" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
<geom name="geom2" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -426,10 +426,10 @@ TEST_F(MjGjkTest, BoxBoxDepth3) {
TEST_F(MjGjkTest, BoxBoxTouching) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 1.859913200000001376466229885409" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 2 1.859913200000001376466229885409" size="1 1 1"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" pos="0 0 1.859913200000001376466229885409" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 2 1.859913200000001376466229885409" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -454,10 +454,10 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
TEST_F(MjGjkTest, BoxBoxMultiCCD) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 1.9" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 0 0" size="10 10 1"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" pos="0 0 1.9" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 0 0" size="10 10 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -492,10 +492,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) {
TEST_F(MjGjkTest, BoxBoxMultiCCD2) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="9.5 9.5 1.9" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 0 0" size="10 10 1"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" pos="9.5 9.5 1.9" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 0 0" size="10 10 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -530,10 +530,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) {
TEST_F(MjGjkTest, BoxBoxMultiCCD3) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom type="box" name="geom1" size="5 5 .1" pos="0 0 0"/>
<geom type="box" name="geom2" size="1 1 1"/>
</worldbody>
<worldbody>
<geom type="box" name="geom1" size="5 5 .1" pos="0 0 0"/>
<geom type="box" name="geom2" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -575,10 +575,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) {
TEST_F(MjGjkTest, BoxBoxMultiCCD4) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" size="0.25 0.25 0.05" type="box"/>
<geom name="geom2" size="0.25 0.25 0.05" type="box"/>
</worldbody>
<worldbody>
<geom name="geom1" size="0.25 0.25 0.05" type="box"/>
<geom name="geom2" size="0.25 0.25 0.05" type="box"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -641,10 +641,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) {
TEST_F(MjGjkTest, BoxBoxMultiCCD5) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" size="0.25 0.25 0.05" type="box"/>
<geom name="geom2" size="0.25 0.25 0.05" type="box"/>
</worldbody>
<worldbody>
<geom name="geom1" size="0.25 0.25 0.05" type="box"/>
<geom name="geom2" size="0.25 0.25 0.05" type="box"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -708,10 +708,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) {
TEST_F(MjGjkTest, BoxBoxMultiCCD6) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".5 .5 .1" pos="0 0 -.1"/>
<geom name="geom2" type="box" size=".1 .1 .1" pos="0 0 0"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" size=".5 .5 .1" pos="0 0 -.1"/>
<geom name="geom2" type="box" size=".1 .1 .1" pos="0 0 0"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -757,10 +757,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) {
TEST_F(MjGjkTest, BoxBoxMultiCCD7) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".25 .25 .05" pos="0 0 0"/>
<geom name="geom2" type="box" size=".25 .25 .05" pos="0 0 0"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" size=".25 .25 .05" pos="0 0 0"/>
<geom name="geom2" type="box" size=".25 .25 .05" pos="0 0 0"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -819,10 +819,10 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) {
TEST_F(MjGjkTest, BoxBoxMultiCCD8) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".25 .25 .05" pos="0 0 0"/>
<geom name="geom2" type="box" size=".25 .25 .05" pos="0 0 0"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" size=".25 .25 .05" pos="0 0 0"/>
<geom name="geom2" type="box" size=".25 .25 .05" pos="0 0 0"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -880,11 +880,11 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) {
TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".025 .025 .025" pos="0 0 0"/>
<geom name="geom2" type="box" size=".025 .025 .025" pos="0 0 0"/>
</worldbody>
</mujoco>)";
<worldbody>
<geom name="geom1" type="box" size=".025 .025 .025" pos="0 0 0"/>
<geom name="geom2" type="box" size=".025 .025 .025" pos="0 0 0"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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"(
<mujoco>
<asset>
<mesh name="box" scale=".5 .5 .1"
vertex="-1 -1 -1
1 -1 -1
1 1 -1
1 1 1
1 -1 1
-1 1 -1
-1 1 1
-1 -1 1"/>
<mesh name="smallbox" scale=".1 .1 .1"
vertex="-1 -1 -1
1 -1 -1
1 1 -1
1 1 1
1 -1 1
-1 1 -1
-1 1 1
-1 -1 1"/>
</asset>
<asset>
<mesh name="box" scale=".5 .5 .1"
vertex="-1 -1 -1
1 -1 -1
1 1 -1
1 1 1
1 -1 1
-1 1 -1
-1 1 1
-1 -1 1"/>
<mesh name="smallbox" scale=".1 .1 .1"
vertex="-1 -1 -1
1 -1 -1
1 1 -1
1 1 1
1 -1 1
-1 1 -1
-1 1 1
-1 -1 1"/>
</asset>
<worldbody>
<geom name="geom1" pos="0 0 -.1" mesh="box" type="mesh"/>
<geom name="geom2" pos="0 0 .1" mesh="smallbox" type="mesh"/>
</worldbody>
<worldbody>
<geom name="geom2" pos="0 0 .1" size=".1 .1 .1" type="mesh" mesh="smallbox"/>
<geom name="geom1" pos="0 0 -.099999999" size=".5 .5 .1" type="mesh" mesh="box"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -998,6 +998,174 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjGjkTest, BoxMesh) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="pentaprism"
vertex="1 0 0 0.309 0.951 0 -0.809 0.588 0 -0.809 -0.588 0 0.309 -0.951 0
1 0 1 0.309 0.951 1 -0.809 0.588 1 -0.809 -0.588 1 0.309 -0.951 1"
scale=".2 .2 .1"/>
</asset>
<worldbody>
<geom name="geom1" type="box" pos="0 0 -.01" size="3 3 .01"/>
<geom name="geom2" pos="0 0 0.157" euler="0 -90 0" type="mesh" mesh="pentaprism"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
std::vector<mjtNum> 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"(
<mujoco>
<asset>
<mesh name="pentaprism"
vertex="1 0 0 0.309 0.951 0 -0.809 0.588 0 -0.809 -0.588 0 0.309 -0.951 0
1 0 1 0.309 0.951 1 -0.809 0.588 1 -0.809 -0.588 1 0.309 -0.951 1"
scale=".2 .2 .1"/>
</asset>
<worldbody>
<geom name="geom1" type="box" pos="0 0 -.01" size="3 3 .01"/>
<geom name="geom2" pos="0 0 -0.001" type="mesh" mesh="pentaprism"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
std::vector<mjtNum> 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"(
<mujoco>
<asset>
<mesh name="pentaprism"
vertex="1 0 0 0.309 0.951 0 -0.809 0.588 0 -0.809 -0.588 0 0.309 -0.951 0
1 0 1 0.309 0.951 1 -0.809 0.588 1 -0.809 -0.588 1 0.309 -0.951 1"
scale=".2 .2 .1"/>
</asset>
<worldbody>
<geom name="geom1" type="box" pos="0 0 -.01" size="3 3 .01"/>
<geom name="geom2" pos="0 0 -0.001" type="mesh" mesh="pentaprism"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
std::vector<mjtNum> 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"(
<mujoco>
<asset>
<mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
scale="1 1 .01"/>
<mesh name="pentaprism"
vertex="1 0 0 0.309 0.951 0 -0.809 0.588 0 -0.809 -0.588 0 0.309 -0.951 0
1 0 1 0.309 0.951 1 -0.809 0.588 1 -0.809 -0.588 1 0.309 -0.951 1"
scale=".2 .2 .1"/>
</asset>
<worldbody>
<geom name="geom1" type="mesh" pos="0 0 -0.01" mesh="box"/>
<geom name="geom2" pos="0 0 -0.001" type="mesh" mesh="pentaprism"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
std::vector<mjtNum> 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"(
<mujoco>
<asset>
<mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
scale="1 1 .01"/>
<mesh name="pentaprism"
vertex="1 0 0 0.309 0.951 0 -0.809 0.588 0 -0.809 -0.588 0 0.309 -0.951 0
1 0 1 0.309 0.951 1 -0.809 0.588 1 -0.809 -0.588 1 0.309 -0.951 1"
scale=".2 .2 .1"/>
</asset>
<worldbody>
<geom name="geom1" type="mesh" pos="0 0 -0.01" mesh="box"/>
<geom name="geom2" pos="0 0 -0.001" type="mesh" mesh="pentaprism"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
std::vector<mjtNum> 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<char, 1000> error;
@@ -1023,10 +1191,10 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
TEST_F(MjGjkTest, EllipsoidEllipsoid) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="ellipsoid" pos="1.5 0 -.5" size=".15 .30 .20"/>
<geom name="geom2" type="ellipsoid" pos="1.5 .5 .5" size=".10 .10 .15"/>
</worldbody>
<worldbody>
<geom name="geom1" type="ellipsoid" pos="1.5 0 -.5" size=".15 .30 .20"/>
<geom name="geom2" type="ellipsoid" pos="1.5 .5 .5" size=".10 .10 .15"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -1048,10 +1216,10 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
TEST_F(MjGjkTest, BoxBox) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="-1.5 .5 0" size="1 1 1"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody>
<worldbody>
<geom name="geom1" type="box" pos="-1.5 .5 0" size="1 1 1"/>
<geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -1071,18 +1239,18 @@ TEST_F(MjGjkTest, BoxBox) {
}
TEST_F(MjGjkTest, LongBox) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="long_box"
vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
scale=".6 .03 .03"/>
</asset>
<worldbody>
<geom name="geom1" type="box" size="1 1 .3" pos="0 0 -.3"/>
<geom name="geom2" type="mesh" mesh="long_box" pos="0 0 .02" euler="0 0 40"/>
</worldbody>
</mujoco>)";
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="long_box"
vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
scale=".6 .03 .03"/>
</asset>
<worldbody>
<geom name="geom1" type="box" size="1 1 .3" pos="0 0 -.3"/>
<geom name="geom2" type="mesh" mesh="long_box" pos="0 0 .02" euler="0 0 40"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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"(
<mujoco>
<worldbody>
<geom name="geom1" type="ellipsoid" pos="1.5 0 -.5" size=".15 .30 .20"/>
<geom name="geom2" type="ellipsoid" pos="1.5 .5 .5" size=".10 .10 .15"/>
</worldbody>
<worldbody>
<geom name="geom1" type="ellipsoid" pos="1.5 0 -.5" size=".15 .30 .20"/>
<geom name="geom2" type="ellipsoid" pos="1.5 .5 .5" size=".10 .10 .15"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;
@@ -1143,10 +1315,10 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
TEST_F(MjGjkTest, CapsuleCapsule) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="geom1" type="capsule" pos="-.3 .2 -.4" size=".15 .30"/>
<geom name="geom2" type="capsule" pos=".3 .2 .4" size=".10 .10"/>
</worldbody>
<worldbody>
<geom name="geom1" type="capsule" pos="-.3 .2 -.4" size=".15 .30"/>
<geom name="geom2" type="capsule" pos=".3 .2 .4" size=".10 .10"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> error;