Verified Commit eeddf558 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

geometry: introduce Compute{Collision,Distance}

parent cf926202
......@@ -271,7 +271,64 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP
friend std::ostream & operator<< (std::ostream & os, const GeometryObject & geomObject);
};
#ifdef PINOCCHIO_WITH_HPP_FCL
struct ComputeCollision
: ::hpp::fcl::ComputeCollision
{
typedef ::hpp::fcl::ComputeCollision Base;
ComputeCollision(const GeometryObject & o1, const GeometryObject & o2)
: Base(o1.geometry.get(),o2.geometry.get())
, o1(o1)
, o2(o2)
{}
virtual ~ComputeCollision() {};
protected:
const GeometryObject & o1;
const GeometryObject & o2;
virtual std::size_t run(const fcl::Transform3f& tf1, const fcl::Transform3f& tf2,
const fcl::CollisionRequest& request, fcl::CollisionResult& result) const
{
typedef ::hpp::fcl::CollisionGeometry const * Pointer;
const_cast<Pointer&>(Base::o1) = o1.geometry.get();
const_cast<Pointer&>(Base::o2) = o2.geometry.get();
return Base::run(tf1, tf2, request, result);
}
};
struct ComputeDistance
: ::hpp::fcl::ComputeDistance
{
typedef ::hpp::fcl::ComputeDistance Base;
ComputeDistance(const GeometryObject & o1, const GeometryObject & o2)
: Base(o1.geometry.get(),o2.geometry.get())
, o1(o1)
, o2(o2)
{}
virtual ~ComputeDistance() {};
protected:
const GeometryObject & o1;
const GeometryObject & o2;
virtual hpp::fcl::FCL_REAL run(const fcl::Transform3f& tf1, const fcl::Transform3f& tf2,
const fcl::DistanceRequest& request, fcl::DistanceResult& result) const
{
typedef ::hpp::fcl::CollisionGeometry const * Pointer;
const_cast<Pointer&>(Base::o1) = o1.geometry.get();
const_cast<Pointer&>(Base::o2) = o2.geometry.get();
return Base::run(tf1, tf2, request, result);
}
};
#endif
} // namespace pinocchio
......
......@@ -189,8 +189,8 @@ namespace pinocchio
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
#ifdef PINOCCHIO_WITH_HPP_FCL
typedef ::hpp::fcl::ComputeCollision ComputeCollision;
typedef ::hpp::fcl::ComputeDistance ComputeDistance;
typedef ComputeCollision ComputeCollision;
typedef ComputeDistance ComputeDistance;
#endif
///
......
......@@ -57,8 +57,8 @@ namespace pinocchio
const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first];
const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second];
collision_functors.push_back(ComputeCollision(obj_1.geometry.get(),obj_2.geometry.get()));
distance_functors.push_back(ComputeDistance(obj_1.geometry.get(),obj_2.geometry.get()));
collision_functors.push_back(ComputeCollision(obj_1,obj_2));
distance_functors.push_back(ComputeDistance(obj_1,obj_2));
}
#endif
fillInnerOuterObjectMaps(geom_model);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment