int collision_detection::refineContactNormals(const World::ObjectConstPtr& object,
CollisionResult &res,
double cell_bbx_search_distance,
double allowed_angle_divergence,
bool estimate_depth,
double iso_value,
double metaball_radius_multiple)
{
if(!object)
{
logError("No valid Object passed in, cannot refine Normals!");
return 0;
}
if(res.contact_count < 1)
{
logWarn("There do not appear to be any contacts, so there is nothing to refine!");
return 0;
}
int modified = 0;
// iterate through contacts
for( collision_detection::CollisionResult::ContactMap::iterator it = res.contacts.begin(); it != res.contacts.end(); ++it)
{
std::string contact1 = it->first.first;
std::string contact2 = it->first.second;
std::string octomap_name = "";
std::vector<collision_detection::Contact>& contact_vector = it->second;
if( contact1.find("octomap") != std::string::npos ) octomap_name = contact1;
else if( contact2.find("octomap") != std::string::npos ) octomap_name = contact2;
else
{
continue;
}
double cell_size = 0;
if(!object->shapes_.empty())
{
const shapes::ShapeConstPtr& shape = object->shapes_[0];
boost::shared_ptr<const shapes::OcTree> shape_octree = boost::dynamic_pointer_cast<const shapes::OcTree>(shape);
if(shape_octree)
{
boost::shared_ptr<const octomap::OcTree> octree = shape_octree->octree;
cell_size = octree->getResolution();
for(size_t contact_index = 0; contact_index < contact_vector.size(); contact_index++)
{
const Eigen::Vector3d& point = contact_vector[contact_index].pos;
const Eigen::Vector3d& normal = contact_vector[contact_index].normal;
octomath::Vector3 contact_point(point[0], point[1], point[2]);
octomath::Vector3 contact_normal(normal[0], normal[1], normal[2]);
octomath::Vector3 diagonal = octomath::Vector3(1,1,1);
octomath::Vector3 bbx_min = contact_point - diagonal*cell_size*cell_bbx_search_distance;
octomath::Vector3 bbx_max = contact_point + diagonal*cell_size*cell_bbx_search_distance;
octomap::point3d_list node_centers;
octomap::OcTreeBaseImpl<octomap::OcTreeNode, octomap::AbstractOccupancyOcTree>::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbx_min, bbx_max);
octomap::OcTreeBaseImpl<octomap::OcTreeNode, octomap::AbstractOccupancyOcTree>::leaf_bbx_iterator leafs_end = octree->end_leafs_bbx();
int count = 0;
for( ; it != leafs_end; ++it)
{
octomap::point3d pt = it.getCoordinate();
//double prob = it->getOccupancy();
if(octree->isNodeOccupied(*it)) // magic number!
{
count++;
node_centers.push_back(pt);
//logInform("Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]", count, prob, pt.x(), pt.y(), pt.z());
}
}
//logInform("Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells %d",
// contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);
//octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
//logError("bad stuff in collision_octomap_filter.cpp; need to port octomap call for groovy");
octomath::Vector3 n;
double depth;
if(getMetaballSurfaceProperties(node_centers, cell_size, iso_value, metaball_radius_multiple,
contact_point, n, depth, estimate_depth))
{
// only modify normal if the refinement predicts a "very different" result.
double divergence = contact_normal.angleTo(n);
if(divergence > allowed_angle_divergence)
{
modified++;
// logInform("Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
// divergence,
// contact_normal.x(), contact_normal.y(), contact_normal.z(),
// n.x(), n.y(), n.z());
contact_vector[contact_index].normal = Eigen::Vector3d(n.x(), n.y(), n.z());
}
if(estimate_depth)
contact_vector[contact_index].depth = depth;
}
}
}
}
}
return modified;
}
void initialize_contact_points(CassCluster* cluster, std::string prefix, unsigned int num_nodes_dc1, unsigned int num_nodes_dc2) {
for (unsigned int i = 0; i < num_nodes_dc1; ++i) {
std::string contact_point(prefix + boost::lexical_cast<std::string>(i + 1));
cass_cluster_set_contact_points(cluster, contact_point.c_str());
}
}
