Add methods to get the AABB of visual shapes and visual models

projectchrono · Oct 6, 2024 · 4b10da3 · 4b10da3
1 parent a172c59
commit 4b10da3
Show file tree

Hide file tree

Showing 21 changed files with 86 additions and 2 deletions.
diff --git a/src/chrono/assets/ChVisualModel.cpp b/src/chrono/assets/ChVisualModel.cpp
@@ -50,6 +50,15 @@ void ChVisualModel::Update(ChPhysicsItem* owner, const ChFrame<>& frame) {
     }
 }
 
+ChAABB ChVisualModel::GetBoundingBox() const {
+    ChAABB aabb;
+    for (auto& shape : m_shapes) {
+        auto shape_aabb = shape.first->GetBoundingBox();
+        aabb += shape_aabb.Transform(shape.second);
+    }
+    return aabb;
+}
+
 void ChVisualModel::ArchiveOut(ChArchiveOut& archive_out) {
     archive_out.VersionWrite<ChVisualModel>();
 

diff --git a/src/chrono/assets/ChVisualModel.h b/src/chrono/assets/ChVisualModel.h
@@ -75,6 +75,9 @@ class ChApi ChVisualModel {
     /// Erase the specified visual shape from this model.
     void Erase(std::shared_ptr<ChVisualShape> shape);
 
+    /// Return the current axis aligned bounding box (AABB) of the visual model.
+    ChAABB GetBoundingBox() const;
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out);
 

diff --git a/src/chrono/assets/ChVisualShape.h b/src/chrono/assets/ChVisualShape.h
@@ -16,6 +16,8 @@
 #include "chrono/core/ChFrame.h"
 #include "chrono/core/ChFrame.h"
 
+#include "chrono/geometry/ChGeometry.h"
+
 #include "chrono/assets/ChColor.h"
 #include "chrono/assets/ChVisualMaterial.h"
 
@@ -90,6 +92,10 @@ class ChApi ChVisualShape {
     /// Get the number of visualization materials.
     unsigned int GetNumMaterials() const { return (unsigned int)material_list.size(); }
 
+    /// Get the shape bounding box.
+    /// The default implementation returns an inverted AABB.
+    virtual ChAABB GetBoundingBox() const { return ChAABB(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out);
 

diff --git a/src/chrono/assets/ChVisualShapeBarrel.cpp b/src/chrono/assets/ChVisualShapeBarrel.cpp
@@ -33,6 +33,12 @@ ChVisualShapeBarrel::ChVisualShapeBarrel(double Y_low,
     SetMutable(false);
 }
 
+ChAABB ChVisualShapeBarrel::GetBoundingBox() const {
+    double y_min = std::min(Hlow, -Rvert);
+    double y_max = std::max(Hsup, +Rvert);
+    return ChAABB(ChVector3d(-Rhor, y_min, -Rhor), ChVector3d(+Rhor, y_max, +Rhor));
+}
+
 void ChVisualShapeBarrel::ArchiveOut(ChArchiveOut& archive_out) {
     // version number
     archive_out.VersionWrite<ChVisualShapeBarrel>();

diff --git a/src/chrono/assets/ChVisualShapeBarrel.h b/src/chrono/assets/ChVisualShapeBarrel.h
@@ -45,6 +45,9 @@ class ChApi ChVisualShapeBarrel : public ChVisualShape {
     double GetRoffset() { return Roffset; }
     void SetRoffset(double ms) { Roffset = ms; }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override;
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeBox.h b/src/chrono/assets/ChVisualShapeBox.h
@@ -42,6 +42,9 @@ class ChApi ChVisualShapeBox : public ChVisualShape {
     /// Get the box dimensions.
     ChVector3d GetLengths() const { return gbox.GetLengths(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gbox.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeCapsule.h b/src/chrono/assets/ChVisualShapeCapsule.h
@@ -44,6 +44,9 @@ class ChApi ChVisualShapeCapsule : public ChVisualShape {
     /// Get the capsule total length.
     double GetLength() const { return gcapsule.GetLength(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gcapsule.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeCone.h b/src/chrono/assets/ChVisualShapeCone.h
@@ -39,6 +39,9 @@ class ChApi ChVisualShapeCone : public ChVisualShape {
     /// Get the cone height.
     double GetHeight() const { return gcone.GetHeight(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gcone.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeCylinder.h b/src/chrono/assets/ChVisualShapeCylinder.h
@@ -41,6 +41,9 @@ class ChApi ChVisualShapeCylinder : public ChVisualShape {
     /// Get the cylinder height.
     double GetHeight() const { return gcylinder.GetHeight(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gcylinder.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeEllipsoid.h b/src/chrono/assets/ChVisualShapeEllipsoid.h
@@ -40,6 +40,9 @@ class ChApi ChVisualShapeEllipsoid : public ChVisualShape {
     /// Get the ellipsoid axes.
     ChVector3d GetAxes() const { return gellipsoid.GetAxes(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gellipsoid.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeLine.h b/src/chrono/assets/ChVisualShapeLine.h
@@ -46,6 +46,9 @@ class ChApi ChVisualShapeLine : public ChVisualShape {
     double GetThickness() const { return thickness; }
     void SetThickness(double mt) { thickness = mt; }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gline->GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeModelFile.cpp b/src/chrono/assets/ChVisualShapeModelFile.cpp
@@ -13,6 +13,9 @@
 // =============================================================================
 
 #include "chrono/assets/ChVisualShapeModelFile.h"
+#include "chrono/geometry/ChTriangleMeshConnected.h"
+
+#include "chrono_thirdparty/filesystem/path.h"
 
 namespace chrono {
 
@@ -27,6 +30,18 @@ ChVisualShapeModelFile::ChVisualShapeModelFile(const std::string& fname) : filen
     SetMutable(false);
 }
 
+ChAABB ChVisualShapeModelFile::GetBoundingBox() const {
+    auto ext = filesystem::path(filename).extension();
+    std::shared_ptr<ChTriangleMeshConnected> trimesh;
+    if (ext == "obj" || ext == "OBJ")
+        trimesh = ChTriangleMeshConnected::CreateFromWavefrontFile(filename, false);
+    else if (ext == "stl" || ext == "STL")
+        trimesh = ChTriangleMeshConnected::CreateFromSTLFile(filename, true);
+    if (trimesh)
+        return trimesh->GetBoundingBox();
+    return ChAABB();
+}
+
 void ChVisualShapeModelFile::ArchiveOut(ChArchiveOut& archive_out) {
     // version number
     archive_out.VersionWrite<ChVisualShapeModelFile>();

diff --git a/src/chrono/assets/ChVisualShapeModelFile.h b/src/chrono/assets/ChVisualShapeModelFile.h
@@ -36,6 +36,10 @@ class ChApi ChVisualShapeModelFile : public ChVisualShape {
     void SetScale(const ChVector3d& s) { scale = s; }
     void SetScale(double s) { scale = ChVector3d(s, s, s); }
 
+    /// Get the shape bounding box.
+    /// Currently, this calculates an updated bounding box only for OBJ and STL files.
+    virtual ChAABB GetBoundingBox() const override;
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapePath.h b/src/chrono/assets/ChVisualShapePath.h
@@ -41,6 +41,9 @@ class ChApi ChVisualShapePath : public ChVisualShape {
     double GetThickness() const { return thickness; }
     void SetThickness(double mt) { thickness = mt; }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gpath->GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeRoundedBox.h b/src/chrono/assets/ChVisualShapeRoundedBox.h
@@ -45,6 +45,9 @@ class ChApi ChVisualShapeRoundedBox : public ChVisualShape {
     /// Get the radius of the sweeping sphere.
     double GetSphereRadius() const { return gbox.GetSphereRadius(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gbox.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeRoundedCylinder.h b/src/chrono/assets/ChVisualShapeRoundedCylinder.h
@@ -44,6 +44,9 @@ class ChApi ChVisualShapeRoundedCylinder : public ChVisualShape {
     /// Get the radius of the sweeping sphere.
     double GetSphereRadius() const { return groundedcyl.GetSphereRadius(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return groundedcyl.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeSphere.h b/src/chrono/assets/ChVisualShapeSphere.h
@@ -38,6 +38,9 @@ class ChApi ChVisualShapeSphere : public ChVisualShape {
     /// Get the sphere radius.
     double GetRadius() const { return gsphere.GetRadius(); }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gsphere.GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeSurface.h b/src/chrono/assets/ChVisualShapeSurface.h
@@ -61,6 +61,9 @@ class ChApi ChVisualShapeSurface : public ChVisualShape {
         resolution_V = mr;
     }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return gsurface->GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/assets/ChVisualShapeTriangleMesh.h b/src/chrono/assets/ChVisualShapeTriangleMesh.h
@@ -58,6 +58,9 @@ class ChApi ChVisualShapeTriangleMesh : public ChVisualShape {
     void SetModifiedVertices(std::vector<int> vertices) { modified_vertices = vertices; }
     const std::vector<int>& GetModifiedVertices() const { return modified_vertices; }
 
+    /// Get the shape bounding box.
+    virtual ChAABB GetBoundingBox() const override { return trimesh->GetBoundingBox(); }
+
     /// Method to allow serialization of transient data to archives.
     virtual void ArchiveOut(ChArchiveOut& archive_out) override;
 

diff --git a/src/chrono/physics/ChAssembly.h b/src/chrono/physics/ChAssembly.h
@@ -16,6 +16,8 @@
 #define CHASSEMBLY_H
 
 #include <cmath>
+#include <vector>
+
 #include "chrono/fea/ChMesh.h"
 #include "chrono/physics/ChBodyAuxRef.h"
 #include "chrono/physics/ChShaft.h"

diff --git a/src/chrono_thirdparty/libstl/stlfile.c b/src/chrono_thirdparty/libstl/stlfile.c
@@ -71,7 +71,7 @@ loadstl(FILE *fp, char *comment, float **vertp, vertex_t *nvertp, vertex_t **tri
 	vht = malloc(vhtcap * sizeof vht[0]);
 	memset(vht, 0, vhtcap * sizeof vht[0]);
 
-	fprintf(stderr, "loadstl: number of triangles: %u, vhtcap %d\n", ntris, vhtcap);
+	////fprintf(stderr, "loadstl: number of triangles: %u, vhtcap %d\n", ntris, vhtcap);
 
 	nverts = 0;
 	for(i = 0; i < ntris; i++){
@@ -100,7 +100,7 @@ loadstl(FILE *fp, char *comment, float **vertp, vertex_t *nvertp, vertex_t **tri
 		attrs[i] = get16(buf + 48);
 	}
 
-	fprintf(stderr, "loadstl: number of verts: %u\n", nverts);
+	////fprintf(stderr, "loadstl: number of verts: %u\n", nverts);
 	free(vht);
 	verts = realloc(verts, nverts * 3*sizeof verts[0]);
 	*vertp = (float *)verts;