Remove const qualifier on mjData in mjfCollision type as mjData is non-const in mjc_SDF via mjpPlugin.
PiperOrigin-RevId: 883225786 Change-Id: I5fecaa1d575b340a6510212db7b3e7db48f361cc
This commit is contained in:
Kyle Bayes
committed by
Copybara-Service
Copybara-Service
parent
7ec4fe2c50
commit
88ba8ffdbc
@@ -529,7 +529,7 @@ typedef mjtNum (*mjfTime)(void);
|
||||
typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id);
|
||||
|
||||
// collision detection
|
||||
typedef int (*mjfCollision)(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
typedef int (*mjfCollision)(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
|
||||
#endif // MUJOCO_MJDATA_H_
|
||||
|
||||
@@ -94,9 +94,7 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
|
||||
|
||||
|
||||
// sphere : box
|
||||
int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con,
|
||||
int g1, int g2, mjtNum margin)
|
||||
{
|
||||
int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO;
|
||||
|
||||
return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
|
||||
@@ -593,9 +591,7 @@ skip:
|
||||
|
||||
|
||||
// capsule : box
|
||||
int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
|
||||
int g1, int g2, mjtNum margin)
|
||||
{
|
||||
int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
|
||||
}
|
||||
@@ -1342,7 +1338,7 @@ edgeedge:
|
||||
|
||||
|
||||
// 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);
|
||||
|
||||
// use dim field to mark: -1: bad, 0: good
|
||||
|
||||
@@ -868,8 +868,7 @@ static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) {
|
||||
|
||||
|
||||
// multi-point convex-convex collision, using libccd
|
||||
int mjc_Convex(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
// init ccd objects
|
||||
mjCCDObj obj1, obj2;
|
||||
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
|
||||
int mjc_PlaneConvex(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
|
||||
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
|
||||
int mjc_ConvexHField(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
// hfield frame
|
||||
const mjtNum* pos1 = d->geom_xpos + 3*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 ---------------------------------------------
|
||||
|
||||
// geom-elem or elem-elem or vert-elem convex collision using ccd
|
||||
int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
|
||||
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
|
||||
int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1,
|
||||
int f2, int e2, mjtNum margin) {
|
||||
mjCCDObj obj1, obj2;
|
||||
mjc_initCCDObj(&obj1, m, d, g1, 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
|
||||
int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
|
||||
int g, int f, int e, mjtNum margin) {
|
||||
int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e,
|
||||
mjtNum margin) {
|
||||
mjtNum vec[3], dx, dy;
|
||||
mjtNum xmin, xmax, ymin, ymax, zmin, zmax;
|
||||
int dr[2], cnt, rmin, rmax, cmin, cmax;
|
||||
|
||||
@@ -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]);
|
||||
|
||||
// pairwise geom collision functions using ccd
|
||||
int mjc_PlaneConvex(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
int mjc_ConvexHField(const mjModel* m, const mjData* d,
|
||||
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);
|
||||
int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
|
||||
int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
|
||||
|
||||
// geom-elem or elem-elem or vert-elem collision function using ccd
|
||||
int mjc_ConvexElem (const mjModel* m, const mjData* d, mjContact* con,
|
||||
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin);
|
||||
int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1,
|
||||
int f2, int e2, mjtNum margin);
|
||||
|
||||
// heightfield-elem collision function using ccd
|
||||
int mjc_HFieldElem (const mjModel* m, const mjData* d, mjContact* con,
|
||||
int g, int f, int e, mjtNum margin);
|
||||
int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e,
|
||||
mjtNum margin);
|
||||
|
||||
// fix contact frame normal
|
||||
void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2);
|
||||
|
||||
@@ -51,16 +51,14 @@ static int mjraw_PlaneSphere(mjContact* con, mjtNum margin,
|
||||
|
||||
|
||||
// plane : sphere
|
||||
int mjc_PlaneSphere(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
|
||||
}
|
||||
|
||||
|
||||
// plane : capsule
|
||||
int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
|
||||
// get capsule axis, segment = scaled axis
|
||||
@@ -89,8 +87,7 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
|
||||
|
||||
|
||||
// plane : cylinder
|
||||
int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
mjtNum normal[3] = {mat1[2], mat1[5], mat1[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
|
||||
int mjc_PlaneBox(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
|
||||
// get normal, difference between centers, normal distance
|
||||
@@ -277,8 +273,7 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
|
||||
|
||||
|
||||
// sphere : sphere
|
||||
int mjc_SphereSphere(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
|
||||
}
|
||||
@@ -304,16 +299,15 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
|
||||
|
||||
|
||||
// sphere : capsule
|
||||
int mjc_SphereCapsule(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO
|
||||
return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
|
||||
}
|
||||
|
||||
|
||||
// sphere : cylinder
|
||||
int mjc_SphereCylinder(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin) {
|
||||
mjGETINFO
|
||||
|
||||
// get cylinder sizes and axis
|
||||
@@ -482,8 +476,8 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
|
||||
|
||||
|
||||
// capsule : capsule
|
||||
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin) {
|
||||
mjGETINFO
|
||||
return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
|
||||
}
|
||||
|
||||
@@ -55,32 +55,32 @@ int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
|
||||
mjtNum rt);
|
||||
|
||||
// plane collisions
|
||||
MJAPI int mjc_PlaneSphere (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_PlaneCapsule (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_PlaneCylinder (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_PlaneBox (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
|
||||
// sphere and capsule collisions
|
||||
MJAPI int mjc_SphereSphere (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_SphereCapsule (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_SphereCylinder (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_CapsuleCapsule (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
|
||||
// box collisions: from engine_collision_box.c
|
||||
MJAPI int mjc_CapsuleBox (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_SphereBox (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_BoxBox (const mjModel* m, const mjData* d,
|
||||
mjContact* con, int g1, int g2, mjtNum margin);
|
||||
MJAPI int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
MJAPI int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
|
||||
mjtNum margin);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
|
||||
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) {
|
||||
// check if there is a collision
|
||||
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
|
||||
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++) {
|
||||
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
|
||||
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 rho = .5; // reduction factor for the gradient scaling (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
|
||||
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 normal[3], center[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
|
||||
static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
|
||||
const mjtNum rotation[9], const mjModel* m,
|
||||
const mjSDF* s, mjData* d) {
|
||||
const mjSDF* s, const mjData* d) {
|
||||
mjtNum candidate[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.
|
||||
// Corners are assumed to already be in SDF local coordinates.
|
||||
// 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,
|
||||
mjtNum* candidate, mjtNum* dist, int* ncandidate) {
|
||||
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
|
||||
typedef struct {
|
||||
const mjModel* m;
|
||||
mjData* d;
|
||||
const mjData* d;
|
||||
const mjSDF* sdf;
|
||||
const mjtNum* offset;
|
||||
const mjtNum* rotation;
|
||||
@@ -903,7 +903,7 @@ typedef int (*BVHLeafCallback)(int leaf_id, int node, void* ctx);
|
||||
// ctx: user context passed to callback
|
||||
static void traverseBVH(const mjtNum* bvh, const int* nodeid, const int* child,
|
||||
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) {
|
||||
int stack[64];
|
||||
int nstack = 0;
|
||||
@@ -952,14 +952,14 @@ static int meshFaceCallback(int face_id, int node, void* ctx) {
|
||||
//------------------------------ collision functions -----------------------------------------------
|
||||
|
||||
// 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!");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
|
||||
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
|
||||
MeshSDFContext ctx;
|
||||
ctx.m = m;
|
||||
ctx.d = (mjData*)d;
|
||||
ctx.d = d;
|
||||
ctx.sdf = &sdf;
|
||||
ctx.offset = offset;
|
||||
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;
|
||||
|
||||
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 (ncandidate <= mjMAXCONPAIR) {
|
||||
for (int i = 0; i < ncandidate; i++) {
|
||||
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;
|
||||
}
|
||||
@@ -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++) {
|
||||
int idx = selected_indices[i];
|
||||
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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
size1 = m->geom_aabb + 6*g1;
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
typedef struct {
|
||||
const mjModel* m;
|
||||
mjData* d;
|
||||
const mjData* d;
|
||||
const mjSDF* sdf;
|
||||
const mjtNum* offset;
|
||||
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) {
|
||||
// g = SDF geom, f = flex
|
||||
int dim = m->flex_dim[f];
|
||||
|
||||
@@ -36,16 +36,16 @@ MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNu
|
||||
const mjtNum x[3]);
|
||||
|
||||
// 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
|
||||
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
|
||||
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)
|
||||
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);
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
Reference in New Issue
Block a user