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

Remove pointers from BVH for caching in mjCMesh.

Browse Source 
PiperOrigin-RevId: 736849392
Change-Id: I684cca918c65718b29a1bcb41ecd6bad8d88f8f9
This commit is contained in:
Kyle Bayes
2025-03-14 07:00:23 -07:00
committed by Copybara-Service
parent f25fc63f0f
commit 1a67aaf1b7
4 changed files with 166 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+31 -34
View File
@@ -21,6 +21,7 @@
#include <cstdio>
#include <cstring>
#include <functional>
#include <limits>
#include <memory>
#include <set>
#include <string>
@@ -648,9 +649,10 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
// make bounding volume hierarchy
if (tree_.Bvh().empty()) {
face_aabb_.assign(6*nface(), 0);
face_aabb_.clear();
face_aabb_.reserve(3*face_.size());
tree_.AllocateBoundingVolumes(nface());
for (int i=0; i < nface(); i++) {
for (int i = 0; i < nface(); i++) {
SetBoundingVolume(i);
}
tree_.CreateBVH();
@@ -667,29 +669,28 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
// get bounding volume
void mjCMesh::SetBoundingVolume(int faceid) {
mjCBoundingVolume* node = tree_.GetBoundingVolume(faceid);
node->SetId(faceid);
node->conaffinity = 1;
node->contype = 1;
node->pos = center_ + 3*faceid;
node->quat = NULL;
double face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
for (int j=0; j<3; j++) {
int vertid = face_[3*faceid+j];
face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]);
face_aamm[1] = mjMIN(face_aamm[1], vert_[3*vertid+1]);
face_aamm[2] = mjMIN(face_aamm[2], vert_[3*vertid+2]);
face_aamm[3] = mjMAX(face_aamm[3], vert_[3*vertid+0]);
face_aamm[4] = mjMAX(face_aamm[4], vert_[3*vertid+1]);
face_aamm[5] = mjMAX(face_aamm[5], vert_[3*vertid+2]);
constexpr double kMaxVal = std::numeric_limits<double>::max();
double face_aamm[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal};
for (int j = 0; j < 3; j++) {
int vertid = face_[3*faceid + j];
face_aamm[0] = std::min(face_aamm[0], vert_[3*vertid + 0]);
face_aamm[1] = std::min(face_aamm[1], vert_[3*vertid + 1]);
face_aamm[2] = std::min(face_aamm[2], vert_[3*vertid + 2]);
face_aamm[3] = std::max(face_aamm[3], vert_[3*vertid + 0]);
face_aamm[4] = std::max(face_aamm[4], vert_[3*vertid + 1]);
face_aamm[5] = std::max(face_aamm[5], vert_[3*vertid + 2]);
}
face_aabb_[6*faceid+0] = .5 * (face_aamm[0] + face_aamm[3]);
face_aabb_[6*faceid+1] = .5 * (face_aamm[1] + face_aamm[4]);
face_aabb_[6*faceid+2] = .5 * (face_aamm[2] + face_aamm[5]);
face_aabb_[6*faceid+3] = .5 * (face_aamm[3] - face_aamm[0]);
face_aabb_[6*faceid+4] = .5 * (face_aamm[4] - face_aamm[1]);
face_aabb_[6*faceid+5] = .5 * (face_aamm[5] - face_aamm[2]);
node->aabb = face_aabb_.data() + 6*faceid;
face_aabb_.push_back(.5 * (face_aamm[0] + face_aamm[3]));
face_aabb_.push_back(.5 * (face_aamm[1] + face_aamm[4]));
face_aabb_.push_back(.5 * (face_aamm[2] + face_aamm[5]));
face_aabb_.push_back(.5 * (face_aamm[3] - face_aamm[0]));
face_aabb_.push_back(.5 * (face_aamm[4] - face_aamm[1]));
face_aabb_.push_back(.5 * (face_aamm[5] - face_aamm[2]));
tree_.AddBoundingVolume(faceid, 1, 1, center_ + 3*faceid, nullptr,
&face_aabb_[6*faceid]);
}
@@ -3565,7 +3566,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// create flex BVH
void mjCFlex::CreateBVH(void) {
void mjCFlex::CreateBVH() {
int nbvh = 0;
// allocate element bounding boxes
@@ -3587,8 +3588,8 @@ void mjCFlex::CreateBVH(void) {
mjuu_copyvec(xmax, vertxpos.data() + 3*edata[0], 3);
for (int i=1; i <= dim; i++) {
for (int j=0; j<3; j++) {
xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]);
xmax[j] = mjMAX(xmax[j], vertxpos[3*edata[i]+j]);
xmin[j] = std::min(xmin[j], vertxpos[3*edata[i]+j]);
xmax[j] = std::max(xmax[j], vertxpos[3*edata[i]+j]);
}
}
@@ -3601,13 +3602,9 @@ void mjCFlex::CreateBVH(void) {
elemaabb_[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element
mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++);
bv->contype = contype;
bv->conaffinity = conaffinity;
bv->quat = NULL;
bv->SetId(e);
bv->aabb = elemaabb_.data() + 6*e;
bv->pos = bv->aabb;
const double* aabb = elemaabb_.data() + 6*e;
tree.AddBoundingVolume(e, contype, conaffinity, aabb, nullptr, aabb);
nbvh++;
}
// create hierarchy
src/user/user_model.cc
+3 -3
View File
@@ -2218,7 +2218,7 @@ void mjCModel::CopyTree(mjModel* m) {
memcpy(m->bvh_child + 2*bvh_adr, pb->tree.Child().data(), 2*pb->tree.Nbvh()*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pb->tree.Level().data(), pb->tree.Nbvh()*sizeof(int));
for (int i=0; i<pb->tree.Nbvh(); i++) {
m->bvh_nodeid[i + bvh_adr] = pb->tree.Nodeid(i) ? *(pb->tree.Nodeid(i)) : -1;
m->bvh_nodeid[i + bvh_adr] = pb->tree.Nodeidptr(i) ? *(pb->tree.Nodeidptr(i)) : -1;
}
}
bvh_adr += pb->tree.Nbvh();
@@ -2838,7 +2838,7 @@ void mjCModel::CopyObjects(mjModel* m) {
memcpy(m->bvh_child + 2*bvh_adr, pme->tree().Child().data(), 2*pme->tree().Nbvh()*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pme->tree().Level().data(), pme->tree().Nbvh()*sizeof(int));
for (int j=0; j<pme->tree().Nbvh(); j++) {
m->bvh_nodeid[j + bvh_adr] = pme->tree().Nodeid(j) ? *(pme->tree().Nodeid(j)) : -1;
m->bvh_nodeid[j + bvh_adr] = pme->tree().Nodeid(j) > -1 ? pme->tree().Nodeid(j) : -1;
}
}
@@ -2955,7 +2955,7 @@ void mjCModel::CopyObjects(mjModel* m) {
memcpy(m->bvh_child + 2*bvh_adr, pfl->tree.Child().data(), 2*pfl->tree.Nbvh()*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pfl->tree.Level().data(), pfl->tree.Nbvh()*sizeof(int));
for (int i=0; i<pfl->tree.Nbvh(); i++) {
m->bvh_nodeid[i+ bvh_adr] = pfl->tree.Nodeid(i) ? *(pfl->tree.Nodeid(i)) : -1;
m->bvh_nodeid[i+ bvh_adr] = pfl->tree.Nodeidptr(i) ? *(pfl->tree.Nodeidptr(i)) : -1;
}
}
src/user/user_objects.cc
+67 -62
View File
@@ -38,6 +38,7 @@
#include "cc/array_safety.h"
#include "engine/engine_passive.h"
#include <mujoco/mjspec.h>
#include <mujoco/mujoco.h>
#include "user/user_api.h"
#include "user/user_cache.h"
#include "user/user_model.h"
@@ -347,12 +348,6 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
//------------------------- class mjCBoundingVolumeHierarchy implementation ------------------------
// constructor
mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() {
mjuu_setvec(ipos_, 0, 0, 0);
mjuu_setvec(iquat_, 1, 0, 0, 0);
}
// assign position and orientation
void mjCBoundingVolumeHierarchy::Set(double ipos_element[3], double iquat_element[4]) {
@@ -369,7 +364,7 @@ void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) {
nodeid_.clear();
level_.clear();
bvleaf_.clear();
bvleaf_.resize(nleaf);
bvleaf_.reserve(nleaf);
}
@@ -377,9 +372,21 @@ void mjCBoundingVolumeHierarchy::RemoveInactiveVolumes(int nmax) {
bvleaf_.erase(bvleaf_.begin() + nmax, bvleaf_.end());
}
const mjCBoundingVolume*
mjCBoundingVolumeHierarchy::AddBoundingVolume(int id, int contype, int conaffinity,
const double* pos, const double* quat,
const double* aabb) {
bvleaf_.emplace_back(id, contype, conaffinity, pos, quat, aabb);
return &bvleaf_.back();
}
mjCBoundingVolume* mjCBoundingVolumeHierarchy::GetBoundingVolume(int id) {
return bvleaf_.data() + id;
const mjCBoundingVolume*
mjCBoundingVolumeHierarchy::AddBoundingVolume(const int* id, int contype, int conaffinity,
const double* pos, const double* quat,
const double* aabb) {
bvleaf_.emplace_back(id, contype, conaffinity, pos, quat, aabb);
return &bvleaf_.back();
}
@@ -391,12 +398,12 @@ void mjCBoundingVolumeHierarchy::CreateBVH() {
elements.reserve(bvleaf_.size());
double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
for (int i = 0; i < bvleaf_.size(); i++) {
if (bvleaf_[i].conaffinity || bvleaf_[i].contype) {
if (bvleaf_[i].Conaffinity() || bvleaf_[i].Contype()) {
BVElement element;
element.e = &bvleaf_[i];
double vert[3] = {element.e->pos[0] - ipos_[0],
element.e->pos[1] - ipos_[1],
element.e->pos[2] - ipos_[2]};
double vert[3] = {element.e->Pos(0) - ipos_[0],
element.e->Pos(1) - ipos_[1],
element.e->Pos(2) - ipos_[2]};
mjuu_rotVecQuat(element.lpos, vert, qinv);
elements.push_back(std::move(element));
}
@@ -412,7 +419,8 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
if (nelements == 0) {
return -1;
}
double AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
constexpr double kMaxVal = std::numeric_limits<double>::max();
double AAMM[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal};
// inverse transformation
double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
@@ -420,26 +428,26 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
// accumulate AAMM over elements
for (auto element = elements_begin; element != elements_end; ++element) {
// transform element aabb to aamm format
double aamm[6] = {element->e->aabb[0] - element->e->aabb[3],
element->e->aabb[1] - element->e->aabb[4],
element->e->aabb[2] - element->e->aabb[5],
element->e->aabb[0] + element->e->aabb[3],
element->e->aabb[1] + element->e->aabb[4],
element->e->aabb[2] + element->e->aabb[5]};
double aamm[6] = {element->e->AABB(0) - element->e->AABB(3),
element->e->AABB(1) - element->e->AABB(4),
element->e->AABB(2) - element->e->AABB(5),
element->e->AABB(0) + element->e->AABB(3),
element->e->AABB(1) + element->e->AABB(4),
element->e->AABB(2) + element->e->AABB(5)};
// update node AAMM
for (int v=0; v<8; v++) {
for (int v=0; v < 8; v++) {
double vert[3], box[3];
vert[0] = (v&1 ? aamm[3] : aamm[0]);
vert[1] = (v&2 ? aamm[4] : aamm[1]);
vert[2] = (v&4 ? aamm[5] : aamm[2]);
// rotate to the body inertial frame if specified
if (element->e->quat) {
mjuu_rotVecQuat(box, vert, element->e->quat);
box[0] += element->e->pos[0] - ipos_[0];
box[1] += element->e->pos[1] - ipos_[1];
box[2] += element->e->pos[2] - ipos_[2];
if (element->e->Quat()) {
mjuu_rotVecQuat(box, vert, element->e->Quat());
box[0] += element->e->Pos(0) - ipos_[0];
box[1] += element->e->Pos(1) - ipos_[1];
box[2] += element->e->Pos(2) - ipos_[2];
mjuu_rotVecQuat(vert, box, qinv);
}
@@ -453,10 +461,10 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
}
// inflate flat AABBs
for (int i=0; i<3; i++) {
if (std::abs(AAMM[i]-AAMM[i+3])<mjEPS) {
AAMM[i+0] -= mjEPS;
AAMM[i+3] += mjEPS;
for (int i = 0; i < 3; i++) {
if (std::abs(AAMM[i] - AAMM[i+3]) < mjEPS) {
AAMM[i + 0] -= mjEPS;
AAMM[i + 3] += mjEPS;
}
}
@@ -464,55 +472,62 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
int index = nbvh_++;
child_.push_back(-1);
child_.push_back(-1);
nodeid_.push_back(nullptr);
nodeid_.push_back(-1);
nodeidptr_.push_back(nullptr);
level_.push_back(lev);
// store bounding box of the current node
for (int i=0; i<3; i++) {
bvh_.push_back((AAMM[3+i] + AAMM[i]) / 2);
}
for (int i=0; i<3; i++) {
bvh_.push_back((AAMM[3+i] - AAMM[i]) / 2);
}
bvh_.push_back((AAMM[3] + AAMM[0]) / 2);
bvh_.push_back((AAMM[4] + AAMM[1]) / 2);
bvh_.push_back((AAMM[5] + AAMM[2]) / 2);
bvh_.push_back((AAMM[3] - AAMM[0]) / 2);
bvh_.push_back((AAMM[4] - AAMM[1]) / 2);
bvh_.push_back((AAMM[5] - AAMM[2]) / 2);
// leaf node, return
if (nelements==1) {
for (int i=0; i<2; i++) {
child_[2*index+i] = -1;
}
nodeid_[index] = (int*)elements_begin->e->GetId();
if (nelements == 1) {
child_[2*index + 0] = -1;
child_[2*index + 1] = -1;
nodeid_[index] = *elements_begin->e->Id();
nodeidptr_[index] = (int*)elements_begin->e->Id();
return index;
}
// find longest axis, by a margin of at least mjEPS, default to 0
int axis = 0;
double edges[3] = { AAMM[3]-AAMM[0], AAMM[4]-AAMM[1], AAMM[5]-AAMM[2] };
double edges[3] = {AAMM[3] - AAMM[0], AAMM[4] - AAMM[1], AAMM[5] - AAMM[2]};
if (edges[1] >= edges[0] + mjEPS) axis = 1;
if (edges[2] >= edges[axis] + mjEPS) axis = 2;
// find median along the axis
auto compare = [&](const BVElement& e1, const BVElement& e2) {
if (std::abs(e1.lpos[axis] - e2.lpos[axis]) > mjEPS) {
return e1.lpos[axis] < e2.lpos[axis];
}
// comparing pointers gives a stable sort, because they both come from the same array
return e1.e < e2.e;
};
// note: nth_element performs a partial sort of elements
BVElementCompare compare;
compare.axis = axis;
int m = nelements / 2;
std::nth_element(elements_begin, elements_begin + m, elements_end, compare);
// recursive calls
if (m > 0) {
child_[2*index+0] = MakeBVH(elements_begin, elements_begin + m, lev+1);
child_[2*index + 0] = MakeBVH(elements_begin, elements_begin + m, lev + 1);
}
if (m != nelements) {
child_[2*index+1] = MakeBVH(elements_begin + m, elements_end, lev+1);
child_[2*index + 1] = MakeBVH(elements_begin + m, elements_end, lev + 1);
}
// SHOULD NOT OCCUR
if (child_[2*index+0]==-1 && child_[2*index+1]==-1) {
if (child_[2*index + 0] == -1 && child_[2*index + 1] == -1) {
mju_error("this should have been a leaf, body=%s nelements=%d",
name_.c_str(), nelements);
}
if (lev>mjMAXTREEDEPTH) {
if (lev > mjMAXTREEDEPTH) {
mju_warning("max tree depth exceeded in body=%s", name_.c_str());
}
@@ -1595,8 +1610,9 @@ void mjCBody::ComputeBVH() {
tree.Set(ipos, iquat);
tree.AllocateBoundingVolumes(geoms.size());
for (int i=0; i<geoms.size(); i++) {
geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
for (const mjCGeom* geom : geoms) {
tree.AddBoundingVolume(&geom->id, geom->contype, geom->conaffinity,
geom->pos, geom->quat, geom->aabb);
}
tree.CreateBVH();
}
@@ -2463,17 +2479,6 @@ double mjCGeom::GetVolume() const {
void mjCGeom::SetBoundingVolume(mjCBoundingVolume* bv) const {
bv->SetId(&id);
bv->contype = contype;
bv->conaffinity = conaffinity;
bv->aabb = aabb;
bv->pos = pos;
bv->quat = quat;
}
// set geom diagonal inertia given density
void mjCGeom::SetInertia(void) {
// get from mesh
src/user/user_objects.h
+65 -36
View File
@@ -16,9 +16,9 @@
#define MUJOCO_SRC_USER_USER_OBJECTS_H_
#include <stdbool.h>
#include <algorithm>
#include <cstddef>
#include <array>
#include <cstdlib>
#include <functional>
#include <map>
#include <string>
@@ -27,6 +27,7 @@
#include <vector>
#include <mujoco/mjspec.h>
#include <mujoco/mujoco.h>
#include "user/user_cache.h"
#include "user/user_util.h"
#include <tiny_obj_loader.h>
@@ -96,21 +97,59 @@ const char* ResolveOrientation(double* quat, // set frame quat
// bounding volume
class mjCBoundingVolume {
public:
mjCBoundingVolume() { id_ = nullptr; };
mjCBoundingVolume(int id, int contype, int conaffinity, const double pos[3],
const double quat[4], const double aabb[6]) : contype_(contype),
conaffinity_(conaffinity), idval_(id) {
std::copy(pos, pos + 3, pos_.begin());
std::copy(aabb, aabb + 6, aabb_.begin());
quat_set_ = quat != nullptr;
if (quat_set_) {
std::copy(quat, quat + 4, quat_.begin());
}
}
int contype; // contact type
int conaffinity; // contact affinity
const double* aabb; // axis-aligned bounding box (center, size)
const double* pos; // position (set by user or Compile1)
const double* quat; // orientation (set by user or Compile1)
mjCBoundingVolume(const int* id, int contype, int conaffinity, const double pos[3],
const double quat[4], const double aabb[6]) : contype_(contype),
conaffinity_(conaffinity), id_(id) {
std::copy(pos, pos + 3, pos_.begin());
std::copy(aabb, aabb + 6, aabb_.begin());
quat_set_ = quat != nullptr;
if (quat_set_) {
std::copy(quat, quat + 4, quat_.begin());
}
}
const int* GetId() const { if (id_) return id_; else return &idval_; }
int Contype() const { return contype_; }
int Conaffinity() const { return conaffinity_; }
const double* AABB() const { return aabb_.data(); }
double AABB(int i) const { return aabb_[i]; }
const double* Pos() const { return pos_.data(); }
double Pos(int i) const { return pos_[i]; }
const double* Quat() const { return quat_set_ ? quat_.data() : nullptr; }
const int* Id() const { return id_ ? id_ : &idval_; }
void SetContype(int val) { contype_ = val; }
void SetConaffinity(int val) { conaffinity_ = val; }
void SetAABB(const double* aabb) { std::copy(aabb, aabb + 6, aabb_.begin()); }
void SetPos(const double* pos) { std::copy(pos, pos + 3, pos_.begin()); }
void SetQuat(const double* quat) {
quat_set_ = true;
std::copy(quat, quat + 4, quat_.begin());
}
void SetId(const int* id) { id_ = id; }
void SetId(int val) { idval_ = val; }
private:
int idval_; // local id copy for nodes not storing their id's (e.g. faces)
const int* id_; // pointer to object id
int contype_; // contact type
int conaffinity_; // contact affinity
std::array<double, 6> aabb_; // axis-aligned bounding box (center, size)
std::array<double, 3> pos_; // position (set by user or Compiler)
std::array<double, 4> quat_; // orientation (set by user or Compiler)
bool quat_set_; // boolean flag is quat_ has been set
int idval_; // local id copy for nodes not storing their id's (e.g. faces)
// pointer to object id
const int* id_ = nullptr;
};
@@ -118,33 +157,38 @@ class mjCBoundingVolume {
struct mjCBoundingVolumeHierarchy_ {
protected:
int nbvh_ = 0;
std::vector<mjtNum> bvh_; // bounding boxes (nbvh x 6)
std::vector<int> child_; // children of each node (nbvh x 2)
std::vector<int*> nodeid_; // id of elem contained by the node (nbvh x 1)
std::vector<int> level_; // levels of each node (nbvh x 1)
std::vector<mjtNum> bvh_; // bounding boxes (nbvh x 6)
std::vector<int> child_; // children of each node (nbvh x 2)
std::vector<int> nodeid_; // id of elem contained by the node (nbvh x 1)
std::vector<int*> nodeidptr_; // ptr to id of elem contained by the node (nbvh x 1)
std::vector<int> level_; // levels of each node (nbvh x 1)
std::vector<mjCBoundingVolume> bvleaf_;
std::string name_;
double ipos_[3];
double iquat_[4];
double ipos_[3] = {0, 0, 0};
double iquat_[4] = {1, 0, 0, 0};
};
class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
public:
mjCBoundingVolumeHierarchy();
// make bounding volume hierarchy
void CreateBVH();
void Set(double ipos_element[3], double iquat_element[4]);
void AllocateBoundingVolumes(int nleaf);
void RemoveInactiveVolumes(int nmax);
mjCBoundingVolume* GetBoundingVolume(int id);
const mjCBoundingVolume*
AddBoundingVolume(int id, int contype, int conaffinity, const double pos[3],
const double quat[4], const double aabb[6]);
const mjCBoundingVolume*
AddBoundingVolume(const int* id, int contype, int conaffinity, const double pos[3],
const double quat[4], const double aabb[6]);
// public accessors
int Nbvh() const { return nbvh_; }
const std::vector<mjtNum>& Bvh() const { return bvh_; }
const std::vector<int>& Child() const { return child_; }
const std::vector<int*>& Nodeid() const { return nodeid_; }
const int* Nodeid(int id) const { return nodeid_[id]; }
const std::vector<int>& Nodeid() const { return nodeid_; }
int Nodeid(int id) const { return nodeid_[id]; }
const int* Nodeidptr(int id) const { return nodeidptr_[id]; }
const std::vector<int>& Level() const { return level_; }
private:
@@ -155,18 +199,6 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
double lpos[3];
};
struct BVElementCompare {
int axis = 0;
bool operator()(const BVElement& e1, const BVElement& e2) const {
if (std::abs(e1.lpos[axis] - e2.lpos[axis]) > mjEPS) {
return e1.lpos[axis] < e2.lpos[axis];
}
// comparing pointers gives a stable sort, because they both come from the same array
return e1.e < e2.e;
}
};
int MakeBVH(std::vector<BVElement>::iterator elements_begin,
std::vector<BVElement>::iterator elements_end, int lev = 0);
};
@@ -578,9 +610,6 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
// Compute the kappa coefs of the added inertia due to the surrounding fluid.
double GetAddedMassKappa(double dx, double dy, double dz);
// sets properties of a bounding volume
void SetBoundingVolume(mjCBoundingVolume* bv) const;
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() const { return userdata_; }
const std::string& get_hfieldname() const { return spec_hfieldname_; }