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

Remove const qualifier on mjData in mjfCollision type as mjData is non-const in mjc_SDF via mjpPlugin.

Browse Source 
PiperOrigin-RevId: 883225786
Change-Id: I5fecaa1d575b340a6510212db7b3e7db48f361cc
This commit is contained in:
Kyle Bayes
2026-03-13 10:19:53 -07:00
committed by Copybara-Service
parent 7ec4fe2c50
commit 88ba8ffdbc
8 changed files with 76 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/mujoco/mjdata.h
+2 -2
View File
@@ -529,7 +529,7 @@ typedef mjtNum (*mjfTime)(void);
typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id); typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id);
// collision detection // collision detection
typedef int (*mjfCollision)(const mjModel* m, const mjData* d, typedef int (*mjfCollision)(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
#endif // MUJOCO_MJDATA_H_ #endif // MUJOCO_MJDATA_H_
src/engine/engine_collision_box.c
+3 -7
View File
@@ -94,9 +94,7 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
// sphere : box // sphere : box
int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con, int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
int g1, int g2, mjtNum margin)
{
mjGETINFO; mjGETINFO;
return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
@@ -593,9 +591,7 @@ skip:
// capsule : box // capsule : box
int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con, int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
int g1, int g2, mjtNum margin)
{
mjGETINFO mjGETINFO
return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
@@ -1342,7 +1338,7 @@ edgeedge:
// box : box // box : box
int mjc_BoxBox(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
int num = _boxbox(m, d, con, g1, g2, margin); int num = _boxbox(m, d, con, g1, g2, margin);
// use dim field to mark: -1: bad, 0: good // use dim field to mark: -1: bad, 0: good
src/engine/engine_collision_convex.c
+7 -10
View File
@@ -868,8 +868,7 @@ static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) {
// multi-point convex-convex collision, using libccd // multi-point convex-convex collision, using libccd
int mjc_Convex(const mjModel* m, const mjData* d, int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
// init ccd objects // init ccd objects
mjCCDObj obj1, obj2; mjCCDObj obj1, obj2;
mjc_initCCDObj(&obj1, m, d, g1, margin); mjc_initCCDObj(&obj1, m, d, g1, margin);
@@ -998,8 +997,7 @@ static int addplanemesh(mjContact* con, const float vertex[3],
// plane-convex collision, using libccd // plane-convex collision, using libccd
int mjc_PlaneConvex(const mjModel* m, const mjData* d, int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO mjGETINFO
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
ccd_vec3_t dir, vec; ccd_vec3_t dir, vec;
@@ -1136,8 +1134,7 @@ static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, m
// entry point for heightfield collisions // entry point for heightfield collisions
int mjc_ConvexHField(const mjModel* m, const mjData* d, int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
// hfield frame // hfield frame
const mjtNum* pos1 = d->geom_xpos + 3*g1; const mjtNum* pos1 = d->geom_xpos + 3*g1;
const mjtNum* mat1 = d->geom_xmat + 9*g1; const mjtNum* mat1 = d->geom_xmat + 9*g1;
@@ -1565,8 +1562,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
//---------------------------- flex collisions --------------------------------------------- //---------------------------- flex collisions ---------------------------------------------
// geom-elem or elem-elem or vert-elem convex collision using ccd // geom-elem or elem-elem or vert-elem convex collision using ccd
int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { int f2, int e2, mjtNum margin) {
mjCCDObj obj1, obj2; mjCCDObj obj1, obj2;
mjc_initCCDObj(&obj1, m, d, g1, margin); mjc_initCCDObj(&obj1, m, d, g1, margin);
mjc_initCCDObj(&obj2, m, d, -1, margin); mjc_initCCDObj(&obj2, m, d, -1, margin);
@@ -1595,8 +1592,8 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
// test a height field and a flex element for collision // test a height field and a flex element for collision
int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e,
int g, int f, int e, mjtNum margin) { mjtNum margin) {
mjtNum vec[3], dx, dy; mjtNum vec[3], dx, dy;
mjtNum xmin, xmax, ymin, ymax, zmin, zmax; mjtNum xmin, xmax, ymin, ymax, zmin, zmax;
int dr[2], cnt, rmin, rmax, cmin, cmax; int dr[2], cnt, rmin, rmax, cmin, cmax;
src/engine/engine_collision_convex.h
+7 -10
View File
@@ -79,20 +79,17 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
// pairwise geom collision functions using ccd // pairwise geom collision functions using ccd
int mjc_PlaneConvex(const mjModel* m, const mjData* d, int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
mjContact* con, int g1, int g2, mjtNum margin); int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
int mjc_ConvexHField(const mjModel* m, const mjData* d, MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
mjContact* con, int g1, int g2, mjtNum margin);
MJAPI int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
// geom-elem or elem-elem or vert-elem collision function using ccd // geom-elem or elem-elem or vert-elem collision function using ccd
int mjc_ConvexElem (const mjModel* m, const mjData* d, mjContact* con, int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin); int f2, int e2, mjtNum margin);
// heightfield-elem collision function using ccd // heightfield-elem collision function using ccd
int mjc_HFieldElem (const mjModel* m, const mjData* d, mjContact* con, int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e,
int g, int f, int e, mjtNum margin); mjtNum margin);
// fix contact frame normal // fix contact frame normal
void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2); void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2);
src/engine/engine_collision_primitive.c
+10 -16
View File
@@ -51,16 +51,14 @@ static int mjraw_PlaneSphere(mjContact* con, mjtNum margin,
// plane : sphere // plane : sphere
int mjc_PlaneSphere(const mjModel* m, const mjData* d, int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO mjGETINFO
return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
// plane : capsule // plane : capsule
int mjc_PlaneCapsule(const mjModel* m, const mjData* d, int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO mjGETINFO
// get capsule axis, segment = scaled axis // get capsule axis, segment = scaled axis
@@ -89,8 +87,7 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
// plane : cylinder // plane : cylinder
int mjc_PlaneCylinder(const mjModel* m, const mjData* d, int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO mjGETINFO
mjtNum normal[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum normal[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]}; mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
@@ -193,8 +190,7 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
// plane : box // plane : box
int mjc_PlaneBox(const mjModel* m, const mjData* d, int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO mjGETINFO
// get normal, difference between centers, normal distance // get normal, difference between centers, normal distance
@@ -277,8 +273,7 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
// sphere : sphere // sphere : sphere
int mjc_SphereSphere(const mjModel* m, const mjData* d, int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO mjGETINFO
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
@@ -304,16 +299,15 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
// sphere : capsule // sphere : capsule
int mjc_SphereCapsule(const mjModel* m, const mjData* d, int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO mjGETINFO
return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
// sphere : cylinder // sphere : cylinder
int mjc_SphereCylinder(const mjModel* m, const mjData* d, int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin) { mjtNum margin) {
mjGETINFO mjGETINFO
// get cylinder sizes and axis // get cylinder sizes and axis
@@ -482,8 +476,8 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
// capsule : capsule // capsule : capsule
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d, int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin) { mjtNum margin) {
mjGETINFO mjGETINFO
return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
} }
src/engine/engine_collision_primitive.h
+22 -22
View File
@@ -55,32 +55,32 @@ int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
mjtNum rt); mjtNum rt);
// plane collisions // plane collisions
MJAPI int mjc_PlaneSphere (const mjModel* m, const mjData* d, MJAPI int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_PlaneCapsule (const mjModel* m, const mjData* d, MJAPI int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_PlaneCylinder (const mjModel* m, const mjData* d, MJAPI int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_PlaneBox (const mjModel* m, const mjData* d, MJAPI int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
// sphere and capsule collisions // sphere and capsule collisions
MJAPI int mjc_SphereSphere (const mjModel* m, const mjData* d, MJAPI int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_SphereCapsule (const mjModel* m, const mjData* d, MJAPI int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_SphereCylinder (const mjModel* m, const mjData* d, MJAPI int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_CapsuleCapsule (const mjModel* m, const mjData* d, MJAPI int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
// box collisions: from engine_collision_box.c // box collisions: from engine_collision_box.c
MJAPI int mjc_CapsuleBox (const mjModel* m, const mjData* d, MJAPI int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_SphereBox (const mjModel* m, const mjData* d, MJAPI int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
MJAPI int mjc_BoxBox (const mjModel* m, const mjData* d, MJAPI int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
mjContact* con, int g1, int g2, mjtNum margin); mjtNum margin);
#ifdef __cplusplus #ifdef __cplusplus
} }
src/engine/engine_collision_sdf.c
+21 -21
View File
@@ -545,7 +545,7 @@ static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
// 1 = normal points OUT of SDF (for flex-SDF where SDF is g1) // 1 = normal points OUT of SDF (for flex-SDF where SDF is g1)
static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist, const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
int cnt, const mjModel* m, const mjSDF* s, mjData* d, int cnt, const mjModel* m, const mjSDF* s, const mjData* d,
int flipNormal) { int flipNormal) {
// check if there is a collision // check if there is a collision
if (dist > 0 || isknown(points, x, cnt)) { if (dist > 0 || isknown(points, x, cnt)) {
@@ -584,7 +584,7 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
// finds minimum of Frank-Wolfe objective // finds minimum of Frank-Wolfe objective
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
const mjModel* m, const mjSDF* sdf, mjData* d) { const mjModel* m, const mjSDF* sdf, const mjData* d) {
for (int step=0; step < m->opt.sdf_iterations; step++) { for (int step=0; step < m->opt.sdf_iterations; step++) {
mjtNum best = mjMAXVAL, fun, s[3], grad[3]; mjtNum best = mjMAXVAL, fun, s[3], grad[3];
@@ -614,7 +614,7 @@ static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
// finds minimum using gradient descent // finds minimum using gradient descent
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s, static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
mjData* d, int niter) { const mjData* d, int niter) {
const mjtNum c = .1; // reduction factor for the target decrease in the objective function const mjtNum c = .1; // reduction factor for the target decrease in the objective function
const mjtNum rho = .5; // reduction factor for the gradient scaling (alpha) const mjtNum rho = .5; // reduction factor for the gradient scaling (alpha)
const mjtNum amin = 1e-4; // minimum value for alpha const mjtNum amin = 1e-4; // minimum value for alpha
@@ -664,7 +664,7 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
// stricter triangle collision // stricter triangle collision
static int triangleIntersect(const mjtNum triangle[9], const mjModel* m, static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
const mjSDF* sdf, mjData* d) { const mjSDF* sdf, const mjData* d) {
mjtNum edges[6]; mjtNum edges[6];
mjtNum normal[3], center[3]; mjtNum normal[3], center[3];
mjtNum v[9], cross[9], p[3]; mjtNum v[9], cross[9], p[3];
@@ -737,7 +737,7 @@ static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
// intersect with circumsphere of bounding box // intersect with circumsphere of bounding box
static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3], static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
const mjtNum rotation[9], const mjModel* m, const mjtNum rotation[9], const mjModel* m,
const mjSDF* s, mjData* d) { const mjSDF* s, const mjData* d) {
mjtNum candidate[3]; mjtNum candidate[3];
mjtNum r = mju_norm3(bvh+3); mjtNum r = mju_norm3(bvh+3);
@@ -806,7 +806,7 @@ static int selectFPS(const mjtNum* candidate, const mjtNum* dist, int ncandidate
// Process triangle corners against SDF using Halton sampling + Frank-Wolfe. // Process triangle corners against SDF using Halton sampling + Frank-Wolfe.
// Corners are assumed to already be in SDF local coordinates. // Corners are assumed to already be in SDF local coordinates.
// Adds penetrating candidates to candidate/dist arrays. // Adds penetrating candidates to candidate/dist arrays.
static void processSdfCorners(const mjtNum corners[9], const mjModel* m, mjData* d, static void processSdfCorners(const mjtNum corners[9], const mjModel* m, const mjData* d,
const mjSDF* sdf, int nstartpts, const mjSDF* sdf, int nstartpts,
mjtNum* candidate, mjtNum* dist, int* ncandidate) { mjtNum* candidate, mjtNum* dist, int* ncandidate) {
mjtNum x[3], depth; mjtNum x[3], depth;
@@ -851,7 +851,7 @@ static void processSdfCorners(const mjtNum corners[9], const mjModel* m, mjData*
// Context for inline face processing during BVH traversal // Context for inline face processing during BVH traversal
typedef struct { typedef struct {
const mjModel* m; const mjModel* m;
mjData* d; const mjData* d;
const mjSDF* sdf; const mjSDF* sdf;
const mjtNum* offset; const mjtNum* offset;
const mjtNum* rotation; const mjtNum* rotation;
@@ -903,7 +903,7 @@ typedef int (*BVHLeafCallback)(int leaf_id, int node, void* ctx);
// ctx: user context passed to callback // ctx: user context passed to callback
static void traverseBVH(const mjtNum* bvh, const int* nodeid, const int* child, static void traverseBVH(const mjtNum* bvh, const int* nodeid, const int* child,
mjtByte* bvh_active, const mjtNum* offset, const mjtNum* rotation, mjtByte* bvh_active, const mjtNum* offset, const mjtNum* rotation,
const mjModel* m, mjData* d, const mjSDF* sdf, const mjModel* m, const mjData* d, const mjSDF* sdf,
BVHLeafCallback callback, void* ctx) { BVHLeafCallback callback, void* ctx) {
int stack[64]; int stack[64];
int nstack = 0; int nstack = 0;
@@ -952,14 +952,14 @@ static int meshFaceCallback(int face_id, int node, void* ctx) {
//------------------------------ collision functions ----------------------------------------------- //------------------------------ collision functions -----------------------------------------------
// collision between a height field and a signed distance field // collision between a height field and a signed distance field
int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mju_warning("HField vs SDF collision not yet supported!"); mju_warning("HField vs SDF collision not yet supported!");
return 0; return 0;
} }
// collision between a mesh and a signed distance field // collision between a mesh and a signed distance field
int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO; mjGETINFO;
mjtNum offset[3], rotation[9]; mjtNum offset[3], rotation[9];
@@ -988,7 +988,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
// set up context for inline BVH processing // set up context for inline BVH processing
MeshSDFContext ctx; MeshSDFContext ctx;
ctx.m = m; ctx.m = m;
ctx.d = (mjData*)d; ctx.d = d;
ctx.sdf = &sdf; ctx.sdf = &sdf;
ctx.offset = offset; ctx.offset = offset;
ctx.rotation = rotation; ctx.rotation = rotation;
@@ -1008,14 +1008,14 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL; mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL;
traverseBVH(bvh, nodeid, child, bvh_active, ctx.offset, ctx.rotation, traverseBVH(bvh, nodeid, child, bvh_active, ctx.offset, ctx.rotation,
m, (mjData*)d, ctx.sdf, meshFaceCallback, &ctx); m, d, ctx.sdf, meshFaceCallback, &ctx);
} }
// if few candidates, add them all directly // if few candidates, add them all directly
if (ncandidate <= mjMAXCONPAIR) { if (ncandidate <= mjMAXCONPAIR) {
for (int i = 0; i < ncandidate; i++) { for (int i = 0; i < ncandidate; i++) {
cnt = addContact(points, con, candidate + 3*i, pos2, sdf_quat, cnt = addContact(points, con, candidate + 3*i, pos2, sdf_quat,
dist[i], cnt, m, &sdf, (mjData*)d, 0); dist[i], cnt, m, &sdf, d, 0);
} }
return cnt; return cnt;
} }
@@ -1028,14 +1028,14 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
for (int i = 0; i < nselected; i++) { for (int i = 0; i < nselected; i++) {
int idx = selected_indices[i]; int idx = selected_indices[i];
cnt = addContact(points, con, candidate + 3*idx, pos2, sdf_quat, cnt = addContact(points, con, candidate + 3*idx, pos2, sdf_quat,
dist[idx], cnt, m, &sdf, (mjData*)d, 0); dist[idx], cnt, m, &sdf, d, 0);
} }
return cnt; return cnt;
} }
// collision between two SDFs // collision between two SDFs
int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO; mjGETINFO;
size1 = m->geom_aabb + 6*g1; size1 = m->geom_aabb + 6*g1;
size2 = m->geom_aabb + 6*g2; size2 = m->geom_aabb + 6*g2;
@@ -1148,20 +1148,20 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
// start counters // start counters
if (sdf_ptr[0]) { if (sdf_ptr[0]) {
sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF); sdf_ptr[0]->compute(m, d, instance[0], mjPLUGIN_SDF);
} }
// gradient descent - we use a special function of the two SDF as objective // gradient descent - we use a special function of the two SDF as objective
sdf.type = mjSDFTYPE_COLLISION; sdf.type = mjSDFTYPE_COLLISION;
dist = stepGradient(x, m, &sdf, (mjData*)d, m->opt.sdf_iterations); dist = stepGradient(x, m, &sdf, d, m->opt.sdf_iterations);
// inexact SDFs can yield spurious collisions, filter them by projecting on the midsurface // inexact SDFs can yield spurious collisions, filter them by projecting on the midsurface
sdf.type = mjSDFTYPE_INTERSECTION; sdf.type = mjSDFTYPE_INTERSECTION;
dist = stepGradient(x, m, &sdf, (mjData*)d, 1); dist = stepGradient(x, m, &sdf, d, 1);
// contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
sdf.type = mjSDFTYPE_MIDSURFACE; sdf.type = mjSDFTYPE_MIDSURFACE;
cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, (mjData*)d, 0); cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, d, 0);
// SHOULD NOT OCCUR // SHOULD NOT OCCUR
if (cnt > mjMAXCONPAIR) { if (cnt > mjMAXCONPAIR) {
@@ -1175,7 +1175,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
// Context for flex element processing during BVH traversal // Context for flex element processing during BVH traversal
typedef struct { typedef struct {
const mjModel* m; const mjModel* m;
mjData* d; const mjData* d;
const mjSDF* sdf; const mjSDF* sdf;
const mjtNum* offset; const mjtNum* offset;
const mjtNum* rotation; const mjtNum* rotation;
@@ -1240,7 +1240,7 @@ static int flexElemCallback(int elem_idx, int node, void* ctx) {
} }
int mjc_FlexSDF(const mjModel* m, mjData* d, mjContact* con, int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
int g, int f, mjtNum margin) { int g, int f, mjtNum margin) {
// g = SDF geom, f = flex // g = SDF geom, f = flex
int dim = m->flex_dim[f]; int dim = m->flex_dim[f];
src/engine/engine_collision_sdf.h
+4 -4
View File
@@ -36,16 +36,16 @@ MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNu
const mjtNum x[3]); const mjtNum x[3]);
// collision between a height field and a signed distance field // collision between a height field and a signed distance field
int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
// collision between a mesh and a signed distance field // collision between a mesh and a signed distance field
int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
// collision between two signed distance fields // collision between two signed distance fields
int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
// collision between entire flex and SDF geom (batched processing) // collision between entire flex and SDF geom (batched processing)
int mjc_FlexSDF(const mjModel* m, mjData* d, mjContact* con, int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
int g, int f, mjtNum margin); int g, int f, mjtNum margin);
#ifdef __cplusplus #ifdef __cplusplus