Commit 30397793 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add replaceGeometryByConvexHull function.

parent 71a06d4d
......@@ -341,6 +341,9 @@ namespace hpp {
Pool<DeviceData> datas_;
}; // class Device
void replaceGeometryByConvexHull (GeomModel& geomModel,
const std::vector<std::string>& geometryNames);
inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device)
{ return device.print(os); }
......
......@@ -23,6 +23,7 @@
#include <Eigen/Core>
#include <hpp/fcl/BV/AABB.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <pinocchio/algorithm/geometry.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
......@@ -503,5 +504,24 @@ namespace hpp {
}
return aabb;
}
void replaceGeometryByConvexHull (GeomModel& gmodel,
const std::vector<std::string>& gnames)
{
for (std::size_t i = 0; i < gnames.size(); ++i) {
if (!gmodel.existGeometryName(gnames[i]))
throw std::invalid_argument("Geometry " + gnames[i] + " does not "
"exist.");
GeomIndex gid = gmodel.getGeometryId(gnames[i]);
GeometryObject& go = gmodel.geometryObjects[gid];
if (go.geometry->getObjectType() == fcl::OT_BVH) {
boost::shared_ptr<fcl::BVHModelBase> bvh =
HPP_DYNAMIC_PTR_CAST(fcl::BVHModelBase, go.geometry);
assert(bvh);
bvh->buildConvexHull(false, "Qx");
go.geometry = bvh->convex;
}
}
}
} // namespace pinocchio
} // namespace hpp
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