void
GlobalOptimizationTDM::updateSurf (double damp)
{
int ncps (0);
for (unsigned i = 0; i < m_nurbs.size (); i++)
{
ON_NurbsSurface* nurbs = m_nurbs[i];
int ncp = nurbs->CVCount ();
for (int A = 0; A < ncp; A++)
{
int I = gl2gr (*nurbs, A);
int J = gl2gc (*nurbs, A);
ON_3dPoint cp_prev;
nurbs->GetCV (I, J, cp_prev);
ON_3dPoint cp;
cp.x = cp_prev.x + damp * (m_solver.x (3 * (ncps + A) + 0, 0) - cp_prev.x);
cp.y = cp_prev.y + damp * (m_solver.x (3 * (ncps + A) + 1, 0) - cp_prev.y);
cp.z = cp_prev.z + damp * (m_solver.x (3 * (ncps + A) + 2, 0) - cp_prev.z);
nurbs->SetCV (I, J, cp);
}
ncps += ncp;
}
}
void
GlobalOptimization::addCageInteriorRegularisation (unsigned id, int ncps, double weight, unsigned &row)
{
ON_NurbsSurface *nurbs = m_nurbs[id];
for (int i = 1; i < (nurbs->CVCount (0) - 1); i++)
{
for (int j = 1; j < (nurbs->CVCount (1) - 1); j++)
{
m_solver.f (row, 0, 0.0);
m_solver.f (row, 1, 0.0);
m_solver.f (row, 2, 0.0);
m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 0), -4.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j - 1), 1.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 1), 1.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i - 1, j + 0), 1.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i + 1, j + 0), 1.0 * weight);
row++;
}
}
// if (!m_quiet && !(row % 100))
// printf("[GlobalOptimization::addCageInteriorRegularisation] row: %d / %d\n", row, m_nrows);
}
static ON_Surface* CreatePlanarSurface(
const ON_3dPoint& SW, const ON_3dPoint& SE,
const ON_3dPoint& NE, const ON_3dPoint& NW
)
{
ON_NurbsSurface* pNurbsSurface = new ON_NurbsSurface(
3, // dimension (>= 1)
FALSE, // not rational
2, // "u" order (>= 2)
2, // "v" order (>= 2)
2, // number of control vertices in "u" dir (>= order)
2 // number of control vertices in "v" dir (>= order)
);
// corner CVs in counter clockwise order starting in the south west
pNurbsSurface->SetCV( 0,0, SW );
pNurbsSurface->SetCV( 1,0, SE );
pNurbsSurface->SetCV( 1,1, NE );
pNurbsSurface->SetCV( 0,1, NW );
// "u" knots
pNurbsSurface->SetKnot( 0,0, 0.0 );
pNurbsSurface->SetKnot( 0,1, 1.0 );
// "v" knots
pNurbsSurface->SetKnot( 1,0, 0.0 );
pNurbsSurface->SetKnot( 1,1, 1.0 );
return pNurbsSurface;
}
static ON_Surface* TwistedCubeSideSurface(
const ON_3dPoint& SW, const ON_3dPoint& SE,
const ON_3dPoint& NE, const ON_3dPoint& NW
)
{
ON_NurbsSurface* pNurbsSurface = new ON_NurbsSurface(
3, // dimension
false, // not rational
2, // "u" order
2, // "v" order
2, // number of control vertices in "u" dir
2 // number of control vertices in "v" dir
);
// corner CVs in counter clockwise order starting in the south west
pNurbsSurface->SetCV( 0,0, SW );
pNurbsSurface->SetCV( 1,0, SE );
pNurbsSurface->SetCV( 1,1, NE );
pNurbsSurface->SetCV( 0,1, NW );
// "u" knots
pNurbsSurface->SetKnot( 0,0, 0.0 );
pNurbsSurface->SetKnot( 0,1, 1.0 );
// "v" knots
pNurbsSurface->SetKnot( 1,0, 0.0 );
pNurbsSurface->SetKnot( 1,1, 1.0 );
return pNurbsSurface;
}
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);
}
osg::Node* RhinoReader::BuildWireFrameFace(const ON_BrepFace* theFace)
{
osg::ref_ptr<osg::Geode> aGeode = new osg::Geode();
ON_NurbsSurface aSurface;
if (theFace->GetNurbForm(aSurface) == 0)
{
return NULL;
}
double u0 = aSurface.Domain(0).Min();
double u1 = aSurface.Domain(0).Max();
double v0 = aSurface.Domain(1).Min();
double v1 = aSurface.Domain(1).Max();
double d0 = 0.0;
double d1 = 0.0;
d0 = (u1 - u0) / 10.0;
d1 = (v1 - v0) / 10.0;
for (double u = u0; (u - u1) < TOLERANCE_FACE; u += d0)
{
osg::ref_ptr<osg::Geometry> aGeometry = new osg::Geometry();
osg::ref_ptr<osg::Vec3Array> aVertices = new osg::Vec3Array();
for (double v = v0; (v - v1) < TOLERANCE_FACE; v += d1)
{
ON_3dPoint aPoint = aSurface.PointAt(u, v);
aVertices->push_back(osg::Vec3(aPoint.x, aPoint.y, aPoint.z));
}
aGeometry->setVertexArray(aVertices);
aGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP, 0, aVertices->size()));
aGeode->addDrawable(aGeometry);
}
for (double v = v0; (v - v1) < TOLERANCE_FACE; v += d1)
{
osg::ref_ptr<osg::Geometry> aGeometry = new osg::Geometry();
osg::ref_ptr<osg::Vec3Array> aVertices = new osg::Vec3Array();
for (double u = u0; (u - u1) < TOLERANCE_FACE; u += d0)
{
ON_3dPoint aPoint = aSurface.PointAt(u, v);
aVertices->push_back(osg::Vec3(aPoint.x, aPoint.y, aPoint.z));
}
aGeometry->setVertexArray(aVertices);
aGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP, 0, aVertices->size()));
aGeode->addDrawable(aGeometry);
}
return aGeode.release();
}
void
NurbsTools::computeBoundingBox (const ON_NurbsSurface &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 (0); i++)
{
for (int j = 0; j < nurbs.CVCount (1); j++)
{
ON_3dPoint p;
nurbs.GetCV (i, j, 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;
}
}
}
Eigen::Vector3d
ClosingBoundary::commonBoundaryPoint2 (ON_NurbsSurface &n1, ON_NurbsSurface &n2, Eigen::Vector2d ¶ms1,
Eigen::Vector2d ¶ms2, Eigen::Vector3d start, unsigned nsteps, double &error,
double accuracy)
{
Eigen::Vector3d current = start;
double error1 (DBL_MAX);
double error2 (DBL_MAX);
Eigen::Vector3d p1, p2, tu1, tu2, tv1, tv2;
params1 = FittingSurface::findClosestElementMidPoint (n1, current);
params2 = FittingSurface::findClosestElementMidPoint (n2, current);
params1 = FittingSurface::inverseMapping (n1, current, params1, error1, p1, tu1, tv1, nsteps, accuracy, true);
params2 = FittingSurface::inverseMapping (n2, current, params2, error2, p2, tu2, tv2, nsteps, accuracy, true);
for (unsigned i = 0; i < nsteps; i++)
{
params1 = FittingSurface::inverseMapping (n1, current, params1, error1, p1, tu1, tv1, nsteps, accuracy, true);
params2 = FittingSurface::inverseMapping (n2, current, params2, error2, p2, tu2, tv2, nsteps, accuracy, true);
// params1 = ntools1.inverseMappingBoundary(current, error1, p1, tu1, tv1, 10, 1e-2, true);
// params2 = ntools2.inverseMappingBoundary(current, error2, p2, tu2, tv2, 10, 1e-2, true);
// dbgWin.AddLine3D(current(0), current(1), current(2), p1(0), p1(1), p1(2), 0, 0, 255);
// dbgWin.AddLine3D(current(0), current(1), current(2), p2(0), p2(1), p2(2), 0, 0, 255);
// dbgWin.AddPoint3D(current(0), current(1), current(2), 0, 0, 255, 3);
// current = (p1 + p2) * 0.5;
Eigen::Vector3d n1 = tu1.cross (tv1);
n1.normalize ();
Eigen::Vector3d n2 = tu2.cross (tv2);
n2.normalize ();
Eigen::Vector3d l1 = (p2 - n1 * n1.dot (p2 - p1)) - p1;
l1.normalize ();
Eigen::Vector3d l2 = (p1 - n2 * n2.dot (p1 - p2)) - p2;
l2.normalize ();
// dbgWin.AddLine3D(p1(0), p1(1), p1(2), p1(0) + l1(0), p1(1) + l1(1), p1(2) + l1(2), 255, 0, 0);
// dbgWin.AddLine3D(p2(0), p2(1), p2(2), p2(0) + l2(0), p2(1) + l2(1), p2(2) + l2(2), 255, 0, 0);
Eigen::Vector3d P, Q;
getLineDistance (p1, l1, p2, l2, P, Q);
current = (P + Q) * 0.5;
}
error = 0.5 * (error1 + error2);
// dbgWin.AddPoint3D(current(0), current(1), current(2), 255, 0, 255, 5);
// dbgWin.Update();
return current;
}
ON_NurbsSurface
FittingCylinder::initNurbsCylinderWithAxes (int order, NurbsDataSurface *data, Eigen::Matrix3d &axes)
{
Eigen::Vector3d mean;
unsigned s = unsigned (data->interior.size ());
mean = NurbsTools::computeMean (data->interior);
data->mean = mean;
Eigen::Vector3d v_max (0.0, 0.0, 0.0);
Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
for (unsigned i = 0; i < s; i++)
{
Eigen::Vector3d p (axes.inverse () * (data->interior[i] - mean));
if (p (0) > v_max (0))
v_max (0) = p (0);
if (p (1) > v_max (1))
v_max (1) = p (1);
if (p (2) > v_max (2))
v_max (2) = p (2);
if (p (0) < v_min (0))
v_min (0) = p (0);
if (p (1) < v_min (1))
v_min (1) = p (1);
if (p (2) < v_min (2))
v_min (2) = p (2);
}
int ncpsU (order);
int ncpsV (2 * order + 4);
ON_NurbsSurface nurbs (3, false, order, order, ncpsU, ncpsV);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakePeriodicUniformKnotVector (1, 1.0 / (ncpsV - order + 1));
double dcu = (v_max (0) - v_min (0)) / (ncpsU - 1);
double dcv = (2.0 * M_PI) / (ncpsV - order + 1);
double ry = std::max<double> (std::fabs (v_min (1)), std::fabs (v_max (1)));
double rz = std::max<double> (std::fabs (v_min (2)), std::fabs (v_max (2)));
Eigen::Vector3d cv_t, cv;
for (int i = 0; i < ncpsU; i++)
{
for (int j = 0; j < ncpsV; j++)
{
cv (0) = v_min (0) + dcu * i;
cv (1) = ry * sin (dcv * j);
cv (2) = rz * cos (dcv * j);
cv_t = axes * cv + mean;
nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
}
}
return nurbs;
}
int
ON_PlaneSurface::GetNurbForm( // returns 0: unable to create NURBS representation
// with desired accuracy.
// 1: success - returned NURBS parameterization
// matches the surface's to wthe desired accuracy
// 2: success - returned NURBS point locus matches
// the surfaces's to the desired accuracy but, on
// the interior of the surface's domain, the
// surface's parameterization and the NURBS
// parameterization may not match to the
// desired accuracy.
ON_NurbsSurface& nurbs,
double tolerance
) const
{
ON_BOOL32 rc = IsValid();
if( !rc )
{
if ( m_plane.origin.x != ON_UNSET_VALUE
&& m_plane.xaxis.x != ON_UNSET_VALUE
&& m_plane.yaxis.x != ON_UNSET_VALUE
&& m_domain[0].IsIncreasing() && m_domain[1].IsIncreasing()
&& m_extents[0].Length() > 0.0 && m_extents[1].Length() > 0.0
)
{
ON_3dVector N = ON_CrossProduct(m_plane.xaxis,m_plane.yaxis);
if ( N.Length() <= 1.0e-4 )
{
ON_WARNING("ON_PlaneSurface::GetNurbForm - using invalid surface.");
rc = true;
}
}
}
if ( rc )
{
nurbs.m_dim = 3;
nurbs.m_is_rat = 0;
nurbs.m_order[0] = nurbs.m_order[1] = 2;
nurbs.m_cv_count[0] = nurbs.m_cv_count[1] = 2;
nurbs.m_cv_stride[1] = nurbs.m_dim;
nurbs.m_cv_stride[0] = nurbs.m_cv_stride[1]*nurbs.m_cv_count[1];
nurbs.ReserveCVCapacity(12);
nurbs.ReserveKnotCapacity(0,2);
nurbs.ReserveKnotCapacity(1,2);
nurbs.m_knot[0][0] = m_domain[0][0];
nurbs.m_knot[0][1] = m_domain[0][1];
nurbs.m_knot[1][0] = m_domain[1][0];
nurbs.m_knot[1][1] = m_domain[1][1];
nurbs.SetCV( 0, 0, PointAt( m_domain[0][0], m_domain[1][0] ));
nurbs.SetCV( 0, 1, PointAt( m_domain[0][0], m_domain[1][1] ));
nurbs.SetCV( 1, 0, PointAt( m_domain[0][1], m_domain[1][0] ));
nurbs.SetCV( 1, 1, PointAt( m_domain[0][1], m_domain[1][1] ));
}
return rc;
}
extern "C" void
rt_nurb_brep(ON_Brep **b, const struct rt_db_internal *ip, const struct bn_tol *)
{
int i, j, k;
struct rt_nurb_internal *nip;
RT_CK_DB_INTERNAL(ip);
nip = (struct rt_nurb_internal *)ip->idb_ptr;
RT_NURB_CK_MAGIC(nip);
ON_TextLog log(stderr);
for (i = 0; i < nip->nsrf; i++) {
struct face_g_snurb *surface = nip->srfs[i];
NMG_CK_SNURB(surface);
ON_NurbsSurface *nurb = ON_NurbsSurface::New(3, true, surface->order[0], surface->order[1], surface->s_size[0], surface->s_size[1]);
/* set 'u' knots */
/* skip first and last (duplicates?) */
for (j = 1; j < surface->u.k_size - 1; j++) {
nurb->SetKnot(0, j-1, surface->u.knots[j]);
/* bu_log("u knot %d is %f\n", j-1, surface->u.knots[j]); */
}
/* set 'v' knots */
/* skip first and last (duplicates?) */
for (j = 1; j < surface->v.k_size - 1; j++) {
nurb->SetKnot(1, j-1, surface->v.knots[j]);
/* bu_log("v knot %d is %f\n", j-1, surface->u.knots[j]); */
}
/* set control points */
for (j = 0; j < surface->s_size[0]; j++) {
for (k = 0; k < surface->s_size[1]; k++) {
ON_3dPoint point = &RT_NURB_GET_CONTROL_POINT(surface, j, k);
nurb->SetCV(k, j, point);
}
}
/* nurb->Dump(log); */
bu_log("NURBS surface %d %s valid\n", i, nurb->IsValid(&log) ? "is" : "is not");
(*b)->m_S.Append(nurb);
int sindex = (*b)->m_S.Count();
(*b)->NewFace(sindex - 1);
int findex = (*b)->m_F.Count();
(*b)->NewOuterLoop(findex - 1);
}
bu_log("BREP object %s a single surface\n", (*b)->IsSurface() ? "is" : "is not");
bu_log("BREP object %s valid\n", (*b)->IsValid(&log) ? "is" : "is not");
bu_log("BREP object %s valid topology\n", (*b)->IsValidTopology(&log) ? "is" : "is not");
bu_log("BREP object %s valid geometry\n", (*b)->IsValidGeometry(&log) ? "is" : "is not");
bu_log("BREP object %s solid\n", (*b)->IsSolid() ? "is" : "is not");
bu_log("BREP object %s manifold\n", (*b)->IsManifold() ? "is" : "is not");
}
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);
}
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
SequentialFitter::compute_quadfit ()
{
ON_NurbsSurface nurbs;
if (m_have_corners)
{
nurbs = FittingSurface::initNurbs4Corners (2, m_corners[0], m_corners[1], m_corners[2], m_corners[3]);
}
else
{
nurbs = FittingSurface::initNurbsPCA (2, &m_data);
nurbs.GetCV (0, 0, m_corners[0]);
nurbs.GetCV (1, 0, m_corners[1]);
nurbs.GetCV (1, 1, m_corners[2]);
nurbs.GetCV (0, 1, m_corners[3]);
Eigen::Vector3d v0 (m_corners[0].x, m_corners[0].y, m_corners[0].z);
Eigen::Vector3d v1 (m_corners[1].x, m_corners[1].y, m_corners[1].z);
Eigen::Vector3d v2 (m_corners[2].x, m_corners[2].y, m_corners[2].z);
Eigen::Vector3d v3 (m_corners[3].x, m_corners[3].y, m_corners[3].z);
if (is_back_facing (v0, v1, v2, v3))
{
ON_3dPoint tmp[4];
tmp[0] = m_corners[0];
tmp[1] = m_corners[1];
tmp[2] = m_corners[2];
tmp[3] = m_corners[3];
m_corners[3] = tmp[0];
m_corners[2] = tmp[1];
m_corners[1] = tmp[2];
m_corners[0] = tmp[3];
nurbs = FittingSurface::initNurbs4Corners (2, m_corners[0], m_corners[1], m_corners[2], m_corners[3]);
}
}
FittingSurface fitting (&m_data, nurbs);
FittingSurface::Parameter paramFP (m_params.forceInterior, 1.0, 0.0, m_params.forceBoundary, 1.0, 0.0);
// Quad fitting
// if( !m_quiet && m_dbgWin != NULL )
// NurbsConvertion::Nurbs2TomGine(m_dbgWin, *fitting->m_nurbs, m_surf_id, m_params.resolution);
for (int r = 0; r < m_params.iterationsQuad; r++)
{
fitting.assemble (paramFP);
fitting.solve ();
}
fitting.m_nurbs.GetCV (0, 0, m_corners[0]);
fitting.m_nurbs.GetCV (1, 0, m_corners[1]);
fitting.m_nurbs.GetCV (1, 1, m_corners[2]);
fitting.m_nurbs.GetCV (0, 1, m_corners[3]);
}
HIDDEN ON_Surface*
sideSurface(const ON_3dPoint& SW, const ON_3dPoint& SE, const ON_3dPoint& NE, const ON_3dPoint& NW)
{
ON_NurbsSurface *surf = ON_NurbsSurface::New(3, 0, 2, 2, 2, 2);
surf->SetCV(0, 0, SW);
surf->SetCV(1, 0, SE);
surf->SetCV(1, 1, NE);
surf->SetCV(0, 1, NW);
surf->SetKnot(0, 0, 0.0);
surf->SetKnot(0, 1, 1.0);
surf->SetKnot(1, 0, 0.0);
surf->SetKnot(1, 1, 1.0);
return surf;
}
void
GlobalOptimization::addCageBoundaryRegularisation (unsigned id, int ncps, double weight, int side, unsigned &row)
{
ON_NurbsSurface *nurbs = m_nurbs[id];
int i = 0;
int j = 0;
switch (side)
{
case SOUTH:
j = nurbs->CVCount (1) - 1;
case NORTH:
for (i = 1; i < (nurbs->CVCount (0) - 1); i++)
{
m_solver.f (row, 0, 0.0);
m_solver.f (row, 1, 0.0);
m_solver.f (row, 2, 0.0);
m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j), -2.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i - 1, j), 1.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i + 1, j), 1.0 * weight);
row++;
}
break;
case EAST:
i = nurbs->CVCount (0) - 1;
case WEST:
for (j = 1; j < (nurbs->CVCount (1) - 1); j++)
{
m_solver.f (row, 0, 0.0);
m_solver.f (row, 1, 0.0);
m_solver.f (row, 2, 0.0);
m_solver.K (row, ncps + grc2gl (*nurbs, i, j + 0), -2.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i, j - 1), 1.0 * weight);
m_solver.K (row, ncps + grc2gl (*nurbs, i, j + 1), 1.0 * weight);
row++;
}
break;
}
// if (!m_quiet && !(row % 100))
// printf("[GlobalOptimization::addCageBoundaryRegularisation] row: %d / %d\n", row, m_nrows);
}
Eigen::Vector3d
ClosingBoundary::commonBoundaryPoint3 (ON_NurbsSurface &n1, ON_NurbsSurface &n2, Eigen::Vector2d ¶ms1,
Eigen::Vector2d ¶ms2, Eigen::Vector3d start, unsigned nsteps, double &error,
double accuracy)
{
Eigen::Vector3d current = start;
double error1 (DBL_MAX);
double error2 (DBL_MAX);
Eigen::Vector3d p1, p2, tu1, tu2, tv1, tv2;
params1 = FittingSurface::findClosestElementMidPoint (n1, current);
params2 = FittingSurface::findClosestElementMidPoint (n2, current);
params1 = FittingSurface::inverseMapping (n1, current, params1, error1, p1, tu1, tv1, nsteps, accuracy, true);
params2 = FittingSurface::inverseMapping (n2, current, params2, error2, p2, tu2, tv2, nsteps, accuracy, true);
for (unsigned i = 0; i < nsteps; i++)
{
params1 = FittingSurface::inverseMapping (n1, current, params1, error1, p1, tu1, tv1, nsteps, accuracy, true);
params2 = FittingSurface::inverseMapping (n2, current, params2, error2, p2, tu2, tv2, nsteps, accuracy, true);
// dbgWin.AddLine3D(current(0), current(1), current(2), p1(0), p1(1), p1(2), 0, 0, 255);
// dbgWin.AddLine3D(current(0), current(1), current(2), p2(0), p2(1), p2(2), 0, 0, 255);
// dbgWin.AddPoint3D(current(0), current(1), current(2), 0, 0, 255, 3);
Eigen::Vector3d n1 = tu1.cross (tv1);
n1.normalize ();
double d1 = n1.dot (p1);
Eigen::Vector3d n2 = tu2.cross (tv2);
n2.normalize ();
double d2 = n2.dot (p2);
Eigen::Vector3d n3 = (p1 - current).cross (p2 - current);
n3.normalize ();
double d3 = n3.dot (current);
current = intersectPlanes (n1, d1, n2, d2, n3, d3);
}
// dbgWin.AddPoint3D(current(0), current(1), current(2), 255, 0, 255, 5);
// dbgWin.Update();
error = 0.5 * (error1 + error2);
return current;
}
int ON_Cylinder::GetNurbForm( ON_NurbsSurface& s ) const
{
int rc = 0;
if ( IsValid() && height[0] != height[1] ) {
ON_NurbsCurve n0, n1;
int i;
ON_Circle c0 = CircleAt(height[0]);
ON_Circle c1 = CircleAt(height[1]);
if ( height[0] <= height[1] ) {
c0.GetNurbForm(n0);
c1.GetNurbForm(n1);
}
else {
c0.GetNurbForm(n1);
c1.GetNurbForm(n0);
}
if ( n0.m_dim != n1.m_dim
|| n0.m_is_rat != n1.m_is_rat
|| n0.m_order != n1.m_order
|| n0.m_cv_count != n1.m_cv_count )
return 0;
s.Create(3,TRUE, n0.m_order, 2, n0.m_cv_count, 2 );
if ( height[0] <= height[1] ) {
s.m_knot[1][0] = height[0];
s.m_knot[1][1] = height[1];
}
else {
s.m_knot[1][0] = height[1];
s.m_knot[1][1] = height[0];
}
for ( i = 0; i < n0.KnotCount(); i++ )
s.m_knot[0][i] = n0.m_knot[i];
for ( i = 0; i < n0.m_cv_count; i++ ) {
s.SetCV(i,0,ON::homogeneous_rational,n0.CV(i));
s.SetCV(i,1,ON::homogeneous_rational,n1.CV(i));
}
rc = 2;
}
return rc;
}
ON_NurbsSurface
FittingSurface::initNurbsPCA (int order, NurbsDataSurface *m_data, ON_3dVector z)
{
ON_3dVector mean;
Eigen::Matrix3d eigenvectors;
Eigen::Vector3d eigenvalues;
unsigned s = m_data->interior.size ();
NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);
m_data->mean = mean;
//m_data->eigenvectors = (*eigenvectors);
bool flip (false);
Eigen::Vector3d ez(z[0],z[1],z[2]);
if (eigenvectors.col (2).dot (ez) < 0.0)
flip = true;
eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
ON_3dVector sigma(sqrt(eigenvalues[0]), sqrt(eigenvalues[1]), sqrt(eigenvalues[2]));
ON_NurbsSurface nurbs (3, false, order, order, order, order);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakeClampedUniformKnotVector (1, 1.0);
// +- 2 sigma -> 95,45 % aller Messwerte
double dcu = (4.0 * sigma[0]) / (nurbs.Order (0) - 1);
double dcv = (4.0 * sigma[1]) / (nurbs.Order (1) - 1);
ON_3dVector cv_t, cv;
Eigen::Vector3d ecv_t, ecv;
Eigen::Vector3d emean(mean[0], mean[1], mean[2]);
for (int i = 0; i < nurbs.Order (0); i++)
{
for (int j = 0; j < nurbs.Order (1); j++)
{
cv[0] = -2.0 * sigma[0] + dcu * i;
cv[1] = -2.0 * sigma[1] + dcv * j;
cv[2] = 0.0;
ecv (0) = -2.0 * sigma[0] + dcu * i;
ecv (1) = -2.0 * sigma[1] + dcv * j;
ecv (2) = 0.0;
ecv_t = eigenvectors * ecv + emean;
cv_t[0] = ecv_t (0);
cv_t[1] = ecv_t (1);
cv_t[2] = ecv_t (2);
if (flip)
nurbs.SetCV (nurbs.Order (0) - 1 - i, j, ON_3dPoint (cv_t[0], cv_t[1], cv_t[2]));
else
nurbs.SetCV (i, j, ON_3dPoint (cv_t[0], cv_t[1], cv_t[2]));
}
}
return nurbs;
}
RH_C_FUNCTION ON_NurbsSurface* ON_NurbsSurface_CreateRuledSurface( const ON_Curve* pConstA, const ON_Curve* pConstB )
{
ON_NurbsSurface* rc = NULL;
if( pConstA && pConstB )
{
rc = new ON_NurbsSurface();
rc->CreateRuledSurface(*pConstA, *pConstB);
if( !rc->IsValid() )
{
delete rc;
rc = NULL;
}
}
return rc;
}
void nurbsfit::IncreaseDimension( const ON_NurbsSurface& src, ON_NurbsSurface& dest, int dim )
{
dest.m_dim = dim;
dest.m_is_rat = src.m_is_rat;
dest.m_order[0] = src.m_order[0];
dest.m_order[1] = src.m_order[1];
dest.m_cv_count[0] = src.m_cv_count[0];
dest.m_cv_count[1] = src.m_cv_count[1];
dest.m_cv_stride[1] = dest.m_is_rat ? dest.m_dim+1 : dest.m_dim;
dest.m_cv_stride[0] = dest.m_cv_count[1]*dest.m_cv_stride[1];
if ( src.m_knot[0] )
{
// copy knot array
dest.ReserveKnotCapacity( 0, dest.KnotCount(0) );
memcpy( dest.m_knot[0], src.m_knot[0], dest.KnotCount(0)*sizeof(*dest.m_knot[0]) );
}
if ( src.m_knot[1] )
{
// copy knot array
dest.ReserveKnotCapacity( 1, dest.KnotCount(1) );
memcpy( dest.m_knot[1], src.m_knot[1], dest.KnotCount(1)*sizeof(*dest.m_knot[1]) );
}
if ( src.m_cv )
{
// copy cv array
dest.ReserveCVCapacity( dest.m_cv_count[0]*dest.m_cv_count[1]*dest.m_cv_stride[1] );
const int dst_cv_size = dest.CVSize()*sizeof(*dest.m_cv);
const int src_stride[2] = {src.m_cv_stride[0],src.m_cv_stride[1]};
if ( src_stride[0] == dest.m_cv_stride[0] && src_stride[1] == dest.m_cv_stride[1] )
{
memcpy( dest.m_cv, src.m_cv, dest.m_cv_count[0]*dest.m_cv_count[1]*dest.m_cv_stride[1]*sizeof(*dest.m_cv) );
}
else
{
const double *src_cv;
double *dst_cv = dest.m_cv;
int i, j;
for ( i = 0; i < dest.m_cv_count[0]; i++ )
{
src_cv = src.CV(i,0);
for ( j = 0; j < dest.m_cv_count[1]; j++ )
{
memcpy( dst_cv, src_cv, dst_cv_size );
dst_cv += dest.m_cv_stride[1];
src_cv += src_stride[1];
}
}
}
}
}
int ON_Torus::GetNurbForm( ON_NurbsSurface& s ) const
{
int rc = 0;
ON_RevSurface revsrf;
if ( RevSurfaceForm(&revsrf) )
{
rc = revsrf.GetNurbForm(s);
}
else
s.Destroy();
return rc;
}
void
FittingSurface::refine (ON_NurbsSurface &nurbs, int dim)
{
std::vector<double> xi;
std::vector<double> elements = getElementVector (nurbs, dim);
for (unsigned i = 0; i < elements.size () - 1; i++)
xi.push_back (elements[i] + 0.5 * (elements[i + 1] - elements[i]));
for (unsigned i = 0; i < xi.size (); i++)
nurbs.InsertKnot (dim, xi[i], 1);
}
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;
}
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;
}
ON_NurbsSurface
FittingSurface::initNurbs4Corners (int order, ON_3dPoint ll, ON_3dPoint lr, ON_3dPoint ur, ON_3dPoint ul)
{
std::map<int, std::map<int, ON_3dPoint> > cv_map;
double dc = 1.0 / (order - 1);
for (int i = 0; i < order; i++)
{
double di = dc * i;
cv_map[i][0] = ll + (lr - ll) * di;
cv_map[0][i] = ll + (ul - ll) * di;
cv_map[i][order - 1] = ul + (ur - ul) * di;
cv_map[order - 1][i] = lr + (ur - lr) * di;
}
for (int i = 1; i < order - 1; i++)
{
for (int j = 1; j < order - 1; j++)
{
ON_3dPoint du = cv_map[0][j] + (cv_map[order - 1][j] - cv_map[0][j]) * dc * i;
ON_3dPoint dv = cv_map[i][0] + (cv_map[i][order - 1] - cv_map[i][0]) * dc * j;
cv_map[i][j] = du * 0.5 + dv * 0.5;
}
}
ON_NurbsSurface nurbs (3, false, order, order, order, order);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakeClampedUniformKnotVector (1, 1.0);
for (int i = 0; i < order; i++)
{
for (int j = 0; j < order; j++)
{
nurbs.SetCV (i, j, cv_map[i][j]);
}
}
return nurbs;
}
ON_Surface*
TwistedCubeSideSurface(const ON_3dPoint& SW, const ON_3dPoint& SE, const ON_3dPoint& NE, const ON_3dPoint& NW)
{
ON_NurbsSurface* pNurbsSurface = new ON_NurbsSurface(3, // dimension
FALSE, // not rational
2, // u order
2, // v order,
2, // number of control vertices in u
2 // number of control verts in v
);
pNurbsSurface->SetCV(0, 0, SW);
pNurbsSurface->SetCV(1, 0, SE);
pNurbsSurface->SetCV(1, 1, NE);
pNurbsSurface->SetCV(0, 1, NW);
// u knots
pNurbsSurface->SetKnot(0, 0, 0.0);
pNurbsSurface->SetKnot(0, 1, 1.0);
// v knots
pNurbsSurface->SetKnot(1, 0, 0.0);
pNurbsSurface->SetKnot(1, 1, 1.0);
return pNurbsSurface;
}
ON_NurbsSurface
FittingSurface::initNurbsPCA (int order, NurbsDataSurface *m_data, Eigen::Vector3d z)
{
Eigen::Vector3d mean;
Eigen::Matrix3d eigenvectors;
Eigen::Vector3d eigenvalues;
unsigned s = m_data->interior.size ();
NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);
m_data->mean = mean;
m_data->eigenvectors = eigenvectors;
bool flip (false);
if (eigenvectors.col (2).dot (z) < 0.0)
flip = true;
eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
Eigen::Vector3d sigma (sqrt (eigenvalues (0)), sqrt (eigenvalues (1)), sqrt (eigenvalues (2)));
ON_NurbsSurface nurbs (3, false, order, order, order, order);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakeClampedUniformKnotVector (1, 1.0);
// +- 2 sigma -> 95,45 % aller Messwerte
double dcu = (4.0 * sigma (0)) / (nurbs.Order (0) - 1);
double dcv = (4.0 * sigma (1)) / (nurbs.Order (1) - 1);
Eigen::Vector3d cv_t, cv;
for (int i = 0; i < nurbs.Order (0); i++)
{
for (int j = 0; j < nurbs.Order (1); j++)
{
cv (0) = -2.0 * sigma (0) + dcu * i;
cv (1) = -2.0 * sigma (1) + dcv * j;
cv (2) = 0.0;
cv_t = eigenvectors * cv + mean;
if (flip)
nurbs.SetCV (nurbs.Order (0) - 1 - i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
else
nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
}
}
return nurbs;
}
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::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);
}
}
}
