Add mjCBoundingVolume class to represent an element of mjCBoundingVolumeHierarchy (for example an mjCGeom).

PiperOrigin-RevId: 527906004
Change-Id: Ibc3bdac0ec6d85c7fd56fd2057f5a63970e8b1d4

src/user/user_objects.h

@@ -114,6 +114,26 @@ class mjCAlternative {
 };
 
 
+
+
+
+
+
+//------------------------- class mjCBoundingVolumeHierarchy ---------------------------------------
+// bounding volume
+class mjCBoundingVolume {
+ public:
+  mjCBoundingVolume() = default;
+
+  int id;                       // object id
+  int contype;                  // contact type
+  int conaffinity;              // contact affinity
+  const mjtNum* aabb;           // half-sizes of axis-aligned bounding box
+  const mjtNum* pos;            // position (set by user or Compile1)
+  const mjtNum* quat;           // orientation (set by user or Compile1)
+};
+
+
 // bounding volume hierarchy
 class mjCBoundingVolumeHierarchy {
  public:
@@ -125,10 +145,15 @@ class mjCBoundingVolumeHierarchy {
   std::vector<int> nodeid;            // id of the geom contained by the node          (nbvh x 1)
   std::vector<int> level;             // levels of each node                           (nbvh x 1)
 
-  int MakeBVH(std::vector<mjCGeom *>&, int lev = 0);  // make bounding volume hierarchy
+  // make bounding volume hierarchy
+  void CreateBVH();
   void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]);
+  void AddBundingVolume(const mjCBoundingVolume& bv);
 
  private:
+  int MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev = 0);
+
+  std::vector<mjCBoundingVolume> bvh_;
   std::string name_;
   double ipos_[3];
   double iquat_[4];
@@ -320,6 +345,9 @@ class mjCGeom : public mjCBase {
   // Compute the kappa coefs of the added inertia due to the surrounding fluid.
   double GetAddedMassKappa(double dx, double dy, double dz);
 
+  // returns a bounding volume
+  mjCBoundingVolume GetBoundingVolume() const;
+
   // variables set by user and copied into mjModel
   mjtGeom type;                   // geom type
   int contype;                    // contact type