Add Signed Distance Field to collision geometries.
PiperOrigin-RevId: 557507088 Change-Id: I358a642407aee1ba8dfc9d405eb9a4f609435fe9
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
6245edae28
commit
fdb041580c
@@ -23,6 +23,8 @@ set(MUJOCO_ENGINE_SRCS
|
||||
engine_collision_driver.h
|
||||
engine_collision_primitive.c
|
||||
engine_collision_primitive.h
|
||||
engine_collision_sdf.c
|
||||
engine_collision_sdf.h
|
||||
engine_core_constraint.c
|
||||
engine_core_constraint.h
|
||||
engine_core_smooth.c
|
||||
|
||||
@@ -23,6 +23,7 @@
|
||||
#include "engine/engine_callback.h"
|
||||
#include "engine/engine_collision_convex.h"
|
||||
#include "engine/engine_collision_primitive.h"
|
||||
#include "engine/engine_collision_sdf.h"
|
||||
#include "engine/engine_core_constraint.h"
|
||||
#include "engine/engine_crossplatform.h"
|
||||
#include "engine/engine_io.h"
|
||||
@@ -34,15 +35,16 @@
|
||||
|
||||
// table of pair-wise collision functions
|
||||
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
|
||||
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH */
|
||||
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex},
|
||||
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField},
|
||||
/*SHPERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex},
|
||||
/*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex},
|
||||
/*ELLIPSOID */ {0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_Convex},
|
||||
/*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex},
|
||||
/*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex},
|
||||
/*MESH */ {0, 0, 0, 0, 0, 0, 0, mjc_Convex}
|
||||
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */
|
||||
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_SDF},
|
||||
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_HFieldSDF},
|
||||
/*SPHERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex, mjc_SDF},
|
||||
/*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex, mjc_SDF},
|
||||
/*ELLIPSOID */ {0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_Convex, mjc_SDF},
|
||||
/*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_SDF},
|
||||
/*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex, mjc_SDF},
|
||||
/*MESH */ {0, 0, 0, 0, 0, 0, 0, mjc_Convex, mjc_MeshSDF},
|
||||
/*SDF */ {0, 0, 0, 0, 0, 0, 0, 0, mjc_SDF}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,695 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "engine/engine_collision_sdf.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include "engine/engine_collision_primitive.h"
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
#include "engine/engine_util_spatial.h"
|
||||
|
||||
|
||||
#define MAXSDFFACE 1300
|
||||
|
||||
|
||||
|
||||
//---------------------------- primitives sdf ---------------------------------------------
|
||||
|
||||
static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p,
|
||||
int i, const mjtNum x[3], mjtGeom type) {
|
||||
mjtNum a[3], b[3];
|
||||
const mjtNum* size = m->geom_size+3*i;
|
||||
|
||||
// see https://iquilezles.org/articles/distfunctions/
|
||||
switch (type) {
|
||||
case mjGEOM_PLANE:
|
||||
return x[2];
|
||||
case mjGEOM_SPHERE:
|
||||
return mju_norm3(x) - size[0];
|
||||
case mjGEOM_BOX:
|
||||
a[0] = mju_abs(x[0]) - size[0];
|
||||
a[1] = mju_abs(x[1]) - size[1];
|
||||
a[2] = mju_abs(x[2]) - size[2];
|
||||
b[0] = mju_max(a[0], 0);
|
||||
b[1] = mju_max(a[1], 0);
|
||||
b[2] = mju_max(a[2], 0);
|
||||
return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0);
|
||||
case mjGEOM_CAPSULE:
|
||||
a[0] = x[0];
|
||||
a[1] = x[1];
|
||||
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
|
||||
return mju_norm3(a) - size[0];
|
||||
case mjGEOM_ELLIPSOID:
|
||||
a[0] = x[0] / size[0];
|
||||
a[1] = x[1] / size[1];
|
||||
a[2] = x[2] / size[2];
|
||||
b[0] = a[0] / size[0];
|
||||
b[1] = a[1] / size[1];
|
||||
b[2] = a[2] / size[2];
|
||||
mjtNum k0 = mju_norm3(a);
|
||||
mjtNum k1 = mju_norm3(b);
|
||||
return k0 * (k0 - 1.0) / k1;
|
||||
case mjGEOM_CYLINDER:
|
||||
a[0] = mju_sqrt(x[0]*x[0]+x[1]*x[1]) - size[0];
|
||||
a[1] = mju_abs(x[2]) - size[1];
|
||||
b[0] = mju_max(a[0], 0);
|
||||
b[1] = mju_max(a[1], 0);
|
||||
return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2);
|
||||
case mjGEOM_SDF:
|
||||
return p->sdf_distance(x, d, i);
|
||||
default:
|
||||
mju_error("sdf collisions not available for geom type %d", type);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
|
||||
const mjpPlugin* p, int i, const mjtNum x[3],
|
||||
mjtGeom type) {
|
||||
mjtNum a[3], b[3], c, e;
|
||||
const mjtNum* size = m->geom_size+3*i;
|
||||
|
||||
// see https://iquilezles.org/articles/distfunctions/
|
||||
switch (type) {
|
||||
case mjGEOM_PLANE:
|
||||
mju_zero3(gradient);
|
||||
gradient[2] = 1;
|
||||
break;
|
||||
case mjGEOM_SPHERE:
|
||||
mju_copy3(gradient, x);
|
||||
c = mju_norm3(x);
|
||||
gradient[0] *= 1. / c;
|
||||
gradient[1] *= 1. / c;
|
||||
gradient[2] *= 1. / c;
|
||||
break;
|
||||
case mjGEOM_BOX:
|
||||
mju_zero3(gradient);
|
||||
a[0] = mju_abs(x[0]) - size[0];
|
||||
a[1] = mju_abs(x[1]) - size[1];
|
||||
a[2] = mju_abs(x[2]) - size[2];
|
||||
int k = a[0] > a[1] ? 0 : 1;
|
||||
int l = a[2] > a[k] ? 2 : k;
|
||||
if (a[l]<0) {
|
||||
gradient[l] = x[l] / mju_abs(x[l]);
|
||||
} else {
|
||||
b[0] = mju_max(a[0], 0);
|
||||
b[1] = mju_max(a[1], 0);
|
||||
b[2] = mju_max(a[2], 0);
|
||||
c = mju_norm3(b);
|
||||
gradient[0] = a[0]>0 ? b[0] / c * x[0] / mju_abs(x[0]) : 0;
|
||||
gradient[1] = a[1]>0 ? b[1] / c * x[1] / mju_abs(x[1]) : 0;
|
||||
gradient[2] = a[2]>0 ? b[2] / c * x[2] / mju_abs(x[2]) : 0;
|
||||
}
|
||||
break;
|
||||
case mjGEOM_CAPSULE:
|
||||
a[0] = x[0];
|
||||
a[1] = x[1];
|
||||
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
|
||||
c = mju_norm3(a);
|
||||
gradient[0] = a[0] / c;
|
||||
gradient[1] = a[1] / c;
|
||||
gradient[2] = a[2] / c;
|
||||
break;
|
||||
case mjGEOM_ELLIPSOID:
|
||||
a[0] = x[0] / size[0];
|
||||
a[1] = x[1] / size[1];
|
||||
a[2] = x[2] / size[2];
|
||||
b[0] = a[0] / size[0];
|
||||
b[1] = a[1] / size[1];
|
||||
b[2] = a[2] / size[2];
|
||||
mjtNum k0 = mju_norm3(a);
|
||||
mjtNum k1 = mju_norm3(b);
|
||||
gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1);
|
||||
gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1);
|
||||
gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1);
|
||||
break;
|
||||
case mjGEOM_CYLINDER:
|
||||
c = mju_sqrt(x[0]*x[0]+x[1]*x[1]);
|
||||
e = mju_abs(x[2]);
|
||||
a[0] = c - size[0];
|
||||
a[1] = e - size[1];
|
||||
mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e};
|
||||
int j = a[0] > a[1] ? 0 : 1;
|
||||
if (a[j] < 0) {
|
||||
gradient[0] = j==0 ? grada[0] : 0;
|
||||
gradient[1] = j==0 ? grada[1] : 0;
|
||||
gradient[2] = j==1 ? grada[2] : 0;
|
||||
} else {
|
||||
b[0] = mju_max(a[0], 0);
|
||||
b[1] = mju_max(a[1], 0);
|
||||
mjtNum bnorm = mju_norm(b, 2);
|
||||
gradient[0] = grada[0] * b[0] / bnorm;
|
||||
gradient[1] = grada[1] * b[0] / bnorm;
|
||||
gradient[2] = grada[2] * b[1] / bnorm;
|
||||
}
|
||||
break;
|
||||
case mjGEOM_SDF:
|
||||
p->sdf_gradient(gradient, x, d, i);
|
||||
break;
|
||||
default:
|
||||
mju_error("sdf collisions not available for geom type %d", type);
|
||||
}
|
||||
}
|
||||
|
||||
//---------------------------- helper functions -------------------------------------------
|
||||
|
||||
// signed distance function
|
||||
mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]) {
|
||||
mjtNum y[3];
|
||||
|
||||
switch (s->type) {
|
||||
case mjSDFTYPE_SINGLE:
|
||||
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
|
||||
case mjSDFTYPE_INTERSECTION:
|
||||
mju_rotVecMat(y, x, s->relmat);
|
||||
mju_addTo3(y, s->relpos);
|
||||
return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]),
|
||||
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]));
|
||||
default:
|
||||
mju_error("SDF type not available");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// gradient of sdf
|
||||
void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
|
||||
mjtNum gradient[3], const mjtNum x[3]) {
|
||||
mjtNum y[3];
|
||||
const mjtNum* point[2] = {x, y};
|
||||
|
||||
switch (s->type) {
|
||||
case mjSDFTYPE_INTERSECTION:
|
||||
mju_rotVecMat(y, x, s->relmat);
|
||||
mju_addTo3(y, s->relpos);
|
||||
int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) >
|
||||
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1;
|
||||
geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]);
|
||||
if (i==1) {
|
||||
mju_rotVecMatT(gradient, gradient, s->relmat);
|
||||
}
|
||||
break;
|
||||
case mjSDFTYPE_AVERAGE:
|
||||
mju_rotVecMat(y, x, s->relmat);
|
||||
mju_addTo3(y, s->relpos);
|
||||
mjtNum grad1[3], grad2[3];
|
||||
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
|
||||
mju_normalize3(grad1);
|
||||
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
|
||||
mju_rotVecMatT(grad2, grad2, s->relmat);
|
||||
mju_normalize3(grad2);
|
||||
mju_sub3(gradient, grad1, grad2);
|
||||
mju_normalize3(gradient);
|
||||
break;
|
||||
case mjSDFTYPE_SINGLE:
|
||||
geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]);
|
||||
break;
|
||||
default:
|
||||
mju_error("SDF type not available");
|
||||
}
|
||||
}
|
||||
|
||||
// get sdf from geom id
|
||||
static const mjpPlugin* getSDF(const mjModel* m, int id) {
|
||||
int instance = m->geom_plugin[id];
|
||||
const int nslot = mjp_pluginCount();
|
||||
const int slot = m->plugin[instance];
|
||||
const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot);
|
||||
if (!sdf) mju_error("invalid plugin slot: %d", slot);
|
||||
if (!(sdf->capabilityflags & mjPLUGIN_SDF)) {
|
||||
mju_error("Plugin is not a signed distance field at slot %d", slot);
|
||||
}
|
||||
return sdf;
|
||||
}
|
||||
|
||||
// map (pos12, mat12) as (xpos2, xmat2)^-1 \circ (xpos1, xmat1)
|
||||
static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4],
|
||||
const mjtNum xpos2[3], const mjtNum xquat2[4],
|
||||
mjtNum pos12[3], mjtNum mat12[9]) {
|
||||
mjtNum negpos[3], negquat[4], quat12[4];
|
||||
mju_negPose(negpos, negquat, xpos2, xquat2);
|
||||
mju_mulPose(pos12, quat12, negpos, negquat, xpos1, xquat1);
|
||||
mju_quat2Mat(mat12, quat12);
|
||||
}
|
||||
|
||||
// subtract mesh position from sdf transformation
|
||||
static void undoTransformation(const mjModel* m, const mjData* d, int g,
|
||||
mjtNum sdf_xpos[3], mjtNum sdf_quat[4]) {
|
||||
mjtNum* xpos = d->geom_xpos + 3 * g;
|
||||
mjtNum* xmat = d->geom_xmat + 9 * g;
|
||||
if (m->geom_type[g]==mjGEOM_MESH || m->geom_type[g]==mjGEOM_SDF) {
|
||||
mjtNum negpos[3], negquat[4], xquat[4];
|
||||
mjtNum* pos = m->geom_pos + 3 * g;
|
||||
mjtNum* quat = m->geom_quat + 4 * g;
|
||||
mju_mat2Quat(xquat, xmat);
|
||||
mju_negPose(negpos, negquat, pos, quat);
|
||||
mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);
|
||||
} else {
|
||||
mju_copy3(sdf_xpos, xpos);
|
||||
mju_mat2Quat(sdf_quat, xmat);
|
||||
}
|
||||
}
|
||||
|
||||
//---------------------------- narrow phase -----------------------------------------------
|
||||
|
||||
// check if the collision point already exists
|
||||
static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
|
||||
for (int i = 0; i < cnt; i++) {
|
||||
if (mju_dist3(x, points + 3*i) < mjMINVAL) {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// adds candidate point to result
|
||||
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) {
|
||||
// check if there is a collision
|
||||
if (dist > 0 || isknown(points, x, cnt)) {
|
||||
return cnt;
|
||||
} else {
|
||||
mju_copy3(points+3*cnt, x);
|
||||
}
|
||||
|
||||
// compute normal in local coordinates
|
||||
mjtNum norm[3], vec[3];
|
||||
mjc_gradient(m, d, s, norm, x);
|
||||
mju_scl3(norm, norm, -1);
|
||||
|
||||
// construct contact
|
||||
con[cnt].dist = dist;
|
||||
mju_rotVecQuat(con[cnt].frame, norm, quat2);
|
||||
mju_zero3(con[cnt].frame+3);
|
||||
mju_makeFrame(con[cnt].frame);
|
||||
mju_scl3(vec, con[cnt].frame, -con[cnt].dist/2);
|
||||
mju_rotVecQuat(con[cnt].pos, x, quat2);
|
||||
mju_addTo3(con[cnt].pos, pos2);
|
||||
mju_addTo3(con[cnt].pos, vec);
|
||||
|
||||
return cnt+1;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
for (int step=0; step<m->opt.sdf_iterations; step++) {
|
||||
mjtNum best = 1e10, fun, s[3], grad[3];
|
||||
|
||||
// evaluate gradient
|
||||
mjc_gradient(m, d, sdf, grad, x);
|
||||
|
||||
// evaluate all corners
|
||||
for (int i=0; i<ncorners; i++) {
|
||||
// compute sdf
|
||||
fun = mju_dot3(corners + 3*i, grad);
|
||||
|
||||
// save argmin
|
||||
if (fun<best) {
|
||||
best = fun;
|
||||
mju_copy3(s, corners + 3*i);
|
||||
}
|
||||
}
|
||||
|
||||
// update collision point
|
||||
mju_subFrom3(s, x);
|
||||
mju_addToScl3(x, s, 2. / (step+2.));
|
||||
}
|
||||
|
||||
// compute distance
|
||||
return mjc_distance(m, d, sdf, x);
|
||||
}
|
||||
|
||||
// finds minimum using gradient descent
|
||||
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
|
||||
mjData* d) {
|
||||
mjtNum alpha = 0.2; // step along the gradient direction
|
||||
|
||||
for (int step=0; step<m->opt.sdf_iterations; step++) {
|
||||
mjtNum grad[3];
|
||||
|
||||
// evaluate gradient
|
||||
mjc_gradient(m, d, s, grad, x);
|
||||
|
||||
// sanity check
|
||||
if (isnan(grad[0]) || isnan(grad[1]) || isnan(grad[2])) {
|
||||
return mjMAXVAL;
|
||||
}
|
||||
|
||||
// update solution
|
||||
mju_addToScl3(x, grad, -alpha/(mjtNum)(step+1));
|
||||
}
|
||||
|
||||
// compute distance
|
||||
return mjc_distance(m, d, s, x);
|
||||
}
|
||||
|
||||
//---------------------------- bounding box vs sdf -------------------------------------------------
|
||||
|
||||
// stricter triangle collision
|
||||
static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
|
||||
const mjSDF* sdf, mjData* d) {
|
||||
mjtNum edges[6];
|
||||
mjtNum normal[3], center[3];
|
||||
mjtNum v[9], cross[9], p[3];
|
||||
mjtNum kDistanceScl = 10.;
|
||||
|
||||
// triangle normal
|
||||
mju_sub3(edges+0, triangle+3, triangle);
|
||||
mju_sub3(edges+3, triangle+6, triangle);
|
||||
mju_cross(normal, edges, edges+3);
|
||||
mju_normalize3(normal);
|
||||
|
||||
// fourth point
|
||||
mju_scl3(p, triangle, 1./3.);
|
||||
mju_addToScl3(p, triangle+3, 1./3.);
|
||||
mju_addToScl3(p, triangle+6, 1./3.);
|
||||
mjtNum h = -mjc_distance(m, d, sdf, p)/kDistanceScl;
|
||||
mju_addToScl3(p, normal, -h);
|
||||
|
||||
// circumsphere center
|
||||
mju_sub3(v+0, triangle+0, p);
|
||||
mju_sub3(v+3, triangle+3, p);
|
||||
mju_sub3(v+6, triangle+6, p);
|
||||
mju_cross(cross+0, v+3, v+6);
|
||||
mju_cross(cross+3, v+6, v+0);
|
||||
mju_cross(cross+6, v+0, v+3);
|
||||
mju_scl3(center, cross, mju_dot3(v, v));
|
||||
mju_addToScl3(center, cross+3, mju_dot3(v+3, v+3));
|
||||
mju_addToScl3(center, cross+6, mju_dot3(v+6, v+6));
|
||||
mju_scl3(center, center, 1./(2.*mju_dot3(v, cross)));
|
||||
|
||||
// circumsphere radius
|
||||
mjtNum r = mju_sqrt(mju_dot3(center, center));
|
||||
|
||||
// coordinate change
|
||||
mju_addTo3(center, p);
|
||||
|
||||
return mjc_distance(m, d, sdf, center)<r;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
mjtNum candidate[3];
|
||||
mjtNum r = mju_norm3(bvh+3);
|
||||
|
||||
mju_rotVecMat(candidate, bvh, rotation);
|
||||
mju_addTo3(candidate, offset);
|
||||
|
||||
// check if inside the bounding box
|
||||
return mjc_distance(m, d, s, candidate)<r;
|
||||
}
|
||||
|
||||
//---------------------------- mesh vs sdf broad phase --------------------------------------------
|
||||
|
||||
// tree vs sdf binary search
|
||||
static void collideBVH(const mjModel* m, mjData* d, int g,
|
||||
const mjtNum offset[3], const mjtNum rotation[9],
|
||||
int* faces, int* npoints, int* n0,
|
||||
const mjSDF* sdf) {
|
||||
const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]];
|
||||
const int* faceid = m->bvh_geomid + bvhadr;
|
||||
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
|
||||
const int* child = m->bvh_child + 2*bvhadr;
|
||||
mjtByte* visited = d->bvh_active + bvhadr;
|
||||
|
||||
mjMARKSTACK;
|
||||
// TODO(quaglino): Store bvh max depths to make this bound tighter.
|
||||
int max_stack = m->mesh_bvhnum[m->geom_dataid[g]];
|
||||
struct CollideTreeArgs_ {
|
||||
int node;
|
||||
};
|
||||
typedef struct CollideTreeArgs_ CollideTreeArgs;
|
||||
CollideTreeArgs* stack = (CollideTreeArgs*)mj_stackAlloc(
|
||||
d, max_stack * sizeof(CollideTreeArgs*) / sizeof(mjtNum));
|
||||
|
||||
int nstack = 0;
|
||||
stack[nstack].node = 0;
|
||||
nstack++;
|
||||
|
||||
while (nstack) {
|
||||
(*n0)++;
|
||||
|
||||
// pop from stack
|
||||
nstack--;
|
||||
int node = stack[nstack].node;
|
||||
|
||||
// node1 is a leaf
|
||||
if (faceid[node] != -1) {
|
||||
if (visited[node]) {
|
||||
continue;
|
||||
}
|
||||
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
||||
faces[*npoints] = faceid[node];
|
||||
if (++(*npoints)==MAXSDFFACE) {
|
||||
mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed");
|
||||
mjFREESTACK;
|
||||
return;
|
||||
}
|
||||
visited[node] = 1;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// if no intersection at intermediate levels, stop
|
||||
if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
visited[node] = 1;
|
||||
|
||||
// recursive call
|
||||
for (int i=0; i<2; i++) {
|
||||
if (child[2*node+i] != -1) {
|
||||
if (nstack >= max_stack) mju_error("BVH stack depth exceeded.");
|
||||
stack[nstack].node = child[2*node+i];
|
||||
nstack++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mjFREESTACK;
|
||||
}
|
||||
|
||||
//------------------------------ 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) {
|
||||
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) {
|
||||
mjtNum* pos1 = d->geom_xpos + 3 * g1;
|
||||
mjtNum* mat1 = d->geom_xmat + 9 * g1;
|
||||
|
||||
mjtNum offset[3], rotation[9];
|
||||
mjtNum corners[9], points[3*MAXSDFFACE], x[3], dist;
|
||||
int vertadr = m->mesh_vertadr[m->geom_dataid[g1]];
|
||||
int faceadr = m->mesh_faceadr[m->geom_dataid[g1]];
|
||||
int cnt=0, npoints=0, n0=0, faces[MAXSDFFACE]={-1};
|
||||
|
||||
// get sdf plugin
|
||||
int instance = m->geom_plugin[g2];
|
||||
const mjpPlugin* sdf_ptr = getSDF(m, g2);
|
||||
mjtGeom geomtype = mjGEOM_SDF;
|
||||
|
||||
// copy into data
|
||||
mjSDF sdf;
|
||||
sdf.id = &instance;
|
||||
sdf.type = mjSDFTYPE_SINGLE;
|
||||
sdf.plugin = &sdf_ptr;
|
||||
sdf.geomtype = &geomtype;
|
||||
|
||||
// compute transformation from g1 to g2
|
||||
mjtNum pos2true[3], sdf_quat[4], quat1[4];
|
||||
mju_mat2Quat(quat1, mat1);
|
||||
undoTransformation(m, d, g2, pos2true, sdf_quat);
|
||||
mapPose(pos1, quat1, pos2true, sdf_quat, offset, rotation);
|
||||
|
||||
// binary tree search
|
||||
collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf);
|
||||
|
||||
// Frank-Wolfe algorithm
|
||||
for (int i=0; i<npoints; i++) {
|
||||
int face = faceadr + faces[i];
|
||||
for (int v=0; v<3; v++) {
|
||||
mjtNum vec[3] = {
|
||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0],
|
||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1],
|
||||
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+2],
|
||||
};
|
||||
|
||||
// transform local 1 (mesh) to local 2 (sdf)
|
||||
mju_rotVecMat(corners+3*v, vec, rotation);
|
||||
mju_addTo3(corners+3*v, offset);
|
||||
}
|
||||
|
||||
// stricter culling
|
||||
if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// starting point
|
||||
x[0] = (corners[0]+corners[3]+corners[6])/3;
|
||||
x[1] = (corners[1]+corners[4]+corners[7])/3;
|
||||
x[2] = (corners[2]+corners[5]+corners[8])/3;
|
||||
|
||||
// SHOULD NOT OCCUR
|
||||
if (cnt==mjMAXCONPAIR) mju_error("mjc_MeshSDF: too many contact points");
|
||||
|
||||
// Frank-Wolfe
|
||||
dist = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d);
|
||||
cnt = addContact(points, con, x, pos2true, sdf_quat, dist, cnt, m, &sdf, (mjData*)d);
|
||||
}
|
||||
|
||||
return cnt;
|
||||
}
|
||||
|
||||
// collision between two SDFs
|
||||
int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
|
||||
mjGETINFO;
|
||||
size1 = m->geom_aabb + 6*g1;
|
||||
size2 = m->geom_aabb + 6*g2;
|
||||
|
||||
int cnt = 0;
|
||||
mjtNum x[3], y[3], dist, vec1[3], vec2[3];
|
||||
mjtNum aabb1[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
|
||||
mjtNum aabb2[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
|
||||
mjtNum aabb[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
|
||||
|
||||
// second geom must be an SDF
|
||||
if (m->geom_type[g2] != mjGEOM_SDF) {
|
||||
mju_error("geom is not an SDF");
|
||||
}
|
||||
|
||||
// compute transformations from/to g1 to/from g2
|
||||
mjtNum quat1[4], quat2[4];
|
||||
mjtNum pos1true[3], offset21[3], rotation21[9], rotation12[9];
|
||||
mjtNum pos2true[3], offset1[3], rotation1[9], offset12[3];
|
||||
mjtNum offset2[3], rotation2[9], squat1[4], squat2[4];
|
||||
undoTransformation(m, d, g1, pos1true, squat1);
|
||||
undoTransformation(m, d, g2, pos2true, squat2);
|
||||
mju_mat2Quat(quat1, mat1);
|
||||
mju_mat2Quat(quat2, mat2);
|
||||
mapPose(pos2, quat2, pos1, quat1, offset1, rotation1);
|
||||
mapPose(pos1, quat1, pos1true, squat1, offset2, rotation2);
|
||||
mapPose(pos2true, squat2, pos1true, squat1, offset21, rotation21);
|
||||
mapPose(pos1true, squat1, pos2true, squat2, offset12, rotation12);
|
||||
|
||||
// axis-aligned bounding boxes in g1 frame
|
||||
for (int i=0; i<8; i++) {
|
||||
vec1[0] = (i&1 ? size1[0]+size1[3] : size1[0]-size1[3]);
|
||||
vec1[1] = (i&2 ? size1[1]+size1[4] : size1[1]-size1[4]);
|
||||
vec1[2] = (i&4 ? size1[2]+size1[5] : size1[2]-size1[5]);
|
||||
|
||||
vec2[0] = (i&1 ? size2[0]+size2[3] : size2[0]-size2[3]);
|
||||
vec2[1] = (i&2 ? size2[1]+size2[4] : size2[1]-size2[4]);
|
||||
vec2[2] = (i&4 ? size2[2]+size2[5] : size2[2]-size2[5]);
|
||||
|
||||
mju_rotVecMat(vec2, vec2, rotation1);
|
||||
mju_addTo3(vec2, offset1);
|
||||
|
||||
for (int k=0; k<3; k++) {
|
||||
aabb1[0+k] = mju_min(aabb1[0+k], vec1[k]);
|
||||
aabb1[3+k] = mju_max(aabb1[3+k], vec1[k]);
|
||||
aabb2[0+k] = mju_min(aabb2[0+k], vec2[k]);
|
||||
aabb2[3+k] = mju_max(aabb2[3+k], vec2[k]);
|
||||
}
|
||||
}
|
||||
|
||||
// intersection of aabbs
|
||||
for (int k=0; k<3; k++) {
|
||||
aabb[0+k] = mju_max(aabb1[0+k], aabb2[0+k]);
|
||||
aabb[3+k] = mju_min(aabb1[3+k], aabb2[3+k]);
|
||||
}
|
||||
|
||||
// no intersection if max < min
|
||||
if (aabb[3]<aabb[0] || aabb[4]<aabb[1] || aabb[5]<aabb[2]) {
|
||||
return cnt;
|
||||
}
|
||||
|
||||
// create sdf pointers
|
||||
int instance[2];
|
||||
const mjpPlugin* sdf_ptr[2];
|
||||
mjtGeom geomtypes[2] = {m->geom_type[g2], m->geom_type[g1]};
|
||||
|
||||
instance[0] = m->geom_plugin[g2];
|
||||
sdf_ptr[0] = getSDF(m, g2);
|
||||
|
||||
// get sdf plugins
|
||||
if (m->geom_type[g1] == mjGEOM_SDF) {
|
||||
instance[1] = m->geom_plugin[g1];
|
||||
sdf_ptr[1] = getSDF(m, g1);
|
||||
} else {
|
||||
instance[1] = g1;
|
||||
sdf_ptr[1] = NULL;
|
||||
}
|
||||
|
||||
// reset visualization count
|
||||
sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]);
|
||||
|
||||
// copy into sdf
|
||||
mjSDF sdf;
|
||||
sdf.id = instance;
|
||||
sdf.relpos = offset21;
|
||||
sdf.relmat = rotation21;
|
||||
sdf.plugin = sdf_ptr;
|
||||
sdf.geomtype = geomtypes;
|
||||
|
||||
// minimize sdf intersection
|
||||
mjtNum contacts[3*mjMAXCONPAIR];
|
||||
|
||||
int i = 0, j = 0;
|
||||
while (i<m->opt.sdf_initpoints) {
|
||||
x[0] = aabb[0] + (aabb[3]-aabb[0]) * mju_Halton(j, 2);
|
||||
x[1] = aabb[1] + (aabb[4]-aabb[1]) * mju_Halton(j, 3);
|
||||
x[2] = aabb[2] + (aabb[5]-aabb[2]) * mju_Halton(j, 5);
|
||||
|
||||
mju_rotVecMat(y, x, rotation2);
|
||||
mju_addTo3(y, offset2);
|
||||
|
||||
mju_rotVecMat(x, y, rotation12);
|
||||
mju_addTo3(x, offset12);
|
||||
|
||||
j++;
|
||||
|
||||
// here a criterion for rejecting points could be inserted
|
||||
|
||||
i++;
|
||||
|
||||
// start counters
|
||||
sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
|
||||
|
||||
// gradient descent
|
||||
sdf.type = mjSDFTYPE_INTERSECTION;
|
||||
dist = stepGradient(x, m, &sdf, (mjData*)d);
|
||||
sdf.type = mjSDFTYPE_AVERAGE;
|
||||
cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d);
|
||||
}
|
||||
|
||||
return cnt;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_
|
||||
#define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjexport.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjplugin.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
typedef enum mjtSDFType_ {
|
||||
mjSDFTYPE_SINGLE = 0,
|
||||
mjSDFTYPE_INTERSECTION,
|
||||
mjSDFTYPE_AVERAGE,
|
||||
} mjtSDFType;
|
||||
|
||||
struct mjSDF_ {
|
||||
const mjpPlugin** plugin;
|
||||
int* id;
|
||||
mjtSDFType type;
|
||||
mjtNum* relpos;
|
||||
mjtNum* relmat;
|
||||
mjtGeom* geomtype;
|
||||
};
|
||||
typedef struct mjSDF_ mjSDF;
|
||||
|
||||
// signed distance function
|
||||
MJAPI mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]);
|
||||
|
||||
// gradient of sdf
|
||||
MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNum gradient[3],
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
// collision between two signed distance fields
|
||||
int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif // MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_
|
||||
@@ -123,6 +123,10 @@ void mj_defaultOption(mjOption* opt) {
|
||||
opt->mpr_iterations = 50;
|
||||
opt->disableflags = 0;
|
||||
opt->enableflags = 0;
|
||||
|
||||
// sdf collisions
|
||||
opt->sdf_initpoints = 40;
|
||||
opt->sdf_iterations = 10;
|
||||
}
|
||||
|
||||
|
||||
@@ -1716,7 +1720,7 @@ const char* mj_validateReferences(const mjModel* m) {
|
||||
if (m->geom_dataid[i] >= m->nhfield || m->geom_dataid[i] < -1) {
|
||||
return "Invalid model: geom_dataid out of bounds.";
|
||||
}
|
||||
} else if (m->geom_type[i] == mjGEOM_MESH) {
|
||||
} else if ((m->geom_type[i]==mjGEOM_MESH) || (m->geom_type[i]==mjGEOM_SDF)) {
|
||||
if (m->geom_dataid[i] >= m->nmesh || m->geom_dataid[i] < -1) {
|
||||
return "Invalid model: geom_dataid out of bounds.";
|
||||
}
|
||||
|
||||
@@ -55,7 +55,7 @@ namespace {
|
||||
constexpr int kMaxNameLength = 1024;
|
||||
constexpr int kMaxAttributes = 255;
|
||||
|
||||
constexpr int kCacheLine = 64;
|
||||
constexpr int kCacheLine = 256;
|
||||
|
||||
// vfs prefix
|
||||
constexpr const char* kVfsPrefix = mjVFS_PREFIX;
|
||||
|
||||
@@ -22,6 +22,7 @@
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include "engine/engine_io.h"
|
||||
#include "engine/engine_plugin.h"
|
||||
#include "engine/engine_util_blas.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
#include "engine/engine_util_misc.h"
|
||||
@@ -721,6 +722,67 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
|
||||
return x;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// intersect ray with signed distance field
|
||||
mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
|
||||
const mjtNum* pnt, const mjtNum* vec) {
|
||||
mjtNum distance_total = 0;
|
||||
mjtNum p[3];
|
||||
|
||||
// exclude using bounding box
|
||||
if (ray_box(d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pnt, vec, NULL) < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// get sdf
|
||||
int instance = m->geom_plugin[g];
|
||||
const int nslot = mjp_pluginCount();
|
||||
const int slot = m->plugin[instance];
|
||||
const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot);
|
||||
if (!sdf) mju_error("invalid plugin slot: %d", slot);
|
||||
if (!(sdf->capabilityflags & mjPLUGIN_SDF)) {
|
||||
mju_error("Plugin is not a sign distance field at slot %d", slot);
|
||||
}
|
||||
|
||||
// reset counter
|
||||
sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
|
||||
|
||||
// compute transformation
|
||||
mjtNum sdf_quat[4], sdf_xmat[9], sdf_xpos[9];
|
||||
mjtNum negpos[3], negquat[4], xquat[4];
|
||||
mjtNum* xpos = d->geom_xpos + 3*g;
|
||||
mjtNum* pos = m->geom_pos + 3*g;
|
||||
mjtNum* quat = m->geom_quat + 4*g;
|
||||
mju_mat2Quat(xquat, d->geom_xmat + 9*g);
|
||||
mju_negPose(negpos, negquat, pos, quat);
|
||||
mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);
|
||||
mju_quat2Mat(sdf_xmat, sdf_quat);
|
||||
|
||||
// unit direction
|
||||
mjtNum dir[3] = {vec[0], vec[1], vec[2]};
|
||||
mju_normalize3(dir);
|
||||
|
||||
// ray marching
|
||||
for (int i=0; i < 40; i++) {
|
||||
mju_addScl3(p, pnt, dir, distance_total);
|
||||
mju_subFrom3(p, sdf_xpos);
|
||||
mju_rotVecMatT(p, p, sdf_xmat);
|
||||
mjtNum distance = sdf->sdf_distance(p, (mjData*)d, instance);
|
||||
distance_total += distance;
|
||||
if (distance < 1e-8) {
|
||||
return distance_total;
|
||||
}
|
||||
}
|
||||
|
||||
// reset counter
|
||||
sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// intersect ray with mesh
|
||||
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
|
||||
const mjtNum* pnt, const mjtNum* vec) {
|
||||
@@ -911,6 +973,8 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
|
||||
newdist = mj_rayMesh(m, d, i, pnt, vec);
|
||||
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
|
||||
newdist = mj_rayHfield(m, d, i, pnt, vec);
|
||||
} else if (m->geom_type[i] == mjGEOM_SDF) {
|
||||
newdist = ray_sdf(m, d, i, pnt, vec);
|
||||
}
|
||||
|
||||
// otherwise general dispatch
|
||||
@@ -1068,6 +1132,8 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
|
||||
newdist = mj_rayMesh(m, d, i, pnt, vec);
|
||||
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
|
||||
newdist = mj_rayHfield(m, d, i, pnt, vec);
|
||||
} else if (m->geom_type[i] == mjGEOM_SDF) {
|
||||
newdist = ray_sdf(m, d, i, pnt, vec);
|
||||
}
|
||||
|
||||
// otherwise general dispatch
|
||||
|
||||
@@ -92,7 +92,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
|
||||
{"Static Bo&dy", "1", "D"},
|
||||
{"Skin", "1", ";"},
|
||||
{"Body Tree", "0", "`"},
|
||||
{"Mesh Tree", "0", ""}
|
||||
{"Mesh Tree", "0", ""},
|
||||
{"SDF iters", "0", ""}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -647,6 +647,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
|
||||
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
|
||||
|
||||
if (!d->bvh_active[i]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
rgba[0] = d->bvh_active[i] ? 1 : 0;
|
||||
rgba[1] = d->bvh_active[i] ? 0 : 1;
|
||||
|
||||
@@ -1053,7 +1057,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
for (int k=geomadr; k < geomadr+geomnum; k++) {
|
||||
int geomtype = m->geom_type[k];
|
||||
// add inflated geom if it is a regular primitive
|
||||
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) {
|
||||
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD &&
|
||||
geomtype != mjGEOM_MESH && geomtype != mjGEOM_SDF) {
|
||||
START
|
||||
// inflate sizes by 5%
|
||||
mju_scl3(sz, m->geom_size+3*k, 1.05);
|
||||
@@ -1183,7 +1188,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
setMaterial(m, thisgeom, geom_matid, rgba, vopt->flags);
|
||||
|
||||
// set texcoord
|
||||
if (m->geom_type[i] == mjGEOM_MESH &&
|
||||
if ((m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) &&
|
||||
m->geom_dataid[i] >= 0 &&
|
||||
m->mesh_texcoordadr[m->geom_dataid[i]] >= 0) {
|
||||
thisgeom->texcoord = 1;
|
||||
@@ -1205,7 +1210,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
}
|
||||
|
||||
// mesh: 2*i is original, 2*i+1 is convex hull
|
||||
if (m->geom_type[i] == mjGEOM_MESH) {
|
||||
if (m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) {
|
||||
thisgeom->dataid *= 2;
|
||||
if (m->mesh_graphadr[m->geom_dataid[i]] >= 0 && vopt->flags[mjVIS_CONVEXHULL] &&
|
||||
(m->geom_contype[i] || m->geom_conaffinity[i])) {
|
||||
|
||||
Reference in New Issue
Block a user