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()); } }