void
Triangulation::convertSurface2Vertices (const ON_NurbsSurface &nurbs, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
std::vector<pcl::Vertices> &vertices, unsigned resolution)
{
// copy knots
if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1)
{
printf ("[Triangulation::convertSurface2Vertices] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
vertices.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;
createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution);
createIndices (vertices, 0, resolution, resolution);
for (auto &v : *cloud)
{
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = static_cast<float> (point[0]);
v.y = static_cast<float> (point[1]);
v.z = static_cast<float> (point[2]);
}
}
void
Triangulation::convertSurface2PolygonMesh (const ON_NurbsSurface &nurbs, PolygonMesh &mesh, unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[1] - 1);
double h = y1 - y0;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
createIndices (mesh.polygons, 0, resolution, resolution);
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &v = cloud->at (i);
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = point[0];
v.y = point[1];
v.z = point[2];
}
toROSMsg (*cloud, mesh.cloud);
}
void
Triangulation::convertSurface2PolygonMesh (const ON_NurbsSurface &nurbs, PolygonMesh &mesh, unsigned resolution)
{
// copy knots
if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1)
{
printf ("[Triangulation::convertSurface2PolygonMesh] Warning: ON knot vector empty.\n");
return;
}
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>);
mesh.polygons.clear ();
createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution);
createIndices (mesh.polygons, 0, resolution, resolution);
for (auto &v : *cloud)
{
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = float (point[0]);
v.y = float (point[1]);
v.z = float (point[2]);
}
toPCLPointCloud2 (*cloud, mesh.cloud);
}
std::vector<double>
FittingCylinder::getElementVector (const ON_NurbsSurface &nurbs, int dim) // !
{
std::vector<double> result;
if (dim == 0)
{
int idx_min = 0;
int idx_max = nurbs.KnotCount (0) - 1;
if (nurbs.IsClosed (0))
{
idx_min = nurbs.Order (0) - 2;
idx_max = nurbs.KnotCount (0) - nurbs.Order (0) + 1;
}
const double* knotsU = nurbs.Knot (0);
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]);
}
}
else if (dim == 1)
{
int idx_min = 0;
int idx_max = nurbs.KnotCount (1) - 1;
if (nurbs.IsClosed (1))
{
idx_min = nurbs.Order (1) - 2;
idx_max = nurbs.KnotCount (1) - nurbs.Order (1) + 1;
}
const double* knotsV = nurbs.Knot (1);
result.push_back (knotsV[idx_min]);
//for(int F=(m_nurbs.m_order[1]-2); F<(m_nurbs.m_knot_capacity[1]-m_nurbs.m_order[1]+2); F++) {
for (int F = idx_min + 1; F <= idx_max; F++)
{
if (knotsV[F] != knotsV[F - 1])
result.push_back (knotsV[F]);
}
}
else
printf ("[FittingCylinder::getElementVector] Error, index exceeds problem dimensions!\n");
return result;
}
std::vector<double>
FittingSurface::getElementVector (const ON_NurbsSurface &nurbs, int dim) // !
{
std::vector<double> result;
int idx_min = 0;
int idx_max = nurbs.KnotCount (dim) - 1;
if (nurbs.IsClosed (dim))
{
idx_min = nurbs.Order (dim) - 2;
idx_max = nurbs.KnotCount (dim) - nurbs.Order (dim) + 1;
}
const double* knots = nurbs.Knot (dim);
result.push_back (knots[idx_min]);
//for(int E=(m_nurbs.Order(0)-2); E<(m_nurbs.KnotCount(0)-m_nurbs.Order(0)+2); E++) {
for (int E = idx_min + 1; E <= idx_max; E++)
{
if (!NEAR_EQUAL(knots[E], knots[E - 1], SMALL_FASTF)) // do not count double knots
result.push_back (knots[E]);
}
return result;
}
void
Triangulation::convertSurface2Vertices (const ON_NurbsSurface &nurbs, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
std::vector<pcl::Vertices> &vertices, unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
vertices.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[1] - 1);
double h = y1 - y0;
createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
createIndices (vertices, 0, resolution, resolution);
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &v = cloud->at (i);
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = static_cast<float> (point[0]);
v.y = static_cast<float> (point[1]);
v.z = static_cast<float> (point[2]);
}
}
void
Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve,
PolygonMesh &mesh, unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
mesh.polygons.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[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);
std::vector<uint32_t> out_idx;
pcl::on_nurbs::vector_vec2d out_pc;
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++)
{
double err;
Eigen::Vector2d pc, tc;
uint32_t &vi = poly.vertices[j];
pcl::PointXYZ &v = cloud->at (vi);
Eigen::Vector2d vp (v.x, v.y);
double param = pcl::on_nurbs::FittingCurve2d::findClosestElementMidPoint (curve, vp);
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);
if (z (2) >= 0.0)
in++;
else
{
out_idx_tmp.push_back (vi);
out_pc_tmp.push_back (pc);
}
}
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[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = point[0];
v.y = point[1];
v.z = point[2];
}
toROSMsg (*cloud, mesh.cloud);
}
void ON_GL( const ON_NurbsSurface& s,
GLUnurbsObj* nobj, // created with gluNewNurbsRenderer )
GLenum type, // = 0 (and type is automatically set)
int bPermitKnotScaling,
double* knot_scale0,
double* knot_scale1
)
{
int i, j, k;
// The "bPermitScaling" parameters to the ON_GL() call that
// fills in the knot vectors is set to false because any
// rescaling that is applied to a surface domain must also
// be applied to parameter space trimming curve geometry.
// GL "s" knots
GLint sknot_count = s.KnotCount(0) + 2;
GLfloat* sknot = (GLfloat*)onmalloc( sknot_count*sizeof(*sknot) );
ON_GL( s.Order(0), s.CVCount(0), s.Knot(0), sknot,
bPermitKnotScaling, knot_scale0 );
// GL "t" knots
GLint tknot_count = s.KnotCount(1) + 2;
GLfloat* tknot = (GLfloat*)onmalloc( tknot_count*sizeof(*tknot) );
ON_GL( s.Order(1), s.CVCount(1), s.Knot(1), tknot,
bPermitKnotScaling, knot_scale1 );
// control vertices
const int cv_size= s.CVSize();
const int cv_count[2] = {s.CVCount(0), s.CVCount(1)};
GLint s_stride = cv_size*cv_count[1];
GLint t_stride = cv_size;
GLfloat* ctlarray = (GLfloat*)onmalloc( s_stride*cv_count[0]*sizeof(*ctlarray) );
for ( i = 0; i < cv_count[0]; i++ ) {
for ( j = 0; j < cv_count[1]; j++ ) {
const double* cv = s.CV(i,j);
GLfloat* gl_cv = ctlarray + s_stride*i + t_stride*j;
for ( k = 0; k < cv_size; k++ ) {
gl_cv[k] = (GLfloat)cv[k];
}
}
}
GLint sorder = s.Order(0);
GLint torder = s.Order(1);
if ( type == 0 ) {
// set GL surface type for 3d CVs in homogeneous/euclidean form.
type = ( s.IsRational() ) ? GL_MAP2_VERTEX_4 : GL_MAP2_VERTEX_3;
}
gluNurbsSurface (
nobj,
sknot_count,
sknot,
tknot_count,
tknot,
s_stride,
t_stride,
ctlarray,
sorder,
torder,
type
);
onfree( ctlarray );
onfree( tknot );
onfree( sknot );
}
void
Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve,
PolygonMesh &mesh, unsigned resolution, vector_vec3d &start,
vector_vec3d &end)
{
// copy knots
if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1 || curve.KnotCount () <= 1)
{
printf ("[Triangulation::convertTrimmedSurface2PolygonMesh] Warning: ON knot vector empty.\n");
return;
}
mesh.polygons.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.KnotCount (0) - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.KnotCount (1) - 1);
double h = y1 - y0;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
std::vector<pcl::Vertices> polygons;
createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
createIndices (polygons, 0, resolution, resolution);
vector_vec2d points (cloud->size (), Eigen::Vector2d ());
std::vector<double> params (cloud->size (), 0.0);
std::vector<bool> pt_is_in (cloud->size (), false);
std::vector<uint32_t> out_idx;
pcl::on_nurbs::vector_vec2d out_pc;
for (unsigned i = 0; i < cloud->size (); i++)
{
double err, param;
Eigen::Vector2d pc, tc;
pcl::PointXYZ &v = cloud->at (i);
Eigen::Vector2d vp (v.x, v.y);
if(curve.Order()==2)
param = pcl::on_nurbs::FittingCurve2d::inverseMappingO2 (curve, vp, err, pc, tc);
else
{
param = pcl::on_nurbs::FittingCurve2d::findClosestElementMidPoint(curve, vp);
param = pcl::on_nurbs::FittingCurve2d::inverseMapping(curve, vp, param, err, pc, tc);
}
Eigen::Vector3d a (vp (0) - pc (0), vp (1) - pc (1), 0.0);
Eigen::Vector3d b (tc (0), tc (1), 0.0);
Eigen::Vector3d z = a.cross (b);
points[i] = pc;
params[i] = param;
pt_is_in[i] = (z (2) >= 0.0);
end.push_back (Eigen::Vector3d (pc (0), pc (1), 0.0));
start.push_back (Eigen::Vector3d (pc (0) + tc (0) * 0.01, pc (1) + tc (1) * 0.01, 0.0));
}
for (unsigned i = 0; i < polygons.size (); i++)
{
unsigned in (0);
pcl::Vertices &poly = polygons[i];
std::vector<uint32_t> out_idx_tmp;
pcl::on_nurbs::vector_vec2d out_pc_tmp;
for (std::size_t j = 0; j < poly.vertices.size (); j++)
{
uint32_t &vi = poly.vertices[j];
if (pt_is_in[vi])
in++;
else
{
out_idx_tmp.push_back (vi);
out_pc_tmp.push_back (points[vi]);
}
}
if (in > 0)
{
mesh.polygons.push_back (poly);
if (in < poly.vertices.size ())
{
for (std::size_t j = 0; j < out_idx_tmp.size (); j++)
{
out_idx.push_back (out_idx_tmp[j]);
out_pc.push_back (out_pc_tmp[j]);
}
}
}
}
for (std::size_t i = 0; i < out_idx.size (); i++)
{
pcl::PointXYZ &v = cloud->at (out_idx[i]);
Eigen::Vector2d &pc = out_pc[i];
v.x = pc (0);
v.y = pc (1);
}
for (std::size_t i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &v = cloud->at (i);
double point[3];
nurbs.Evaluate (v.x, v.y, 0, 3, point);
v.x = point[0];
v.y = point[1];
v.z = point[2];
}
for (std::size_t i = 0; i < start.size (); i++)
{
Eigen::Vector3d &p1 = start[i];
Eigen::Vector3d &p2 = end[i];
double point[3];
nurbs.Evaluate (p1 (0), p1 (1), 0, 3, point);
p1 (0) = point[0];
p1 (1) = point[1];
p1 (2) = point[2];
nurbs.Evaluate (p2 (0), p2 (1), 0, 3, point);
p2 (0) = point[0];
p2 (1) = point[1];
p2 (2) = point[2];
}
toROSMsg (*cloud, mesh.cloud);
}
void
Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve,
PolygonMesh &mesh, unsigned resolution)
{
// copy knots
if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1 || curve.KnotCount () <= 1)
{
printf ("[Triangulation::convertTrimmedSurface2PolygonMesh] Warning: ON knot vector empty.\n");
return;
}
mesh.polygons.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.KnotCount (0) - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.KnotCount (1) - 1);
double h = y1 - y0;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
std::vector<pcl::Vertices> polygons;
createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution);
createIndices (polygons, 0, resolution, resolution);
vector_vec2d points (cloud->size (), Eigen::Vector2d ());
std::vector<bool> pt_is_in (cloud->size (), false);
Eigen::Vector3d a0, a1;
pcl::on_nurbs::NurbsTools::computeBoundingBox (curve, a0, a1);
double rScale = 1.0 / pcl::on_nurbs::NurbsTools::computeRScale (a0, a1);
std::vector<uint32_t> out_idx;
pcl::on_nurbs::vector_vec2d out_pc;
for (size_t i = 0; i < cloud->size (); i++)
{
double err;
Eigen::Vector2d pc, tc;
pcl::PointXYZ &v = cloud->at (i);
Eigen::Vector2d vp (v.x, v.y);
if (curve.Order () == 2)
pcl::on_nurbs::FittingCurve2dAPDM::inverseMappingO2 (curve, vp, err, pc, tc);
else
{
double param = pcl::on_nurbs::FittingCurve2dAPDM::findClosestElementMidPoint (curve, vp);
pcl::on_nurbs::FittingCurve2dAPDM::inverseMapping (curve, vp, param, err, pc, tc, rScale);
}
Eigen::Vector3d a (vp (0) - pc (0), vp (1) - pc (1), 0.0);
Eigen::Vector3d b (tc (0), tc (1), 0.0);
Eigen::Vector3d z = a.cross (b);
points[i] = pc;
pt_is_in[i] = (z (2) >= 0.0);
}
for (const auto &poly : polygons)
{
unsigned in (0);
std::vector<uint32_t> out_idx_tmp;
pcl::on_nurbs::vector_vec2d out_pc_tmp;
for (const unsigned int &vi : poly.vertices)
{
if (pt_is_in[vi])
in++;
else
{
out_idx_tmp.push_back (vi);
out_pc_tmp.push_back (points[vi]);
}
}
if (in > 0)
{
mesh.polygons.push_back (poly);
if (in < poly.vertices.size ())
{
for (std::size_t j = 0; j < out_idx_tmp.size (); j++)
{
out_idx.push_back (out_idx_tmp[j]);
out_pc.push_back (out_pc_tmp[j]);
}
}
}
}
for (std::size_t i = 0; i < out_idx.size (); i++)
{
pcl::PointXYZ &v = cloud->at (out_idx[i]);
Eigen::Vector2d &pc = out_pc[i];
v.x = float (pc (0));
v.y = float (pc (1));
}
for (auto &v : *cloud)
{
Eigen::Vector3d tu, tv;
double point[3];
nurbs.Evaluate (v.x, v.y, 0, 3, point);
v.x = float (point[0]);
v.y = float (point[1]);
v.z = float (point[2]);
// tu[0] = point[3];
// tu[1] = point[4];
// tu[2] = point[5];
// tv[0] = point[6];
// tv[1] = point[7];
// tv[2] = point[8];
// TODO: add normals to mesh
}
toPCLPointCloud2 (*cloud, mesh.cloud);
}