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_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_; }