diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index 036785af..5167baa2 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -37,8 +37,8 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) static int _SphereBox(mjContact* con, mjtNum mindist, - mjtNum* pos1, mjtNum* mat1, mjtNum* size1, - mjtNum* pos2, mjtNum* mat2, mjtNum* size2) + const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, + const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { int i, k; mjtNum tmp[3], center[3], clamped[3], deepest[3], nearest[3]; diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 2f8472f1..5b3a7e2b 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -578,7 +578,7 @@ static void addVert(int* nvert, mjtPrism* prism, mjtNum x, mjtNum y, mjtNum z) { // entry point for heightfield collisions int mjc_ConvexHField(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO + mjGETINFO_HFIELD mjtNum mat[9], savemat2[9], savepos2[3], pos[3], vec[3], r2, dx, dy; mjtNum xmin, xmax, ymin, ymax, zmin, zmax; int hid = m->geom_dataid[g1]; diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index f4dde4d2..6eed5f3d 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -26,6 +26,14 @@ #include #include +#define mjGETINFO_HFIELD \ + const mjtNum* pos1 = d->geom_xpos + 3*g1; \ + const mjtNum* mat1 = d->geom_xmat + 9*g1; \ + const mjtNum* size1 = m->geom_size + 3*g1; \ + mjtNum* pos2 = d->geom_xpos + 3*g2; \ + mjtNum* mat2 = d->geom_xmat + 9*g2; +// mjc_ConvexHField modifies and then restores pos2 and mat2 + #ifdef __cplusplus extern "C" { #endif diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index ba077e35..a56be368 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -353,12 +353,12 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d, // cap collision: use plane-sphere if (collide_cap) { - mjtNum flipmat[9] = { + const mjtNum flipmat[9] = { -mat2[0], mat2[1], -mat2[2], -mat2[3], mat2[4], -mat2[5], -mat2[6], mat2[7], -mat2[8] }; - mjtNum* mat_cap; + const mjtNum* mat_cap; mjtNum pos_cap[3]; if (x > 0) { // top cap mju_addScl3(pos_cap, pos2, axis, height); diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h index 5eb3d5ba..d0089ae1 100644 --- a/src/engine/engine_collision_primitive.h +++ b/src/engine/engine_collision_primitive.h @@ -20,13 +20,13 @@ // define and extract geom info #define mjGETINFO \ - mjtNum* pos1 = d->geom_xpos + 3*g1; \ - mjtNum* mat1 = d->geom_xmat + 9*g1; \ - mjtNum* size1= m->geom_size + 3*g1; \ - mjtNum* pos2 = d->geom_xpos + 3*g2; \ - mjtNum* mat2 = d->geom_xmat + 9*g2; \ - mjtNum* size2= m->geom_size + 3*g2; \ - (void) size1; (void) size2; + const mjtNum* pos1 = d->geom_xpos + 3*g1; \ + const mjtNum* mat1 = d->geom_xmat + 9*g1; \ + const mjtNum* size1 = m->geom_size + 3*g1; \ + const mjtNum* pos2 = d->geom_xpos + 3*g2; \ + const mjtNum* mat2 = d->geom_xmat + 9*g2; \ + const mjtNum* size2 = m->geom_size + 3*g2; \ + (void) size1; (void) size2; // size1 and size2 are sometimes unused #ifdef __cplusplus extern "C" {