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