char *ft_ftoa(double n, long prec) { char *str; long t; long int pprec; if (n != n) return (ft_strdup("nan")); str = NULL; pprec = s_get_pprec(prec); s_strcal(&str, n); if ((n - (long)(n)) * pprec > 0) t = (n - (long)n) * pprec + (double)1 / 10000; else t = (n - (long)n) * pprec + (double)1 / 10000; if (pprec == 0) return (str); t = (t < 0) ? -t : t; s_point(t, pprec, prec, &str); return (str); }
bool HeightmapSampling::calculateNormalsFromHullSurface() { normals_.points.clear(); shared_ptr < PointCloud<PointXYZ> > sp; sp.reset(new PointCloud<PointXYZ> ()); Vector3d search_point_center(0, 0, 0); vector < Vector3d > normals_prestep; for (unsigned int i = 0; i < convex_hull_vertices_.size(); i++) { PointXYZ p(0, 0, 0); for (unsigned int j = 0; j < convex_hull_vertices_[i].vertices.size(); j++) { unsigned int index = convex_hull_vertices_[i].vertices[j]; p.x += convex_hull_points_->points[index].x; p.y += convex_hull_points_->points[index].y; p.z += convex_hull_points_->points[index].z; } p.x /= convex_hull_vertices_[i].vertices.size(); p.y /= convex_hull_vertices_[i].vertices.size(); p.z /= convex_hull_vertices_[i].vertices.size(); search_point_center += Vector3d(p.x, p.y, p.z); sp->push_back(p); /* normal */ Vector3d p_centr(p.x, p.y, p.z); const PointXYZ& p_a = convex_hull_points_-> points[convex_hull_vertices_[i].vertices[0]]; const PointXYZ& p_b = convex_hull_points_-> points[convex_hull_vertices_[i].vertices[1]]; const PointXYZ& p_c = convex_hull_points_-> points[convex_hull_vertices_[i].vertices[2]]; Vector3d va(p_b.x - p_a.x, p_b.y - p_a.y, p_b.z - p_a.z); Vector3d vb(p_c.x - p_a.x, p_c.y - p_a.y, p_c.z - p_a.z); Vector3d vn = va.cross(vb); vn.normalize(); normals_prestep.push_back(vn); } search_point_center /= sp->points.size(); /* flip normals away from center */ vector<bool> backside; backside.resize(normals_prestep.size(), false); for (unsigned int i = 0; i < normals_prestep.size(); i++) { Vector3d s_point(sp->points[i].x - search_point_center.x(), sp->points[i].y - search_point_center.y(), sp->points[i].z - search_point_center.z()); if (s_point.dot(normals_prestep[i]) < 0) { normals_prestep[i] = -normals_prestep[i]; } Vector3d vec_vs(sp->points[i].x - viewp_trans_.x(), sp->points[i].y - viewp_trans_.y(), sp->points[i].z - viewp_trans_.z()); vec_vs.normalize(); if (vec_vs.dot(normals_prestep[i]) > 0) { backside[i] = true; } } /* set normals */ for (unsigned int i = 0; i < normals_prestep.size(); i++) { if (backside[i]) { continue; } Normal pcl_n; pcl_n.normal_x = normals_prestep[i].x(); pcl_n.normal_y = normals_prestep[i].y(); pcl_n.normal_z = normals_prestep[i].z(); normals_.push_back(pcl_n); } /* filter and set search-points */ for (int i = backside.size() - 1; i >= 0; i--) { if (backside[i]) { sp->points.erase(sp->points.begin() + i); } } search_points_ = sp; ROS_DEBUG_STREAM("grasp_template::HeightmapSampling: #normals: " << normals_.size() << ", #search_points: " << search_points_->points.size() << endl); return true; }