BRL-CAD
opennurbs_fit.cpp
Go to the documentation of this file.
1 /** @file opennurbs_fit.cpp
2  *
3  * Extensions to the openNURBS library, based off of Thomas Mörwald's
4  * surface fitting code in the Point Cloud Library (pcl). His code is
5  # BSD licensed:
6  *
7  * Copyright (c) 2011, Thomas Mörwald, Jonathan Balzer, Inc.
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Thomas Mörwald or Jonathan Balzer nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #include "common.h"
40 
41 #include <iostream>
42 #include <stdexcept>
43 #include <map>
44 #include <algorithm>
45 
46 #include "opennurbs_fit.h"
47 #include "vmath.h"
48 
49 using namespace on_fit;
50 
51 // ************************
52 // * from nurbs_tools.cpp *
53 // ************************
54 
55 void
56 NurbsTools::downsample_random (const vector_vec3d &data1, vector_vec3d &data2, unsigned size)
57 {
58  if (data1.size () <= size && size > 0) {
59  data2 = data1;
60  return;
61  }
62 
63  unsigned s = data1.size ();
64  data2.clear ();
65 
66  for (unsigned i = 0; i < size; i++) {
67  unsigned rnd = unsigned (s * (double (rand ()) / RAND_MAX));
68  data2.push_back (data1[rnd]);
69  }
70 }
71 
72 
73 void
75 {
76  if (data.size () <= size && size > 0)
77  return;
78 
79  unsigned s = data.size ();
80 
81  vector_vec3d data_tmp;
82 
83  for (unsigned i = 0; i < size; i++) {
84  unsigned rnd = unsigned ((s - 1) * (double (rand ()) / RAND_MAX));
85  data_tmp.push_back (data[rnd]);
86  }
87 
88  data = data_tmp;
89 }
90 
91 
92 unsigned
93 NurbsTools::getClosestPoint (const ON_2dPoint &p, const vector_vec2d &data)
94 {
95  if (data.empty ())
96  throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");
97 
98  unsigned idx (0);
99  double dist2 (DBL_MAX);
100  for (unsigned i = 0; i < data.size (); i++) {
101  ON_2dVector v = (data[i] - p);
102  double d2 = ON_DotProduct(v, v); // Was the squaredNorm in Eigen
103  if (d2 < dist2) {
104  idx = i;
105  dist2 = d2;
106  }
107  }
108  return idx;
109 }
110 
111 
112 unsigned
113 NurbsTools::getClosestPoint (const ON_3dPoint &p, const vector_vec3d &data)
114 {
115  if (data.empty ())
116  throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");
117 
118  unsigned idx (0);
119  double dist2 (DBL_MAX);
120  for (unsigned i = 0; i < data.size (); i++) {
121  ON_3dVector v = (data[i] - p);
122  double d2 = ON_DotProduct(v, v); // Was the squaredNorm in Eigen
123  if (d2 < dist2) {
124  idx = i;
125  dist2 = d2;
126  }
127  }
128  return idx;
129 }
130 
131 
132 unsigned
133 NurbsTools::getClosestPoint (const ON_2dPoint &p, const ON_2dVector &dir, const vector_vec2d &data,
134  unsigned &idxcp)
135 {
136  if (data.empty ())
137  throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");
138 
139  unsigned idx (0);
140  idxcp = 0;
141  double dist2 (0.0);
142  double dist2cp (DBL_MAX);
143  for (unsigned i = 0; i < data.size (); i++) {
144  ON_2dVector v = (data[i] - p);
145  double d2 = ON_DotProduct(v, v);
146 
147  if (d2 < dist2cp) {
148  idxcp = i;
149  dist2cp = d2;
150  }
151 
152  if (NEAR_ZERO(d2, SMALL_FASTF))
153  return i;
154 
155  v.Unitize();
156 
157  double d1 = ON_DotProduct(dir, v);
158  if (d1 / d2 > dist2) {
159  idx = i;
160  dist2 = d1 / d2;
161  }
162  }
163  return idx;
164 }
165 
166 
167 ON_3dVector
169 {
170  ON_3dVector u(0.0, 0.0, 0.0);
171 
172  unsigned s = data.size ();
173  double ds = 1.0 / s;
174 
175  for (unsigned i = 0; i < s; i++)
176  u += (data[i] * ds);
177 
178  return u;
179 }
180 
181 
182 ON_2dVector
184 {
185  ON_2dVector u (0.0, 0.0);
186 
187  unsigned s = data.size ();
188  double ds = 1.0 / s;
189 
190  for (unsigned i = 0; i < s; i++)
191  u += (data[i] * ds);
192 
193  return u;
194 }
195 
196 
197 void
198 NurbsTools::pca (const vector_vec3d &data, ON_3dVector &mean, Eigen::Matrix3d &eigenvectors,
199  Eigen::Vector3d &eigenvalues)
200 {
201  if (data.empty ()) {
202  printf ("[NurbsTools::pca] Error, data is empty\n");
203  abort ();
204  }
205 
206  mean = computeMean (data);
207 
208  unsigned s = data.size ();
209 
210  ON_Matrix Q(3, s);
211 
212  for (unsigned i = 0; i < s; i++) {
213  Q[0][i] = data[i].x - mean.x;
214  Q[1][i] = data[i].y - mean.y;
215  Q[2][i] = data[i].z - mean.z;
216  }
217 
218  ON_Matrix Qt = Q;
219  Qt.Transpose();
220 
221  ON_Matrix oC;
222  oC.Multiply(Q, Qt);
223 
224  Eigen::Matrix3d C(3, 3);
225  for (unsigned i = 0; i < 3; i++) {
226  for (unsigned j = 0; j < 3; j++) {
227  C(i, j) = oC[i][j];
228  }
229  }
230 
231  Eigen::SelfAdjointEigenSolver < Eigen::Matrix3d > eigensolver (C);
232  if (eigensolver.info () != Eigen::Success) {
233  printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
234  abort ();
235  }
236 
237  for (int i = 0; i < 3; ++i) {
238  eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
239  if (i == 2)
240  eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
241  else
242  eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
243  }
244 }
245 
246 
247 // ******************************
248 // * from nurbs_solve_eigen.cpp *
249 // ******************************
250 
251 void
252 NurbsSolve::assign (unsigned rows, unsigned cols, unsigned dims)
253 {
254  m_Ksparse.resize(rows, cols);
255  //std::cout << "m_Ksparse(rows=" << rows << ", cols=" << cols << "\n";
256  m_Ksparse.setZero();
257  m_xeig = new ON_Matrix(cols, dims);
258  m_xeig->Zero();
259  //std::cout << "m_xeig(rows=" << cols << ", cols=" << dims << "\n";
260  m_feig = new ON_Matrix(rows, dims);
261  m_feig->Zero();
262  //std::cout << "m_feig(rows=" << rows << ", cols=" << dims << "\n";
263 }
264 
265 
266 void
267 NurbsSolve::K (unsigned i, unsigned j, double v)
268 {
269  m_Ksparse.insert(i, j) = v;
270  //std::cout << "m_Ksparse count: " << m_Ksparse.nonZeros() << "\n";
271  //std::cout << "m_Ksparse item(" << i << ", " << j << "):" << m_Ksparse.coeffRef(i, j) << "\n";
272 }
273 void
274 NurbsSolve::x (unsigned i, unsigned j, double v)
275 {
276  (*m_xeig)[i][j] = v;
277 }
278 void
279 NurbsSolve::f (unsigned i, unsigned j, double v)
280 {
281  (*m_feig)[i][j] = v;
282 }
283 
284 
285 double
286 NurbsSolve::K (unsigned i, unsigned j)
287 {
288  return m_Ksparse.coeffRef(i, j);
289 }
290 double
291 NurbsSolve::x (unsigned i, unsigned j)
292 {
293  return (*m_xeig)[i][j];
294 }
295 double
296 NurbsSolve::f (unsigned i, unsigned j)
297 {
298  return (*m_feig)[i][j];
299 }
300 
301 
302 void
303 NurbsSolve::resizeF (unsigned rows)
304 {
305  ON_Matrix *new_feig = new ON_Matrix(rows, (*m_feig).ColCount());
306  for (int i = 0; i < (*m_feig).RowCount(); i++) {
307  for (int j = 0; j < (*m_feig).ColCount(); j++) {
308  (*new_feig)[i][j] = (*m_feig)[i][j];
309  }
310  }
311  delete m_feig;
312  m_feig = new_feig;
313 }
314 
315 
316 bool
317 solveSparseLinearSystemLQ (Eigen::SparseMatrix<double>* A, Eigen::MatrixXd* b, Eigen::MatrixXd* x)
318 {
319  Eigen::SparseMatrix<double> At(A->cols(), A->rows());
320  Eigen::MatrixXd Atb(A->cols(), b->cols());
321 
322  At = A->transpose();
323  Eigen::SparseMatrix<double> AtA = At * (*A);
324  Atb = At * (*b);
325 
326  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
327  solver.compute(AtA);
328  if (solver.info()!=Eigen::Success) {
329  // decomposition failed
330  std::cout << "decomposition failed\n";
331  }
332 
333  (*x) = solver.solve(Atb);
334 
335  if (solver.info()!=Eigen::Success) {
336  std::cout << "solver failed: " << solver.info() << "\n";
337  return false;
338  } else {
339  return true;
340  }
341 }
342 
343 
344 bool
346 {
347 
348  clock_t time_start, time_end;
349  time_start = clock ();
350 
351  Eigen::MatrixXd e_m_xeig = Eigen::MatrixXd::Zero ((*m_xeig).RowCount(), (*m_xeig).ColCount());
352  Eigen::MatrixXd e_m_feig = Eigen::MatrixXd::Zero ((*m_feig).RowCount(), (*m_feig).ColCount());
353 
354  for (int i = 0; i < (*m_xeig).RowCount(); i++) {
355  for (int j = 0; j < (*m_xeig).ColCount(); j++) {
356  e_m_xeig (i, j) = (*m_xeig)[i][j];
357  }
358  }
359  for (int i = 0; i < (*m_feig).RowCount(); i++) {
360  for (int j = 0; j < (*m_feig).ColCount(); j++) {
361  e_m_feig (i, j) = (*m_feig)[i][j];
362  }
363  }
364 
365  bool success = solveSparseLinearSystemLQ (&m_Ksparse, &e_m_feig, &e_m_xeig);
366  if (!success) {
367  std::cout << "solver failed!\n";
368  return false;
369  }
370 
371  for (int i = 0; i < (*m_xeig).RowCount(); i++) {
372  for (int j = 0; j < (*m_xeig).ColCount(); j++) {
373  (*m_xeig)[i][j] = e_m_xeig (i, j);
374  }
375  }
376 
377  time_end = clock ();
378 
379  if (!m_quiet) {
380  double solve_time = (double)(time_end - time_start) / (double)(CLOCKS_PER_SEC);
381  printf ("[NurbsSolve[Eigen::SimplicialLDLT]::solve_()] solution found! (%f sec)\n", solve_time);
382  }
383 
384  return true;
385 }
386 
387 
388 // ********************************
389 // * from fitting_surface_pdm.cpp *
390 // ********************************
391 
392 FittingSurface::FittingSurface (int order, NurbsDataSurface *in_m_data, ON_3dVector z)
393 {
394  if (order < 2)
395  throw std::runtime_error ("[FittingSurface::FittingSurface] Error order too low (order<2).");
396 
397  ON::Begin ();
398 
399  this->m_data = in_m_data;
400  m_nurbs = initNurbsPCA (order, m_data, z);
401 
402  this->init ();
403 }
404 
405 
406 FittingSurface::FittingSurface (NurbsDataSurface *in_m_data, const ON_NurbsSurface &ns)
407 {
408  ON::Begin ();
409 
410  this->m_nurbs = ON_NurbsSurface (ns);
411  this->m_data = in_m_data;
412 
413  this->init ();
414 }
415 
416 
417 void
419 {
420  std::vector<double> xi;
421  std::vector<double> elements = getElementVector (m_nurbs, dim);
422 
423  for (unsigned i = 0; i < elements.size () - 1; i++)
424  xi.push_back (elements[i] + 0.5 * (elements[i + 1] - elements[i]));
425 
426  for (unsigned i = 0; i < xi.size (); i++)
427  m_nurbs.InsertKnot (dim, xi[i], 1);
428 
431  m_minU = m_elementsU[0];
432  m_minV = m_elementsV[0];
433  m_maxU = m_elementsU[m_elementsU.size () - 1];
434  m_maxV = m_elementsV[m_elementsV.size () - 1];
435 }
436 
437 
438 void
440 {
441  clock_t time_start, time_end;
442  time_start = clock ();
443 
444  int nBnd = m_data->boundary.size ();
445  int nInt = m_data->interior.size ();
446  int nCurInt = param.regularisation_resU * param.regularisation_resV;
447  int nCurBnd = 2 * param.regularisation_resU + 2 * param.regularisation_resV;
448  int nCageReg = (m_nurbs.m_cv_count[0] - 2) * (m_nurbs.m_cv_count[1] - 2);
449  int nCageRegBnd = 2 * (m_nurbs.m_cv_count[0] - 1) + 2 * (m_nurbs.m_cv_count[1] - 1);
450 
451  if (param.boundary_weight <= 0.0)
452  nBnd = 0;
453  if (param.interior_weight <= 0.0)
454  nInt = 0;
455  if (param.boundary_regularisation <= 0.0)
456  nCurBnd = 0;
457  if (param.interior_regularisation <= 0.0)
458  nCurInt = 0;
459  if (param.interior_smoothness <= 0.0)
460  nCageReg = 0;
461  if (param.boundary_smoothness <= 0.0)
462  nCageRegBnd = 0;
463 
464  int ncp = m_nurbs.m_cv_count[0] * m_nurbs.m_cv_count[1];
465  int nrows = nBnd + nInt + nCurInt + nCurBnd + nCageReg + nCageRegBnd;
466 
467  m_solver.assign (nrows, ncp, 3);
468 
469  unsigned row = 0;
470 
471  // boundary points should lie on edges of surface
472  if (nBnd > 0)
473  assembleBoundary (param.boundary_weight, row);
474 
475  // interior points should lie on surface
476  if (nInt > 0)
477  assembleInterior (param.interior_weight, row);
478 
479  // minimal curvature on surface
480  if (nCurInt > 0) {
481  if (m_nurbs.Order (0) < 3 || m_nurbs.Order (1) < 3)
482  printf ("[FittingSurface::assemble] Error insufficient NURBS order to add curvature regularisation.\n");
483  else
485  param.interior_regularisation / param.regularisation_resU, row);
486  }
487 
488  // minimal curvature on boundary
489  if (nCurBnd > 0) {
490  if (m_nurbs.Order (0) < 3 || m_nurbs.Order (1) < 3)
491  printf ("[FittingSurface::assemble] Error insufficient NURBS order to add curvature regularisation.\n");
492  else
494  param.boundary_regularisation / param.regularisation_resU, row);
495  }
496 
497  // cage regularisation
498  if (nCageReg > 0)
500 
501  if (nCageRegBnd > 0) {
507  }
508 
509  time_end = clock ();
510  if (!m_quiet) {
511  double solve_time = (double)(time_end - time_start) / (double)(CLOCKS_PER_SEC);
512  printf ("[FittingSurface::assemble()] (assemble (%d, %d): %f sec)\n", nrows, ncp, solve_time);
513  }
514 }
515 
516 
517 void
519 {
522  m_minU = m_elementsU[0];
523  m_minV = m_elementsV[0];
524  m_maxU = m_elementsU[m_elementsU.size () - 1];
525  m_maxV = m_elementsV[m_elementsV.size () - 1];
526 
527  in_max_steps = 100;
528  in_accuracy = 1e-4;
529 
530  m_quiet = true;
531 }
532 
533 
534 void
536 {
537  if (m_solver.solve ())
538  updateSurf (damp);
539 }
540 
541 
542 void
544 {
545  int ncp = m_nurbs.m_cv_count[0] * m_nurbs.m_cv_count[1];
546 
547  for (int A = 0; A < ncp; A++) {
548 
549  int I = gl2gr (A);
550  int J = gl2gc (A);
551 
552  ON_3dPoint cp_prev;
553  m_nurbs.GetCV (I, J, cp_prev);
554 
555  ON_3dPoint cp;
556  cp.x = cp_prev.x + damp * (m_solver.x (A, 0) - cp_prev.x);
557  cp.y = cp_prev.y + damp * (m_solver.x (A, 1) - cp_prev.y);
558  cp.z = cp_prev.z + damp * (m_solver.x (A, 2) - cp_prev.z);
559 
560  m_nurbs.SetCV (I, J, cp);
561 
562  }
563 
564 }
565 
566 
567 void
568 FittingSurface::setInvMapParams (unsigned max_steps, double accuracy)
569 {
570  this->in_max_steps = max_steps;
571  this->in_accuracy = accuracy;
572 }
573 
574 
575 std::vector<double>
576 FittingSurface::getElementVector (const ON_NurbsSurface &nurbs, int dim) // !
577 {
578  std::vector<double> result;
579 
580  int idx_min = 0;
581  int idx_max = nurbs.KnotCount (dim) - 1;
582  if (nurbs.IsClosed (dim)) {
583  idx_min = nurbs.Order (dim) - 2;
584  idx_max = nurbs.KnotCount (dim) - nurbs.Order (dim) + 1;
585  }
586 
587  const double* knots = nurbs.Knot (dim);
588 
589  result.push_back (knots[idx_min]);
590 
591  //for (int E=(m_nurbs.Order(0)-2); E<(m_nurbs.KnotCount(0)-m_nurbs.Order(0)+2); E++) {
592  for (int E = idx_min + 1; E <= idx_max; E++) {
593 
594  if (!NEAR_EQUAL(knots[E], knots[E - 1], SMALL_FASTF)) // do not count double knots
595  result.push_back (knots[E]);
596 
597  }
598 
599  return result;
600 }
601 
602 
603 void
604 FittingSurface::assembleInterior (double wInt, unsigned &row)
605 {
606  m_data->interior_line_start.clear ();
607  m_data->interior_line_end.clear ();
608  m_data->interior_error.clear ();
609  m_data->interior_normals.clear ();
610  unsigned nInt = m_data->interior.size ();
611  for (unsigned p = 0; p < nInt; p++) {
612  ON_3dVector &pcp = m_data->interior[p];
613 
614  // inverse mapping
615  ON_2dVector params;
616  ON_3dPoint pt;
617  ON_3dVector tu, tv, n;
618  double error;
619  if (p < m_data->interior_param.size ()) {
620  params = inverseMapping (m_nurbs, pcp, m_data->interior_param[p], error, pt, tu, tv, in_max_steps, in_accuracy);
621  m_data->interior_param[p] = params;
622  } else {
623  params = findClosestElementMidPoint (m_nurbs, pcp);
624  params = inverseMapping (m_nurbs, pcp, params, error, pt, tu, tv, in_max_steps, in_accuracy);
625  m_data->interior_param.push_back (params);
626  }
627  m_data->interior_error.push_back (error);
628 
629  n = ON_CrossProduct(tu, tv);
630  n.Unitize();
631 
632  m_data->interior_normals.push_back (n);
633  m_data->interior_line_start.push_back (pcp);
634  m_data->interior_line_end.push_back (pt);
635 
636  double w (wInt);
637  if (p < m_data->interior_weight.size ())
638  w = m_data->interior_weight[p];
639 
641  }
642 }
643 
644 
645 void
646 FittingSurface::assembleBoundary (double wBnd, unsigned &row)
647 {
648  m_data->boundary_line_start.clear ();
649  m_data->boundary_line_end.clear ();
650  m_data->boundary_error.clear ();
651  m_data->boundary_normals.clear ();
652  unsigned nBnd = m_data->boundary.size ();
653  for (unsigned p = 0; p < nBnd; p++) {
654  ON_3dVector &pcp = m_data->boundary[p];
655 
656  double error;
657  ON_3dPoint pt;
658  ON_3dVector tu, tv, n;
659  ON_2dVector params = inverseMappingBoundary (m_nurbs, pcp, error, pt, tu, tv, in_max_steps, in_accuracy);
660  m_data->boundary_error.push_back (error);
661 
662  if (p < m_data->boundary_param.size ()) {
663  m_data->boundary_param[p] = params;
664  } else {
665  m_data->boundary_param.push_back (params);
666  }
667 
668  n = ON_CrossProduct(tu, tv);
669  n.Unitize();
670 
671  m_data->boundary_normals.push_back (n);
672  m_data->boundary_line_start.push_back (pcp);
673  m_data->boundary_line_end.push_back (pt);
674 
675  double w (wBnd);
676  if (p < m_data->boundary_weight.size ())
677  w = m_data->boundary_weight[p];
678 
680  }
681 }
682 
683 
684 ON_NurbsSurface
685 FittingSurface::initNurbs4Corners (int order, ON_3dPoint ll, ON_3dPoint lr, ON_3dPoint ur, ON_3dPoint ul)
686 {
687  std::map<int, std::map<int, ON_3dPoint> > cv_map;
688 
689  double dc = 1.0 / (order - 1);
690 
691  for (int i = 0; i < order; i++) {
692  double di = dc * i;
693  cv_map[i][0] = ll + (lr - ll) * di;
694  cv_map[0][i] = ll + (ul - ll) * di;
695  cv_map[i][order - 1] = ul + (ur - ul) * di;
696  cv_map[order - 1][i] = lr + (ur - lr) * di;
697  }
698 
699  for (int i = 1; i < order - 1; i++) {
700  for (int j = 1; j < order - 1; j++) {
701  ON_3dPoint du = cv_map[0][j] + (cv_map[order - 1][j] - cv_map[0][j]) * dc * i;
702  ON_3dPoint dv = cv_map[i][0] + (cv_map[i][order - 1] - cv_map[i][0]) * dc * j;
703  cv_map[i][j] = du * 0.5 + dv * 0.5;
704  }
705  }
706 
707  ON_NurbsSurface nurbs (3, false, order, order, order, order);
708  nurbs.MakeClampedUniformKnotVector (0, 1.0);
709  nurbs.MakeClampedUniformKnotVector (1, 1.0);
710 
711  for (int i = 0; i < order; i++) {
712  for (int j = 0; j < order; j++) {
713  nurbs.SetCV (i, j, cv_map[i][j]);
714  }
715  }
716  return nurbs;
717 }
718 
719 
720 ON_NurbsSurface
721 FittingSurface::initNurbsPCA (int order, NurbsDataSurface *m_data, ON_3dVector z)
722 {
723  ON_3dVector mean;
724  Eigen::Matrix3d eigenvectors;
725  Eigen::Vector3d eigenvalues;
726 
727  unsigned s = m_data->interior.size ();
728 
729  NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);
730 
731  m_data->mean = mean;
732  //m_data->eigenvectors = (*eigenvectors);
733 
734  bool flip (false);
735  Eigen::Vector3d ez(z[0], z[1], z[2]);
736  if (eigenvectors.col (2).dot (ez) < 0.0)
737  flip = true;
738 
739  eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
740 
741  ON_3dVector sigma(sqrt(eigenvalues[0]), sqrt(eigenvalues[1]), sqrt(eigenvalues[2]));
742 
743  ON_NurbsSurface nurbs (3, false, order, order, order, order);
744  nurbs.MakeClampedUniformKnotVector (0, 1.0);
745  nurbs.MakeClampedUniformKnotVector (1, 1.0);
746 
747  // +- 2 sigma -> 95, 45 % aller Messwerte
748  double dcu = (4.0 * sigma[0]) / (nurbs.Order (0) - 1);
749  double dcv = (4.0 * sigma[1]) / (nurbs.Order (1) - 1);
750 
751  ON_3dVector cv_t, cv;
752  Eigen::Vector3d ecv_t, ecv;
753  Eigen::Vector3d emean(mean[0], mean[1], mean[2]);
754  for (int i = 0; i < nurbs.Order (0); i++) {
755  for (int j = 0; j < nurbs.Order (1); j++) {
756  cv[0] = -2.0 * sigma[0] + dcu * i;
757  cv[1] = -2.0 * sigma[1] + dcv * j;
758  cv[2] = 0.0;
759  ecv (0) = -2.0 * sigma[0] + dcu * i;
760  ecv (1) = -2.0 * sigma[1] + dcv * j;
761  ecv (2) = 0.0;
762  ecv_t = eigenvectors * ecv + emean;
763  cv_t[0] = ecv_t (0);
764  cv_t[1] = ecv_t (1);
765  cv_t[2] = ecv_t (2);
766  if (flip)
767  nurbs.SetCV (nurbs.Order (0) - 1 - i, j, ON_3dPoint (cv_t[0], cv_t[1], cv_t[2]));
768  else
769  nurbs.SetCV (i, j, ON_3dPoint (cv_t[0], cv_t[1], cv_t[2]));
770  }
771  }
772  return nurbs;
773 }
774 
775 
776 ON_NurbsSurface
778 {
779  ON_3dVector mean;
780  Eigen::Matrix3d eigenvectors;
781  Eigen::Vector3d eigenvalues;
782 
783  unsigned s = m_data->interior.size ();
784  m_data->interior_param.clear ();
785 
786  NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);
787 
788  m_data->mean = mean;
789  //m_data->eigenvectors = (*eigenvectors);
790 
791  bool flip (false);
792  Eigen::Vector3d ez(z[0], z[1], z[2]);
793  if (eigenvectors.col (2).dot (ez) < 0.0)
794  flip = true;
795 
796  eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
797  Eigen::Matrix3d eigenvectors_inv = eigenvectors.inverse ();
798 
799  ON_3dVector v_max(0.0, 0.0, 0.0);
800  ON_3dVector v_min(DBL_MAX, DBL_MAX, DBL_MAX);
801  Eigen::Vector3d emean(mean[0], mean[1], mean[2]);
802  for (unsigned i = 0; i < s; i++) {
803  Eigen::Vector3d eint(m_data->interior[i][0], m_data->interior[i][1], m_data->interior[i][2]);
804  Eigen::Vector3d ep = eigenvectors_inv * (eint - emean);
805  ON_3dPoint p(ep (0), ep (1), ep(2));
806  m_data->interior_param.push_back (ON_2dPoint(p[0], p[1]));
807 
808  V_MAX(v_max[0], p[0]);
809  V_MAX(v_max[1], p[1]);
810  V_MAX(v_max[2], p[2]);
811 
812  V_MIN(v_min[0], p[0]);
813  V_MIN(v_min[1], p[1]);
814  V_MIN(v_min[2], p[2]);
815  }
816 
817  for (unsigned i = 0; i < s; i++) {
818  ON_2dVector &p = m_data->interior_param[i];
819  if (v_max[0] > v_min[0] && v_max[0] > v_min[0]) {
820  p[0] = (p[0] - v_min[0]) / (v_max[0] - v_min[0]);
821  p[1] = (p[1] - v_min[1]) / (v_max[1] - v_min[1]);
822  } else {
823  throw std::runtime_error ("[NurbsTools::initNurbsPCABoundingBox] Error: v_max <= v_min");
824  }
825  }
826 
827  ON_NurbsSurface nurbs (3, false, order, order, order, order);
828 
829  nurbs.MakeClampedUniformKnotVector (0, 1.0);
830  nurbs.MakeClampedUniformKnotVector (1, 1.0);
831 
832  double dcu = (v_max[0] - v_min[0]) / (nurbs.Order (0) - 1);
833  double dcv = (v_max[1] - v_min[1]) / (nurbs.Order (1) - 1);
834 
835  ON_3dPoint cv_t, cv;
836  Eigen::Vector3d ecv_t2, ecv2;
837  Eigen::Vector3d emean2(mean[0], mean[1], mean[2]);
838  for (int i = 0; i < nurbs.Order (0); i++) {
839  for (int j = 0; j < nurbs.Order (1); j++) {
840  cv[0] = v_min[0] + dcu * i;
841  cv[1] = v_min[1] + dcv * j;
842  cv[2] = 0.0;
843  ecv2 (0) = cv[0];
844  ecv2 (1) = cv[1];
845  ecv2 (2) = cv[2];
846  ecv_t2 = eigenvectors * ecv2 + emean2;
847  if (flip)
848  nurbs.SetCV (nurbs.Order (0) - 1 - i, j, cv_t);
849  else
850  nurbs.SetCV (i, j, cv_t);
851  }
852  }
853  return nurbs;
854 }
855 
856 
857 void
858 FittingSurface::addPointConstraint (const ON_2dVector &params, const ON_3dPoint &point, double weight,
859  unsigned &row)
860 {
861  double *N0 = new double[m_nurbs.Order (0) * m_nurbs.Order (0)];
862  double *N1 = new double[m_nurbs.Order (1) * m_nurbs.Order (1)];
863 
864  int E = ON_NurbsSpanIndex (m_nurbs.m_order[0], m_nurbs.m_cv_count[0], m_nurbs.m_knot[0], params[0], 0, 0);
865  int F = ON_NurbsSpanIndex (m_nurbs.m_order[1], m_nurbs.m_cv_count[1], m_nurbs.m_knot[1], params[1], 0, 0);
866 
867  ON_EvaluateNurbsBasis (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, params[0], N0);
868  ON_EvaluateNurbsBasis (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, params[1], N1);
869 
870  m_solver.f (row, 0, point[0] * weight);
871  m_solver.f (row, 1, point[1] * weight);
872  m_solver.f (row, 2, point[2] * weight);
873 
874  for (int i = 0; i < m_nurbs.Order (0); i++) {
875 
876  for (int j = 0; j < m_nurbs.Order (1); j++) {
877 
878  m_solver.K (row, lrc2gl (E, F, i, j), weight * N0[i] * N1[j]);
879 
880  } // j
881 
882  } // i
883 
884  row++;
885 
886  delete [] N0;
887  delete [] N1;
888 
889 }
890 
891 
892 void
893 FittingSurface::addCageInteriorRegularisation (double weight, unsigned &row)
894 {
895  for (int i = 1; i < (m_nurbs.m_cv_count[0] - 1); i++) {
896  for (int j = 1; j < (m_nurbs.m_cv_count[1] - 1); j++) {
897 
898  m_solver.f (row, 0, 0.0);
899  m_solver.f (row, 1, 0.0);
900  m_solver.f (row, 2, 0.0);
901 
902  m_solver.K (row, grc2gl (i + 0, j + 0), -4.0 * weight);
903  m_solver.K (row, grc2gl (i + 0, j - 1), 1.0 * weight);
904  m_solver.K (row, grc2gl (i + 0, j + 1), 1.0 * weight);
905  m_solver.K (row, grc2gl (i - 1, j + 0), 1.0 * weight);
906  m_solver.K (row, grc2gl (i + 1, j + 0), 1.0 * weight);
907 
908  row++;
909  }
910  }
911 }
912 
913 
914 void
915 FittingSurface::addCageBoundaryRegularisation (double weight, int side, unsigned &row)
916 {
917  int i = 0;
918  int j = 0;
919 
920  switch (side) {
921  case SOUTH:
922  j = m_nurbs.m_cv_count[1] - 1;
923  case NORTH:
924  for (i = 1; i < (m_nurbs.m_cv_count[0] - 1); i++) {
925 
926  m_solver.f (row, 0, 0.0);
927  m_solver.f (row, 1, 0.0);
928  m_solver.f (row, 2, 0.0);
929 
930  m_solver.K (row, grc2gl (i + 0, j), -2.0 * weight);
931  m_solver.K (row, grc2gl (i - 1, j), 1.0 * weight);
932  m_solver.K (row, grc2gl (i + 1, j), 1.0 * weight);
933 
934  row++;
935  }
936  break;
937 
938  case EAST:
939  i = m_nurbs.m_cv_count[0] - 1;
940  case WEST:
941  for (j = 1; j < (m_nurbs.m_cv_count[1] - 1); j++) {
942 
943  m_solver.f (row, 0, 0.0);
944  m_solver.f (row, 1, 0.0);
945  m_solver.f (row, 2, 0.0);
946 
947  m_solver.K (row, grc2gl (i, j + 0), -2.0 * weight);
948  m_solver.K (row, grc2gl (i, j - 1), 1.0 * weight);
949  m_solver.K (row, grc2gl (i, j + 1), 1.0 * weight);
950 
951  row++;
952  }
953  break;
954  }
955 }
956 
957 
958 void
959 FittingSurface::addCageCornerRegularisation (double weight, unsigned &row)
960 {
961  { // NORTH-WEST
962  int i = 0;
963  int j = 0;
964 
965  m_solver.f (row, 0, 0.0);
966  m_solver.f (row, 1, 0.0);
967  m_solver.f (row, 2, 0.0);
968 
969  m_solver.K (row, grc2gl (i + 0, j + 0), -2.0 * weight);
970  m_solver.K (row, grc2gl (i + 1, j + 0), 1.0 * weight);
971  m_solver.K (row, grc2gl (i + 0, j + 1), 1.0 * weight);
972 
973  row++;
974  }
975 
976  { // NORTH-EAST
977  int i = m_nurbs.m_cv_count[0] - 1;
978  int j = 0;
979 
980  m_solver.f (row, 0, 0.0);
981  m_solver.f (row, 1, 0.0);
982  m_solver.f (row, 2, 0.0);
983 
984  m_solver.K (row, grc2gl (i + 0, j + 0), -2.0 * weight);
985  m_solver.K (row, grc2gl (i - 1, j + 0), 1.0 * weight);
986  m_solver.K (row, grc2gl (i + 0, j + 1), 1.0 * weight);
987 
988  row++;
989  }
990 
991  { // SOUTH-EAST
992  int i = m_nurbs.m_cv_count[0] - 1;
993  int j = m_nurbs.m_cv_count[1] - 1;
994 
995  m_solver.f (row, 0, 0.0);
996  m_solver.f (row, 1, 0.0);
997  m_solver.f (row, 2, 0.0);
998 
999  m_solver.K (row, grc2gl (i + 0, j + 0), -2.0 * weight);
1000  m_solver.K (row, grc2gl (i - 1, j + 0), 1.0 * weight);
1001  m_solver.K (row, grc2gl (i + 0, j - 1), 1.0 * weight);
1002 
1003  row++;
1004  }
1005 
1006  { // SOUTH-WEST
1007  int i = 0;
1008  int j = m_nurbs.m_cv_count[1] - 1;
1009 
1010  m_solver.f (row, 0, 0.0);
1011  m_solver.f (row, 1, 0.0);
1012  m_solver.f (row, 2, 0.0);
1013 
1014  m_solver.K (row, grc2gl (i + 0, j + 0), -2.0 * weight);
1015  m_solver.K (row, grc2gl (i + 1, j + 0), 1.0 * weight);
1016  m_solver.K (row, grc2gl (i + 0, j - 1), 1.0 * weight);
1017 
1018  row++;
1019  }
1020 
1021 }
1022 
1023 
1024 void
1025 FittingSurface::addInteriorRegularisation (int order, int resU, int resV, double UNUSED(weight), unsigned &row)
1026 {
1027  double *N0 = new double[m_nurbs.Order (0) * m_nurbs.Order (0)];
1028  double *N1 = new double[m_nurbs.Order (1) * m_nurbs.Order (1)];
1029 
1030  double dU = (m_maxU - m_minU) / resU;
1031  double dV = (m_maxV - m_minV) / resV;
1032 
1033  for (int u = 0; u < resU; u++) {
1034  for (int v = 0; v < resV; v++) {
1035 
1036  ON_2dPoint params;
1037  params[0] = m_minU + u * dU + 0.5 * dU;
1038  params[1] = m_minV + v * dV + 0.5 * dV;
1039 
1040  // printf("%f %f, %f %f\n", m_minU, dU, params(0), params(1));
1041 
1042  int E = ON_NurbsSpanIndex (m_nurbs.m_order[0], m_nurbs.m_cv_count[0], m_nurbs.m_knot[0], params[0], 0, 0);
1043  int F = ON_NurbsSpanIndex (m_nurbs.m_order[1], m_nurbs.m_cv_count[1], m_nurbs.m_knot[1], params[1], 0, 0);
1044 
1045  ON_EvaluateNurbsBasis (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, params[0], N0);
1046  ON_EvaluateNurbsBasis (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, params[1], N1);
1047  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, order, N0); // derivative order?
1048  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, order, N1);
1049 
1050  m_solver.f (row, 0, 0.0);
1051  m_solver.f (row, 1, 0.0);
1052  m_solver.f (row, 2, 0.0);
1053 
1054  for (int i = 0; i < m_nurbs.Order (0); i++) {
1055 
1056  for (int j = 0; j < m_nurbs.Order (1); j++) {
1057 
1058  //m_solver.K (row, lrc2gl (E, F, i, j),
1059  // weight * (N0[order * m_nurbs.Order (0) + i] * N1[j] + N0[i] * N1[order * m_nurbs.Order (1) + j]));
1060 
1061  } // i
1062 
1063  } // j
1064 
1065  row++;
1066 
1067  }
1068  }
1069  delete [] N0;
1070  delete [] N1;
1071 }
1072 
1073 
1074 void
1075 FittingSurface::addBoundaryRegularisation (int order, int resU, int resV, double weight, unsigned &row)
1076 {
1077  double *N0 = new double[m_nurbs.Order (0) * m_nurbs.Order (0)];
1078  double *N1 = new double[m_nurbs.Order (1) * m_nurbs.Order (1)];
1079 
1080  double dU = (m_maxU - m_minU) / resU;
1081  double dV = (m_maxV - m_minV) / resV;
1082 
1083  for (int u = 0; u < resU; u++) {
1084 
1085  ON_2dPoint params;
1086  params[0] = m_minU + u * dU + 0.5 * dU;
1087  params[1] = m_minV;
1088 
1089  int E = ON_NurbsSpanIndex (m_nurbs.m_order[0], m_nurbs.m_cv_count[0], m_nurbs.m_knot[0], params[0], 0, 0);
1090  int F = ON_NurbsSpanIndex (m_nurbs.m_order[1], m_nurbs.m_cv_count[1], m_nurbs.m_knot[1], params[1], 0, 0);
1091 
1092  ON_EvaluateNurbsBasis (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, params[0], N0);
1093  ON_EvaluateNurbsBasis (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, params[1], N1);
1094  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, order, N0); // derivative order?
1095  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, order, N1);
1096 
1097  m_solver.f (row, 0, 0.0);
1098  m_solver.f (row, 1, 0.0);
1099  m_solver.f (row, 2, 0.0);
1100 
1101  for (int i = 0; i < m_nurbs.Order (0); i++) {
1102 
1103  for (int j = 0; j < m_nurbs.Order (1); j++) {
1104 
1105  m_solver.K (row, lrc2gl (E, F, i, j),
1106  weight * (N0[order * m_nurbs.Order (0) + i] * N1[j] + N0[i] * N1[order * m_nurbs.Order (1) + j]));
1107 
1108  } // i
1109 
1110  } // j
1111 
1112  row++;
1113 
1114  }
1115 
1116  for (int u = 0; u < resU; u++) {
1117 
1118  ON_2dPoint params;
1119  params[0] = m_minU + u * dU + 0.5 * dU;
1120  params[1] = m_maxV;
1121 
1122  int E = ON_NurbsSpanIndex (m_nurbs.m_order[0], m_nurbs.m_cv_count[0], m_nurbs.m_knot[0], params[0], 0, 0);
1123  int F = ON_NurbsSpanIndex (m_nurbs.m_order[1], m_nurbs.m_cv_count[1], m_nurbs.m_knot[1], params[1], 0, 0);
1124 
1125  ON_EvaluateNurbsBasis (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, params[0], N0);
1126  ON_EvaluateNurbsBasis (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, params[1], N1);
1127  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, order, N0); // derivative order?
1128  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, order, N1);
1129 
1130  m_solver.f (row, 0, 0.0);
1131  m_solver.f (row, 1, 0.0);
1132  m_solver.f (row, 2, 0.0);
1133 
1134  for (int i = 0; i < m_nurbs.Order (0); i++) {
1135 
1136  for (int j = 0; j < m_nurbs.Order (1); j++) {
1137 
1138  m_solver.K (row, lrc2gl (E, F, i, j),
1139  weight * (N0[order * m_nurbs.Order (0) + i] * N1[j] + N0[i] * N1[order * m_nurbs.Order (1) + j]));
1140 
1141  } // i
1142 
1143  } // j
1144 
1145  row++;
1146 
1147  }
1148 
1149  for (int v = 0; v < resV; v++) {
1150 
1151  ON_2dPoint params;
1152  params[0] = m_minU;
1153  params[1] = m_minV + v * dV + 0.5 * dV;
1154 
1155  int E = ON_NurbsSpanIndex (m_nurbs.m_order[0], m_nurbs.m_cv_count[0], m_nurbs.m_knot[0], params[0], 0, 0);
1156  int F = ON_NurbsSpanIndex (m_nurbs.m_order[1], m_nurbs.m_cv_count[1], m_nurbs.m_knot[1], params[1], 0, 0);
1157 
1158  ON_EvaluateNurbsBasis (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, params[0], N0);
1159  ON_EvaluateNurbsBasis (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, params[1], N1);
1160  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, order, N0); // derivative order?
1161  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, order, N1);
1162 
1163  m_solver.f (row, 0, 0.0);
1164  m_solver.f (row, 1, 0.0);
1165  m_solver.f (row, 2, 0.0);
1166 
1167  for (int i = 0; i < m_nurbs.Order (0); i++) {
1168 
1169  for (int j = 0; j < m_nurbs.Order (1); j++) {
1170 
1171  m_solver.K (row, lrc2gl (E, F, i, j),
1172  weight * (N0[order * m_nurbs.Order (0) + i] * N1[j] + N0[i] * N1[order * m_nurbs.Order (1) + j]));
1173 
1174  } // i
1175 
1176  } // j
1177 
1178  row++;
1179 
1180  }
1181 
1182  for (int v = 0; v < resV; v++) {
1183 
1184  ON_2dPoint params;
1185  params[0] = m_maxU;
1186  params[1] = m_minV + v * dV + 0.5 * dV;
1187 
1188  int E = ON_NurbsSpanIndex (m_nurbs.m_order[0], m_nurbs.m_cv_count[0], m_nurbs.m_knot[0], params[0], 0, 0);
1189  int F = ON_NurbsSpanIndex (m_nurbs.m_order[1], m_nurbs.m_cv_count[1], m_nurbs.m_knot[1], params[1], 0, 0);
1190 
1191  ON_EvaluateNurbsBasis (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, params[0], N0);
1192  ON_EvaluateNurbsBasis (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, params[1], N1);
1193  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, order, N0); // derivative order?
1194  ON_EvaluateNurbsBasisDerivatives (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, order, N1);
1195 
1196  m_solver.f (row, 0, 0.0);
1197  m_solver.f (row, 1, 0.0);
1198  m_solver.f (row, 2, 0.0);
1199 
1200  for (int i = 0; i < m_nurbs.Order (0); i++) {
1201 
1202  for (int j = 0; j < m_nurbs.Order (1); j++) {
1203 
1204  m_solver.K (row, lrc2gl (E, F, i, j),
1205  weight * (N0[order * m_nurbs.Order (0) + i] * N1[j] + N0[i] * N1[order * m_nurbs.Order (1) + j]));
1206 
1207  } // i
1208 
1209  } // j
1210 
1211  row++;
1212 
1213  }
1214  delete [] N0;
1215  delete [] N1;
1216 }
1217 
1218 
1219 ON_2dPoint
1220 FittingSurface::inverseMapping (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, const ON_2dPoint &hint, double &error,
1221  ON_3dPoint &p, ON_3dVector &tu, ON_3dVector &tv, int maxSteps, double accuracy, bool quiet)
1222 {
1223 
1224  double pointAndTangents[9];
1225 
1226  ON_2dVector current, delta;
1227  ON_Matrix A(2, 2);
1228  ON_2dVector b;
1229  ON_3dVector r;
1230  std::vector<double> elementsU = getElementVector (nurbs, 0);
1231  std::vector<double> elementsV = getElementVector (nurbs, 1);
1232  double minU = elementsU[0];
1233  double minV = elementsV[0];
1234  double maxU = elementsU[elementsU.size () - 1];
1235  double maxV = elementsV[elementsV.size () - 1];
1236 
1237  current = hint;
1238 
1239  for (int k = 0; k < maxSteps; k++) {
1240 
1241  nurbs.Evaluate (current[0], current[1], 1, 3, pointAndTangents);
1242  p[0] = pointAndTangents[0];
1243  p[1] = pointAndTangents[1];
1244  p[2] = pointAndTangents[2];
1245  tu[0] = pointAndTangents[3];
1246  tu[1] = pointAndTangents[4];
1247  tu[2] = pointAndTangents[5];
1248  tv[0] = pointAndTangents[6];
1249  tv[1] = pointAndTangents[7];
1250  tv[2] = pointAndTangents[8];
1251 
1252  r = p - pt;
1253 
1254  b[0] = -ON_DotProduct(r, tu);
1255  b[1] = -ON_DotProduct(r, tv);
1256 
1257  A[0][0] = ON_DotProduct(tu, tu);
1258  A[0][1] = ON_DotProduct(tu, tv);
1259  A[1][0] = A[0][1];
1260  A[1][1] = ON_DotProduct(tv, tv);
1261 
1262  Eigen::Vector2d eb(b[0], b[1]);
1263  Eigen::Vector2d edelta;
1264  Eigen::Matrix2d eA;
1265 
1266  eA (0, 0) = A[0][0];
1267  eA (0, 1) = A[0][1];
1268  eA (1, 0) = A[1][0];
1269  eA (1, 1) = A[1][1];
1270 
1271  edelta = eA.ldlt ().solve (eb);
1272 
1273  delta[0] = edelta (0);
1274  delta[1] = edelta (1);
1275 
1276  if (sqrt(ON_DotProduct(delta, delta)) < accuracy) {
1277 
1278  error = sqrt(ON_DotProduct(r, r));
1279  return current;
1280 
1281  } else {
1282  current = current + delta;
1283 
1284  CLAMP(current[0], minU, maxU);
1285  CLAMP(current[1], minV, maxV);
1286  }
1287 
1288  }
1289 
1290  error = sqrt(ON_DotProduct(r, r));
1291  if (!quiet) {
1292  printf ("[FittingSurface::inverseMapping] Warning: Method did not converge (%e %d)\n", accuracy, maxSteps);
1293  printf (" %f %f ... %f %f\n", hint[0], hint[1], current[0], current[1]);
1294  }
1295 
1296  return current;
1297 
1298 }
1299 
1300 
1301 ON_2dPoint
1302 FittingSurface::findClosestElementMidPoint (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt)
1303 {
1304  ON_2dPoint hint;
1305  ON_3dVector r;
1306  std::vector<double> elementsU = getElementVector (nurbs, 0);
1307  std::vector<double> elementsV = getElementVector (nurbs, 1);
1308 
1309  double d_shortest (DBL_MAX);
1310  for (unsigned i = 0; i < elementsU.size () - 1; i++) {
1311  for (unsigned j = 0; j < elementsV.size () - 1; j++) {
1312  double points[3];
1313  double d;
1314 
1315  double xi = elementsU[i] + 0.5 * (elementsU[i + 1] - elementsU[i]);
1316  double eta = elementsV[j] + 0.5 * (elementsV[j + 1] - elementsV[j]);
1317 
1318  nurbs.Evaluate (xi, eta, 0, 3, points);
1319  r[0] = points[0] - pt[0];
1320  r[1] = points[1] - pt[1];
1321  r[2] = points[2] - pt[2];
1322 
1323  d = ON_DotProduct(r, r);
1324 
1325  if ((i == 0 && j == 0) || d < d_shortest) {
1326  d_shortest = d;
1327  hint[0] = xi;
1328  hint[1] = eta;
1329  }
1330  }
1331  }
1332 
1333  return hint;
1334 }
1335 
1336 
1337 ON_2dPoint
1338 FittingSurface::inverseMappingBoundary (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, double &error, ON_3dPoint &p,
1339  ON_3dVector &tu, ON_3dVector &tv, int maxSteps, double accuracy, bool quiet)
1340 {
1341 
1342  ON_2dPoint result;
1343  double min_err = 100.0;
1344  std::vector<myvec> ini_points;
1345  double err_tmp;
1346  ON_3dPoint p_tmp;
1347  ON_3dVector tu_tmp, tv_tmp;
1348 
1349  std::vector<double> elementsU = getElementVector (nurbs, 0);
1350  std::vector<double> elementsV = getElementVector (nurbs, 1);
1351 
1352  // NORTH - SOUTH
1353  for (unsigned i = 0; i < (elementsV.size () - 1); i++) {
1354  ini_points.push_back (myvec (WEST, elementsV[i] + 0.5 * (elementsV[i + 1] - elementsV[i])));
1355  ini_points.push_back (myvec (EAST, elementsV[i] + 0.5 * (elementsV[i + 1] - elementsV[i])));
1356  }
1357 
1358  // WEST - EAST
1359  for (unsigned i = 0; i < (elementsU.size () - 1); i++) {
1360  ini_points.push_back (myvec (NORTH, elementsU[i] + 0.5 * (elementsU[i + 1] - elementsU[i])));
1361  ini_points.push_back (myvec (SOUTH, elementsU[i] + 0.5 * (elementsU[i + 1] - elementsU[i])));
1362  }
1363 
1364  for (unsigned i = 0; i < ini_points.size (); i++) {
1365 
1366  ON_2dPoint params = inverseMappingBoundary (nurbs, pt, ini_points[i].side, ini_points[i].hint, err_tmp, p_tmp,
1367  tu_tmp, tv_tmp, maxSteps, accuracy, quiet);
1368 
1369  if (i == 0 || err_tmp < min_err) {
1370  min_err = err_tmp;
1371  result = params;
1372  p = p_tmp;
1373  tu = tu_tmp;
1374  tv = tv_tmp;
1375  }
1376  }
1377 
1378  error = min_err;
1379  return result;
1380 
1381 }
1382 
1383 
1384 ON_2dPoint
1385 FittingSurface::inverseMappingBoundary (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, int side, double hint,
1386  double &error, ON_3dPoint &p, ON_3dVector &tu, ON_3dVector &tv, int maxSteps,
1387  double accuracy, bool quiet)
1388 {
1389 
1390  double pointAndTangents[9];
1391  double current, delta;
1392  ON_3dVector r, t;
1393  ON_2dPoint params;
1394 
1395  current = hint;
1396 
1397  std::vector<double> elementsU = getElementVector (nurbs, 0);
1398  std::vector<double> elementsV = getElementVector (nurbs, 1);
1399  double minU = elementsU[0];
1400  double minV = elementsV[0];
1401  double maxU = elementsU[elementsU.size () - 1];
1402  double maxV = elementsV[elementsV.size () - 1];
1403 
1404  for (int k = 0; k < maxSteps; k++) {
1405 
1406  switch (side) {
1407 
1408  case WEST:
1409 
1410  params[0] = minU;
1411  params[1] = current;
1412  nurbs.Evaluate (minU, current, 1, 3, pointAndTangents);
1413  p[0] = pointAndTangents[0];
1414  p[1] = pointAndTangents[1];
1415  p[2] = pointAndTangents[2];
1416  tu[0] = pointAndTangents[3];
1417  tu[1] = pointAndTangents[4];
1418  tu[2] = pointAndTangents[5];
1419  tv[0] = pointAndTangents[6];
1420  tv[1] = pointAndTangents[7];
1421  tv[2] = pointAndTangents[8];
1422 
1423  t = tv; // use tv
1424 
1425  break;
1426  case SOUTH:
1427 
1428  params[0] = current;
1429  params[1] = maxV;
1430  nurbs.Evaluate (current, maxV, 1, 3, pointAndTangents);
1431  p[0] = pointAndTangents[0];
1432  p[1] = pointAndTangents[1];
1433  p[2] = pointAndTangents[2];
1434  tu[0] = pointAndTangents[3];
1435  tu[1] = pointAndTangents[4];
1436  tu[2] = pointAndTangents[5];
1437  tv[0] = pointAndTangents[6];
1438  tv[1] = pointAndTangents[7];
1439  tv[2] = pointAndTangents[8];
1440 
1441  t = tu; // use tu
1442 
1443  break;
1444  case EAST:
1445 
1446  params[0] = maxU;
1447  params[1] = current;
1448  nurbs.Evaluate (maxU, current, 1, 3, pointAndTangents);
1449  p[0] = pointAndTangents[0];
1450  p[1] = pointAndTangents[1];
1451  p[2] = pointAndTangents[2];
1452  tu[0] = pointAndTangents[3];
1453  tu[1] = pointAndTangents[4];
1454  tu[2] = pointAndTangents[5];
1455  tv[0] = pointAndTangents[6];
1456  tv[1] = pointAndTangents[7];
1457  tv[2] = pointAndTangents[8];
1458 
1459  t = tv; // use tv
1460 
1461  break;
1462  case NORTH:
1463 
1464  params[0] = current;
1465  params[1] = minV;
1466  nurbs.Evaluate (current, minV, 1, 3, pointAndTangents);
1467  p[0] = pointAndTangents[0];
1468  p[1] = pointAndTangents[1];
1469  p[2] = pointAndTangents[2];
1470  tu[0] = pointAndTangents[3];
1471  tu[1] = pointAndTangents[4];
1472  tu[2] = pointAndTangents[5];
1473  tv[0] = pointAndTangents[6];
1474  tv[1] = pointAndTangents[7];
1475  tv[2] = pointAndTangents[8];
1476 
1477  t = tu; // use tu
1478 
1479  break;
1480  default:
1481  throw std::runtime_error ("[FittingSurface::inverseMappingBoundary] ERROR: Specify a boundary!");
1482 
1483  }
1484 
1485  r[0] = pointAndTangents[0] - pt[0];
1486  r[1] = pointAndTangents[1] - pt[1];
1487  r[2] = pointAndTangents[2] - pt[2];
1488 
1489  delta = -0.5 * ON_DotProduct(r, t) / ON_DotProduct(t, t);
1490 
1491  if (fabs (delta) < accuracy) {
1492 
1493  error = sqrt(ON_DotProduct(r, r));
1494  return params;
1495 
1496  } else {
1497 
1498  current = current + delta;
1499 
1500  bool stop = false;
1501 
1502  switch (side) {
1503 
1504  case WEST:
1505  case EAST:
1506  if (current < minV) {
1507  params[1] = minV;
1508  stop = true;
1509  } else if (current > maxV) {
1510  params[1] = maxV;
1511  stop = true;
1512  }
1513 
1514  break;
1515 
1516  case NORTH:
1517  case SOUTH:
1518  if (current < minU) {
1519  params[0] = minU;
1520  stop = true;
1521  } else if (current > maxU) {
1522  params[0] = maxU;
1523  stop = true;
1524  }
1525 
1526  break;
1527  }
1528 
1529  if (stop) {
1530  error = sqrt(ON_DotProduct(r, r));
1531  return params;
1532  }
1533 
1534  }
1535 
1536  }
1537 
1538  error = sqrt(ON_DotProduct(r, r));
1539  if (!quiet)
1540  printf (
1541  "[FittingSurface::inverseMappingBoundary] Warning: Method did not converge! (residual: %f, delta: %f, params: %f %f)\n",
1542  error, delta, params[0], params[1]);
1543 
1544  return params;
1545 }
1546 
1547 
1548 /*
1549  * Local Variables:
1550  * tab-width: 8
1551  * mode: C++
1552  * indent-tabs-mode: t
1553  * c-file-style: "stroustrup"
1554  * coding: utf-8
1555  * End:
1556  * ex: shiftwidth=4 tabstop=8
1557  */
virtual void addCageBoundaryRegularisation(double weight, int side, unsigned &row)
Add minimization constraint: boundary smoothness by control point regularisation. ...
std::vector< double > boundary_error
std::vector< double > boundary_weight
< input
fastf_t C[2 *MAX_CNT+1][2 *MAX_CNT+1]
Definition: dsp_brep.cpp:38
void assign(unsigned rows, unsigned cols, unsigned dims)
Assign size and dimension (2D, 3D) of system of equations.
std::vector< double > interior_weight
< input
static ON_2dPoint findClosestElementMidPoint(const ON_NurbsSurface &nurbs, const ON_3dPoint &pt)
Given a point pt, the function finds the closest midpoint of the elements of the surface.
static ON_NurbsSurface initNurbsPCABoundingBox(int order, NurbsDataSurface *data, ON_3dVector z=ON_3dVector(0.0, 0.0, 1.0))
Initializing a B-Spline surface using principal-component-analysis and bounding box of points...
bool solve()
Solves the system of equations with respect to x.
if lu s
Definition: nmg_mod.c:3860
#define SMALL_FASTF
Definition: defines.h:342
Header file for the BRL-CAD common definitions.
void init()
Initialisation of member variables.
virtual void assembleBoundary(double wBnd, unsigned &row)
Assemble point-to-surface constraints for boundary points.
void K(unsigned i, unsigned j, double v)
Set value for system matrix K (stiffness matrix, basis functions)
static unsigned getClosestPoint(const ON_2dPoint &point, const vector_vec2d &data)
Get the closest point with respect to 'point'.
vector_vec3d boundary_line_start
output
std::vector< double > interior_error
void refine(int dim)
Refines surface by inserting a knot in the middle of each element.
virtual void assemble(Parameter param=Parameter())
Assemble the system of equations for fitting.
ON_NurbsSurface m_nurbs
static std::vector< double > getElementVector(const ON_NurbsSurface &nurbs, int dim)
Get the elements of a B-Spline surface.
virtual void addCageCornerRegularisation(double weight, unsigned &row)
Add minimization constraint: corner smoothness by control point regularisation.
COMPLEX data[64]
Definition: fftest.c:34
vector_vec3d interior
< input
std::vector< double > m_elementsU
Parameters for fitting.
#define NEAR_ZERO(val, epsilon)
Definition: color.c:55
vector_vec2d interior_param
output
Data structure for NURBS surface fitting (FittingSurface, FittingSurfaceTDM, FittingCylinder, GlobalOptimization, GlobalOptimizationTDM)
Definition: opennurbs_fit.h:96
std::vector< ON_3dVector > vector_vec3d
Definition: opennurbs_fit.h:92
vector_vec3d interior_line_start
output
virtual void updateSurf(double damp)
Update surface according to the current system of equations.
static ON_2dPoint inverseMappingBoundary(const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, double &error, ON_3dPoint &p, ON_3dVector &tu, ON_3dVector &tv, int maxSteps=100, double accuracy=1e-6, bool quiet=true)
Inverse mapping / point inversion: Given a point pt, this function finds the closest point on the bou...
Coord * point
Definition: chull3d.cpp:52
#define UNUSED(parameter)
Definition: common.h:239
void f(unsigned i, unsigned j, double v)
Set value for target vector f (force vector)
virtual void addInteriorRegularisation(int order, int resU, int resV, double weight, unsigned &row)
Add minimization constraint: interior smoothness by derivatives regularisation.
int lrc2gl(int E, int F, int i, int j)
std::vector< ON_2dVector > vector_vec2d
Definition: opennurbs_fit.h:91
FittingSurface(NurbsDataSurface *data, const ON_NurbsSurface &ns)
Constructor initializing with the B-Spline surface given in argument 2.
vector_vec3d boundary_normals
output
virtual void addBoundaryRegularisation(int order, int resU, int resV, double weight, unsigned &row)
Add minimization constraint: boundary smoothness by derivatives regularisation.
vector_vec3d boundary
output
virtual void addPointConstraint(const ON_2dVector &params, const ON_3dPoint &point, double weight, unsigned &row)
Add minimization constraint: point-to-surface distance (point-distance-minimization).
vector_vec3d boundary_line_end
output
virtual void assembleInterior(double wInt, unsigned &row)
Assemble point-to-surface constraints for interior points.
static ON_NurbsSurface initNurbs4Corners(int order, ON_3dPoint ll, ON_3dPoint lr, ON_3dPoint ur, ON_3dPoint ul)
Initializing a B-Spline surface using 4 corners.
void x(unsigned i, unsigned j, double v)
Set value for state vector x (control points)
static ON_2dPoint inverseMapping(const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, const ON_2dPoint &hint, double &error, ON_3dPoint &p, ON_3dVector &tu, ON_3dVector &tv, int maxSteps=100, double accuracy=1e-6, bool quiet=true)
Inverse mapping / point inversion: Given a point pt, this function finds the closest point on the B-S...
void setInvMapParams(unsigned in_max_steps, double in_accuracy)
Set parameters for inverse mapping.
std::vector< double > m_elementsV
static ON_3dVector computeMean(const vector_vec3d &data)
Compute the mean of a set of points.
#define A
Definition: msr.c:51
vector_vec3d interior_line_end
output
static ON_NurbsSurface initNurbsPCA(int order, NurbsDataSurface *data, ON_3dVector z=ON_3dVector(0.0, 0.0, 1.0))
Initializing a B-Spline surface using principal-component-analysis and eigen values.
vector_vec2d boundary_param
output
#define Q
Definition: msr.c:54
NurbsDataSurface * m_data
void resizeF(unsigned rows)
Resize target vector f (force vector)
static void pca(const vector_vec3d &data, ON_3dVector &mean, Eigen::Matrix3d &eigenvectors, Eigen::Vector3d &eigenvalues)
PCA - principal-component-analysis.
int grc2gl(int I, int J)
HIDDEN const point_t delta
Definition: sh_prj.c:618
bool solveSparseLinearSystemLQ(Eigen::SparseMatrix< double > *A, Eigen::MatrixXd *b, Eigen::MatrixXd *x)
vector_vec3d interior_normals
output
static void downsample_random(const vector_vec3d &data1, vector_vec3d &data2, unsigned size)
Downsample data points to a certain size.
virtual void addCageInteriorRegularisation(double weight, unsigned &row)
Add minimization constraint: interior smoothness by control point regularisation. ...
virtual void solve(double damp=1.0)
Solve system of equations using Eigen or UmfPack (can be defined in on_nurbs.cmake), and updates B-Spline surface if a solution can be obtained.