change the interface of collide: separate geometry and pose. See collide.h and related files

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@55 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/collision_space_fcl/src/environmentFCL.cpp

@@ -266,9 +266,92 @@ fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape
       ROS_FATAL_STREAM("This shape type is not supported using FCL yet");
   }
 
-  return g;
+  fcl::CollisionObject* co = new fcl::CollisionObject(g);
+  return co;
 }
 
+/*
+fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape, double scale, double padding)
+{
+  fcl::CollisionObject* g = NULL;
+  switch(shape->type)
+  {
+    case shapes::SPHERE:
+    {
+      g = new fcl::Sphere(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding);
+    }
+    break;
+    case shapes::BOX:
+    {
+      const double *size = static_cast<const shapes::Box*>(shape)->size;
+      g = new fcl::Box(size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0);
+    }
+    break;
+    case shapes::CYLINDER:
+    {
+      g = new fcl::Cylinder(static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding,
+                            static_cast<const shapes::Cylinder*>(shape)->length * scale + padding * 2.0);
+    }
+    break;
+    case shapes::MESH:
+    {
+      fcl::BVHModel<fcl::OBB>* gobb = new fcl::BVHModel<fcl::OBB>();
+      const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
+      if(mesh->vertexCount > 0 && mesh->triangleCount > 0)
+      {
+        std::vector<fcl::Triangle> tri_indices(mesh->triangleCount);
+        for(unsigned int i = 0; i < mesh->triangleCount; ++i)
+          tri_indices[i] = fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
+
+        std::vector<fcl::Vec3f> points(mesh->vertexCount);
+        double sx = 0.0, sy = 0.0, sz = 0.0;
+        for(unsigned int i = 0; i < mesh->vertexCount; ++i)
+        {
+          points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
+          sx += points[i][0];
+          sy += points[i][1];
+          sz += points[i][2];
+        }
+        // the center of the mesh
+        sx /= (double)mesh->vertexCount;
+        sy /= (double)mesh->vertexCount;
+        sz /= (double)mesh->vertexCount;
+
+        // scale the mesh
+        for(unsigned int i = 0; i < mesh->vertexCount; ++i)
+        {
+          // vector from center to the vertex
+          double dx = points[i][0] - sx;
+          double dy = points[i][1] - sy;
+          double dz = points[i][2] - sz;
+
+          // length of vector
+          //double norm = sqrt(dx * dx + dy * dy + dz * dz);
+
+          double ndx = ((dx > 0) ? dx+padding : dx-padding);
+          double ndy = ((dy > 0) ? dy+padding : dy-padding);
+          double ndz = ((dz > 0) ? dz+padding : dz-padding);
+
+          // the new distance of the vertex from the center
+          //double fact = scale + padding/norm;
+          points[i] = fcl::Vec3f(sx + ndx, sy + ndy, sz + ndz);
+        }
+
+        gobb->beginModel();
+        gobb->addSubModel(points, tri_indices);
+        gobb->endModel();
+        gobb->computeLocalAABB();
+        g = gobb;
+      }
+    }
+    break;
+    default:
+      ROS_FATAL_STREAM("This shape type is not supported using FCL yet");
+  }
+
+  return g;
+}
+*/
 
 void EnvironmentModelFCL::updateGeom(fcl::CollisionObject* geom,  const btTransform &pose) const
 {

trunk/fcl/include/fcl/BVH_model.h

@@ -54,7 +54,7 @@ namespace fcl
 
 /** \brief A class describing the bounding hierarchy of a mesh model */
 template<typename BV>
-class BVHModel : public CollisionObject
+class BVHModel : public CollisionGeometry
 {
 private:
   int num_tris_allocated;

trunk/fcl/include/fcl/collision.h

@@ -54,6 +54,11 @@ namespace fcl
 int collide(const CollisionObject* o1, const CollisionObject* o2,
              int num_max_contacts, bool exhaustive, bool enable_contact,
              std::vector<Contact>& contacts);
+
+int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
+            const CollisionGeometry* o2, const SimpleTransform& tf2,
+            int num_max_contacts, bool exhaustive, bool enable_contact,
+            std::vector<Contact>& contacts);
 }
 
 #endif

trunk/fcl/include/fcl/collision_data.h

@@ -14,8 +14,8 @@ struct Contact
   BVH_REAL penetration_depth;
   Vec3f normal;
   Vec3f pos;
-  const CollisionObject* o1;
-  const CollisionObject* o2;
+  const CollisionGeometry* o1;
+  const CollisionGeometry* o2;
   int b1;
   int b2;
 
@@ -27,7 +27,7 @@ struct Contact
     b2 = -1;
   }
 
-  Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_)
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
   {
     o1 = o1_;
     o2 = o2_;
@@ -35,7 +35,7 @@ struct Contact
     b2 = b2_;
   }
 
