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;
}
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;
}
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);
}
}
}
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;
}
double
FittingCurve2d::inverseMappingO2 (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double &error,
Eigen::Vector2d &p, Eigen::Vector2d &t)
{
if (nurbs.Order () != 2)
printf ("[FittingCurve2d::inverseMappingO2] Error, order not 2 (polynomial degree 1)\n");
std::vector<double> elements = getElementVector (nurbs);
Eigen::Vector2d min_pt;
double min_param (DBL_MAX);
double min_dist (DBL_MAX);
error = DBL_MAX;
int is_corner (-1);
for (unsigned i = 0; i < elements.size () - 1; i++)
{
Eigen::Vector2d p1;
nurbs.Evaluate (elements[i], 0, 2, &p1 (0));
Eigen::Vector2d p2;
nurbs.Evaluate (elements[i + 1], 0, 2, &p2 (0));
Eigen::Vector2d d1 (p2 (0) - p1 (0), p2 (1) - p1 (1));
Eigen::Vector2d d2 (pt (0) - p1 (0), pt (1) - p1 (1));
double d1_norm = d1.norm ();
double d0_norm = d1.dot (d2) / d1_norm;
Eigen::Vector2d d0 = d1 * d0_norm / d1_norm;
Eigen::Vector2d p0 = p1 + d0;
if (d0_norm < 0.0)
{
double tmp_dist = (p1 - pt).norm ();
if (tmp_dist < min_dist)
{
min_dist = tmp_dist;
min_pt = p1;
min_param = elements[i];
is_corner = i;
}
}
else if (d0_norm >= d1_norm)
{
double tmp_dist = (p2 - pt).norm ();
if (tmp_dist < min_dist)
{
min_dist = tmp_dist;
min_pt = p2;
min_param = elements[i + 1];
is_corner = i + 1;
}
}
else
{ // p0 lies on line segment
double tmp_dist = (p0 - pt).norm ();
if (tmp_dist < min_dist)
{
min_dist = tmp_dist;
min_pt = p0;
min_param = elements[i] + (d0_norm / d1_norm) * (elements[i + 1] - elements[i]);
is_corner = -1;
}
}
}
if (is_corner >= 0)
{
double param1, param2;
if (is_corner == 0 || is_corner == elements.size () - 1)
{
double x0a = elements[0];
double x0b = elements[elements.size () - 1];
double xa = elements[1];
double xb = elements[elements.size () - 2];
param1 = x0a + 0.5 * (xa - x0a);
param2 = x0b + 0.5 * (xb - x0b);
}
else
{
double x0 = elements[is_corner];
double x1 = elements[is_corner - 1];
double x2 = elements[is_corner + 1];
param1 = x0 + 0.5 * (x1 - x0);
param2 = x0 + 0.5 * (x2 - x0);
}
double pt1[4];
nurbs.Evaluate (param1, 1, 2, pt1);
Eigen::Vector2d t1 (pt1[2], pt1[3]);
t1.normalize ();
double pt2[4];
nurbs.Evaluate (param2, 1, 2, pt2);
Eigen::Vector2d t2 (pt2[2], pt2[3]);
t2.normalize ();
t = 0.5 * (t1 + t2);
}
else
{
double point_tangent[4];
nurbs.Evaluate (min_param, 1, 2, point_tangent);
t (0) = point_tangent[2];
t (1) = point_tangent[3];
}
t.normalize ();
p = min_pt;
return min_param;
}
