Kyle Bayes
ec506dbde3
PiperOrigin-RevId: 965949465 Change-Id: I099fe1b5dfd722958c339429ddffe33fb66759d2
1729 lines
50 KiB
C
1729 lines
50 KiB
C
// 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_convex.h"
|
|
|
|
#include <float.h>
|
|
#include <stddef.h>
|
|
|
|
#include <ccd/ccd.h>
|
|
#include <ccd/vec3.h>
|
|
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mjmacro.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include "engine/engine_collision_gjk.h"
|
|
#include "engine/engine_macro.h"
|
|
#include "engine/engine_memory.h"
|
|
#include "engine/engine_inline.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 mjMINVAL2 (mjMINVAL * mjMINVAL)
|
|
|
|
// CCD internal buffer used for batched processing; if NULL, stack memory allocated on each
|
|
// mjc_penetration call
|
|
static mjTHREADLOCAL void* ccd_buffer = NULL;
|
|
|
|
// set CCD internal buffer
|
|
void mjc_setCCDBuffer(void* buffer) {
|
|
ccd_buffer = buffer;
|
|
}
|
|
|
|
// ccd prism first dir
|
|
static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) {
|
|
ccdVec3Set(vec, 0, 0, 1);
|
|
}
|
|
|
|
// wrapper around libccd; returns number of collisions found
|
|
static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
|
|
mjPreContact* con, mjtNum margin) {
|
|
ccd_t ccd;
|
|
CCD_INIT(&ccd);
|
|
ccd.mpr_tolerance = m->opt.ccd_tolerance;
|
|
ccd.epa_tolerance = m->opt.ccd_tolerance; // use MPR tolerance for EPA
|
|
ccd.max_iterations = m->opt.ccd_iterations;
|
|
ccd.support1 = mjccd_support;
|
|
ccd.support2 = mjccd_support;
|
|
ccd.center1 = mjccd_center;
|
|
ccd.center2 = mjccd_center;
|
|
if (obj1->geom_type == mjGEOM_HFIELD || obj2->geom_type == mjGEOM_HFIELD) {
|
|
ccd.first_dir = prism_firstdir;
|
|
} else {
|
|
ccd.first_dir = ccdFirstDirDefault;
|
|
}
|
|
|
|
ccd_real_t ccd_depth;
|
|
ccd_vec3_t ccd_dir, ccd_pos;
|
|
int ret = ccdMPRPenetration(obj1, obj2, &ccd, &ccd_depth, &ccd_dir, &ccd_pos);
|
|
if (ret == 0) {
|
|
if (ccdVec3Eq(&ccd_dir, ccd_vec3_origin)) {
|
|
return 0;
|
|
}
|
|
con[0].dist = margin - ccd_depth;
|
|
mji_copy3(con[0].normal, ccd_dir.v);
|
|
mji_copy3(con[0].pos, ccd_pos.v);
|
|
mji_zero3(con[0].tangent);
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
// find penetration info between two geoms; returns number of collisions found
|
|
static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj* obj2,
|
|
mjPreContact* con, int nconmax, mjtNum margin) {
|
|
if (mjDISABLED(mjDSBL_NATIVECCD)) {
|
|
return _libccd_wrapper(m, obj1, obj2, con, margin);
|
|
}
|
|
|
|
// nativeccd
|
|
mjCCDConfig config;
|
|
mjCCDStatus status;
|
|
mjtNum dist;
|
|
void* buffer = ccd_buffer;
|
|
|
|
// set config
|
|
config.max_iterations = m->opt.ccd_iterations;
|
|
config.tolerance = m->opt.ccd_tolerance;
|
|
config.max_contacts = nconmax;
|
|
config.dist_cutoff = 0; // no geom distances needed
|
|
config.npolygonmax = m->npolygonmax;
|
|
config.nmeshdegmax = m->nmeshdegmax;
|
|
if (buffer) {
|
|
config.buffer = buffer;
|
|
} else {
|
|
mj_markStack(d);
|
|
int npolygonmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->npolygonmax;
|
|
int nmeshdegmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->nmeshdegmax;
|
|
config.buffer = mj_stackAllocByte(d, mjc_ccdSize(npolygonmax, nmeshdegmax,
|
|
config.max_iterations), sizeof(mjtNum));
|
|
}
|
|
|
|
int ncon = 0;
|
|
if ((dist = mjc_ccd(&config, &status, obj1, obj2)) < 0) {
|
|
int nwitness = status.nx;
|
|
for (int i = 0; i < nwitness; i++) {
|
|
con[i].dist = margin + status.dist[i];
|
|
mji_add3(con[i].pos, status.x1 + 3*i, status.x2 + 3*i);
|
|
mju_scl3(con[i].pos, con[i].pos, 0.5);
|
|
mji_sub3(con[i].normal, status.x1 + 3*i, status.x2 + 3*i);
|
|
mju_normalize3(con[i].normal);
|
|
mji_zero3(con[i].tangent);
|
|
}
|
|
ncon = nwitness;
|
|
}
|
|
if (!buffer) {
|
|
mj_freeStack(d);
|
|
}
|
|
return ncon;
|
|
}
|
|
|
|
|
|
// ccd center function
|
|
void mjccd_center(const void *obj, ccd_vec3_t *center) {
|
|
mjc_center(center->v, (const mjCCDObj*) obj);
|
|
}
|
|
|
|
// center function for convex collision algorithms
|
|
void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
|
|
int g = obj->geom;
|
|
int f = obj->flex;
|
|
int e = obj->elem;
|
|
int v = obj->vert;
|
|
|
|
if (obj->geom_type == mjGEOM_HFIELD) {
|
|
mju_zero3(res);
|
|
for (int i=0; i < 6; i++) {
|
|
mji_addTo3(res, obj->data.hfield.prism[i]);
|
|
}
|
|
mju_scl3(res, res, 1.0/6.0);
|
|
return;
|
|
}
|
|
|
|
// return geom position
|
|
if (g >= 0) {
|
|
mji_copy3(res, obj->pos);
|
|
return;
|
|
}
|
|
|
|
// return flex element position
|
|
if (e >= 0) {
|
|
mji_copy3(res, obj->data.flex.aabb + 6*(obj->data.flex.elemadr[f]+e));
|
|
return;
|
|
}
|
|
|
|
// return flex vertex position
|
|
if (f >= 0) {
|
|
mji_copy3(res, obj->data.flex.vert_xpos + 3*(obj->data.flex.vertadr[f]+v));
|
|
return;
|
|
}
|
|
}
|
|
|
|
// ------------------------------------ Support functions -----------------------------------------
|
|
|
|
// transform a vector from global to local frame
|
|
static inline void mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum dir[3]) {
|
|
// perform matT * dir
|
|
res[0] = mat[0]*dir[0] + mat[3]*dir[1] + mat[6]*dir[2];
|
|
res[1] = mat[1]*dir[0] + mat[4]*dir[1] + mat[7]*dir[2];
|
|
res[2] = mat[2]*dir[0] + mat[5]*dir[1] + mat[8]*dir[2];
|
|
}
|
|
|
|
|
|
// transform a vector from local to global frame
|
|
static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNum dir[3],
|
|
const mjtNum pos[3]) {
|
|
// perform mat * dir + pos
|
|
res[0] = mat[0]*dir[0] + mat[1]*dir[1] + mat[2]*dir[2];
|
|
res[1] = mat[3]*dir[0] + mat[4]*dir[1] + mat[5]*dir[2];
|
|
res[2] = mat[6]*dir[0] + mat[7]*dir[1] + mat[8]*dir[2];
|
|
res[0] += pos[0];
|
|
res[1] += pos[1];
|
|
res[2] += pos[2];
|
|
}
|
|
|
|
|
|
// point support function
|
|
void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
mji_copy3(res, obj->pos);
|
|
}
|
|
|
|
|
|
// sphere support function
|
|
static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
// sphere data
|
|
const mjtNum* pos = obj->pos;
|
|
mjtNum radius = obj->size[0];
|
|
|
|
res[0] = radius*dir[0] + pos[0];
|
|
res[1] = radius*dir[1] + pos[1];
|
|
res[2] = radius*dir[2] + pos[2];
|
|
}
|
|
|
|
|
|
// line support function (capsule)
|
|
void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
// capsule data
|
|
const mjtNum* mat = obj->mat;
|
|
const mjtNum* pos = obj->pos;
|
|
mjtNum length = obj->size[1];
|
|
|
|
mjtNum dot = mat[2]*dir[0] + mat[5]*dir[1] + mat[8]*dir[2];
|
|
mjtNum scl = dot >= 0 ? length : -length;
|
|
|
|
// transform result to global frame
|
|
res[0] = mat[2]*scl + pos[0];
|
|
res[1] = mat[5]*scl + pos[1];
|
|
res[2] = mat[8]*scl + pos[2];
|
|
}
|
|
|
|
|
|
// capsule support function
|
|
static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
// capsule data
|
|
const mjtNum* mat = obj->mat;
|
|
const mjtNum* pos = obj->pos;
|
|
mjtNum radius = obj->size[0];
|
|
mjtNum length = obj->size[1];
|
|
|
|
// rotate dir to geom local frame
|
|
mjtNum local_dir[3], local_supp[3];
|
|
mulMatTVec3(local_dir, mat, dir);
|
|
|
|
// start with sphere
|
|
local_supp[0] = local_dir[0] * radius;
|
|
local_supp[1] = local_dir[1] * radius;
|
|
local_supp[2] = local_dir[2] * radius;
|
|
|
|
// add cylinder contribution
|
|
local_supp[2] += (local_dir[2] >= 0 ? length : -length);
|
|
|
|
// transform result to global frame
|
|
localToGlobal(res, mat, local_supp, pos);
|
|
}
|
|
|
|
|
|
// ellipsoid support function
|
|
static void mjc_ellipsoidSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
// ellipsoid data
|
|
const mjtNum* mat = obj->mat;
|
|
const mjtNum* pos = obj->pos;
|
|
const mjtNum* size = obj->size;
|
|
|
|
// rotate dir to geom local frame
|
|
mjtNum local_dir[3], local_supp[3];
|
|
mulMatTVec3(local_dir, mat, dir);
|
|
|
|
// find support point on unit sphere: scale dir by ellipsoid sizes
|
|
local_supp[0] = local_dir[0] * size[0];
|
|
local_supp[1] = local_dir[1] * size[1];
|
|
local_supp[2] = local_dir[2] * size[2];
|
|
|
|
mjtNum norm2 = local_supp[0]*local_supp[0] + local_supp[1]*local_supp[1] + local_supp[2]*local_supp[2];
|
|
|
|
// too small to normalize
|
|
if (norm2 < mjMINVAL2) {
|
|
res[0] = mat[0]*size[0] + pos[0];
|
|
res[1] = mat[3]*size[0] + pos[1];
|
|
res[2] = mat[6]*size[0] + pos[2];
|
|
return;
|
|
}
|
|
|
|
// normalize and transform to ellipsoid
|
|
mjtNum norm_inv = 1/mju_sqrt(norm2);
|
|
local_supp[0] *= norm_inv * size[0];
|
|
local_supp[1] *= norm_inv * size[1];
|
|
local_supp[2] *= norm_inv * size[2];
|
|
|
|
// transform result to global frame
|
|
localToGlobal(res, mat, local_supp, pos);
|
|
}
|
|
|
|
|
|
// cylinder support function
|
|
static void mjc_cylinderSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
// cylinder data
|
|
const mjtNum* mat = obj->mat;
|
|
const mjtNum* pos = obj->pos;
|
|
const mjtNum* size = obj->size;
|
|
|
|
// rotate dir to geom local frame
|
|
mjtNum local_dir[3], local_supp[3];
|
|
mulMatTVec3(local_dir, mat, dir);
|
|
|
|
mjtNum n2 = local_dir[0]*local_dir[0] + local_dir[1]*local_dir[1];
|
|
mjtNum scl = n2 >= mjMINVAL2 ? size[0] / mju_sqrt(n2) : 0;
|
|
local_supp[0] = scl * local_dir[0];
|
|
local_supp[1] = scl * local_dir[1];
|
|
|
|
// set result in Z direction
|
|
local_supp[2] = local_dir[2] >= 0 ? size[1] : -size[1];
|
|
|
|
// transform result to global frame
|
|
localToGlobal(res, mat, local_supp, pos);
|
|
}
|
|
|
|
|
|
// box support function
|
|
static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
// box data
|
|
const mjtNum* mat = obj->mat;
|
|
const mjtNum* pos = obj->pos;
|
|
const mjtNum* size = obj->size;
|
|
|
|
// rotate dir to geom local frame
|
|
mjtNum local_dir[3], local_supp[3];
|
|
mulMatTVec3(local_dir, mat, dir);
|
|
|
|
// find support point in local frame
|
|
local_supp[0] = local_dir[0] >= 0 ? size[0] : -size[0];
|
|
local_supp[1] = local_dir[1] >= 0 ? size[1] : -size[1];
|
|
local_supp[2] = local_dir[2] >= 0 ? size[2] : -size[2];
|
|
|
|
// mark the index of the corner of the box for fast lookup
|
|
obj->vertindex = (local_supp[0] > 0) ? 1 : 0;
|
|
obj->vertindex |= (local_supp[1] > 0) ? 2 : 0;
|
|
obj->vertindex |= (local_supp[2] > 0) ? 4 : 0;
|
|
|
|
// transform support point to global frame
|
|
localToGlobal(res, mat, local_supp, pos);
|
|
}
|
|
|
|
|
|
// dot product between mjtNum and float
|
|
static inline mjtNum dot3f(const mjtNum a[3], const float b[3]) {
|
|
return a[0]*(mjtNum)b[0] + a[1]*(mjtNum)b[1] + a[2]*(mjtNum)b[2];
|
|
}
|
|
|
|
|
|
// mesh support function via exhaustive search
|
|
static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
const mjtNum* mat = obj->mat;
|
|
const mjtNum* pos = obj->pos;
|
|
const float* verts = obj->data.mesh.vert;
|
|
int nverts = obj->data.mesh.nvert;
|
|
|
|
mjtNum local_dir[3];
|
|
mulMatTVec3(local_dir, mat, dir);
|
|
|
|
mjtNum max = -FLT_MAX;
|
|
int imax = 0;
|
|
|
|
// used cached results from previous search
|
|
if (obj->vertindex >= 0) {
|
|
imax = obj->vertindex;
|
|
max = dot3f(local_dir, verts + 3*imax);
|
|
}
|
|
|
|
// search all vertices, find maximum dot product
|
|
for (int i=0; i < nverts; i++) {
|
|
mjtNum vdot = dot3f(local_dir, verts + 3*i);
|
|
|
|
// update max
|
|
if (vdot > max) {
|
|
max = vdot;
|
|
imax = i;
|
|
}
|
|
}
|
|
|
|
// record vertex index of maximum
|
|
obj->vertindex = imax;
|
|
|
|
local_dir[0] = (mjtNum)verts[3*imax + 0];
|
|
local_dir[1] = (mjtNum)verts[3*imax + 1];
|
|
local_dir[2] = (mjtNum)verts[3*imax + 2];
|
|
|
|
// transform result to global frame
|
|
localToGlobal(res, mat, local_dir, pos);
|
|
}
|
|
|
|
|
|
// mesh support function via hill climbing
|
|
static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
int numvert = obj->data.mesh.graph[0];
|
|
const int* vert_edgeadr = obj->data.mesh.graph + 2;
|
|
const int* vert_globalid = obj->data.mesh.graph + 2 + numvert;
|
|
const int* edge_localid = obj->data.mesh.graph + 2 + 2*numvert;
|
|
const float* verts = obj->data.mesh.vert;
|
|
const mjtNum* pos = obj->pos;
|
|
const mjtNum* mat = obj->mat;
|
|
|
|
// rotate dir to geom local frame
|
|
mjtNum local_dir[3];
|
|
mulMatTVec3(local_dir, mat, dir);
|
|
|
|
int prev = -1;
|
|
int imax;
|
|
|
|
// map continuous direction to discrete 3x3x3 grid (-1, 0, 1) indices
|
|
int cx = (local_dir[0] > 0.4) - (local_dir[0] < -0.4) + 1;
|
|
int cy = (local_dir[1] > 0.4) - (local_dir[1] < -0.4) + 1;
|
|
int cz = (local_dir[2] > 0.4) - (local_dir[2] < -0.4) + 1;
|
|
int grid_idx = obj->data.mesh.extrema[cx*9 + cy*3 + cz];
|
|
|
|
if (obj->meshindex >= 0) {
|
|
// warm start: pick the better of cached vertex vs grid seed
|
|
mjtNum cached_dot = dot3f(local_dir, verts + 3*vert_globalid[obj->meshindex]);
|
|
mjtNum seed_dot = dot3f(local_dir, verts + 3*vert_globalid[grid_idx]);
|
|
imax = (seed_dot > cached_dot) ? grid_idx : obj->meshindex;
|
|
} else {
|
|
// cold start: use grid seed
|
|
imax = grid_idx;
|
|
}
|
|
|
|
mjtNum max = dot3f(local_dir, verts + 3*vert_globalid[imax]);
|
|
|
|
// hillclimb until no change
|
|
while (imax != prev) {
|
|
prev = imax;
|
|
int subidx;
|
|
for (int i = vert_edgeadr[imax]; (subidx = edge_localid[i]) >= 0; i++) {
|
|
mjtNum vdot = dot3f(local_dir, verts + 3*vert_globalid[subidx]);
|
|
if (vdot > max) {
|
|
max = vdot;
|
|
imax = subidx; // update maximum vertex index
|
|
}
|
|
}
|
|
}
|
|
|
|
// record vertex index of maximum (local id)
|
|
obj->meshindex = imax;
|
|
|
|
// get resulting support vertex
|
|
obj->vertindex = imax = vert_globalid[imax];
|
|
local_dir[0] = (mjtNum)verts[3*imax + 0];
|
|
local_dir[1] = (mjtNum)verts[3*imax + 1];
|
|
local_dir[2] = (mjtNum)verts[3*imax + 2];
|
|
|
|
// transform result to global frame
|
|
localToGlobal(res, mat, local_dir, pos);
|
|
}
|
|
|
|
|
|
// prism support function
|
|
static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
int istart, ibest;
|
|
mjtNum best, tmp;
|
|
mjtNum (*prism)[3] = obj->data.hfield.prism;
|
|
|
|
// find best vertex in halfspace determined by dir.z
|
|
istart = dir[2] < 0 ? 0 : 3;
|
|
ibest = istart;
|
|
best = mju_dot3(prism[istart], dir);
|
|
for (int i=1; i < 3; i++) {
|
|
if ((tmp = mju_dot3(prism[istart + i], dir)) > best) {
|
|
ibest = istart + i;
|
|
best = tmp;
|
|
}
|
|
}
|
|
|
|
// copy best point
|
|
mji_copy3(res, prism[ibest]);
|
|
}
|
|
|
|
|
|
// flex support function
|
|
static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
|
|
int f = obj->flex;
|
|
int dim = obj->data.flex.dim[f];
|
|
|
|
// flex element
|
|
if (obj->elem >= 0) {
|
|
int e = obj->elem;
|
|
const int* edata = obj->data.flex.elem + obj->data.flex.elemdataadr[f] + e*(dim+1);
|
|
const mjtNum* vert = obj->data.flex.vert_xpos + 3*obj->data.flex.vertadr[f];
|
|
|
|
// find element vertex with largest projection along dir
|
|
mji_copy3(res, vert+3*edata[0]);
|
|
mjtNum best = mju_dot3(res, dir);
|
|
for (int i=1; i <= dim; i++) {
|
|
mjtNum dot = mju_dot3(vert+3*edata[i], dir);
|
|
|
|
// better vertex found: assign
|
|
if (dot > best) {
|
|
best = dot;
|
|
mji_copy3(res, vert+3*edata[i]);
|
|
}
|
|
}
|
|
|
|
// add radius and margin/2
|
|
mji_addToScl3(res, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
|
|
return;
|
|
}
|
|
|
|
// flex vertex
|
|
else {
|
|
const mjtNum* vert = obj->data.flex.vert_xpos + 3*(obj->data.flex.vertadr[f] + obj->vert);
|
|
mji_addScl3(res, vert, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
// libccd support function
|
|
void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
|
|
mjCCDObj *obj = (mjCCDObj *)_obj;
|
|
mjtNum *res = vec->v;
|
|
const mjtNum *dir = _dir->v;
|
|
int g = obj->geom;
|
|
|
|
if (g < 0) {
|
|
int f = obj->flex;
|
|
int dim = obj->data.flex.dim[f];
|
|
|
|
// flex element
|
|
if (obj->elem >= 0) {
|
|
int e = obj->elem;
|
|
const int* edata = obj->data.flex.elem + obj->data.flex.elemdataadr[f] + e*(dim+1);
|
|
const mjtNum* vert = obj->data.flex.vert_xpos + 3*obj->data.flex.vertadr[f];
|
|
|
|
// find element vertex with largest projection along dir
|
|
mji_copy3(res, vert+3*edata[0]);
|
|
mjtNum best = mju_dot3(res, dir);
|
|
for (int i=1; i <= dim; i++) {
|
|
mjtNum dot = mju_dot3(vert+3*edata[i], dir);
|
|
|
|
// better vertex found: assign
|
|
if (dot > best) {
|
|
best = dot;
|
|
mji_copy3(res, vert+3*edata[i]);
|
|
}
|
|
}
|
|
|
|
// add radius and margin/2
|
|
mji_addToScl3(res, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
|
|
return;
|
|
}
|
|
|
|
// flex vertex
|
|
else {
|
|
const mjtNum* vert = obj->data.flex.vert_xpos + 3*(obj->data.flex.vertadr[f] + obj->vert);
|
|
mji_addScl3(res, vert, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
|
|
return;
|
|
}
|
|
}
|
|
|
|
const float* vertdata;
|
|
int ibest, numvert, change, locid;
|
|
mjtNum tmp, vdot;
|
|
|
|
const mjtNum* size = obj->size; // geom sizes
|
|
mjtNum local_dir[3]; // direction in geom local frame
|
|
|
|
// rotate dir to geom local frame
|
|
mju_mulMatTVec3(local_dir, obj->mat, dir);
|
|
|
|
// compute result according to geom type
|
|
switch ((mjtGeom) obj->geom_type) {
|
|
case mjGEOM_SPHERE:
|
|
mji_scl3(res, local_dir, size[0]);
|
|
break;
|
|
|
|
case mjGEOM_CAPSULE:
|
|
// start with sphere
|
|
mji_scl3(res, local_dir, size[0]);
|
|
|
|
// add cylinder contribution
|
|
res[2] += mju_sign(local_dir[2]) * size[1];
|
|
break;
|
|
|
|
case mjGEOM_ELLIPSOID:
|
|
// find support point on unit sphere: scale dir by ellipsoid sizes and renormalize
|
|
for (int i=0; i < 3; i++) {
|
|
res[i] = local_dir[i] * size[i];
|
|
}
|
|
mju_normalize3(res);
|
|
|
|
// transform to ellipsoid
|
|
for (int i=0; i < 3; i++) {
|
|
res[i] *= size[i];
|
|
}
|
|
break;
|
|
|
|
case mjGEOM_CYLINDER:
|
|
// set result in XY plane: support on circle
|
|
tmp = mju_sqrt(local_dir[0]*local_dir[0] + local_dir[1]*local_dir[1]);
|
|
if (tmp > mjMINVAL) {
|
|
res[0] = local_dir[0]/tmp*size[0];
|
|
res[1] = local_dir[1]/tmp*size[0];
|
|
} else {
|
|
res[0] = res[1] = 0;
|
|
}
|
|
|
|
// set result in Z direction
|
|
res[2] = mju_sign(local_dir[2]) * size[1];
|
|
break;
|
|
|
|
case mjGEOM_BOX:
|
|
for (int i=0; i < 3; i++) {
|
|
res[i] = mju_sign(local_dir[i]) * size[i];
|
|
}
|
|
break;
|
|
|
|
case mjGEOM_MESH:
|
|
case mjGEOM_SDF:
|
|
// init search
|
|
vertdata = obj->data.mesh.vert;
|
|
tmp = -1E+10;
|
|
ibest = -1;
|
|
|
|
// no graph data: exhaustive search
|
|
if (obj->data.mesh.graph == NULL) {
|
|
// search all vertices, find best
|
|
for (int i=0; i < obj->data.mesh.nvert; i++) {
|
|
// vdot = dot(vertex, dir)
|
|
vdot = local_dir[0] * (mjtNum)vertdata[3*i] +
|
|
local_dir[1] * (mjtNum)vertdata[3*i+1] +
|
|
local_dir[2] * (mjtNum)vertdata[3*i+2];
|
|
|
|
// update best
|
|
if (vdot > tmp) {
|
|
tmp = vdot;
|
|
ibest = i;
|
|
}
|
|
}
|
|
|
|
// record best vertex index, in globalid format
|
|
obj->meshindex = ibest;
|
|
}
|
|
|
|
// hill-climb using graph data
|
|
else {
|
|
// get info
|
|
numvert = obj->data.mesh.graph[0];
|
|
const int* vert_edgeadr = obj->data.mesh.graph + 2;
|
|
const int* vert_globalid = obj->data.mesh.graph + 2 + numvert;
|
|
const int* edge_localid = obj->data.mesh.graph + 2 + 2*numvert;
|
|
|
|
// init with first vertex in convex hull or warmstart
|
|
ibest = obj->meshindex < 0 ? 0 : obj->meshindex;
|
|
tmp = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[ibest]+0] +
|
|
local_dir[1] * (mjtNum)vertdata[3*vert_globalid[ibest]+1] +
|
|
local_dir[2] * (mjtNum)vertdata[3*vert_globalid[ibest]+2];
|
|
|
|
// hill-climb until no change
|
|
change = 1;
|
|
while (change) {
|
|
// look for improvement in ibest neighborhood
|
|
change = 0;
|
|
int i = vert_edgeadr[ibest];
|
|
while ((locid=edge_localid[i]) >= 0) {
|
|
// vdot = dot(vertex, local_dir)
|
|
vdot = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] +
|
|
local_dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] +
|
|
local_dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2];
|
|
|
|
// update best
|
|
if (vdot > tmp) {
|
|
tmp = vdot;
|
|
ibest = locid;
|
|
change = 1;
|
|
}
|
|
|
|
// advance to next edge
|
|
i++;
|
|
}
|
|
}
|
|
|
|
// record best vertex index, in locid format
|
|
obj->meshindex = ibest;
|
|
|
|
// map best index to globalid
|
|
ibest = vert_globalid[ibest];
|
|
}
|
|
|
|
// sanity check, SHOULD NOT OCCUR
|
|
if (ibest < 0) {
|
|
mju_warning("mesh_support could not find support vertex");
|
|
mju_zero3(res);
|
|
}
|
|
|
|
// copy best vertex
|
|
else {
|
|
for (int i=0; i < 3; i++) {
|
|
res[i] = (mjtNum)vertdata[3*ibest + i];
|
|
}
|
|
}
|
|
break;
|
|
|
|
case mjGEOM_HFIELD:
|
|
mjc_prism_support(res, obj, dir);
|
|
return;
|
|
|
|
default:
|
|
mjERROR("ccd support function is undefined for geom type %d", obj->geom_type);
|
|
}
|
|
|
|
// add local_dir*margin/2 to result
|
|
for (int i=0; i < 3; i++) {
|
|
res[i] += local_dir[i] * obj->margin/2;
|
|
}
|
|
|
|
// rotate result to global frame
|
|
mju_mulMatVec3(res, obj->mat, res);
|
|
|
|
// add geom position
|
|
mji_addTo3(res, obj->pos);
|
|
}
|
|
|
|
// ------------------------------------------------------------------------------------------------
|
|
|
|
// initialize a CCD object
|
|
void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjtNum margin) {
|
|
int graphadr, vertadr, polyadr;
|
|
obj->geom = g;
|
|
obj->margin = margin;
|
|
obj->center = mjc_center;
|
|
obj->vertindex = -1;
|
|
obj->meshindex = -1;
|
|
obj->flex = -1;
|
|
obj->elem = -1;
|
|
obj->vert = -1;
|
|
mju_zero4(obj->rotate);
|
|
obj->rotate[0] = 1;
|
|
if (g >= 0) {
|
|
mju_copy(obj->size, m->geom_size+3*g, 3);
|
|
mju_copy(obj->pos, d->geom_xpos+3*g, 3);
|
|
mju_copy(obj->mat, d->geom_xmat+9*g, 9);
|
|
obj->geom_type = m->geom_type[g];
|
|
switch ((mjtGeom) obj->geom_type) {
|
|
case mjGEOM_ELLIPSOID:
|
|
obj->support = mjc_ellipsoidSupport;
|
|
break;
|
|
case mjGEOM_MESH:
|
|
case mjGEOM_SDF:
|
|
graphadr = m->mesh_graphadr[m->geom_dataid[g]];
|
|
vertadr = m->mesh_vertadr[m->geom_dataid[g]];
|
|
polyadr = m->mesh_polyadr[m->geom_dataid[g]];
|
|
if (graphadr < 0 || m->mesh_vertnum[m->geom_dataid[g]] < mjMESH_HILLCLIMB_MIN) {
|
|
obj->data.mesh.graph = NULL;
|
|
obj->data.mesh.extrema = NULL;
|
|
obj->support = mjc_meshSupport;
|
|
} else {
|
|
obj->data.mesh.graph = m->mesh_graph + graphadr;
|
|
obj->data.mesh.extrema = m->mesh_extrema + 27 * m->geom_dataid[g];
|
|
obj->support = mjc_hillclimbSupport;
|
|
}
|
|
obj->data.mesh.vert = m->mesh_vert + 3*vertadr;
|
|
obj->data.mesh.nvert = m->mesh_vertnum[m->geom_dataid[g]];
|
|
obj->data.mesh.mpolymapadr = m->mesh_polymapadr + vertadr;
|
|
obj->data.mesh.mpolymapnum = m->mesh_polymapnum + vertadr;
|
|
obj->data.mesh.polymap = m->mesh_polymap;
|
|
obj->data.mesh.polynormal = m->mesh_polynormal + 3*polyadr;
|
|
obj->data.mesh.polyvertadr = m->mesh_polyvertadr + polyadr;
|
|
obj->data.mesh.polyvertnum = m->mesh_polyvertnum + polyadr;
|
|
obj->data.mesh.polyvert = m->mesh_polyvert;
|
|
obj->data.mesh.mesh_polynum = m->mesh_polynum[m->geom_dataid[g]];
|
|
break;
|
|
case mjGEOM_SPHERE:
|
|
obj->support = mjc_sphereSupport;
|
|
break;
|
|
case mjGEOM_CAPSULE:
|
|
obj->support = mjc_capsuleSupport;
|
|
break;
|
|
case mjGEOM_CYLINDER:
|
|
obj->support = mjc_cylinderSupport;
|
|
break;
|
|
case mjGEOM_BOX:
|
|
obj->support = mjc_boxSupport;
|
|
break;
|
|
case mjGEOM_HFIELD:
|
|
obj->center = mjc_center;
|
|
obj->support = mjc_prism_support;
|
|
|
|
int hid = m->geom_dataid[g];
|
|
obj->data.hfield.hfield_nrow = m->hfield_nrow[hid];
|
|
obj->data.hfield.hfield_ncol = m->hfield_ncol[hid];
|
|
mju_copy(obj->size, m->hfield_size + 4*hid, 4);
|
|
obj->data.hfield.hfield_data = m->hfield_data + m->hfield_adr[hid];
|
|
break;
|
|
default:
|
|
obj->support = NULL;
|
|
break;
|
|
}
|
|
} else {
|
|
obj->geom_type = mjGEOM_FLEX;
|
|
obj->data.flex.dim = m->flex_dim;
|
|
obj->support = mjc_flexSupport;
|
|
obj->data.flex.aabb = d->flexelem_aabb;
|
|
obj->data.flex.elemadr = m->flex_elemadr;
|
|
obj->data.flex.vert_xpos = d->flexvert_xpos;
|
|
obj->data.flex.vertadr = m->flex_vertadr;
|
|
obj->data.flex.xradius = m->flex_radius;
|
|
obj->data.flex.elemdataadr = m->flex_elemdataadr;
|
|
obj->data.flex.elem = m->flex_elem;
|
|
}
|
|
}
|
|
|
|
|
|
// set flex data for CCD object
|
|
static void mjc_setCCDObjFlex(mjCCDObj* obj, int flex, int elem, int vert) {
|
|
obj->flex = flex;
|
|
obj->elem = elem;
|
|
obj->vert = vert;
|
|
}
|
|
|
|
|
|
// compare new contact to previous contacts, return 1 if it is far from all of them
|
|
static int mjc_isDistinctContact(const mjPreContact* con, int ncon, mjtNum tolerance) {
|
|
const mjtNum* last_pos = con[ncon - 1].pos;
|
|
for (int i=0; i < ncon-1; i++) {
|
|
if (mju_dist3(con[i].pos, last_pos) <= tolerance) {
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
// in-place rotation of spatial frame around given point of origin
|
|
static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
|
|
mjtNum xmat[9], mjtNum xpos[3]) {
|
|
mjtNum mat[9], vec[3], rel[3];
|
|
|
|
// rotate frame: xmat = rot*xmat
|
|
mju_mulMatMat3(mat, rot, xmat);
|
|
mju_copy(xmat, mat, 9);
|
|
|
|
// vector to rotation origin: rel = origin - xpos
|
|
mji_sub3(rel, origin, xpos);
|
|
|
|
// displacement of origin due to rotation: vec = rot*rel - rel
|
|
mju_mulMatVec3(vec, rot, rel);
|
|
mju_subFrom3(vec, rel);
|
|
|
|
// correct xpos by subtracting displacement: xpos = xpos - vec
|
|
mji_subFrom3(xpos, vec);
|
|
}
|
|
|
|
|
|
// return number of contacts supported by a single pass of narrowphase
|
|
static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* obj2) {
|
|
// single pass not supported for margins
|
|
if (obj1->margin > 0 || obj2->margin > 0) {
|
|
return 1;
|
|
}
|
|
|
|
// can return 8 contacts for box-box collision in one pass
|
|
int type1 = obj1->geom_type;
|
|
int type2 = obj2->geom_type;
|
|
if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) {
|
|
return 8;
|
|
}
|
|
|
|
// reduce mesh collisions to 4 contacts max
|
|
if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) {
|
|
if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) {
|
|
return mjDISABLED(mjDSBL_MULTICCD) ? 1 : 4;
|
|
}
|
|
}
|
|
|
|
// not supported for other geom types
|
|
return 1;
|
|
}
|
|
|
|
|
|
// multi-point convex-convex collision, using libccd
|
|
int mjc_Convex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
|
|
// init ccd objects
|
|
mjCCDObj obj1, obj2;
|
|
mjc_initCCDObj(&obj1, m, d, g1, margin);
|
|
mjc_initCCDObj(&obj2, m, d, g2, margin);
|
|
int max_contacts = maxContacts(m, &obj1, &obj2);
|
|
|
|
// find initial contact
|
|
int ncon = mjc_penetration(m, d, &obj1, &obj2, con, max_contacts, margin);
|
|
if (mjDISABLED(mjDSBL_NATIVECCD) && ncon && g1 >= 0 && g2 >= 0) {
|
|
mjc_fixNormal(m, d, con, g1, g2);
|
|
}
|
|
|
|
// no additional contacts needed
|
|
if (!mjDISABLED(mjDSBL_NATIVECCD) && max_contacts > 1) {
|
|
return ncon;
|
|
}
|
|
|
|
// look for additional contacts
|
|
if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD)
|
|
&& m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE
|
|
&& m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) {
|
|
// multiCCD parameters
|
|
const mjtNum relative_tolerance = 1e-3;
|
|
const mjtNum perturbation_angle = 1e-3;
|
|
|
|
// complete frame of initial contact
|
|
mjtNum frame[9];
|
|
mji_copy3(frame, con[0].normal);
|
|
mju_zero(frame+3, 6);
|
|
mju_makeFrame(frame);
|
|
|
|
// tolerance for determining if newly found contacts are distinct
|
|
const mjtNum tolerance = relative_tolerance * mju_min(m->geom_rbound[g1], m->geom_rbound[g2]);
|
|
|
|
// axes and rotation angles for perturbation test
|
|
mjtNum* axes[2] = {frame+3, frame+6};
|
|
mjtNum angles[2] = {-perturbation_angle, perturbation_angle};
|
|
|
|
// rotate both geoms, search for new contacts
|
|
for (int axis_id = 0; axis_id < 2; ++axis_id) {
|
|
for (int angle_id = 0; angle_id < 2; ++angle_id) {
|
|
mjtNum* axis = axes[axis_id];
|
|
mjtNum angle = angles[angle_id];
|
|
|
|
// make rotation matrix rot
|
|
mjtNum quat[4], rot[9];
|
|
mji_axisAngle2Quat(quat, axis, angle);
|
|
mju_quat2Mat(rot, quat);
|
|
|
|
// rotate g1 around initial contact point
|
|
mju_rotateFrame(con[0].pos, rot, obj1.mat, obj1.pos);
|
|
|
|
// inversely rotate g2 around initial contact point
|
|
mjtNum invrot[9];
|
|
mju_transpose(invrot, rot, 3, 3);
|
|
mju_rotateFrame(con[0].pos, invrot, obj2.mat, obj2.pos);
|
|
|
|
// search for new contact
|
|
int n = mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, margin);
|
|
if (mjDISABLED(mjDSBL_NATIVECCD) && n && g1 >= 0 && g2 >= 0) {
|
|
mjc_fixNormal(m, d, con + ncon, g1, g2);
|
|
}
|
|
|
|
// check new contact
|
|
if (n && mjc_isDistinctContact(con, ncon + 1, tolerance)) {
|
|
// set penetration of new point to equal that of initial point
|
|
con[ncon].dist = con[0].dist;
|
|
// add new point
|
|
ncon += 1;
|
|
}
|
|
|
|
// reset positions and orientations of g1 and g2
|
|
mji_copy3(obj1.pos, d->geom_xpos+3*g1);
|
|
mji_copy9(obj1.mat, d->geom_xmat+9*g1);
|
|
mji_copy3(obj2.pos, d->geom_xpos+3*g2);
|
|
mji_copy9(obj2.mat, d->geom_xmat+9*g2);
|
|
}
|
|
}
|
|
}
|
|
return ncon;
|
|
}
|
|
|
|
|
|
// parameters for plane-mesh extra contacts
|
|
const int maxplanemesh = 3;
|
|
const mjtNum tolplanemesh = 0.3;
|
|
|
|
// add one plane-mesh contact
|
|
static int addplanemesh(mjPreContact* con, const float vertex[3],
|
|
const mjtNum pos1[3], const mjtNum normal1[3],
|
|
const mjtNum pos2[3], const mjtNum mat2[9],
|
|
const mjtNum first[3], mjtNum rbound) {
|
|
// compute point in global coordinates
|
|
mjtNum pnt[3], v[3] = {vertex[0], vertex[1], vertex[2]};
|
|
mju_mulMatVec3(pnt, mat2, v);
|
|
mju_addTo3(pnt, pos2);
|
|
|
|
// skip if too close to first contact
|
|
if (mju_dist3(pnt, first) < tolplanemesh*rbound) {
|
|
return 0;
|
|
}
|
|
|
|
// pnt-pos difference vector
|
|
mjtNum dif[3];
|
|
mji_sub3(dif, pnt, pos1);
|
|
|
|
// set distance
|
|
con[0].dist = mju_dot3(normal1, dif);
|
|
|
|
// set position
|
|
mji_copy3(con[0].pos, pnt);
|
|
mji_addToScl3(con[0].pos, normal1, -0.5*con[0].dist);
|
|
|
|
// set frame
|
|
mji_copy3(con[0].normal, normal1);
|
|
mji_zero3(con[0].tangent);
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
// plane-convex collision, using libccd
|
|
int mjc_PlaneConvex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
|
|
const mjtNum* pos1 = d->geom_xpos + 3*g1;
|
|
const mjtNum* mat1 = d->geom_xmat + 9*g1;
|
|
const mjtNum* pos2 = d->geom_xpos + 3*g2;
|
|
const mjtNum* mat2 = d->geom_xmat + 9*g2;
|
|
|
|
mjtNum dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
|
|
ccd_vec3_t ccd_dir, ccd_vec;
|
|
mjCCDObj obj;
|
|
mjc_initCCDObj(&obj, m, d, g2, 0);
|
|
// get support point in -normal direction
|
|
ccdVec3Set(&ccd_dir, -mat1[2], -mat1[5], -mat1[8]);
|
|
mjccd_support(&obj, &ccd_dir, &ccd_vec);
|
|
|
|
// compute normal distance, return if too far
|
|
mji_sub3(dif, ccd_vec.v, pos1);
|
|
con[0].dist = mju_dot3(normal, dif);
|
|
if (con[0].dist > margin) {
|
|
return 0;
|
|
}
|
|
|
|
// fill in contact data
|
|
mji_copy3(con[0].pos, ccd_vec.v);
|
|
mji_addToScl3(con[0].pos, normal, -0.5*con[0].dist);
|
|
mji_copy3(con[0].normal, normal);
|
|
mji_zero3(con[0].tangent);
|
|
|
|
//--------------- add all/connected vertices below margin
|
|
float* vertdata;
|
|
int graphadr, numvert, locid;
|
|
int *vert_edgeadr, *vert_globalid, *edge_localid;
|
|
mjtNum vdot;
|
|
int count = 1, g = g2;
|
|
|
|
// g is an ellipsoid: no need for further mesh-specific processing
|
|
if (m->geom_dataid[g] == -1) {
|
|
return count;
|
|
}
|
|
|
|
// init
|
|
vertdata = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
|
|
|
|
// express dir in geom local frame
|
|
mjtNum locdir[3];
|
|
mju_mulMatTVec3(locdir, d->geom_xmat+9*g, ccd_dir.v);
|
|
|
|
// inclusion threshold along locdir, relative to geom2 center
|
|
mji_sub3(dif, pos2, pos1);
|
|
mjtNum threshold = mju_dot3(normal, dif) - margin;
|
|
|
|
// no graph data: exhaustive search
|
|
if (m->mesh_graphadr[m->geom_dataid[g]] < 0) {
|
|
// search all vertices, find best
|
|
for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]] && count < maxplanemesh; i++) {
|
|
// vdot = dot(vertex, dir)
|
|
vdot = locdir[0] * (mjtNum)vertdata[3*i] +
|
|
locdir[1] * (mjtNum)vertdata[3*i+1] +
|
|
locdir[2] * (mjtNum)vertdata[3*i+2];
|
|
|
|
// detect contact, skip best
|
|
if (vdot > threshold && i != obj.meshindex) {
|
|
count += addplanemesh(con+count, vertdata+3*i,
|
|
pos1, normal, pos2, mat2,
|
|
con->pos, m->geom_rbound[g2]);
|
|
}
|
|
}
|
|
}
|
|
|
|
// use graph data
|
|
else if (obj.meshindex >= 0) {
|
|
// get info
|
|
graphadr = m->mesh_graphadr[m->geom_dataid[g]];
|
|
numvert = m->mesh_graph[graphadr];
|
|
vert_edgeadr = m->mesh_graph + graphadr + 2;
|
|
vert_globalid = m->mesh_graph + graphadr + 2 + numvert;
|
|
edge_localid = m->mesh_graph + graphadr + 2 + 2*numvert;
|
|
|
|
// look for contacts in ibest neighborhood
|
|
int i = vert_edgeadr[obj.meshindex];
|
|
while ((locid=edge_localid[i]) >= 0 && count < maxplanemesh) {
|
|
// vdot = dot(vertex, dir)
|
|
vdot = locdir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] +
|
|
locdir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] +
|
|
locdir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2];
|
|
|
|
// detect contact
|
|
if (vdot > threshold) {
|
|
count += addplanemesh(con+count, vertdata+3*vert_globalid[locid],
|
|
pos1, normal, pos2, mat2,
|
|
con->pos, m->geom_rbound[g2]);
|
|
}
|
|
|
|
// advance to next edge
|
|
i++;
|
|
}
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
|
|
//---------------------------- heightfield collisions ---------------------------------------------
|
|
|
|
|
|
// add vertex to prism
|
|
static inline void addVert(mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) {
|
|
// move old data
|
|
mji_copy3(obj->data.hfield.prism[0], obj->data.hfield.prism[1]);
|
|
mji_copy3(obj->data.hfield.prism[1], obj->data.hfield.prism[2]);
|
|
mji_copy3(obj->data.hfield.prism[3], obj->data.hfield.prism[4]);
|
|
mji_copy3(obj->data.hfield.prism[4], obj->data.hfield.prism[5]);
|
|
|
|
// add new vertex at last position
|
|
obj->data.hfield.prism[2][0] = obj->data.hfield.prism[5][0] = x;
|
|
obj->data.hfield.prism[2][1] = obj->data.hfield.prism[5][1] = y;
|
|
obj->data.hfield.prism[5][2] = z;
|
|
}
|
|
|
|
|
|
// add vertex to prism
|
|
static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, mjtNum dy, mjtNum margin) {
|
|
// move old data
|
|
mji_copy3(obj->data.hfield.prism[0], obj->data.hfield.prism[1]);
|
|
mji_copy3(obj->data.hfield.prism[1], obj->data.hfield.prism[2]);
|
|
mji_copy3(obj->data.hfield.prism[3], obj->data.hfield.prism[4]);
|
|
mji_copy3(obj->data.hfield.prism[4], obj->data.hfield.prism[5]);
|
|
|
|
int dr = 1 - i;
|
|
|
|
// add new vertex at last position
|
|
obj->data.hfield.prism[2][0] = obj->data.hfield.prism[5][0] = dx*c - obj->size[0];
|
|
obj->data.hfield.prism[2][1] = obj->data.hfield.prism[5][1] = dy*(r + dr) - obj->size[1];
|
|
obj->data.hfield.prism[5][2] = obj->data.hfield.hfield_data[(r + dr)*obj->data.hfield.hfield_ncol + c]*obj->size[2];
|
|
|
|
// factor in margin
|
|
obj->data.hfield.prism[5][2] += margin;
|
|
}
|
|
|
|
|
|
// entry point for heightfield collisions
|
|
int mjc_ConvexHField(const mjModel* m, mjData* d, mjPreContact* 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;
|
|
|
|
// geom2 frame
|
|
mjtNum* pos2 = d->geom_xpos + 3*g2;
|
|
mjtNum* mat2 = d->geom_xmat + 9*g2;
|
|
|
|
// hfield data
|
|
int hid = m->geom_dataid[g1];
|
|
int nrow = m->hfield_nrow[hid];
|
|
int ncol = m->hfield_ncol[hid];
|
|
mjtNum size0 = m->hfield_size[4*hid + 0], size1 = m->hfield_size[4*hid + 1];
|
|
mjtNum size2 = m->hfield_size[4*hid + 2], size3 = m->hfield_size[4*hid + 3];
|
|
|
|
// try early return using box-sphere test
|
|
|
|
// express geom2 pos in hfield frame
|
|
mjtNum local_pos[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
|
|
mju_mulMatTVec3(local_pos, mat1, local_pos);
|
|
|
|
// sphere radius is geom2 rbound + margin
|
|
mjtNum radius = m->geom_rbound[g2] + margin;
|
|
|
|
// box-sphere test
|
|
if ((size0 < local_pos[0] - radius) || (-size0 > local_pos[0] + radius) ||
|
|
(size1 < local_pos[1] - radius) || (-size1 > local_pos[1] + radius) ||
|
|
(size2 < local_pos[2] - radius) || (-size3 > local_pos[2] + radius)) {
|
|
return 0;
|
|
}
|
|
|
|
// ccd set up
|
|
mjCCDObj obj1, obj2;
|
|
mjc_initCCDObj(&obj1, m, d, g1, 0);
|
|
mjc_initCCDObj(&obj2, m, d, g2, 0);
|
|
|
|
|
|
// try early return using AABB box-box test
|
|
|
|
// express geom2 mat in hfield frame
|
|
mjtNum mat[9];
|
|
mji_mulMatTMat3(mat, mat1, mat2);
|
|
|
|
mji_copy9(obj2.mat, mat);
|
|
mji_copy3(obj2.pos, local_pos);
|
|
|
|
mjtNum local_dir[3] = {0, 0, 0}, res[3];
|
|
|
|
// get support point in +X
|
|
local_dir[0] = 1;
|
|
obj2.support(res, &obj2, local_dir);
|
|
mjtNum xmax = res[0];
|
|
|
|
// get support point in -X
|
|
local_dir[0] = -1;
|
|
obj2.support(res, &obj2, local_dir);
|
|
mjtNum xmin = res[0];
|
|
|
|
local_dir[0] = 0;
|
|
|
|
// get support point in +Y
|
|
local_dir[1] = 1;
|
|
obj2.support(res, &obj2, local_dir);
|
|
mjtNum ymax = res[1];
|
|
|
|
// get support point in -Y
|
|
local_dir[1] = -1;
|
|
obj2.support(res, &obj2, local_dir);
|
|
mjtNum ymin = res[1];
|
|
|
|
local_dir[1] = 0;
|
|
|
|
// get support point in +Z
|
|
local_dir[2] = 1;
|
|
obj2.support(res, &obj2, local_dir);
|
|
mjtNum zmax = res[2];
|
|
|
|
// get support point in -Z
|
|
local_dir[2] = -1;
|
|
obj2.support(res, &obj2, local_dir);
|
|
mjtNum zmin = res[2];
|
|
|
|
// AABB box-box test
|
|
if ((xmin - margin > size0) || (xmax + margin < -size0) ||
|
|
(ymin - margin > size1) || (ymax + margin < -size1) ||
|
|
(zmin - margin > size2) || (zmax + margin < -size3)) {
|
|
return 0;
|
|
}
|
|
|
|
// compute sub-grid bounds
|
|
int cmin = (int) mju_floor((xmin + size0) / (2.0*size0) * (ncol-1));
|
|
int cmax = (int) mju_ceil ((xmax + size0) / (2.0*size0) * (ncol-1));
|
|
int rmin = (int) mju_floor((ymin + size1) / (2.0*size1) * (nrow-1));
|
|
int rmax = (int) mju_ceil ((ymax + size1) / (2.0*size1) * (nrow-1));
|
|
cmin = mjMAX(0, cmin);
|
|
cmax = mjMIN(ncol-1, cmax);
|
|
rmin = mjMAX(0, rmin);
|
|
rmax = mjMIN(nrow-1, rmax);
|
|
|
|
// geom margin needed for actual collision test
|
|
obj2.margin = margin;
|
|
|
|
// compute real-valued grid step
|
|
mjtNum dx = (2.0*size0) / (ncol-1);
|
|
mjtNum dy = (2.0*size1) / (nrow-1);
|
|
|
|
// set zbottom value using base size
|
|
mjtNum (*prism)[3] = obj1.data.hfield.prism;
|
|
prism[0][2] = prism[1][2] = prism[2][2] = -size3;
|
|
|
|
// process all prisms in subgrid
|
|
int ncon = 0;
|
|
for (int r=rmin; r < rmax; r++) {
|
|
addPrismVert(&obj1, r, cmin, 0, dx, dy, margin);
|
|
addPrismVert(&obj1, r, cmin, 1, dx, dy, margin);
|
|
for (int c=cmin + 1; c <= cmax; c++) {
|
|
for (int i=0; i < 2; i++) {
|
|
// send vertex to prism constructor
|
|
addPrismVert(&obj1, r, c, i, dx, dy, margin);
|
|
|
|
// prism height test
|
|
if (prism[3][2] < zmin && prism[4][2] < zmin && prism[5][2] < zmin) {
|
|
continue;
|
|
}
|
|
|
|
// run penetration function, save contact
|
|
if (mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, 0.0)) {
|
|
// transform to global coordinates
|
|
mji_copy3(local_dir, con[ncon].normal);
|
|
mji_copy3(local_pos, con[ncon].pos);
|
|
mji_mulMatVec3(con[ncon].normal, mat1, local_dir);
|
|
mji_mulMatVec3(con[ncon].pos, mat1, local_pos);
|
|
mji_addTo3(con[ncon].pos, pos1);
|
|
|
|
// force out of all loops if max contacts reached
|
|
if (++ncon >= mjMAXCONPAIR) {
|
|
r = rmax+1;
|
|
c = cmax+1;
|
|
i = 3;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (mjDISABLED(mjDSBL_NATIVECCD)) {
|
|
// fix contact normals
|
|
for (int i=0; i < ncon; i++) {
|
|
mjc_fixNormal(m, d, con + i, g1, g2);
|
|
}
|
|
}
|
|
|
|
return ncon;
|
|
}
|
|
|
|
|
|
//--------------------------- fix contact frame normal ---------------------------------------------
|
|
|
|
// compute normal for point outside ellipsoid, using ray-projection SQP
|
|
static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum size[3]) {
|
|
// algorithm constants
|
|
const int maxiter = 30;
|
|
const mjtNum tolerance = 1e-6;
|
|
|
|
// precompute quantities
|
|
mjtNum S2inv[3] = {1/(size[0]*size[0]), 1/(size[1]*size[1]), 1/(size[2]*size[2])};
|
|
mjtNum C = pos[0]*pos[0]*S2inv[0] + pos[1]*pos[1]*S2inv[1] + pos[2]*pos[2]*S2inv[2] - 1;
|
|
if (C > 0) {
|
|
return 0;
|
|
}
|
|
|
|
// normalize initial normal (just in case)
|
|
mju_normalize3(nrm);
|
|
|
|
// main iteration
|
|
int iter;
|
|
for (iter=0; iter < maxiter; iter++) {
|
|
// coefficients and determinant of quadratic
|
|
mjtNum A = nrm[0]*nrm[0]*S2inv[0] + nrm[1]*nrm[1]*S2inv[1] + nrm[2]*nrm[2]*S2inv[2];
|
|
mjtNum B = pos[0]*nrm[0]*S2inv[0] + pos[1]*nrm[1]*S2inv[1] + pos[2]*nrm[2]*S2inv[2];
|
|
mjtNum det = B*B - A*C;
|
|
if (det < mjMINVAL || A < mjMINVAL) {
|
|
return (iter > 0);
|
|
}
|
|
|
|
// ray intersection with ellipse: pos + x*nrm, x>=0
|
|
mjtNum x = (-B + mju_sqrt(det))/A;
|
|
if (x < 0) {
|
|
return (iter > 0);
|
|
}
|
|
|
|
// new point on ellipsoid
|
|
mjtNum pnt[3];
|
|
mji_addScl3(pnt, pos, nrm, x);
|
|
|
|
// normal at new point
|
|
mjtNum newnrm[3] = {pnt[0]*S2inv[0], pnt[1]*S2inv[1], pnt[2]*S2inv[2]};
|
|
mju_normalize3(newnrm);
|
|
|
|
// save change and assign
|
|
mjtNum change = mju_dist3(nrm, newnrm);
|
|
mji_copy3(nrm, newnrm);
|
|
|
|
// terminate if converged
|
|
if (change < tolerance) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
// compute normal for point inside ellipsoid, using diagonal QCQP
|
|
static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum size[3]) {
|
|
// algorithm constants
|
|
const int maxiter = 30;
|
|
const mjtNum tolerance = 1e-6;
|
|
|
|
// precompute quantities
|
|
mjtNum S2[3] = {size[0]*size[0], size[1]*size[1], size[2]*size[2]};
|
|
mjtNum PS2[3] = {pos[0]*pos[0]*S2[0], pos[1]*pos[1]*S2[1], pos[2]*pos[2]*S2[2]};
|
|
|
|
// main iteration
|
|
mjtNum la = 0;
|
|
int iter;
|
|
for (iter=0; iter < maxiter; iter++) {
|
|
// precompute 1/(s^2+la)
|
|
mjtNum R[3] = {1/(S2[0]+la), 1/(S2[1]+la), 1/(S2[2]+la)};
|
|
|
|
// value
|
|
mjtNum val = PS2[0]*R[0]*R[0] + PS2[1]*R[1]*R[1] + PS2[2]*R[2]*R[2] - 1;
|
|
if (val < tolerance) {
|
|
break;
|
|
}
|
|
|
|
// derivative
|
|
mjtNum deriv = -2*(PS2[0]*R[0]*R[0]*R[0] + PS2[1]*R[1]*R[1]*R[1] + PS2[2]*R[2]*R[2]*R[2]);
|
|
if (deriv > -mjMINVAL) {
|
|
break;
|
|
}
|
|
|
|
// delta
|
|
mjtNum delta = -val/deriv;
|
|
if (delta < tolerance) {
|
|
break;
|
|
}
|
|
|
|
// update
|
|
la += delta;
|
|
}
|
|
|
|
// compute normal given lambda
|
|
nrm[0] = pos[0]/(S2[0]+la);
|
|
nrm[1] = pos[1]/(S2[1]+la);
|
|
nrm[2] = pos[2]/(S2[2]+la);
|
|
mju_normalize3(nrm);
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
// fix normals if required
|
|
void mjc_fixNormal(const mjModel* m, const mjData* d, mjPreContact* con, int g1, int g2) {
|
|
mjtNum dst1, dst2;
|
|
|
|
// get geom ids and types
|
|
int gid[2] = {g1, g2};
|
|
mjtGeom type[2];
|
|
for (int i=0; i < 2; i++) {
|
|
if (gid[i] < 0) {
|
|
type[i] = mjGEOM_NONE;
|
|
} else {
|
|
type[i] = m->geom_type[gid[i]];
|
|
}
|
|
|
|
// set to mjGEOM_NONE if type cannot be processed
|
|
if (type[i] != mjGEOM_SPHERE && type[i] != mjGEOM_CAPSULE &&
|
|
type[i] != mjGEOM_ELLIPSOID && type[i] != mjGEOM_CYLINDER) {
|
|
type[i] = mjGEOM_NONE;
|
|
}
|
|
}
|
|
|
|
// neither type can be processed: nothing to do
|
|
if (type[0] == mjGEOM_NONE && type[1] == mjGEOM_NONE) {
|
|
return;
|
|
}
|
|
|
|
// init normals
|
|
mjtNum normal[2][3] = {
|
|
{con->normal[0], con->normal[1], con->normal[2]},
|
|
{-con->normal[0], -con->normal[1], -con->normal[2]}
|
|
};
|
|
|
|
// process geoms in type range
|
|
int processed[2] = {0, 0};
|
|
for (int i=0; i < 2; i++) {
|
|
if (type[i] != mjGEOM_NONE) {
|
|
// get geom mat and size
|
|
mjtNum* mat = d->geom_xmat + 9*gid[i];
|
|
mjtNum* size = m->geom_size + 3*gid[i];
|
|
|
|
// map contact point and normal to local frame
|
|
mjtNum dif[3], pos1[3], nrm[3];
|
|
mju_sub3(dif, con->pos, d->geom_xpos+3*gid[i]);
|
|
mju_mulMatTVec3(pos1, mat, dif);
|
|
mju_mulMatTVec3(nrm, mat, normal[i]);
|
|
|
|
// process according to type
|
|
switch (type[i]) {
|
|
case mjGEOM_SPHERE:
|
|
mji_copy3(nrm, pos1);
|
|
processed[i] = 1;
|
|
break;
|
|
|
|
case mjGEOM_CAPSULE:
|
|
// Z: bottom cap
|
|
if (pos1[2] < -size[1]) {
|
|
nrm[2] = pos1[2]+size[1];
|
|
}
|
|
|
|
// Z: top cap
|
|
else if (pos1[2] > size[1]) {
|
|
nrm[2] = pos1[2]-size[1];
|
|
}
|
|
|
|
// Z: cylinder
|
|
else {
|
|
nrm[2] = 0;
|
|
}
|
|
|
|
// copy XY
|
|
nrm[0] = pos1[0];
|
|
nrm[1] = pos1[1];
|
|
processed[i] = 1;
|
|
break;
|
|
|
|
case mjGEOM_ELLIPSOID:
|
|
// guard against invalid ellipsoid size (just in case)
|
|
if (size[0] < mjMINVAL || size[1] < mjMINVAL || size[2] < mjMINVAL) {
|
|
break;
|
|
}
|
|
|
|
// compute elliptic distance^2
|
|
dst1 = pos1[0]*pos1[0]/(size[0]*size[0]) +
|
|
pos1[1]*pos1[1]/(size[1]*size[1]) +
|
|
pos1[2]*pos1[2]/(size[2]*size[2]);
|
|
|
|
// dispatch to inside or outside solver
|
|
if (dst1 <= 1) {
|
|
processed[i] = mjc_ellipsoidInside(nrm, pos1, size);
|
|
} else {
|
|
processed[i] = mjc_ellipsoidOutside(nrm, pos1, size);
|
|
}
|
|
break;
|
|
|
|
case mjGEOM_CYLINDER:
|
|
// skip if within 5% length of flat wall
|
|
if (mju_abs(pos1[2]) > 0.95*size[1]) {
|
|
break;
|
|
}
|
|
|
|
// compute distances to flat and round wall
|
|
dst1 = mju_abs(size[1]-mju_abs(pos1[2]));
|
|
dst2 = mju_abs(size[0]-mju_norm(pos1, 2));
|
|
|
|
// require 4x closer to round than flat wall
|
|
if (dst1 < 0.25*dst2) {
|
|
break;
|
|
}
|
|
|
|
// set normal for round wall
|
|
nrm[0] = pos1[0];
|
|
nrm[1] = pos1[1];
|
|
nrm[2] = 0;
|
|
processed[i] = 1;
|
|
break;
|
|
|
|
default:
|
|
// do nothing: only sphere, capsule, ellipsoid and cylinder are processed
|
|
break;
|
|
}
|
|
|
|
// normalize and map normal to global frame
|
|
if (processed[i]) {
|
|
mju_normalize3(nrm);
|
|
mji_mulMatVec3(normal[i], mat, nrm);
|
|
}
|
|
}
|
|
}
|
|
|
|
// both processed: average
|
|
if (processed[0] && processed[1]) {
|
|
mji_sub3(con->normal, normal[0], normal[1]);
|
|
mju_normalize3(con->normal);
|
|
}
|
|
|
|
// first processed: copy
|
|
else if (processed[0]) {
|
|
mji_copy3(con->normal, normal[0]);
|
|
}
|
|
|
|
// second processed: copy reverse
|
|
else if (processed[1]) {
|
|
mji_scl3(con->normal, normal[1], -1);
|
|
}
|
|
}
|
|
|
|
|
|
//---------------------------- flex collisions ---------------------------------------------
|
|
|
|
// geom-elem or elem-elem or vert-elem convex collision using ccd
|
|
int mjc_ConvexElem(const mjModel* m, mjData* d, mjPreContact* 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);
|
|
mjc_setCCDObjFlex(&obj1, f1, e1, v1);
|
|
mjc_setCCDObjFlex(&obj2, f2, e2, -1);
|
|
|
|
// find contacts
|
|
int ncon = mjc_penetration(m, d, &obj1, &obj2, con, 1, margin);
|
|
|
|
// fix normals for 2D flex
|
|
if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) {
|
|
// check if either object is a 2D flex
|
|
int isflex2d = 0;
|
|
if (f1 >= 0 && m->flex_dim[f1] == 2) isflex2d = 1;
|
|
if (f2 >= 0 && m->flex_dim[f2] == 2) isflex2d = 1;
|
|
|
|
if (isflex2d) {
|
|
for (int i = 0; i < ncon; i++) {
|
|
mjc_fixNormal(m, d, con + i, g1, -1);
|
|
}
|
|
}
|
|
}
|
|
return ncon;
|
|
}
|
|
|
|
|
|
// test a height field and a flex element for collision
|
|
int mjc_HFieldElem(const mjModel* m, mjData* d, mjPreContact* 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;
|
|
mjCCDObj obj1;
|
|
mjc_initCCDObj(&obj1, m, d, g, 0);
|
|
|
|
// get hfield info
|
|
int hid = m->geom_dataid[g];
|
|
int nrow = m->hfield_nrow[hid];
|
|
int ncol = m->hfield_ncol[hid];
|
|
const mjtNum* hpos = d->geom_xpos + 3*g;
|
|
const mjtNum* hmat = d->geom_xmat + 9*g;
|
|
const mjtNum* hsize = m->hfield_size + 4*hid;
|
|
const float* hdata = m->hfield_data + m->hfield_adr[hid];
|
|
|
|
// get elem indo
|
|
int dim = m->flex_dim[f];
|
|
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
|
|
mjtNum* evert[4] = {NULL, NULL, NULL, NULL};
|
|
for (int i=0; i <= dim; i++) {
|
|
evert[i] = d->flexvert_xpos + 3*(m->flex_vertadr[f] + edata[i]);
|
|
}
|
|
mjtNum* ecenter = d->flexelem_aabb + 6*(m->flex_elemadr[f]+e);
|
|
|
|
// ccd-related
|
|
mjCCDObj obj2;
|
|
mjc_initCCDObj(&obj2, m, d, -1, margin);
|
|
mjc_setCCDObjFlex(&obj2, f, e, -1);
|
|
//------------------------------------- AABB computation, box-box test
|
|
|
|
// save elem vertices, transform to hfield frame
|
|
mjtNum savevert[4][3];
|
|
for (int i=0; i <= dim; i++) {
|
|
mji_copy3(savevert[i], evert[i]);
|
|
mji_sub3(vec, evert[i], hpos);
|
|
mji_mulMatTVec3(evert[i], hmat, vec);
|
|
}
|
|
|
|
// save elem center, transform to hfield frame
|
|
mjtNum savecenter[3];
|
|
mji_copy3(savecenter, ecenter);
|
|
mji_sub3(vec, ecenter, hpos);
|
|
mji_mulMatTVec3(ecenter, hmat, vec);
|
|
|
|
// compute elem bounding box (in hfield frame)
|
|
xmin = xmax = evert[0][0];
|
|
ymin = ymax = evert[0][1];
|
|
zmin = zmax = evert[0][2];
|
|
for (int i=1; i <= dim; i++) {
|
|
xmin = mju_min(xmin, evert[i][0]);
|
|
xmax = mju_max(xmax, evert[i][0]);
|
|
ymin = mju_min(ymin, evert[i][1]);
|
|
ymax = mju_max(ymax, evert[i][1]);
|
|
zmin = mju_min(zmin, evert[i][2]);
|
|
zmax = mju_max(zmax, evert[i][2]);
|
|
}
|
|
|
|
// box-box test
|
|
if ((xmin-margin > hsize[0]) || (xmax+margin < -hsize[0]) ||
|
|
(ymin-margin > hsize[1]) || (ymax+margin < -hsize[1]) ||
|
|
(zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) {
|
|
// restore vertices and center
|
|
for (int i=0; i <= dim; i++) {
|
|
mji_copy3(evert[i], savevert[i]);
|
|
}
|
|
mji_copy3(ecenter, savecenter);
|
|
|
|
return 0;
|
|
}
|
|
|
|
// compute sub-grid bounds
|
|
cmin = (int) mju_floor((xmin + hsize[0]) / (2*hsize[0]) * (ncol-1));
|
|
cmax = (int) mju_ceil ((xmax + hsize[0]) / (2*hsize[0]) * (ncol-1));
|
|
rmin = (int) mju_floor((ymin + hsize[1]) / (2*hsize[1]) * (nrow-1));
|
|
rmax = (int) mju_ceil ((ymax + hsize[1]) / (2*hsize[1]) * (nrow-1));
|
|
cmin = mjMAX(0, cmin);
|
|
cmax = mjMIN(ncol-1, cmax);
|
|
rmin = mjMAX(0, rmin);
|
|
rmax = mjMIN(nrow-1, rmax);
|
|
|
|
//------------------------------------- collision testing
|
|
|
|
// compute real-valued grid step, and triangulation direction
|
|
dx = (2.0*hsize[0]) / (ncol-1);
|
|
dy = (2.0*hsize[1]) / (nrow-1);
|
|
dr[0] = 1;
|
|
dr[1] = 0;
|
|
|
|
// set zbottom value using base size
|
|
mjtNum (*prism)[3] = obj1.data.hfield.prism;
|
|
prism[0][2] = prism[1][2] = prism[2][2] = -hsize[3];
|
|
|
|
// process all prisms in sub-grid
|
|
cnt = 0;
|
|
for (int r=rmin; r < rmax; r++) {
|
|
int nvert = 0;
|
|
for (int c=cmin; c <= cmax; c++) {
|
|
for (int k=0; k < 2; k++) {
|
|
// send vertex to prism constructor
|
|
addVert(&obj1, dx*c-hsize[0], dy*(r+dr[k])-hsize[1],
|
|
hdata[(r+dr[k])*ncol+c]*hsize[2]+margin);
|
|
|
|
// check for enough vertices
|
|
if (++nvert > 2) {
|
|
// prism height test
|
|
if (prism[3][2] < zmin && prism[4][2] < zmin && prism[5][2] < zmin) {
|
|
continue;
|
|
}
|
|
|
|
// run ccd, save contact
|
|
if (mjc_penetration(m, d, &obj1, &obj2, con + cnt, 1, 0.0)) {
|
|
// transform to global coordinates
|
|
mji_zero3(con[cnt].tangent);
|
|
mju_mulMatVec3(con[cnt].normal, hmat, con[cnt].normal);
|
|
mju_mulMatVec3(con[cnt].pos, hmat, con[cnt].pos);
|
|
mji_addTo3(con[cnt].pos, hpos);
|
|
|
|
// count, stop if max number reached
|
|
cnt++;
|
|
if (cnt >= mjMAXCONPAIR) {
|
|
r = rmax+1;
|
|
c = cmax+1;
|
|
k = 3;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// restore elem vertices and center
|
|
for (int i=0; i <= dim; i++) {
|
|
mji_copy3(evert[i], savevert[i]);
|
|
}
|
|
mji_copy3(ecenter, savecenter);
|
|
|
|
return cnt;
|
|
}
|