void NurbsTools::computeBoundingBox (const ON_NurbsCurve &nurbs, Eigen::Vector3d &_min, Eigen::Vector3d &_max) { _min = Eigen::Vector3d (DBL_MAX, DBL_MAX, DBL_MAX); _max = Eigen::Vector3d (-DBL_MAX, -DBL_MAX, -DBL_MAX); for (int i = 0; i < nurbs.CVCount (); i++) { ON_3dPoint p; nurbs.GetCV (i, p); if (p.x < _min (0)) _min (0) = p.x; if (p.y < _min (1)) _min (1) = p.y; if (p.z < _min (2)) _min (2) = p.z; if (p.x > _max (0)) _max (0) = p.x; if (p.y > _max (1)) _max (1) = p.y; if (p.z > _max (2)) _max (2) = p.z; } }
int ON_LineCurve::GetNurbForm( ON_NurbsCurve& c, double tolerance, const ON_Interval* subdomain ) const { int rc = 0; if ( c.Create( m_dim==2?2:3, false, 2, 2 ) ) { rc = 1; double t0 = m_t[0]; double t1 = m_t[1]; if (subdomain ) { if ( t0 < t1 ) { const ON_Interval& sd = *subdomain; double s0 = sd[0]; double s1 = sd[1]; if (s0 < t0) s0 = t0; if (s1 > t1) s1 = t1; if (s0 < s1) { t0 = s0; t1 = s1; } else rc = 0; } else { rc = 0; } } if ( t0 < t1 ) { c.m_knot[0] = t0; c.m_knot[1] = t1; c.SetCV( 0, PointAt(t0)); c.SetCV( 1, PointAt(t1)); } else if ( t0 > t1 ) { rc = 0; c.m_knot[0] = t1; c.m_knot[1] = t0; c.SetCV( 0, PointAt(t1)); c.SetCV( 1, PointAt(t0)); } else { rc = 0; c.m_knot[0] = 0.0; c.m_knot[1] = 1.0; c.SetCV( 0, m_line.from ); c.SetCV( 1, m_line.to ); } } return rc; }
/** * \return List of bezier spline segments which together represent this curve. */ QList<RSpline> RSpline::getBezierSegments() const { // spline is a single bezier segment: if (countControlPoints()==getDegree()+1) { return QList<RSpline>() << *this; } updateInternal(); QList<RSpline> ret; #ifndef R_NO_OPENNURBS ON_NurbsCurve* dup = dynamic_cast<ON_NurbsCurve*>(curve.DuplicateCurve()); if (dup==NULL) { return ret; } dup->MakePiecewiseBezier(); for (int i=0; i<=dup->CVCount() - dup->Order(); ++i) { ON_BezierCurve bc; if (!dup->ConvertSpanToBezier(i, bc)) { continue; } QList<RVector> ctrlPts; for (int cpi=0; cpi<bc.CVCount(); cpi++) { ON_3dPoint onp; bc.GetCV(cpi, onp); ctrlPts.append(RVector(onp.x, onp.y, onp.z)); } ret.append(RSpline(ctrlPts, degree)); } delete dup; #endif return ret; }
ON_NurbsCurve FittingCurve2d::initCPsNurbsCurve2D (int order, const vector_vec2d &cps) { int cp_red = order - 2; ON_NurbsCurve nurbs; if (cps.size () < 3 || cps.size () < (2 * cp_red + 1)) { printf ("[FittingCurve2d::initCPsNurbsCurve2D] Warning, number of control points too low.\n"); return nurbs; } int ncps = cps.size () + 2 * cp_red; // +2*cp_red for smoothness and +1 for closing nurbs = ON_NurbsCurve (2, false, order, ncps); nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1)); for (int j = 0; j < cps.size (); j++) nurbs.SetCV (cp_red + j, ON_3dPoint (cps[j] (0), cps[j] (1), 0.0)); // close nurbs nurbs.SetCV (cp_red + cps.size (), ON_3dPoint (cps[0] (0), cps[0] (1), 0.0)); // make smooth at closing point for (int j = 0; j < cp_red; j++) { ON_3dPoint cp; nurbs.GetCV (nurbs.CVCount () - 1 - cp_red + j, cp); nurbs.SetCV (j, cp); nurbs.GetCV (cp_red - j, cp); nurbs.SetCV (nurbs.CVCount () - 1 - j, cp); } return nurbs; }
void VisualizeCurve (ON_NurbsCurve &curve, double r, double g, double b, bool show_cps) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, cloud, 8); for (std::size_t i = 0; i < cloud->size () - 1; i++) { pcl::PointXYZRGB &p1 = cloud->at (i); pcl::PointXYZRGB &p2 = cloud->at (i + 1); std::ostringstream os; os << "line_" << r << "_" << g << "_" << b << "_" << i; viewer.addLine<pcl::PointXYZRGB> (p1, p2, r, g, b, os.str ()); } if (show_cps) { pcl::PointCloud<pcl::PointXYZ>::Ptr cps (new pcl::PointCloud<pcl::PointXYZ>); for (int i = 0; i < curve.CVCount (); i++) { ON_3dPoint cp; curve.GetCV (i, cp); pcl::PointXYZ p; p.x = float (cp.x); p.y = float (cp.y); p.z = float (cp.z); cps->push_back (p); } pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler (cps, 255 * r, 255 * g, 255 * b); viewer.addPointCloud<pcl::PointXYZ> (cps, handler, "cloud_cps"); } }
std::vector<double> FittingCurve::getElementVector (const ON_NurbsCurve &nurbs) { std::vector<double> result; int idx_min = 0; int idx_max = nurbs.m_knot_capacity - 1; if (nurbs.IsClosed ()) { idx_min = nurbs.m_order - 2; idx_max = nurbs.m_knot_capacity - nurbs.m_order + 1; } const double* knotsU = nurbs.Knot (); result.push_back (knotsU[idx_min]); //for(int E=(m_nurbs.m_order[0]-2); E<(m_nurbs.m_knot_capacity[0]-m_nurbs.m_order[0]+2); E++) { for (int E = idx_min + 1; E <= idx_max; E++) { if (knotsU[E] != knotsU[E - 1]) // do not count double knots result.push_back (knotsU[E]); } return result; }
ON_BoundingBox ON_Arc::BoundingBox() const { // TODO - compute tight arc bounding box // Using these knot[] and cv[] arrays makes this function // not use any heap memory. double knot[10]; ON_4dPoint cv[9]; ON_NurbsCurve c; c.m_knot = knot; c.m_cv = &cv[0].x; if ( GetNurbForm(c) ) return c.BoundingBox(); return ON_Circle::BoundingBox(); }
double FittingCurve2d::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double hint) { // evaluate hint double param = hint; double points[2]; nurbs.Evaluate (param, 0, 2, points); Eigen::Vector2d p (points[0], points[1]); Eigen::Vector2d r = p - pt; double d_shortest_hint = r.squaredNorm (); double d_shortest_elem (DBL_MAX); // evaluate elements std::vector<double> elements = pcl::on_nurbs::FittingCurve2d::getElementVector (nurbs); double seg = 1.0 / (nurbs.Order () - 1); for (unsigned i = 0; i < elements.size () - 1; i++) { double &xi0 = elements[i]; double &xi1 = elements[i + 1]; double dxi = xi1 - xi0; for (unsigned j = 0; j < nurbs.Order (); j++) { double xi = xi0 + (seg * j) * dxi; nurbs.Evaluate (xi, 0, 2, points); p (0) = points[0]; p (1) = points[1]; r = p - pt; double d = r.squaredNorm (); if (d < d_shortest_elem) { d_shortest_elem = d; param = xi; } } } if(d_shortest_hint < d_shortest_elem) return hint; else return param; }
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); } } }
double FittingCurve::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector3d &pt) { double hint (0.0); Eigen::Vector3d p, r; std::vector<double> elements = getElementVector (nurbs); double points[3]; double d_shortest (DBL_MAX); for (unsigned i = 0; i < elements.size () - 1; i++) { double xi = elements[i] + 0.5 * (elements[i + 1] - elements[i]); nurbs.Evaluate (xi, 0, 3, points); p (0) = points[0]; p (1) = points[1]; p (2) = points[2]; r = p - pt; double d = r.squaredNorm (); if (d < d_shortest) { d_shortest = d; hint = xi; } } return hint; }
bool Triangulation::isInside(const ON_NurbsCurve &curve, const pcl::PointXYZ &v) { Eigen::Vector2d vp (v.x, v.y); Eigen::Vector3d a0, a1; pcl::on_nurbs::NurbsTools::computeBoundingBox (curve, a0, a1); double rScale = 1.0 / pcl::on_nurbs::NurbsTools::computeRScale (a0, a1); Eigen::Vector2d pc, tc; double err, param; if (curve.Order () == 2) param = pcl::on_nurbs::FittingCurve2dAPDM::inverseMappingO2 (curve, vp, err, pc, tc); else { param = pcl::on_nurbs::FittingCurve2dAPDM::findClosestElementMidPoint (curve, vp); param = 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); return (z (2) >= 0.0); }
int ON_ArcCurve::GetNurbForm( // returns 0: unable to create NURBS representation // with desired accuracy. // 1: success - returned NURBS parameterization // matches the curve's to wthe desired accuracy // 2: success - returned NURBS point locus matches // the curve's to the desired accuracy but, on // the interior of the curve's domain, the // curve's parameterization and the NURBS // parameterization may not match to the // desired accuracy. ON_NurbsCurve& c, double tolerance, const ON_Interval* subdomain // OPTIONAL subdomain of arc ) const { int rc = 0; if ( subdomain ) { ON_ArcCurve trimmed_arc(*this); if ( trimmed_arc.Trim(*subdomain) ) { rc = trimmed_arc.GetNurbForm( c, tolerance, NULL ); } } else if ( m_t.IsIncreasing() && m_arc.IsValid() ) { if ( NurbsCurveArc( m_arc, m_dim, c ) ) { rc = 2; c.SetDomain( m_t[0], m_t[1] ); } } return rc; }
ON_BOOL32 ON_Ellipse::GetNurbForm( ON_NurbsCurve& nurbscurve ) const { int rc = 0; if ( IsValid() ) { nurbscurve.Create( 3, true, 3, 9 ); nurbscurve.m_knot[0] = nurbscurve.m_knot[1] = 0.0; nurbscurve.m_knot[2] = nurbscurve.m_knot[3] = 0.5*ON_PI; nurbscurve.m_knot[4] = nurbscurve.m_knot[5] = ON_PI; nurbscurve.m_knot[6] = nurbscurve.m_knot[7] = 1.5*ON_PI; nurbscurve.m_knot[8] = nurbscurve.m_knot[9] = 2.0*ON_PI; ON_4dPoint* CV = (ON_4dPoint*)nurbscurve.m_cv; CV[0] = plane.PointAt( radius[0], 0.0); CV[1] = plane.PointAt( radius[0], radius[1]); CV[2] = plane.PointAt( 0.0, radius[1]); CV[3] = plane.PointAt(-radius[0], radius[1]); CV[4] = plane.PointAt(-radius[0], 0.0); CV[5] = plane.PointAt(-radius[0], -radius[1]); CV[6] = plane.PointAt( 0.0, -radius[1]); CV[7] = plane.PointAt( radius[0], -radius[1]); CV[8] = CV[0]; const double w = 1.0/sqrt(2.0); int i; for ( i = 1; i < 8; i += 2 ) { CV[i].x *= w; CV[i].y *= w; CV[i].z *= w; CV[i].w = w; } rc = 2; } return rc; }
ON_NurbsCurve FittingCurve2d::initCPsNurbsCurve2D (int order, const vector_vec2d &cps) { ON_NurbsCurve nurbs; if ((int)cps.size () < (2 * order)) { printf ("[FittingCurve2d::initCPsNurbsCurve2D] Warning, number of control points too low.\n"); return nurbs; } int cp_red = order - 2; int ncps = cps.size () + cp_red; nurbs = ON_NurbsCurve (2, false, order, ncps); nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1)); for (int j = 0; j < ncps; j++) nurbs.SetCV (j, ON_3dPoint (cps[j] (0), cps[j] (1), 0.0)); for (int j = 0; j < cp_red; j++) { ON_3dPoint cp; nurbs.GetCV (nurbs.m_cv_count - 1 - cp_red + j, cp); nurbs.SetCV (j, cp); nurbs.GetCV (cp_red - j, cp); nurbs.SetCV (nurbs.m_cv_count - 1 - j, cp); } return nurbs; }
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); } } }
RH_C_FUNCTION ON_NurbsCurve* ON_NurbsCurve_CreateControlPointCurve(int count, /*ARRAY*/const ON_3dPoint* points, int degree) { if( count < 2 || NULL == points ) return NULL; int order = ( count <= degree ) ? count : degree + 1; ON_NurbsCurve* pNC = ON_NurbsCurve::New(); if( points[0].DistanceTo(points[count-1]) < ON_SQRT_EPSILON ) pNC->CreatePeriodicUniformNurbs( 3, order, count-1, points ); else pNC->CreateClampedUniformNurbs( 3, order, count, points ); if( !pNC->IsValid() ) { delete pNC; return NULL; } return pNC; }
int ON_Cone::GetNurbForm( ON_NurbsSurface& s ) const { int rc = 0; if ( IsValid() ) { ON_Circle c = CircleAt(height); ON_NurbsCurve n; c.GetNurbForm(n); ON_3dPoint apex = ApexPoint(); ON_4dPoint cv; int i, j0, j1; s.Create(3,TRUE,3,2,9,2); for ( i = 0; i < 10; i++ ) s.m_knot[0][i] = n.m_knot[i]; if ( height >= 0.0 ) { s.m_knot[1][0] = 0.0; s.m_knot[1][1] = height; j0 = 0; j1 = 1; } else { s.m_knot[1][0] = height; s.m_knot[1][1] = 0.0; j0 = 1; j1 = 0; } for ( i = 0; i < 9; i++ ) { cv = n.CV(i); s.SetCV(i, j1, ON::homogeneous_rational, &cv.x ); cv.x = apex.x*cv.w; cv.y = apex.y*cv.w; cv.z = apex.z*cv.w; s.SetCV(i, j0, cv); } rc = 2; } return rc; }
void ON_GL( const ON_NurbsCurve& nurbs_curve, GLUnurbsObj* nobj, // created with gluNewNurbsRenderer ) GLenum type, // = 0 (and type is automatically set) int bPermitKnotScaling, double* knot_scale, double xform[][4] ) { ON_GL( nurbs_curve.Dimension(), nurbs_curve.IsRational(), nurbs_curve.Order(), nurbs_curve.CVCount(), nurbs_curve.Knot(), nurbs_curve.m_cv_stride, nurbs_curve.m_cv, nobj, type, bPermitKnotScaling, knot_scale, xform ); }
ON_NurbsCurve FittingCurve2d::initNurbsCurve2D (int order, const vector_vec2d &data, int ncps, double radiusF) { if (data.empty ()) printf ("[FittingCurve2d::initNurbsCurve2D] Warning, no boundary parameters available\n"); Eigen::Vector2d mean = NurbsTools::computeMean (data); unsigned s = data.size (); double r (0.0); for (unsigned i = 0; i < s; i++) { Eigen::Vector2d d = data[i] - mean; double sn = d.squaredNorm (); if (sn > r) r = sn; } r = radiusF * sqrt (r); if (ncps < 2 * order) ncps = 2 * order; ON_NurbsCurve nurbs = ON_NurbsCurve (2, false, order, ncps); nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1)); double dcv = (2.0 * M_PI) / (ncps - order + 1); Eigen::Vector2d cv; for (int j = 0; j < ncps; j++) { cv (0) = r * sin (dcv * j); cv (1) = r * cos (dcv * j); cv = cv + mean; nurbs.SetCV (j, ON_3dPoint (cv (0), cv (1), 0.0)); } return nurbs; }
void visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4); for (std::size_t i = 0; i < curve_cloud->size () - 1; i++) { pcl::PointXYZRGB &p1 = curve_cloud->at (i); pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1); std::ostringstream os; os << "line" << i; viewer.removeShape (os.str ()); viewer.addLine<pcl::PointXYZRGB> (p1, p2, 1.0, 0.0, 0.0, os.str ()); } pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cps (new pcl::PointCloud<pcl::PointXYZRGB>); for (int i = 0; i < curve.CVCount (); i++) { ON_3dPoint p1; curve.GetCV (i, p1); double pnt[3]; surface.Evaluate (p1.x, p1.y, 0, 3, pnt); pcl::PointXYZRGB p2; p2.x = float (pnt[0]); p2.y = float (pnt[1]); p2.z = float (pnt[2]); p2.r = 255; p2.g = 0; p2.b = 0; curve_cps->push_back (p2); } viewer.removePointCloud ("cloud_cps"); viewer.addPointCloud (curve_cps, "cloud_cps"); }
ON_NurbsCurve FittingCurve::initNurbsCurvePCA (int order, const vector_vec3d &data, int ncps, double rf) { if (data.empty ()) printf ("[FittingCurve::initNurbsCurvePCA] Warning, no boundary parameters available\n"); Eigen::Vector3d mean; Eigen::Matrix3d eigenvectors; Eigen::Vector3d eigenvalues; unsigned s = unsigned (data.size ()); NurbsTools::pca (data, mean, eigenvectors, eigenvalues); eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???) double r = rf * sqrt (eigenvalues (0)); if (ncps < 2 * order) ncps = 2 * order; ON_NurbsCurve nurbs = ON_NurbsCurve (3, false, order, ncps); nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1)); double dcv = (2.0 * M_PI) / (ncps - order + 1); Eigen::Vector3d cv, cv_t; for (int j = 0; j < ncps; j++) { cv (0) = r * sin (dcv * j); cv (1) = r * cos (dcv * j); cv (2) = 0.0; cv_t = eigenvectors * cv + mean; nurbs.SetCV (j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2))); } return nurbs; }
int ON_CurveProxy::GetNurbForm( // returns 0: unable to create NURBS representation // with desired accuracy. // 1: success - returned NURBS parameterization // matches the curve's to wthe desired accuracy // 2: success - returned NURBS point locus matches // the curve's to the desired accuracy but, on // the interior of the curve's domain, the // curve's parameterization and the NURBS // parameterization may not match to the // desired accuracy. ON_NurbsCurve& nurbs, double tolerance, // (>=0) const ON_Interval* sub_domain // OPTIONAL subdomain of ON::ProxyCurve::Domain() ) const { ON_BOOL32 rc = false; if ( m_real_curve ) { ON_Interval scratch_domain = RealCurveInterval( sub_domain ); rc = m_real_curve->GetNurbForm(nurbs,tolerance,&scratch_domain); if ( rc ) { if ( m_bReversed ) nurbs.Reverse(); ON_Interval d = m_this_domain; if ( sub_domain ) d.Intersection( *sub_domain ); nurbs.SetDomain( d[0], d[1] ); if ( nurbs.m_dim <= 3 && nurbs.m_dim >= 1 ) { double t0 = Domain()[0]; double t1 = Domain()[1]; if ( 0 != sub_domain ) { if ( t0 < sub_domain->Min() ) t0 = sub_domain->Min(); if ( sub_domain->Max() < t1 ) t1 = sub_domain->Max(); } // set ends of NURBS curve to be exactly on ends of proxy curve ON_3dPoint P0 = PointAt(t0); ON_3dPoint P1 = PointAt(t1); ON_3dPoint N0 = nurbs.PointAtStart(); ON_3dPoint N1 = nurbs.PointAtEnd(); // 22 September 2003, GBA. The end tuning code below should only be applied // to clamped nurbs curves. In particular it should not be used on // periodic nurbs curves. Fixes TRR#11502. ON_BOOL32 clamped = nurbs.IsClamped(2); if ( clamped && (P0 != N0 || P1 != N1) ) { if ( 0==nurbs.m_is_rat ) { nurbs.SetCV(0,P0); nurbs.SetCV(nurbs.m_cv_count-1,P1); } else { ON_4dPoint H0, H1; H0 = P0; H0.w = nurbs.Weight(0); H0.x *= H0.w; H0.y *= H0.w; H0.z *= H0.w; nurbs.SetCV(0,H0); H1 = P1; H1.w = nurbs.Weight(nurbs.m_cv_count-1); H1.x *= H1.w; H1.y *= H1.w; H1.z *= H1.w; nurbs.SetCV(nurbs.m_cv_count-1,H1); } } } } } return rc; }
double FittingCurve::inverseMapping (const ON_NurbsCurve &nurbs, const Eigen::Vector3d &pt, const double &hint, double &error, Eigen::Vector3d &p, Eigen::Vector3d &t, int maxSteps, double accuracy, bool quiet) { //int cp_red = (nurbs.m_order - 2); //int ncpj = int (nurbs.m_cv_count - 2 * cp_red); double pointAndTangents[6]; double current, delta; Eigen::Vector3d r; std::vector<double> elements = getElementVector (nurbs); double minU = elements[0]; double maxU = elements[elements.size () - 1]; current = hint; for (int k = 0; k < maxSteps; k++) { nurbs.Evaluate (current, 1, 3, pointAndTangents); p (0) = pointAndTangents[0]; p (1) = pointAndTangents[1]; p (2) = pointAndTangents[2]; t (0) = pointAndTangents[3]; t (1) = pointAndTangents[4]; t (2) = pointAndTangents[5]; r = p - pt; // step width control int E = findElement (current, elements); double e = elements[E + 1] - elements[E]; delta = -(0.5 * e) * r.dot (t) / t.norm (); // A.ldlt().solve(b); if (std::fabs (delta) < accuracy) { error = r.norm (); return current; } else { current = current + delta; if (current < minU) current = maxU - (minU - current); else if (current > maxU) current = minU + (current - maxU); } } error = r.norm (); if (!quiet) { printf ("[FittingCurve::inverseMapping] Warning: Method did not converge (%e %d).\n", accuracy, maxSteps); printf ("[FittingCurve::inverseMapping] hint: %f current: %f delta: %f error: %f\n", hint, current, delta, error); } return current; }
extern "C" void rt_hyp_brep(ON_Brep **b, const struct rt_db_internal *ip, const struct bn_tol *) { struct rt_hyp_internal *eip; RT_CK_DB_INTERNAL(ip); eip = (struct rt_hyp_internal *)ip->idb_ptr; RT_HYP_CK_MAGIC(eip); point_t p1_origin, p2_origin; ON_3dPoint plane1_origin, plane2_origin; ON_3dVector plane_x_dir, plane_y_dir; // First, find planes corresponding to the top and bottom faces - initially vect_t x_dir, y_dir; VMOVE(x_dir, eip->hyp_A); VCROSS(y_dir, eip->hyp_A, eip->hyp_Hi); VREVERSE(y_dir, y_dir); VMOVE(p1_origin, eip->hyp_Vi); plane1_origin = ON_3dPoint(p1_origin); plane_x_dir = ON_3dVector(x_dir); plane_y_dir = ON_3dVector(y_dir); const ON_Plane hyp_bottom_plane(plane1_origin, plane_x_dir, plane_y_dir); VADD2(p2_origin, eip->hyp_Vi, eip->hyp_Hi); plane2_origin = ON_3dPoint(p2_origin); const ON_Plane hyp_top_plane(plane2_origin, plane_x_dir, plane_y_dir); // Next, create ellipses in the planes corresponding to the edges of the hyp ON_Ellipse b_ell(hyp_bottom_plane, MAGNITUDE(eip->hyp_A), eip->hyp_b); ON_NurbsCurve* bcurve = ON_NurbsCurve::New(); b_ell.GetNurbForm((*bcurve)); bcurve->SetDomain(0.0, 1.0); ON_Ellipse t_ell(hyp_top_plane, MAGNITUDE(eip->hyp_A), eip->hyp_b); ON_NurbsCurve* tcurve = ON_NurbsCurve::New(); t_ell.GetNurbForm((*tcurve)); tcurve->SetDomain(0.0, 1.0); // Generate the bottom cap ON_SimpleArray<ON_Curve*> boundary; boundary.Append(ON_Curve::Cast(bcurve)); ON_PlaneSurface* bp = new ON_PlaneSurface(); bp->m_plane = hyp_bottom_plane; bp->SetDomain(0, -100.0, 100.0); bp->SetDomain(1, -100.0, 100.0); bp->SetExtents(0, bp->Domain(0)); bp->SetExtents(1, bp->Domain(1)); (*b)->m_S.Append(bp); const int bsi = (*b)->m_S.Count() - 1; ON_BrepFace& bface = (*b)->NewFace(bsi); (*b)->NewPlanarFaceLoop(bface.m_face_index, ON_BrepLoop::outer, boundary, true); const ON_BrepLoop* bloop = (*b)->m_L.Last(); bp->SetDomain(0, bloop->m_pbox.m_min.x, bloop->m_pbox.m_max.x); bp->SetDomain(1, bloop->m_pbox.m_min.y, bloop->m_pbox.m_max.y); bp->SetExtents(0, bp->Domain(0)); bp->SetExtents(1, bp->Domain(1)); (*b)->FlipFace(bface); (*b)->SetTrimIsoFlags(bface); boundary.Empty(); delete bcurve; // Generate the top cap boundary.Append(ON_Curve::Cast(tcurve)); ON_PlaneSurface* tp = new ON_PlaneSurface(); tp->m_plane = hyp_top_plane; tp->SetDomain(0, -100.0, 100.0); tp->SetDomain(1, -100.0, 100.0); tp->SetExtents(0, bp->Domain(0)); tp->SetExtents(1, bp->Domain(1)); (*b)->m_S.Append(tp); int tsi = (*b)->m_S.Count() - 1; ON_BrepFace& tface = (*b)->NewFace(tsi); (*b)->NewPlanarFaceLoop(tface.m_face_index, ON_BrepLoop::outer, boundary, true); ON_BrepLoop* tloop = (*b)->m_L.Last(); tp->SetDomain(0, tloop->m_pbox.m_min.x, tloop->m_pbox.m_max.x); tp->SetDomain(1, tloop->m_pbox.m_min.y, tloop->m_pbox.m_max.y); tp->SetExtents(0, bp->Domain(0)); tp->SetExtents(1, bp->Domain(1)); (*b)->SetTrimIsoFlags(tface); delete tcurve; // Now, the hard part. Need an elliptical hyperbolic NURBS surface. // First step is to create a nurbs curve. double MX = eip->hyp_b * eip->hyp_bnr; point_t ep1, ep2, ep3; VSET(ep1, -eip->hyp_b, 0, 0.5*MAGNITUDE(eip->hyp_Hi)); VSET(ep2, -MX*eip->hyp_bnr, 0, 0); VSET(ep3, -eip->hyp_b, 0, -0.5*MAGNITUDE(eip->hyp_Hi)); ON_3dPoint onp1 = ON_3dPoint(ep1); ON_3dPoint onp2 = ON_3dPoint(ep2); ON_3dPoint onp3 = ON_3dPoint(ep3); ON_3dPointArray cpts(3); cpts.Append(onp1); cpts.Append(onp2); cpts.Append(onp3); ON_BezierCurve *bezcurve = new ON_BezierCurve(cpts); bezcurve->MakeRational(); bezcurve->SetWeight(1, bezcurve->Weight(0)/eip->hyp_bnr); ON_NurbsCurve* tnurbscurve = ON_NurbsCurve::New(); bezcurve->GetNurbForm(*tnurbscurve); delete bezcurve; ON_3dPoint revpnt1 = ON_3dPoint(0, 0, -0.5*MAGNITUDE(eip->hyp_Hi)); ON_3dPoint revpnt2 = ON_3dPoint(0, 0, 0.5*MAGNITUDE(eip->hyp_Hi)); ON_Line revaxis = ON_Line(revpnt1, revpnt2); ON_RevSurface* hyp_surf = ON_RevSurface::New(); hyp_surf->m_curve = tnurbscurve; hyp_surf->m_axis = revaxis; hyp_surf->m_angle = ON_Interval(0, 2*ON_PI); // Get the NURBS form of the surface ON_NurbsSurface *hypcurvedsurf = ON_NurbsSurface::New(); hyp_surf->GetNurbForm(*hypcurvedsurf, 0.0); delete hyp_surf; for (int i = 0; i < hypcurvedsurf->CVCount(0); i++) { for (int j = 0; j < hypcurvedsurf->CVCount(1); j++) { point_t cvpt; ON_4dPoint ctrlpt; hypcurvedsurf->GetCV(i, j, ctrlpt); // Scale and shear vect_t proj_ah; vect_t proj_ax; fastf_t factor; VPROJECT(eip->hyp_A, eip->hyp_Hi, proj_ah, proj_ax); VSET(cvpt, ctrlpt.x * MAGNITUDE(proj_ax)/eip->hyp_b, ctrlpt.y, ctrlpt.z); factor = VDOT(eip->hyp_A, eip->hyp_Hi)>0 ? 1.0 : -1.0; cvpt[2] += factor*cvpt[0]/MAGNITUDE(proj_ax)*MAGNITUDE(proj_ah) + 0.5*MAGNITUDE(eip->hyp_Hi)*ctrlpt.w; // Rotate vect_t Au, Bu, Hu; mat_t R; point_t new_cvpt; VSCALE(Bu, y_dir, 1/MAGNITUDE(y_dir)); VSCALE(Hu, eip->hyp_Hi, 1/MAGNITUDE(eip->hyp_Hi)); VCROSS(Au, Bu, Hu); VUNITIZE(Au); MAT_IDN(R); VMOVE(&R[0], Au); VMOVE(&R[4], Bu); VMOVE(&R[8], Hu); VEC3X3MAT(new_cvpt, cvpt, R); VMOVE(cvpt, new_cvpt); // Translate vect_t scale_v; VSCALE(scale_v, eip->hyp_Vi, ctrlpt.w); VADD2(cvpt, cvpt, scale_v); ON_4dPoint newpt = ON_4dPoint(cvpt[0], cvpt[1], cvpt[2], ctrlpt.w); hypcurvedsurf->SetCV(i, j, newpt); } } (*b)->m_S.Append(hypcurvedsurf); int surfindex = (*b)->m_S.Count(); ON_BrepFace& face = (*b)->NewFace(surfindex - 1); (*b)->FlipFace(face); int faceindex = (*b)->m_F.Count(); (*b)->NewOuterLoop(faceindex-1); }
extern "C" void rt_ehy_brep(ON_Brep **b, const struct rt_db_internal *ip, const struct bn_tol *) { struct rt_ehy_internal *eip; RT_CK_DB_INTERNAL(ip); eip = (struct rt_ehy_internal *)ip->idb_ptr; RT_EHY_CK_MAGIC(eip); // Check the parameters if (!NEAR_ZERO(VDOT(eip->ehy_Au, eip->ehy_H), RT_DOT_TOL)) { bu_log("rt_ehy_brep: Au and H are not perpendicular!\n"); return; } if (!NEAR_EQUAL(MAGNITUDE(eip->ehy_Au), 1.0, RT_LEN_TOL)) { bu_log("rt_ehy_brep: Au not a unit vector!\n"); return; } if (MAGNITUDE(eip->ehy_H) < RT_LEN_TOL || eip->ehy_c < RT_LEN_TOL || eip->ehy_r1 < RT_LEN_TOL || eip->ehy_r2 < RT_LEN_TOL) { bu_log("rt_ehy_brep: not all dimensions positive!\n"); return; } if (eip->ehy_r2 > eip->ehy_r1) { bu_log("rt_ehy_brep: semi-minor axis cannot be longer than semi-major axis!\n"); return; } point_t p1_origin; ON_3dPoint plane1_origin, plane2_origin; ON_3dVector plane_x_dir, plane_y_dir; // First, find plane in 3 space corresponding to the bottom face of the EPA. vect_t x_dir, y_dir; VMOVE(x_dir, eip->ehy_Au); VCROSS(y_dir, eip->ehy_Au, eip->ehy_H); VUNITIZE(y_dir); VMOVE(p1_origin, eip->ehy_V); plane1_origin = ON_3dPoint(p1_origin); plane_x_dir = ON_3dVector(x_dir); plane_y_dir = ON_3dVector(y_dir); const ON_Plane ehy_bottom_plane(plane1_origin, plane_x_dir, plane_y_dir); // Next, create an ellipse in the plane corresponding to the edge of the ehy. ON_Ellipse ellipse1(ehy_bottom_plane, eip->ehy_r1, eip->ehy_r2); ON_NurbsCurve* ellcurve1 = ON_NurbsCurve::New(); ellipse1.GetNurbForm((*ellcurve1)); ellcurve1->SetDomain(0.0, 1.0); // Generate the bottom cap ON_SimpleArray<ON_Curve*> boundary; boundary.Append(ON_Curve::Cast(ellcurve1)); ON_PlaneSurface* bp = new ON_PlaneSurface(); bp->m_plane = ehy_bottom_plane; bp->SetDomain(0, -100.0, 100.0); bp->SetDomain(1, -100.0, 100.0); bp->SetExtents(0, bp->Domain(0)); bp->SetExtents(1, bp->Domain(1)); (*b)->m_S.Append(bp); const int bsi = (*b)->m_S.Count() - 1; ON_BrepFace& bface = (*b)->NewFace(bsi); (*b)->NewPlanarFaceLoop(bface.m_face_index, ON_BrepLoop::outer, boundary, true); const ON_BrepLoop* bloop = (*b)->m_L.Last(); bp->SetDomain(0, bloop->m_pbox.m_min.x, bloop->m_pbox.m_max.x); bp->SetDomain(1, bloop->m_pbox.m_min.y, bloop->m_pbox.m_max.y); bp->SetExtents(0, bp->Domain(0)); bp->SetExtents(1, bp->Domain(1)); (*b)->SetTrimIsoFlags(bface); delete ellcurve1; // Now, the hard part. Need an elliptical hyperbolic NURBS surface // First step is to create a nurbs curve. double intercept_calc = (eip->ehy_c)*(eip->ehy_c)/(MAGNITUDE(eip->ehy_H) + eip->ehy_c); double intercept_dist = MAGNITUDE(eip->ehy_H) + eip->ehy_c - intercept_calc; double intercept_length = intercept_dist - MAGNITUDE(eip->ehy_H); double MX = MAGNITUDE(eip->ehy_H); double MP = MX + intercept_length; double w = (MX/MP)/(1-MX/MP); point_t ep1, ep2, ep3; VSET(ep1, -eip->ehy_r1, 0, 0); VSET(ep2, 0, 0, w*intercept_dist); VSET(ep3, eip->ehy_r1, 0, 0); ON_3dPoint onp1 = ON_3dPoint(ep1); ON_3dPoint onp2 = ON_3dPoint(ep2); ON_3dPoint onp3 = ON_3dPoint(ep3); ON_3dPointArray cpts(3); cpts.Append(onp1); cpts.Append(onp2); cpts.Append(onp3); ON_BezierCurve *bcurve = new ON_BezierCurve(cpts); bcurve->MakeRational(); bcurve->SetWeight(1, w); ON_NurbsCurve* tnurbscurve = ON_NurbsCurve::New(); bcurve->GetNurbForm(*tnurbscurve); ON_NurbsCurve* hypbnurbscurve = ON_NurbsCurve::New(); const ON_Interval subinterval = ON_Interval(0, 0.5); tnurbscurve->GetNurbForm(*hypbnurbscurve, 0.0, &subinterval); // Next, rotate that curve around the height vector. point_t revpoint1, revpoint2; VSET(revpoint1, 0, 0, 0); VSET(revpoint2, 0, 0, MX); ON_3dPoint rpnt1 = ON_3dPoint(revpoint1); ON_3dPoint rpnt2 = ON_3dPoint(revpoint2); ON_Line revaxis = ON_Line(rpnt1, rpnt2); ON_RevSurface* hyp_surf = ON_RevSurface::New(); hyp_surf->m_curve = hypbnurbscurve; hyp_surf->m_axis = revaxis; hyp_surf->m_angle = ON_Interval(0, 2*ON_PI); // Get the NURBS form of the surface ON_NurbsSurface *ehycurvedsurf = ON_NurbsSurface::New(); hyp_surf->GetNurbForm(*ehycurvedsurf, 0.0); delete hyp_surf; delete tnurbscurve; delete bcurve; // Transformations for (int i = 0; i < ehycurvedsurf->CVCount(0); i++) { for (int j = 0; j < ehycurvedsurf->CVCount(1); j++) { point_t cvpt; ON_4dPoint ctrlpt; ehycurvedsurf->GetCV(i, j, ctrlpt); // Scale the control points of the // resulting surface to map to the shorter axis. VSET(cvpt, ctrlpt.x, ctrlpt.y * eip->ehy_r2/eip->ehy_r1, ctrlpt.z); // Rotate according to the directions of Au and H vect_t Hu; mat_t R; point_t new_cvpt; VSCALE(Hu, eip->ehy_H, 1/MAGNITUDE(eip->ehy_H)); MAT_IDN(R); VMOVE(&R[0], eip->ehy_Au); VMOVE(&R[4], y_dir); VMOVE(&R[8], Hu); VEC3X3MAT(new_cvpt, cvpt, R); VMOVE(cvpt, new_cvpt); // Translate according to V vect_t scale_v; VSCALE(scale_v, eip->ehy_V, ctrlpt.w); VADD2(cvpt, cvpt, scale_v); ON_4dPoint newpt = ON_4dPoint(cvpt[0], cvpt[1], cvpt[2], ctrlpt.w); ehycurvedsurf->SetCV(i, j, newpt); } } (*b)->m_S.Append(ehycurvedsurf); int surfindex = (*b)->m_S.Count(); ON_BrepFace& face = (*b)->NewFace(surfindex - 1); (*b)->FlipFace(face); int faceindex = (*b)->m_F.Count(); (*b)->NewOuterLoop(faceindex-1); }
bool ON_Arc::GetRadianFromNurbFormParameter(double NurbParameter, double* RadianParameter ) const { // TRR#53994. // 16-Sept-09 Replaced this code so we dont use LocalClosestPoint. // In addition to being slower than neccessary the old method suffered from getting the // wrong answer at the seam of a full circle, This probably only happened with large // coordinates where many digits of precision get lost. ON_NurbsCurve crv; if( !IsValid()|| RadianParameter==NULL) return false; ON_Interval dom= Domain(); if( fabs(NurbParameter- dom[0])<=2.0*ON_EPSILON*fabs(dom[0])) { *RadianParameter=dom[0]; return true; } else if( fabs(NurbParameter- dom[1])<=2.0*ON_EPSILON*fabs(dom[1])) { *RadianParameter=dom[1]; return true; } if( !dom.Includes(NurbParameter) ) return false; if( !GetNurbForm(crv) ) return false; ON_3dPoint cp; cp = crv.PointAt(NurbParameter); cp -= Center(); double x = ON_DotProduct(Plane().Xaxis(), cp); double y = ON_DotProduct(Plane().Yaxis(), cp); double theta = atan2(y,x); theta -= floor( (theta-dom[0])/(2*ON_PI)) * 2* ON_PI; if( theta<dom[0] || theta>dom[1]) { // 24-May-2010 GBA // We got outside of the domain because of a numerical error somewhere. // The only case that matters is because we are right near an endpoint. // So we need to decide which endpoint to return. (Other possibilities // are that the radius is way to small relative to the coordinates of the center. // In this case the circle is just numerical noise around the center anyway.) if( NurbParameter< (dom[0]+dom[1])/2.0) theta = dom[0]; else theta = dom[1]; } // Carefully handle the potential discontinuity of this function // when the domain is a full circle if(dom.Length()>.99999*2.0*ON_PI) { double np_theta = dom.NormalizedParameterAt(theta); double np_nurb = dom.NormalizedParameterAt(NurbParameter); if( np_nurb<.01 && np_theta>.99) theta = dom[0]; else if( np_nurb>.99 && np_theta<.01) theta = dom[1]; } *RadianParameter = theta; //#if defined(ON_DEBUG) // double np2; // ON_3dPoint AP = PointAt(*RadianParameter); // // GetNurbFormParameterFromRadian( *RadianParameter, &np2); // ON_ASSERT(fabs(np2-NurbParameter)<=100* ON_EPSILON*( fabs(NurbParameter) + AP.MaximumCoordinate()+1.0) ); //#endif return true; }
// Add the isocurves of a BrepFace to the partial boundingbox result. static void ON_Brep_GetTightIsoCurveBoundingBox_Helper( const TL_Brep& tlbrep, const ON_BrepFace& face, ON_BoundingBox& bbox, const ON_Xform* xform, int dir ) { ON_Interval domain = face.Domain(1 - dir); int degree = face.Degree(1 - dir); int spancount = face.SpanCount(1 - dir); int spansamples = degree * (degree + 1) - 1; if( spansamples < 2 ) spansamples = 2; // pbox delineates the extremes of the face interior. // We can use it to trivially reject spans and isocurves. ON_BrepLoop* pOuterLoop = face.OuterLoop(); if( NULL==pOuterLoop ) return; const ON_BoundingBox& pbox = pOuterLoop->m_pbox; double t0 = ((dir == 0) ? pbox.Min().y : pbox.Min().x); double t1 = ((dir == 0) ? pbox.Max().y : pbox.Max().x); // Get the surface span vector. ON_SimpleArray<double> spanvector(spancount + 1); spanvector.SetCount(spancount + 1); face.GetSpanVector(1 - dir, spanvector.Array()); // Generate a list of all the sampling parameters. ON_SimpleArray<double> samples(spancount * spansamples); for( int s = 0; s < spancount; s++) { double s0 = spanvector[s]; double s1 = spanvector[s+1]; // Reject span if it does not intersect the pbox. if( s1 < t0 ) { continue; } if( s0 > t1 ) { continue; } ON_Interval span(s0, s1); for( int i = 1; i < spansamples; i++ ) { double t = span.ParameterAt((double)i / (double)(spansamples - 1)); // Reject iso if it does not intersect the pbox. if( t < t0 ) continue; if( t > t1 ) break; samples.Append(t); } } //Iterate over samples int sample_count = samples.Count(); ON_BoundingBox loose_box; ON_SimpleArray<ON_Interval> intervals; ON_NurbsCurve isosubcrv; for( int i = 0; i<sample_count; i++) { // Retrieve iso-curve. ON_Curve* isocrv = face.IsoCurve(dir, samples[i]); while( NULL!=isocrv ) { // Transform isocurve if necessary, this is better than transforming downstream boundingboxes. if( xform ) isocrv->Transform(*xform); // Compute loose box. if( !isocrv->GetBoundingBox(loose_box, false)) break; // Determine whether the loose box is already contained within the partial result. if( bbox.Includes(loose_box, false) ) break; // Solve trimming domains for the iso-curve. intervals.SetCount(0); if( !tlbrep.GetIsoIntervals(face, dir, samples[i], intervals)) break; // Iterate over trimmed iso-curves. int interval_count = intervals.Count(); for( int k=0; k<interval_count; k++ ) { //this to mask a bug in Rhino4. GetNurbForm does not destroy the Curve Tree. It does now. isosubcrv.DestroyCurveTree(); isocrv->GetNurbForm(isosubcrv, 0.0, &intervals[k]); ON_Brep_GetTightCurveBoundingBox_Helper(isosubcrv, bbox, NULL, NULL); } break; } if( isocrv ) { delete isocrv; isocrv = NULL; } } }
static ON_BOOL32 NurbsCurveArc ( const ON_Arc& arc, int dim, ON_NurbsCurve& nurb ) { if ( !arc.IsValid() ) return false; // makes a quadratic nurbs arc const ON_3dPoint center = arc.Center(); double angle = arc.AngleRadians(); ON_Interval dom = arc.DomainRadians(); const double angle0 = dom[0]; const double angle1 = dom[1]; ON_3dPoint start_point = arc.StartPoint(); //ON_3dPoint mid_point = arc.PointAt(angle0 + 0.5*angle); ON_3dPoint end_point = arc.IsCircle() ? start_point : arc.EndPoint(); ON_4dPoint CV[9]; double knot[10]; double a, b, c, w, winv; double *cv; int j, span_count, cv_count; a = (0.5 + ON_SQRT_EPSILON)*ON_PI; if (angle <= a) span_count = 1; else if (angle <= 2.0*a) span_count = 2; else if (angle <= 3.0*a) span_count = 4; // TODO - make a 3 span case else span_count = 4; cv_count = 2*span_count + 1; switch(span_count) { case 1: CV[0] = start_point; CV[1] = arc.PointAt(angle0 + 0.50*angle); CV[2] = end_point; break; case 2: CV[0] = start_point; CV[1] = arc.PointAt(angle0 + 0.25*angle); CV[2] = arc.PointAt(angle0 + 0.50*angle); CV[3] = arc.PointAt(angle0 + 0.75*angle); CV[4] = end_point; angle *= 0.5; break; default: // 4 spans CV[0] = start_point; CV[1] = arc.PointAt(angle0 + 0.125*angle); CV[2] = arc.PointAt(angle0 + 0.250*angle); CV[3] = arc.PointAt(angle0 + 0.375*angle); CV[4] = arc.PointAt(angle0 + 0.500*angle); CV[5] = arc.PointAt(angle0 + 0.625*angle); CV[6] = arc.PointAt(angle0 + 0.750*angle); CV[7] = arc.PointAt(angle0 + 0.875*angle); CV[8] = end_point; angle *= 0.25; break; } a = cos(0.5*angle); b = a - 1.0; //c = (radius > 0.0) ? radius*angle : angle; c = angle; span_count *= 2; knot[0] = knot[1] = angle0; //0.0; for (j = 1; j < span_count; j += 2) { CV[j].x += b * center.x; CV[j].y += b * center.y; CV[j].z += b * center.z; CV[j].w = a; CV[j+1].w = 1.0; knot[j+1] = knot[j+2] = knot[j-1] + c; } knot[cv_count-1] = knot[cv_count] = angle1; for ( j = 1; j < span_count; j += 2 ) { w = CV[j].w; winv = 1.0/w; a = CV[j].x*winv; b = ArcDeFuzz(a); if ( a != b ) { CV[j].x = b*w; } a = CV[j].y*winv; b = ArcDeFuzz(a); if ( a != b ) { CV[j].y = b*w; } a = CV[j].z*winv; b = ArcDeFuzz(a); if ( a != b ) { CV[j].z = b*w; } } nurb.m_dim = (dim==2) ? 2 : 3; nurb.m_is_rat = 1; nurb.m_order = 3; nurb.m_cv_count = cv_count; nurb.m_cv_stride = (dim==2 ? 3 : 4); nurb.ReserveCVCapacity( nurb.m_cv_stride*cv_count ); nurb.ReserveKnotCapacity( cv_count+1 ); for ( j = 0; j < cv_count; j++ ) { cv = nurb.CV(j); cv[0] = CV[j].x; cv[1] = CV[j].y; if ( dim == 2 ) { cv[2] = CV[j].w; } else { cv[2] = CV[j].z; cv[3] = CV[j].w; } nurb.m_knot[j] = knot[j]; } nurb.m_knot[cv_count] = knot[cv_count]; 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); }
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; }