From 3f74d67bb7dfac12f9174882b4dc2f5ae906a6aa Mon Sep 17 00:00:00 2001
From: EnricoMingo <enricomingo@gmail.com>
Date: Wed, 2 Apr 2025 18:35:25 +0200
Subject: [PATCH 1/2] Added MeshRaw struct to handle mesh passed as a set of
 triangles and points

---
 include/xbot2_interface/collision.h |  9 +++++++-
 src/collision/collision.cpp         | 32 +++++++++++++++++++++++++++++
 2 files changed, 40 insertions(+), 1 deletion(-)

diff --git a/include/xbot2_interface/collision.h b/include/xbot2_interface/collision.h
index aca762d..aa85d61 100644
--- a/include/xbot2_interface/collision.h
+++ b/include/xbot2_interface/collision.h
@@ -58,6 +58,12 @@ struct XBOT2IFC_API Shape
         std::shared_ptr<Eigen::MatrixXf> height;
     };
 
+    struct MeshRaw
+    {
+        std::vector<Eigen::Vector3d> vertices;
+        std::vector<Eigen::Vector3i> triangles;
+    };
+
     using Variant = std::variant<
         Sphere,
         Capsule,
@@ -65,7 +71,8 @@ struct XBOT2IFC_API Shape
         Cylinder,
         Mesh,
         Octree,
-        HeightMap
+        HeightMap,
+        MeshRaw
         >;
 };
 
diff --git a/src/collision/collision.cpp b/src/collision/collision.cpp
index 634f9bf..de45916 100644
--- a/src/collision/collision.cpp
+++ b/src/collision/collision.cpp
@@ -550,6 +550,38 @@ bool CollisionModel::Impl::addCollisionShape(string_const_ref name,
 
             return false;
         },
+        [&](const Shape::MeshRaw& mr)
+        {
+            // fill vertices and triangles
+            std::vector<fcl::Vec3f> vertices;
+            std::vector<fcl::Triangle> triangles;
+
+            for(unsigned int i = 0; i < mr.vertices.size(); ++i)
+            {
+                fcl::Vec3f v(mr.vertices[i].x(),
+                             mr.vertices[i].y(),
+                             mr.vertices[i].z());
+                vertices.push_back(v);
+            }
+
+            for(unsigned int i = 0; i < mr.triangles.size(); ++i)
+            {
+                fcl::Triangle t(mr.triangles[i][0], mr.triangles[i][1], mr.triangles[i][2]);
+                triangles.push_back(t);
+            }
+
+            // add the mesh data into the BVHModel structure
+            auto bvhModel = std::make_shared<fcl::BVHModel<fcl::OBBRSS>>();
+            fcl_geom = bvhModel;
+            bvhModel->beginModel(vertices.size(), triangles.size());
+            bvhModel->addSubModel(vertices, triangles);
+            bvhModel->endModel();
+
+            std::cout << "mesh raw";
+
+
+            return true;
+        },
         [&](const Shape::Mesh& m)
         {
             // read mesh file

From 885dec40da5a2394ada03c9e67f3908d2b6d05b1 Mon Sep 17 00:00:00 2001
From: EnricoMingo <enricomingo@gmail.com>
Date: Thu, 3 Apr 2025 16:27:46 +0200
Subject: [PATCH 2/2] fixed direction of normals when flipped

---
 src/collision/collision.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/collision/collision.cpp b/src/collision/collision.cpp
index de45916..a87d0f1 100644
--- a/src/collision/collision.cpp
+++ b/src/collision/collision.cpp
@@ -1205,6 +1205,14 @@ void CollisionModel::Impl::CollisionPairData::compute_distance(const ModelInterf
     {
         dresult.normal = (dresult.nearest_points[1]-dresult.nearest_points[0]).normalized();
     }
+
+    //hack to fix normal direction when not pointing from o1 to o2
+    double dir = dresult.normal.dot(dresult.nearest_points[1] - dresult.nearest_points[0]);
+    if(dir < 1e-6)
+    {
+        dresult.normal = -dresult.normal;
+    }
+
 }
 
 void CollisionModel::Impl::CollisionPairData::compute_collision(const ModelInterface &model,