-  Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_,
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
           const Vec3f& pos_, const Vec3f& normal_, BVH_REAL depth_)
   {
     o1 = o1_;

trunk/fcl/include/fcl/collision_func_matrix.h

@@ -46,7 +46,7 @@ namespace fcl
 {
 
 
-typedef int (*CollisionFunc)(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
+typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
 
 
 struct CollisionFunctionMatrix

trunk/fcl/include/fcl/collision_object.h

@@ -40,6 +40,7 @@
 
 #include "fcl/AABB.h"
 #include "fcl/transform.h"
+#include <boost/shared_ptr.hpp>
 
 namespace fcl
 {
@@ -49,11 +50,181 @@ enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
 enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
                 GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
 
-/** \brief Base class for all objects participating in collision */
+class CollisionGeometry
+{
+public:
+  virtual ~CollisionGeometry() {}
+
+  virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
+
+  virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+  virtual void computeLocalAABB() = 0;
+
+  /** AABB center in local coordinate */
+  Vec3f aabb_center;
+
+  /** AABB radius */
+  BVH_REAL aabb_radius;
+};
+
 class CollisionObject
 {
 public:
-  virtual ~CollisionObject() {}
+  CollisionObject(CollisionGeometry* cgeom_)
+  {
+    cgeom.reset(cgeom_);
+    computeAABB();
+  }
+
+  CollisionObject(CollisionGeometry* cgeom_, const SimpleTransform& tf)
+  {
+    cgeom.reset(cgeom_);
+    t = tf;
+    computeAABB();
+  }
+
+  CollisionObject(CollisionGeometry* cgeom_, const Matrix3f& R, const Vec3f& T)
+  {
+    cgeom.reset(cgeom_);
+    t = SimpleTransform(R, T);
+    computeAABB();
+  }
+
+  CollisionObject()
+  {
+  }
+
+  ~CollisionObject()
+  {
+  }
+
+  OBJECT_TYPE getObjectType() const
+  {
+    return cgeom->getObjectType();
+  }
+
+  NODE_TYPE getNodeType() const
+  {
+    return cgeom->getNodeType();
+  }
+
+  inline const AABB& getAABB() const
+  {
+    return aabb;
+  }
+
+  inline void computeAABB()
+  {
+    Vec3f center = t.transform(cgeom->aabb_center);
+    Vec3f delta(cgeom->aabb_radius, cgeom->aabb_radius, cgeom->aabb_radius);
+    aabb.min_ = center - delta;
+    aabb.max_ = center + delta;
+  }
+
+  inline void computeAABB2()
+  {
+    Vec3f center = t.transform(cgeom->aabb_center);
+    // compute new r1, r2, r3
+  }
+
+  void* getUserData() const
+  {
+    return user_data;
+  }
+
+  void setUserData(void *data)
+  {
+    user_data = data;
+  }
+
+  inline const Vec3f& getTranslation() const
+  {
+    return t.getTranslation();
+  }
+
+  inline const Matrix3f& getRotation() const
+  {
+    return t.getRotation();
+  }
+
+  inline const SimpleQuaternion& getQuatRotation() const
+  {
+    return t.getQuatRotation();
+  }
+
+  inline const SimpleTransform& getTransform() const
+  {
+    return t;
+  }
+
+  void setRotation(const Matrix3f& R)
+  {
+    t.setRotation(R);
+  }
+
+  void setTranslation(const Vec3f& T)
+  {
+    t.setTranslation(T);
+  }
+
+  void setQuatRotation(const SimpleQuaternion& q)
+  {
+    t.setQuatRotation(q);
+  }
+
+  void setTransform(const Matrix3f& R, const Vec3f& T)
+  {
+    t.setTransform(R, T);
+  }
+
+  void setTransform(const SimpleQuaternion& q, const Vec3f& T)
+  {
+    t.setTransform(q, T);
+  }
+
+  void setTransform(const SimpleTransform& tf)
+  {
+    t = tf;
+  }
+
+  bool isIdentityTransform() const
+  {
+    return t.isIdentity();
+  }
+
+  void setIdentityTransform()
+  {
+    t.setIdentity();
+  }
+
+  const CollisionGeometry* getCollisionGeometry() const
+  {
+    return cgeom.get();
+  }
+
+protected:
+
+  // const CollisionGeometry* cgeom;
+  boost::shared_ptr<CollisionGeometry> cgeom;
+
+  SimpleTransform t;
+
+  /** AABB in global coordinate */
+  mutable AABB aabb;
+
+  /** pointer to user defined data specific to this object */
+  void *user_data;
+};
+
+
+
+
+/** \brief Base class for all objects participating in collision */
+class CollisionObject2
+{
+public:
+  virtual ~CollisionObject2() {}
 
   virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
 

trunk/fcl/include/fcl/conservative_advancement.h

@@ -48,9 +48,9 @@ namespace fcl
 {
 
 template<typename BV>
-int conservativeAdvancement(const CollisionObject* o1,
+int conservativeAdvancement(const CollisionGeometry* o1,
                             MotionBase<BV>* motion1,
-                            const CollisionObject* o2,
+                            const CollisionGeometry* o2,
                             MotionBase<BV>* motion2,
                             int num_max_contacts, bool exhaustive, bool enable_contact,
                             std::vector<Contact>& contacts,

trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h

@@ -48,7 +48,7 @@ namespace fcl
 
 /** \brief Generate BVH model from box */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box& shape)
+void generateBVHModel(BVHModel<BV>& model, const Box& shape, const SimpleTransform& pose = SimpleTransform())
 {
   double a = shape.side[0];
   double b = shape.side[1];
@@ -80,7 +80,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -92,7 +92,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
 
 /** Generate BVH model from sphere */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int seg = 16, unsigned int ring = 16)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -147,7 +147,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -162,7 +162,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
  * then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
  */
 template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_faces_for_unit_sphere)
+void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int n_faces_for_unit_sphere = 100)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -222,7 +222,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -235,7 +235,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
 
 /** \brief Generate BVH model from cylinder */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -299,7 +299,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -314,7 +314,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
  * larger radius, the number of circle split number is r * tot.
  */
 template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot_for_unit_cylinder)
+void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot_for_unit_cylinder = 100)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -379,7 +379,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 
@@ -392,7 +392,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
 
 /** \brief Generate BVH model from cone */
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
 {
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
@@ -454,7 +454,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot =
   for(unsigned int i = 0; i < points.size(); ++i)
   {
     Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
-    v = shape.getRotation() * v + shape.getTranslation();
+    v = pose.transform(v);
     points[i] = v;
   }
 

trunk/fcl/include/fcl/geometric_shapes.h

@@ -47,7 +47,7 @@ namespace fcl
 {
 
 /** \brief Base class for all basic geometric shapes */
-class ShapeBase : public CollisionObject
+class ShapeBase : public CollisionGeometry
 {
 public:
   /** \brief Default Constructor */

trunk/fcl/include/fcl/geometric_shapes_intersect.h

@@ -66,7 +66,7 @@ public:
    * Notice that only local transformation is applied.
    * Gloal transformation are considered later
    */
-  static void* createGJKObject(const T& s) { return NULL; }
+  static void* createGJKObject(const T& s, const SimpleTransform& tf) { return NULL; }
 
   /** \brief Delete GJK object */
   static void deleteGJKObject(void* o) {}
@@ -79,7 +79,7 @@ class GJKInitializer<Cylinder>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Cylinder& s);
+  static void* createGJKObject(const Cylinder& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -90,7 +90,7 @@ class GJKInitializer<Sphere>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Sphere& s);
+  static void* createGJKObject(const Sphere& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -101,7 +101,7 @@ class GJKInitializer<Box>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Box& s);
+  static void* createGJKObject(const Box& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -112,7 +112,7 @@ class GJKInitializer<Capsule>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Capsule& s);
+  static void* createGJKObject(const Capsule& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -123,7 +123,7 @@ class GJKInitializer<Cone>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Cone& s);
+  static void* createGJKObject(const Cone& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -134,7 +134,7 @@ class GJKInitializer<Convex>
 public:
   static GJKSupportFunction getSupportFunction();
   static GJKCenterFunction getCenterFunction();
-  static void* createGJKObject(const Convex& s);
+  static void* createGJKObject(const Convex& s, const SimpleTransform& tf);
   static void deleteGJKObject(void* o);
 };
 
@@ -157,10 +157,12 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
 
 /** intersection checking between two shapes */
 template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
+                    const S2& s2, const SimpleTransform& tf2,
+                    Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
 {
-  void* o1 = GJKInitializer<S1>::createGJKObject(s1);
-  void* o2 = GJKInitializer<S2>::createGJKObject(s2);
+  void* o1 = GJKInitializer<S1>::createGJKObject(s1, tf1);
+  void* o2 = GJKInitializer<S2>::createGJKObject(s2, tf2);
 
   return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
                     o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
@@ -172,9 +174,10 @@ bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BV
 
 /** \brief intersection checking between one shape and a triangle */
 template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+                            const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
 {
-  void* o1 = GJKInitializer<S>::createGJKObject(s);
+  void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
   void* o2 = triCreateGJKObject(P1, P2, P3);
 
   return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
@@ -187,10 +190,11 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
 
 /** \brief intersection checking between one shape and a triangle with transformation */
 template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+                            const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
                             Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
 {
-  void* o1 = GJKInitializer<S>::createGJKObject(s);
+  void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
   void* o2 = triCreateGJKObject(P1, P2, P3, R, T);
 
   return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),

trunk/fcl/include/fcl/geometric_shapes_utility.h

@@ -44,71 +44,71 @@
 namespace fcl
 {
   template<typename BV>
-  void computeBV(const Box& s, BV& bv) {}
+  void computeBV(const Box& s, const SimpleTransform& tf, BV& bv) {}
 
   template<typename BV>
-  void computeBV(const Sphere& s, BV& bv) {}
+  void computeBV(const Sphere& s, const SimpleTransform& tf, BV& bv) {}
 
   template<typename BV>
-  void computeBV(const Capsule& s, BV& bv) {}
+  void computeBV(const Capsule& s, const SimpleTransform&