void Triangulation::convertCurve2PointCloud (const ON_NurbsCurve &curve, const ON_NurbsSurface &surf, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, unsigned resolution) { // copy knots if (curve.m_knot_capacity <= 1) { printf ("[Triangulation::Convert] Warning: ON knot vector empty.\n"); return; } cloud->clear (); if (resolution < 2) resolution = 2; int cp_red = curve.Order () - 2; // for each element of the nurbs curve for (int i = 1; i < curve.KnotCount () - 1 - cp_red; i++) { double dr = 1.0 / (resolution - 1); double xi0 = curve.m_knot[i]; double xid = (curve.m_knot[i + 1] - xi0); for (unsigned j = 0; j < resolution; j++) { pcl::PointXYZRGB pt; double xi = (xi0 + j * dr * xid); double p[3]; double pp[3]; curve.Evaluate (xi, 0, 2, pp); surf.Evaluate (pp[0], pp[1], 0, 3, p); pt.x = p[0]; pt.y = p[1]; pt.z = p[2]; pt.r = 255; pt.g = 0; pt.b = 0; cloud->push_back (pt); } } }
void Triangulation::convertCurve2PointCloud (const ON_NurbsCurve &nurbs, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, unsigned resolution) { // copy knots if (nurbs.m_knot_capacity <= 1) { printf ("[Triangulation::convert] Warning: ON knot vector empty.\n"); return; } cloud->clear (); if (resolution < 2) resolution = 2; int cp_red = nurbs.Order () - 2; // for each element in the nurbs curve for (int i = 1; i < nurbs.KnotCount () - 1 - cp_red; i++) { double dr = 1.0 / (resolution - 1); double xi0 = nurbs.m_knot[i]; double xid = (nurbs.m_knot[i + 1] - xi0); for (unsigned j = 0; j < resolution; j++) { double xi = (xi0 + j * dr * xid); pcl::PointXYZRGB p; double points[3]; nurbs.Evaluate (xi, 0, 3, points); p.x = static_cast<float> (points[0]); p.y = static_cast<float> (points[1]); p.z = static_cast<float> (points[2]); p.r = 255; p.g = 0; p.b = 0; cloud->push_back (p); } } }
bool ON_Arc::GetNurbFormParameterFromRadian(double RadianParameter, double* NurbParameter ) const { if(!IsValid() || NurbParameter==NULL) return false; ON_Interval ADomain = DomainRadians(); double endtol = 10.0*ON_EPSILON*(fabs(ADomain[0]) + fabs(ADomain[1])); double del = RadianParameter - ADomain[0]; if(del <= endtol && del >= -ON_SQRT_EPSILON) { *NurbParameter=ADomain[0]; return true; } else { del = ADomain[1] - RadianParameter; if(del <= endtol && del >= -ON_SQRT_EPSILON){ *NurbParameter=ADomain[1]; return true; } } if( !ADomain.Includes(RadianParameter ) ) return false; ON_NurbsCurve crv; if( !GetNurbForm(crv)) return false; //Isolate a bezier that contains the solution int cnt = crv.SpanCount(); int si =0; //get span index int ki=0; //knot index double ang = ADomain[0]; ON_3dPoint cp; cp = crv.PointAt( crv.Knot(0) ) - Center(); double x = ON_DotProduct(Plane().Xaxis(),cp); double y = ON_DotProduct(Plane().Yaxis(),cp); double at = atan2( y, x); //todo make sure we dont go to far for( si=0, ki=0; si<cnt; si++, ki+=crv.KnotMultiplicity(ki) ){ cp = crv.PointAt( crv.Knot(ki+2)) - Center(); x = ON_DotProduct(Plane().Xaxis(),cp); y = ON_DotProduct(Plane().Yaxis(),cp); double at2 = atan2(y,x); if(at2>at) ang+=(at2-at); else ang += (2*ON_PI + at2 - at); at = at2; if( ang>RadianParameter) break; } // Crash Protection trr#55679 if( ki+2>= crv.KnotCount()) { *NurbParameter=ADomain[1]; return true; } ON_Interval BezDomain(crv.Knot(ki), crv.Knot(ki+2)); ON_BezierCurve bez; if(!crv.ConvertSpanToBezier(ki,bez)) return false; ON_Xform COC; COC.ChangeBasis( ON_Plane(),Plane()); bez.Transform(COC); // change coordinates to circles local frame double a[3]; // Bez coefficients of a quadratic to solve for(int i=0; i<3; i++) a[i] = tan(RadianParameter)* bez.CV(i)[0] - bez.CV(i)[1]; //Solve the Quadratic double descrim = (a[1]*a[1]) - a[0]*a[2]; double squared = a[0]-2*a[1]+a[2]; double tbez; if(fabs(squared)> ON_ZERO_TOLERANCE){ ON_ASSERT(descrim>=0); descrim = sqrt(descrim); tbez = (a[0]-a[1] + descrim)/(a[0]-2*a[1]+a[2]); if( tbez<0 || tbez>1){ double tbez2 = (a[0]-a[1]-descrim)/(a[0] - 2*a[1] + a[2]); if( fabs(tbez2 - .5)<fabs(tbez-.5) ) tbez = tbez2; } ON_ASSERT(tbez>=-ON_ZERO_TOLERANCE && tbez<=1+ON_ZERO_TOLERANCE); } else{ // Quadratic degenerates to linear tbez = 1.0; if(a[0]-a[2]) tbez = a[0]/(a[0]-a[2]); } if(tbez<0) tbez=0.0; else if(tbez>1.0) tbez=1.0; //Debug ONLY Code - check the result // double aa = a[0]*(1-tbez)*(1-tbez) + 2*a[1]*tbez*(1-tbez) + a[2]*tbez*tbez; // double tantheta= tan(RadianParameter); // ON_3dPoint bezp; // bez.Evaluate(tbez, 0, 3, bezp); // double yx = bezp.y/bezp.x; *NurbParameter = BezDomain.ParameterAt(tbez); return true; }
void Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve, PolygonMesh &mesh, unsigned resolution, vector_vec3d &start, vector_vec3d &end) { // copy knots if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1 || curve.KnotCount () <= 1) { printf ("[Triangulation::convertTrimmedSurface2PolygonMesh] Warning: ON knot vector empty.\n"); return; } mesh.polygons.clear (); double x0 = nurbs.Knot (0, 0); double x1 = nurbs.Knot (0, nurbs.KnotCount (0) - 1); double w = x1 - x0; double y0 = nurbs.Knot (1, 0); double y1 = nurbs.Knot (1, nurbs.KnotCount (1) - 1); double h = y1 - y0; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); std::vector<pcl::Vertices> polygons; createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution); createIndices (polygons, 0, resolution, resolution); vector_vec2d points (cloud->size (), Eigen::Vector2d ()); std::vector<double> params (cloud->size (), 0.0); std::vector<bool> pt_is_in (cloud->size (), false); std::vector<uint32_t> out_idx; pcl::on_nurbs::vector_vec2d out_pc; for (unsigned i = 0; i < cloud->size (); i++) { double err, param; Eigen::Vector2d pc, tc; pcl::PointXYZ &v = cloud->at (i); Eigen::Vector2d vp (v.x, v.y); if(curve.Order()==2) param = pcl::on_nurbs::FittingCurve2d::inverseMappingO2 (curve, vp, err, pc, tc); else { param = pcl::on_nurbs::FittingCurve2d::findClosestElementMidPoint(curve, vp); param = pcl::on_nurbs::FittingCurve2d::inverseMapping(curve, vp, param, err, pc, tc); } Eigen::Vector3d a (vp (0) - pc (0), vp (1) - pc (1), 0.0); Eigen::Vector3d b (tc (0), tc (1), 0.0); Eigen::Vector3d z = a.cross (b); points[i] = pc; params[i] = param; pt_is_in[i] = (z (2) >= 0.0); end.push_back (Eigen::Vector3d (pc (0), pc (1), 0.0)); start.push_back (Eigen::Vector3d (pc (0) + tc (0) * 0.01, pc (1) + tc (1) * 0.01, 0.0)); } for (unsigned i = 0; i < polygons.size (); i++) { unsigned in (0); pcl::Vertices &poly = polygons[i]; std::vector<uint32_t> out_idx_tmp; pcl::on_nurbs::vector_vec2d out_pc_tmp; for (std::size_t j = 0; j < poly.vertices.size (); j++) { uint32_t &vi = poly.vertices[j]; if (pt_is_in[vi]) in++; else { out_idx_tmp.push_back (vi); out_pc_tmp.push_back (points[vi]); } } if (in > 0) { mesh.polygons.push_back (poly); if (in < poly.vertices.size ()) { for (std::size_t j = 0; j < out_idx_tmp.size (); j++) { out_idx.push_back (out_idx_tmp[j]); out_pc.push_back (out_pc_tmp[j]); } } } } for (std::size_t i = 0; i < out_idx.size (); i++) { pcl::PointXYZ &v = cloud->at (out_idx[i]); Eigen::Vector2d &pc = out_pc[i]; v.x = pc (0); v.y = pc (1); } for (std::size_t i = 0; i < cloud->size (); i++) { pcl::PointXYZ &v = cloud->at (i); double point[3]; nurbs.Evaluate (v.x, v.y, 0, 3, point); v.x = point[0]; v.y = point[1]; v.z = point[2]; } for (std::size_t i = 0; i < start.size (); i++) { Eigen::Vector3d &p1 = start[i]; Eigen::Vector3d &p2 = end[i]; double point[3]; nurbs.Evaluate (p1 (0), p1 (1), 0, 3, point); p1 (0) = point[0]; p1 (1) = point[1]; p1 (2) = point[2]; nurbs.Evaluate (p2 (0), p2 (1), 0, 3, point); p2 (0) = point[0]; p2 (1) = point[1]; p2 (2) = point[2]; } toROSMsg (*cloud, mesh.cloud); }
void Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve, PolygonMesh &mesh, unsigned resolution) { // copy knots if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1 || curve.KnotCount () <= 1) { printf ("[Triangulation::convertTrimmedSurface2PolygonMesh] Warning: ON knot vector empty.\n"); return; } mesh.polygons.clear (); double x0 = nurbs.Knot (0, 0); double x1 = nurbs.Knot (0, nurbs.KnotCount (0) - 1); double w = x1 - x0; double y0 = nurbs.Knot (1, 0); double y1 = nurbs.Knot (1, nurbs.KnotCount (1) - 1); double h = y1 - y0; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); std::vector<pcl::Vertices> polygons; createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution); createIndices (polygons, 0, resolution, resolution); vector_vec2d points (cloud->size (), Eigen::Vector2d ()); std::vector<bool> pt_is_in (cloud->size (), false); Eigen::Vector3d a0, a1; pcl::on_nurbs::NurbsTools::computeBoundingBox (curve, a0, a1); double rScale = 1.0 / pcl::on_nurbs::NurbsTools::computeRScale (a0, a1); std::vector<uint32_t> out_idx; pcl::on_nurbs::vector_vec2d out_pc; for (size_t i = 0; i < cloud->size (); i++) { double err; Eigen::Vector2d pc, tc; pcl::PointXYZ &v = cloud->at (i); Eigen::Vector2d vp (v.x, v.y); if (curve.Order () == 2) pcl::on_nurbs::FittingCurve2dAPDM::inverseMappingO2 (curve, vp, err, pc, tc); else { double param = pcl::on_nurbs::FittingCurve2dAPDM::findClosestElementMidPoint (curve, vp); pcl::on_nurbs::FittingCurve2dAPDM::inverseMapping (curve, vp, param, err, pc, tc, rScale); } Eigen::Vector3d a (vp (0) - pc (0), vp (1) - pc (1), 0.0); Eigen::Vector3d b (tc (0), tc (1), 0.0); Eigen::Vector3d z = a.cross (b); points[i] = pc; pt_is_in[i] = (z (2) >= 0.0); } for (const auto &poly : polygons) { unsigned in (0); std::vector<uint32_t> out_idx_tmp; pcl::on_nurbs::vector_vec2d out_pc_tmp; for (const unsigned int &vi : poly.vertices) { if (pt_is_in[vi]) in++; else { out_idx_tmp.push_back (vi); out_pc_tmp.push_back (points[vi]); } } if (in > 0) { mesh.polygons.push_back (poly); if (in < poly.vertices.size ()) { for (std::size_t j = 0; j < out_idx_tmp.size (); j++) { out_idx.push_back (out_idx_tmp[j]); out_pc.push_back (out_pc_tmp[j]); } } } } for (std::size_t i = 0; i < out_idx.size (); i++) { pcl::PointXYZ &v = cloud->at (out_idx[i]); Eigen::Vector2d &pc = out_pc[i]; v.x = float (pc (0)); v.y = float (pc (1)); } for (auto &v : *cloud) { Eigen::Vector3d tu, tv; double point[3]; nurbs.Evaluate (v.x, v.y, 0, 3, point); v.x = float (point[0]); v.y = float (point[1]); v.z = float (point[2]); // tu[0] = point[3]; // tu[1] = point[4]; // tu[2] = point[5]; // tv[0] = point[6]; // tv[1] = point[7]; // tv[2] = point[8]; // TODO: add normals to mesh } toPCLPointCloud2 (*cloud, mesh.cloud); }