void Scene::getGridStats(real &mean, real &stdDeviation, real &variance, int &min, int &max, real &meanCells, real &stdDeviationCells, real &varianceCells, int &minCells, int &maxCells) { int total = 0; int squareTotal = 0; int maximum = std::numeric_limits<int>::min(); int minimum = std::numeric_limits<int>::max(); int count; for (size_t x = 0; x < width; ++x) { for (size_t y = 0; y < height ; ++y) { for (size_t z = 0; z < depth ; ++z) { count = theGrid[x][y][z]->getNumObjs(); squareTotal += count*count; total += count; minimum = std::min(minimum, count); maximum = std::max(maximum, count); } } } mean = real(total)/getNumCells(); variance = real(squareTotal)/getNumCells()-mean*mean; stdDeviation = std::sqrt(variance); min = minimum; max = maximum; maximum = std::numeric_limits<int>::min(); minimum = std::numeric_limits<int>::max(); total = 0; squareTotal = 0; for (ObjectList::iterator iter = objects.begin(); iter != objects.end(); ++iter) { count = (*iter)->getNumCells(); total += count; squareTotal += count*count; minimum = std::min(minimum, count); maximum = std::max(maximum, count); } meanCells = real(total)/objects.size(); minCells = minimum; maxCells = maximum; varianceCells = real(squareTotal)/objects.size()-mean*mean; stdDeviationCells = std::sqrt(varianceCells); }
void collision_detection::StaticDistanceField::initialize( const bodies::Body& body, double resolution, double space_around_body, bool save_points) { points_.clear(); inv_twice_resolution_ = 1.0 / (2.0 * resolution); logInform(" create points at res=%f",resolution); EigenSTL::vector_Vector3d points; determineCollisionPoints(body, resolution, points); if (points.empty()) { logWarn(" StaticDistanceField::initialize: No points in body. Using origin."); points.push_back(body.getPose().translation()); if (body.getType() == shapes::MESH) { const bodies::ConvexMesh& mesh = dynamic_cast<const bodies::ConvexMesh&>(body); const EigenSTL::vector_Vector3d& verts = mesh.getVertices(); logWarn(" StaticDistanceField::initialize: also using %d vertices.", int(verts.size())); EigenSTL::vector_Vector3d::const_iterator it = verts.begin(); EigenSTL::vector_Vector3d::const_iterator it_end = verts.end(); for ( ; it != it_end ; ++it) { points.push_back(*it); } } } logInform(" StaticDistanceField::initialize: Using %d points.", points.size()); AABB aabb; aabb.add(points); logInform(" space_around_body = %f",space_around_body); logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-space)", aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), aabb.max_.x(), aabb.max_.y(), aabb.max_.z()); aabb.min_ -= Eigen::Vector3d(space_around_body, space_around_body, space_around_body); aabb.max_ += Eigen::Vector3d(space_around_body, space_around_body, space_around_body); logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-adjust)", aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), aabb.max_.x(), aabb.max_.y(), aabb.max_.z()); aabb.min_.x() = std::floor(aabb.min_.x() / resolution) * resolution; aabb.min_.y() = std::floor(aabb.min_.y() / resolution) * resolution; aabb.min_.z() = std::floor(aabb.min_.z() / resolution) * resolution; logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (post-adjust)", aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), aabb.max_.x(), aabb.max_.y(), aabb.max_.z()); Eigen::Vector3d size = aabb.max_ - aabb.min_; double diagonal = size.norm(); logInform(" DF: sz=(%7.3f %7.3f %7.3f) cnt=(%d %d %d) diag=%f", size.x(), size.y(), size.z(), int(size.x()/resolution), int(size.y()/resolution), int(size.z()/resolution), diagonal); distance_field::PropagationDistanceField df( size.x(), size.y(), size.z(), resolution, aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), diagonal * 2.0, true); df.addPointsToField(points); DistPosEntry default_entry; default_entry.distance_ = diagonal * 2.0; default_entry.cell_id_ = -1; resize(size.x(), size.y(), size.z(), resolution, aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), default_entry); logInform(" copy %d points.", getNumCells(distance_field::DIM_X) * getNumCells(distance_field::DIM_Y) * getNumCells(distance_field::DIM_Z)); int pdf_x,pdf_y,pdf_z; int sdf_x,sdf_y,sdf_z; Eigen::Vector3d pdf_p, sdf_p; df.worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), pdf_x,pdf_y,pdf_z); worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), sdf_x,sdf_y,sdf_z); df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z()); gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z()); logInform(" DF: min=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)", aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), pdf_x, pdf_y, pdf_z); logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)", pdf_p.x(), pdf_p.y(), pdf_p.z(), pdf_x, pdf_y, pdf_z); logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)", sdf_p.x(), sdf_p.y(), sdf_p.z(), sdf_x, sdf_y, sdf_z); df.worldToGrid(0,0,0, pdf_x,pdf_y,pdf_z); worldToGrid(0,0,0, sdf_x,sdf_y,sdf_z); df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z()); gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z()); logInform(" DF: org=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)", 0.0, 0.0, 0.0, pdf_x, pdf_y, pdf_z); logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)", pdf_p.x(), pdf_p.y(), pdf_p.z(), pdf_x, pdf_y, pdf_z); logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)", sdf_p.x(), sdf_p.y(), sdf_p.z(), sdf_x, sdf_y, sdf_z); df.worldToGrid(points[0].x(), points[0].y(), points[0].z(), pdf_x,pdf_y,pdf_z); worldToGrid(points[0].x(), points[0].y(), points[0].z(), sdf_x,sdf_y,sdf_z); df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z()); gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z()); logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)", points[0].x(), points[0].y(), points[0].z(), pdf_x, pdf_y, pdf_z); logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)", pdf_p.x(), pdf_p.y(), pdf_p.z(), pdf_x, pdf_y, pdf_z); logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)", sdf_p.x(), sdf_p.y(), sdf_p.z(), sdf_x, sdf_y, sdf_z); for (int z = 0 ; z < df.getZNumCells() ; ++z) { for (int y = 0 ; y < df.getYNumCells() ; ++y) { for (int x = 0 ; x < df.getXNumCells() ; ++x) { DistPosEntry entry; double dist = df.getDistance(x, y, z); const distance_field::PropDistanceFieldVoxel& voxel = df.getCell(x,y,z); if (dist < 0) { // propogation distance field has a bias of -1*resolution on points inside the object if (dist <= -resolution) { dist += resolution; } else { logError("PropagationDistanceField returned distance=%f between 0 and -resolution=%f." " Did someone fix it?" " Need to remove workaround from static_distance_field.cpp", dist,-resolution); dist = 0.0; } entry.distance_ = dist; entry.cell_id_ = getCellId( voxel.closest_negative_point_.x(), voxel.closest_negative_point_.y(), voxel.closest_negative_point_.z()); } else { entry.distance_ = dist; entry.cell_id_ = getCellId( voxel.closest_point_.x(), voxel.closest_point_.y(), voxel.closest_point_.z()); } setCell(x, y, z, entry); } } } if (save_points) std::swap(points, points_); }