• R/O
  • SSH
  • HTTPS

ongeo: Commit


Commit MetaInfo

Revisão60 (tree)
Hora2020-05-13 01:30:00
Autormocchi_2012

Mensagem de Log

・点列からNurbs曲線求める関数(トレランスで精度を制御するタイプ)の追加

Mudança Sumário

Diff

--- trunk/FitNurbsToPoints.cpp (revision 59)
+++ trunk/FitNurbsToPoints.cpp (revision 60)
@@ -61,7 +61,11 @@
6161 prm[i] /= sq_chord_length;
6262 }
6363 }
64+ break;
6465 }
66+ case ONGEO_NI_Manual: {
67+ break;
68+ }
6569 }
6670 return true;
6771 }
@@ -68,7 +72,10 @@
6872
6973 struct IEquationSolver{
7074 virtual void Initialize(const ON_Matrix &N) = 0;
71- virtual void Solve(int pt_cnt, const ON_3dPoint *pts, ON_3dPoint *Q, int stride) const = 0;
75+ void Solve(int pt_cnt, const ON_3dPoint *pts, ON_3dPoint *Q, int stride) const {
76+ Solve(pt_cnt, pts, 0, 0, Q, stride);
77+ }
78+ virtual void Solve(int pt_cnt, const ON_3dPoint *pts, int *constraint_indices, int constraint_cnt, ON_3dPoint *Q, int stride) const = 0;
7279 };
7380
7481 // 通常は LU 分解を使うところであるが、 まずは有り物(OpenNurbs の逆行列を求める関数)で実現。
@@ -79,12 +86,13 @@
7986 Ninv = N;
8087 Ninv.Invert(ON_ZERO_TOLERANCE);
8188 }
82- virtual void Solve(int pt_cnt, const ON_3dPoint *pts, ON_3dPoint *Q, int stride) const{
89+ virtual void Solve(int pt_cnt, const ON_3dPoint *pts, int *constraint_indices, int constraint_cnt, ON_3dPoint *Q, int stride) const{
8390 int n = pt_cnt - 1;
8491 ON_Matrix P(n+1, 1), Qm(n+1, 1);
8592 for (int dim = 0; dim < 3; ++dim){
8693 for (int i = 0; i <= n; ++i){
87- P[i][0] = pts[i][dim];
94+ int ii = constraint_indices ? constraint_indices[i] : i;
95+ P[i][0] = pts[ii][dim];
8896 }
8997 Qm.Multiply(Ninv, P);
9098 for (int i = 0; i <= n; ++i){
@@ -94,18 +102,20 @@
94102 }
95103 };
96104
97-void InitializeSolver_ExactlyPassThroughPoints(int pt_cnt, int order, double *prm, double *knot, int knot_count, IEquationSolver *solver){
98- int n = pt_cnt - 1;
105+void InitializeSolver_ExactlyPassThroughPoints(int pt_cnt, int order, double *prm, int *constraint_indices, int constraint_cnt, double *knot, int knot_count, IEquationSolver *solver){
106+ int n = (constraint_indices) ? constraint_cnt - 1 : pt_cnt - 1;
99107 int m = knot_count;
100108 int p = order - 1;
109+ int prm_first_idx = 0, prm_last_idx = pt_cnt - 1;
110+ if (constraint_indices) prm_first_idx = constraint_indices[0], prm_last_idx = constraint_indices[n];
101111 for (int i = 0; i < p; ++i){
102- knot[i] = 0;
103- knot[m-i-1] = 1;
112+ knot[i] = prm[prm_first_idx];
113+ knot[m-i-1] = prm[prm_last_idx];
104114 }
105115 for (int j = 1; j <= n - p; ++j){
106116 double s = 0;
107117 for (int i = j; i < j + p; ++i){
108- s += prm[i];
118+ s += prm[constraint_indices ? constraint_indices[i] : i];
109119 }
110120 knot[j+p-1] = s / static_cast<double>(p);
111121 }
@@ -120,7 +130,7 @@
120130
121131 ON_SimpleArray<double> basis(order * order); basis.SetCount(basis.Capacity());
122132 for (int j = 1; j < n; ++j){
123- double t = prm[j];
133+ double t = prm[constraint_indices ? constraint_indices[j] : j];
124134 int span_index = ON_NurbsSpanIndex(order, n+1, knot, t, 0, 0);
125135 for (int i = 0; i < span_index; ++i){
126136 N[j][i] = 0;
@@ -150,9 +160,10 @@
150160
151161 double d = static_cast<double>(m + 1) / static_cast<double>(n - p + 1);
152162 for (double j = 1.0; j <= n - p; j += 1.0){
153- double i = std::floor(j * d);
163+ double jd = j * d;
164+ double i = std::floor(jd);
154165 int ii = static_cast<int>(i);
155- double a = j * d - i;
166+ double a = jd - i;
156167 knot[static_cast<int>(j)+p-1] = (1.0 - a) * prm[ii-1] + a * prm[ii];
157168 }
158169
@@ -194,8 +205,52 @@
194205 solver->Initialize(NN);
195206 }
196207
208+bool FitNurbsCurveToPointArray(const ON_3dPoint *pts, int pt_cnt, int order, int *constraint_indices, int constraint_cnt, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, double *prm_, ONGEO_NI_Solver solver_id){
209+ if (pt_cnt < order || !pts) return false;
210+ int n = (constraint_indices) ? constraint_cnt - 1 : pt_cnt - 1;
211+
212+ Solver_ON_Matrix_Inverse solver_omi;
213+ IEquationSolver *solver = 0;
214+ if (solver_id == ONGEO_NI_Solver_ON_Matrix_Inverse){
215+ solver = &solver_omi;
216+ }
217+
218+ nc.Create(3, false, order, n+1);
219+
220+ ON_SimpleArray<double> prm;
221+ if (prm_ == 0){
222+ if (pmethod == ONGEO_NI_Manual) return false;
223+ prm.SetCapacity(pt_cnt); prm.SetCount(pt_cnt);
224+ prm_ = prm;
225+ }
226+ CreateParameter(pts, pt_cnt, 1, pmethod, prm_);
227+
228+ InitializeSolver_ExactlyPassThroughPoints(pt_cnt, order, prm_, constraint_indices, constraint_cnt, nc.m_knot, nc.KnotCount(), solver);
229+
230+ ON_ClassArray<ON_3dPoint> Q(n+1); Q.SetCount(n+1);
231+ if (order == 2){
232+ if (constraint_indices){
233+ for (int i = 0; i < constraint_cnt; ++i) Q[i] = pts[constraint_indices[i]];
234+ }else{
235+ for (int i = 0; i < pt_cnt; ++i) Q[i] = pts[i];
236+ }
237+ }else{
238+ solver->Solve(n+1, pts, constraint_indices, constraint_cnt, Q.First(), 1);
239+ }
240+ for (int i = 0; i <= n; ++i){
241+ nc.SetCV(i, Q[i]);
242+ }
243+
244+ return true;
197245 }
198246
247+}
248+
249+bool ONGEO_FitNurbsCurveToPointArray(const ON_3dPoint *pts, int pt_cnt, int order, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, double *prm_, ONGEO_NI_Solver solver_id){
250+ return FitNurbsCurveToPointArray(pts, pt_cnt, order, 0, 0, pmethod, nc, prm_, solver_id);
251+}
252+
253+#if 0
199254 bool ONGEO_FitNurbsCurveToPointArray(const ON_3dPoint *pts, int pt_cnt, int order, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, ONGEO_NI_Solver solver_id){
200255 if (pt_cnt < order || !pts) return false;
201256 int n = pt_cnt - 1;
@@ -208,10 +263,14 @@
208263
209264 nc.Create(3, false, order, n+1);
210265
211- ON_SimpleArray<double> prm(n+1); prm.SetCount(n+1);
212- CreateParameter(pts, pt_cnt, 1, pmethod, prm);
266+ ON_SimpleArray<double> prm;
267+ if (prm_ == 0){
268+ prm.SetCapacity(n+1); prm.SetCount(n+1);
269+ prm_ = prm;
270+ }
271+ CreateParameter(pts, pt_cnt, 1, pmethod, prm_);
213272
214- InitializeSolver_ExactlyPassThroughPoints(pt_cnt, order, prm.First(), nc.m_knot, nc.KnotCount(), solver);
273+ InitializeSolver_ExactlyPassThroughPoints(pt_cnt, order, prm_, nc.m_knot, nc.KnotCount(), solver);
215274
216275 ON_ClassArray<ON_3dPoint> Q(n+1); Q.SetCount(n+1);
217276 if (order == 2){
@@ -225,6 +284,7 @@
225284
226285 return true;
227286 }
287+#endif
228288
229289 double ONGEO_FitNurbsCurveToPointArray_LeastSquare(const ON_3dPoint *pts, int pt_cnt, int order, int cpt_cnt, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, double *prm_, double *err_, ONGEO_NI_Solver solver_id){
230290 Solver_ON_Matrix_Inverse solver_omi;
@@ -271,172 +331,251 @@
271331 return err_max;
272332 }
273333
274-#if 0
275-double CalculateBr(const ON_NurbsCurve &nc, int r, int s){
276- int ord = nc.Order();
277- int p = ord - 1;
278- int last = r - s;
279- int first = r - p;
280- int off = first - 1;
281- const double *knot = nc.Knot();
334+// Reference : Razdan, A.
335+// Knot Placement for B-Spline Curve Approximation
336+// Arizona State University, 1999.
337+namespace{
338+// pt_indices_marked は呼び出し元で pt_cnt 分の領域を確保すること。戻り値の数だけ書き込まれる。
339+int EstimateNumberOfPoints(const ON_3dPoint *pts, int pt_cnt, double sigma, int *pt_indices_marked){
340+ if (pt_cnt < 2) return 0;
341+ double arc_length = 0;
342+ int n = 0;
343+ if (pt_indices_marked) pt_indices_marked[n] = 0;
344+ n++;
345+ ON_3dPoint pt_marked = pts[0], pt_prev = pts[0];
346+ for (int i = 1; i < pt_cnt - 1; ++i){
347+ ON_3dPoint pt_cur = pts[i];
348+ double chord_length = pt_cur.DistanceTo(pt_marked);
349+ arc_length += pt_cur.DistanceTo(pt_prev);
350+ pt_prev = pt_cur;
351+ double ratio = arc_length / chord_length;
352+ if (sigma < ratio){
353+ if (pt_indices_marked) pt_indices_marked[n] =i;
354+ arc_length = 0;
355+ pt_marked = pt_cur;
356+ n++;
357+ }
358+ }
359+ if (pt_indices_marked) pt_indices_marked[n] = pt_cnt - 1;
360+ n++;
361+ return n;
362+}
282363
283- ON_ClassArray<ON_3dPoint> temp(last + 2 - off);
284- temp.SetCount(temp.Capacity());
285- nc.GetCV(off, temp[0]);
286- nc.GetCV(last+1, temp[last + 1 - off]);
287- int i = first, j = last;
288- int ii = 1, jj = last - off;
289- while(j-i > 0){
290- double alfi = (knot[r] - knot[i]) / (knot[i+ord] - knot[i]);
291- double alfj = (knot[r] - knot[j]) / (knot[j+ord] - knot[j]);
292- ON_3dPoint Pi, Pj;
293- nc.GetCV(i, Pi); nc.GetCV(j, Pj);
294- temp[ii] = (Pi - temp[ii-1] * (1.0 - alfi)) / alfi;
295- temp[jj] = (Pj - temp[jj+1] * alfj) / (1.0 - alfj);
296- i = i + 1, ii = ii + 1;
297- j = j - 1, jj = jj - 1;
364+// arc_integral 及び kappa_integral は呼び出し元で pt_cnt - 1 の領域を確保すること。
365+void ArcKappaIntegral(const ON_3dPoint *pts, int pt_cnt, double *arc_integral, double *kappa_integral){
366+ if (pt_cnt <= 2) return;
367+ ON_3dPoint pt1 = pts[0], pt2 = pts[1];
368+ kappa_integral[0] = 0;
369+ arc_integral[0] = pt2.DistanceTo(pt1);
370+ for (int i = 1; i < pt_cnt - 1; ++i){
371+ ON_3dPoint pt3 = pts[i+1];
372+ double r = ON_Circle(pt1, pt2, pt3).Radius();
373+ double arc = pt3.DistanceTo(pt2);
374+ arc_integral[i] = arc + arc_integral[i-1];
375+
376+ // pt1、pt2、pt3 が同一線上にある場合は曲率 0 だが、逆関数を求めづらいため、トレランスを足す。
377+ double kappa = (r == 0) ? ON_ZERO_CURVATURE_TOLERANCE : 1.0 / r;
378+ pt1 = pt2, pt2 = pt3;
379+ kappa_integral[i] = kappa + kappa_integral[i-1];
298380 }
299- if (j - 1 < 0){
300- return temp[ii-1].DistanceTo(temp[jj+1]);
301- }else{
302- double alfi = (knot[r] - knot[i]) / (knot[i+ord] - knot[i]);
303- ON_3dPoint Pi;
304- nc.GetCV(i, Pi);
305- return Pi.DistanceTo(temp[ii+1] * alfi + (1.0 - alfi) * temp[ii-1]);
381+}
382+
383+// kappa_integral は KappaIntegral の計算結果を渡す。 pt_cnt - 1 個の値が入っているはず。
384+// pt_indices_selected は呼び出し元で pt_cnt 分の領域を確保すること。戻り値の数だけ書き込まれる。
385+int KappaParametrization(const ON_3dPoint *pts, int pt_cnt, int enp, const double *kappa_integral, int *pt_indices_selected){
386+ if (pt_cnt < 2) return 0;
387+ if (enp == 2){
388+ pt_indices_selected[0] = 0;
389+ pt_indices_selected[1] = pt_cnt - 1;
390+ return 2;
306391 }
392+ int ki_count = pt_cnt - 1;
393+ int n = 0;
394+
395+ ON_Interval ki_range(kappa_integral[0], kappa_integral[pt_cnt - 2]);
396+
397+ int ki_cursor = 0;
398+ pt_indices_selected[n++] = ki_cursor;
399+ for (int j = 1; j < enp; ++j){
400+ double normalized_interval = static_cast<double>(j) / static_cast<double>(enp - 1);
401+ double ki = ki_range.ParameterAt(normalized_interval);
402+ int ki_cnt_search = ki_count - ki_cursor;
403+ int ki_inv = ON_SearchMonotoneArray(kappa_integral + ki_cursor, ki_cnt_search, ki);
404+ if (ki_inv < 0 || ki_inv == ki_cnt_search) return -1;
405+ if (ki_inv > 0) ki_cursor += ki_inv, pt_indices_selected[n++] = ki_cursor;
406+ }
407+ return n;
307408 }
308409
309-bool ONGEO_FitNurbsCurveToPointArray(const ON_3dPoint *pts, int pt_cnt, int order, double tolerance, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, ONGEO_NI_Solver solver_id){
310- if (tolerance <= 0) return ONGEO_FitNurbsCurveToPointArray(pts, pt_cnt, order, pmethod, nc, solver_id);
410+// pt_indices_added は呼び出し元で pt_cnt - pt_indices_cnt_already_selected だけ確保しておくこと。戻り値の数だけ書き込まれる。
411+int AdaptiveKnotSequenceGeneration(const ON_3dPoint *pts, int pt_cnt, const double *prm, const int *pt_indices_already_selected, int pt_indices_cnt_already_selected, double rho, int *pt_indices_added){
412+ if (pt_indices_cnt_already_selected == 0) return 0;
413+ double rho_inv = 1.0 / rho;
414+ double m = pt_indices_cnt_already_selected - 1;
415+ int n = 0;
311416
312- Solver_ON_Matrix_Inverse solver_omi;
313- IEquationSolver *solver = 0;
314- if (solver_id == ONGEO_NI_Solver_ON_Matrix_Inverse){
315- solver = &solver_omi;
417+ ON_3dPoint pt1 = pts[pt_indices_already_selected[0]];
418+ ON_3dPoint pt2 = pts[pt_indices_already_selected[1]];
419+ double delta1 = pt2.DistanceTo(pt1);
420+ int ridx_prev = -1;
421+ for (int i = 2; i <= m; ++i){
422+ ON_3dPoint pt3 = pts[pt_indices_already_selected[i]];
423+ double delta2 = pt3.DistanceTo(pt2);
424+ double ratio_delta = delta1 < ON_ZERO_TOLERANCE ? 0 : delta2 / delta1;
425+ int ridx = delta2 > delta1 ? i - 1 : i - 2;
426+ delta1 = delta2;
427+ pt2 = pt3;
428+
429+ if (ratio_delta >= rho_inv && ratio_delta <= rho) continue;
430+
431+ // 既に分割済のセグメントを選択した場合は飛ばす。
432+ // 短い、長い、短い と並んだセグメント列に到達した場合に、同一セグメントを2回分割してしまうのを防止するための措置。
433+ if (ridx == ridx_prev) continue;
434+ ridx_prev = ridx;
435+
436+ // 2つの長さのバランスが悪いところを見つけたら、
437+ // 長い方のセグメントの両端のパラメータの中央値に最も近いインデックス点を探し、
438+ // 未選択の点であれば追加。選択済の点の場合は追加しない。
439+ double prm1 = prm[pt_indices_already_selected[ridx]];
440+ double prm2 = prm[pt_indices_already_selected[ridx+1]];
441+ double prm_mid = (prm1 + prm2) * 0.5;
442+ int prm_mid_idx[2];
443+ prm_mid_idx[0] = ON_SearchMonotoneArray(prm, pt_cnt, prm_mid);
444+ bool last_idx = prm_mid_idx[0] == pt_cnt - 1;
445+
446+ if (!last_idx){
447+ prm_mid_idx[1] = (prm_mid_idx[0] < pt_cnt - 1) ? prm_mid_idx[0] + 1 : prm_mid_idx[0];
448+ // prm_mid に近い順に並べ替え
449+ double d1 = prm[prm_mid_idx[0]] - prm_mid;
450+ double d2 = prm_mid - prm[prm_mid_idx[1]];
451+ if (d1 > d2){
452+ std::swap(prm_mid_idx[0], prm_mid_idx[1]);
453+ }
454+ }
455+
456+ const int *pidx[2];
457+ pidx[0] = ON_BinarySearchIntArray(prm_mid_idx[0], pt_indices_already_selected, pt_indices_cnt_already_selected);
458+ if (pidx[0]){
459+ pidx[1] = last_idx ? pidx[0] : ON_BinarySearchIntArray(prm_mid_idx[1], pt_indices_already_selected, pt_indices_cnt_already_selected);
460+ }
461+
462+ int prm_idx_toadd;
463+ if (!pidx[0]) prm_idx_toadd = prm_mid_idx[0];
464+ else if (!pidx[1]) prm_idx_toadd = prm_mid_idx[1];
465+ else continue;
466+
467+ if (n > 0 && ON_BinarySearchIntArray(prm_idx_toadd, pt_indices_added, n)) continue;
468+
469+ pt_indices_added[n++] = prm_idx_toadd;
316470 }
317471
318- ON_SimpleArray<double> basis(order * order); basis.SetCount(basis.Capacity());
472+ return n;
473+}
474+}
319475
320- ON_NurbsCurve nc_tmp;
321- ONGEO_FitNurbsCurveToPointArray(pts, pt_cnt, 2, pmethod, nc_tmp, solver_id);
476+double ONGEO_FitNurbsCurveToPointArray_AdaptiveKnotSelection(const ON_3dPoint *pts, int pt_cnt, int order_want, double tolerance, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, double *prm_, double *err_, ON_SimpleArray<int> *selected_indices_, ONGEO_NI_Solver solver_id){
477+ ON_SimpleArray<double> prm;
478+ if (prm_ == 0){
479+ if (pmethod == ONGEO_NI_Manual) return -1;
480+ prm.SetCapacity(pt_cnt); prm.SetCount(pt_cnt);
481+ prm_ = prm;
482+ }
483+ CreateParameter(pts, pt_cnt, 1, pmethod, prm_);
322484
323- int n = pt_cnt - 1;
324- ON_SimpleArray<double> prm(n+1); prm.SetCount(n+1);
325- ON_SimpleArray<double> err(n+1); err.SetCount(n+1);
326- for (int i = 0; i < n+1; ++i){
327- prm[i] = nc_tmp.Knot(i);
328- err[i] = 0;
485+ ON_SimpleArray<double> arc_integral(pt_cnt - 1), kappa_integral(pt_cnt - 1);
486+ arc_integral.SetCount(pt_cnt - 1);
487+ kappa_integral.SetCount(pt_cnt - 1);
488+ ArcKappaIntegral(pts, pt_cnt, arc_integral.First(), kappa_integral.First());
489+
490+ double ai_last = (*arc_integral.Last());
491+
492+ ON_SimpleArray<int> selected_indices;
493+ if (selected_indices_ == 0){
494+ selected_indices_ = &selected_indices;
495+ selected_indices.SetCapacity(pt_cnt); selected_indices.SetCount(pt_cnt);
329496 }
330497
331- int p = order - 1;
332- // ノット除去
333- for (int deg = 1; deg <= p; ++deg){
334- ON_NurbsCurve nc_tmp2;
335- nc_tmp2.Create(3, false, deg+1, nc_tmp.CVCount());
336- // ノット除去
337- while(true){
338- // 最小の Br 値と、そのBrに対応するノット番号を取得
339- double Br_min = -1, err_min = -1;
340- const double *knot = nc_tmp.Knot();
341- int s = nc_tmp.KnotMultiplicity(0);
342- int r_min = -1, r = s-1;
343- s = nc_tmp.KnotMultiplicity(r+1);
344- r += s;
345- while(s > 0){
346- double Br = CalculateBr(nc_tmp, r, s);
498+ // トレランスから大まかな sigma を求める。
499+ double sigma;
500+ {
501+ double seg_len = ai_last / static_cast<double>(pt_cnt - 1);
502+ ON_3dPoint pta(0, 0, 0), ptb(seg_len * 0.5, tolerance, 0), ptc(seg_len, 0, 0);
503+ sigma = ON_Arc(pta, ptb, ptc).Length() / seg_len;
504+ }
505+ // トレランスで制御したいため、最初は少なめに見積もる。
506+ int enp = EstimateNumberOfPoints(pts, pt_cnt, sigma, 0);
507+ if (enp > 50) enp /= order_want * 2;
508+ else if (enp > 10) enp /= order_want;
347509
348- int p_s = deg + s;
349- s = nc_tmp.KnotMultiplicity(r+1);
350- if (r + s >= nc_tmp.KnotCount()-1) break;
351-#if 0
352- nc_tmp2 = nc_tmp;
353- nc_tmp2.RemoveSpan(r-(p-1));
354- double Br;
355- if ((p_s & 1) == 0){
356- int k = p_s / 2;
357- double a = (knot[r] - knot[r-k]) / (knot[r-k+p+1] - knot[r-k]);
358- ON_3dPoint P, P1r, P1l;
359- nc_tmp.GetCV(r-k, P);
360- nc_tmp2.GetCV(r-k+1, P1r);
361- int rk_1 = r-k-1;
362- nc_tmp2.GetCV(rk_1 >= 0 ? rk_1 : 0, P1l);
363- Br = P.DistanceTo(P1r * a + P1l * (1.0-a));
364- }else{
365- int k = (p_s + 1) / 2;
366- ON_3dPoint P1r, P1l;
367- nc_tmp2.GetCV(r-k+1, P1r);
368- nc_tmp2.GetCV(r-k, P1l);
369- Br = P1r.DistanceTo(P1l);
370- }
371-#endif
372- if (Br_min < 0 || Br_min > Br) Br_min = Br, r_min = r;
373- r += s;
510+ if (enp < 2) enp = 2;
511+
512+ int kp_cnt = KappaParametrization(pts, pt_cnt, enp, kappa_integral.First(), selected_indices_->First());
513+ int kp_aksg_cnt = kp_cnt;
514+ for(;;){
515+ int aksg_cnt = AdaptiveKnotSequenceGeneration(pts, pt_cnt, prm_, selected_indices_->First(), kp_aksg_cnt, 4.0, selected_indices_->First() + kp_aksg_cnt);
516+ if (aksg_cnt == 0) break;
517+ kp_aksg_cnt += aksg_cnt;
518+ ON_SortIntArray(ON::heap_sort, selected_indices_->First(), kp_aksg_cnt);
519+ }
520+
521+ // 収束計算
522+ double err_max;
523+ for (;;){
524+ int order = (order_want > kp_aksg_cnt) ? kp_aksg_cnt : order_want;
525+ FitNurbsCurveToPointArray(pts, pt_cnt, order, selected_indices_->First(), kp_aksg_cnt, ONGEO_NI_Manual, nc, prm_, solver_id);
526+
527+ ON_SimpleArray<double> segment_over_tol(kp_aksg_cnt-1); segment_over_tol.SetCount(kp_aksg_cnt-1);
528+ for (int i = 0; i < segment_over_tol.Count(); ++i) segment_over_tol[i] = 0;
529+
530+ int segment_idx = 0;
531+ err_max = 0;
532+ bool need_converge = false;
533+ for (int i = 1; i < pt_cnt - 1; ++i){
534+ while((*selected_indices_)[segment_idx+1] <= i && segment_idx < kp_aksg_cnt - 1){
535+ ++segment_idx;
374536 }
375- if (Br_min == -1) break;
376- {
377- int span_index = ON_NurbsSpanIndex(order, nc_tmp.CVCount(), knot, knot[r_min], 0, 0);
378- ON_EvaluateNurbsBasis(order, knot + span_index, knot[r_min], basis.First());
379- int s_min = nc_tmp.KnotMultiplicity(r_min);
380- int p_s = deg + s_min;
381- if ((p_s & 1) == 0){
382- int k = p_s / 2;
383- err_min = basis[r_min - k - span_index+1] * Br_min;
384- }else{
385- int k = (p_s + 1) / 2;
386- double a = (knot[r_min] - knot[r_min-k+1])/(knot[r_min-k+deg+2] - knot[r_min-k+1]);
387- err_min = (1.0 - a) * basis[r_min - k + 1 - span_index+1] * Br_min;
537+ double err = -1;
538+ double t = prm_[i];
539+ if (ONGEO_NearestPointNurbsCurve_Newton(nc, pts[i], t)){
540+ prm_[i] = t;
541+ err = pts[i].DistanceTo(nc.PointAt(t));
542+ if (err > tolerance){
543+ need_converge = true;
544+ if (segment_over_tol[segment_idx] < err) segment_over_tol[segment_idx] = err;
388545 }
546+ if (err_) err_[i] = err;
389547 }
390- if (err_min > tolerance) break;
391- int idx_s = ON_NurbsSpanIndex(order, nc_tmp.CVCount(), knot, knot[r_min], 0, 0);
392- int idx_n = idx_s + 2 * deg - 1;
393-
394- ptrdiff_t diff_s = std::lower_bound(prm.First(), prm.Last()+1, knot[idx_s]) - prm.First();
395- ptrdiff_t diff_e = std::upper_bound(prm.First(), prm.Last()+1, knot[idx_n]) - prm.First();
396- double *es = err.First() + diff_s, *ee = err.First() + diff_e;
397-
398- bool knot_removable = true;
399- for (double *e = es; e != ee; ++e){
400- if (*e + err_min < tolerance) continue;
401- knot_removable = false;
402- break;
403- }
404- if (knot_removable){
405- for (double *e = es; e != ee; ++e) *e += err_min;
406- nc_tmp.RemoveSpan(r_min-(order-2));
407- }else break;
548+ if (err_max < err) err_max = err;
408549 }
409- if (deg == p){
410- nc = nc_tmp;
411- break;
412- }
413550
414- // 次数を1上げて最小二乗法で曲線を再生成
415- nc_tmp.Create(3, false, deg+2, nc_tmp.CVCount());
416- ON_ClassArray<ON_3dPoint> R(nc_tmp.CVCount()-2); R.SetCount(R.Capacity());
417- InitializeSolver_PassThroughPoints_LSQ_EndPointsConstraint(pt_cnt, deg+2, nc_tmp.CVCount(), prm, nc_tmp.m_knot, nc_tmp.KnotCount(), pts, R, solver);
418- ON_ClassArray<ON_3dPoint> P(nc_tmp.CVCount()); P.SetCount(P.Capacity());
419- P[0] = pts[0];
420- P[P.Count()-1] = pts[pt_cnt-1];
421- solver->Solve(R.Count(), R.First(), P.First()+1, 1);
422- for (int i = 0; i < nc_tmp.CVCount(); ++i) nc_tmp.SetCV(i, P[i]);
551+ if (!need_converge) break;
423552
424- double err_max = 0;
425- for (int i = 1; i < pt_cnt - 1; ++i){
426- double t = prm[i];
427- if (ONGEO_NearestPointNurbsCurve_Newton(nc_tmp, pts[i], t)){
428- prm[i] = t;
429- err[i] = pts[i].DistanceTo(nc_tmp.PointAt(t));
553+ int *selected_indices_to_add = selected_indices_->First() + kp_aksg_cnt;
554+ int add_cnt = 0;
555+ for (int i = 0; i < segment_over_tol.Count(); ++i){
556+ if (segment_over_tol[i] == 0) continue;
557+ ON_Interval idx_range((*selected_indices_)[i], (*selected_indices_)[i+1]);
558+ double divide = 0.5;
559+ if (segment_over_tol[i] / tolerance > 100.0){
560+ divide = 0.25;
430561 }
431- if (err_max < err[i]) err_max = err[i];
562+ int idx_prev = static_cast<int>(idx_range.m_t[0]);
563+ for (double nrm_p = divide; nrm_p < 1.0; nrm_p += divide){
564+ int midx = idx_range.ParameterAt(nrm_p);
565+ if (idx_prev == midx) continue;
566+ selected_indices_to_add[add_cnt++] = midx;
567+ idx_prev = midx;
568+ }
432569 }
570+ if (add_cnt == 0) break;
571+ kp_aksg_cnt += add_cnt;
572+ ON_SortIntArray(ON::heap_sort, selected_indices_->First(), kp_aksg_cnt);
433573 }
574+ selected_indices_->SetCount(kp_aksg_cnt);
434575
435- return true;
576+ return err_max;
436577 }
437-#endif
438578
439-
440579 bool ONGEO_FitNurbsSurfaceToPointGrid(const ON_3dPoint *pts, int u_count, int v_count, int u_order, int v_order, ONGEO_NI_ParameterMethod pmethod, ON_NurbsSurface &nf, ONGEO_NI_Solver solver_id){
441580 if (u_count < u_order || v_count < v_order || !pts) return false;
442581 int n = u_count - 1, m = v_count - 1;
@@ -461,7 +600,7 @@
461600 }
462601 for (int i = 0; i <= n; ++i) prm_u[i] /= static_cast<double>(m+1);
463602
464- InitializeSolver_ExactlyPassThroughPoints(n + 1, u_order, prm_u.First(), nf.m_knot[0], nf.KnotCount(0), solver);
603+ InitializeSolver_ExactlyPassThroughPoints(n + 1, u_order, prm_u.First(), 0, 0, nf.m_knot[0], nf.KnotCount(0), solver);
465604
466605 ON_ClassArray<ON_3dPoint> R((n+1) * (m+1)); R.SetCount(R.Capacity());
467606 for (int j = 0; j <= m; ++j){
@@ -477,7 +616,7 @@
477616 }
478617 for (int j = 0; j <= m; ++j) prm_v[j] /= static_cast<double>(n+1);
479618
480- InitializeSolver_ExactlyPassThroughPoints(m + 1, v_order, prm_v.First(), nf.m_knot[1], nf.KnotCount(1), solver);
619+ InitializeSolver_ExactlyPassThroughPoints(m + 1, v_order, prm_v.First(), 0, 0, nf.m_knot[1], nf.KnotCount(1), solver);
481620
482621 ON_ClassArray<ON_3dPoint> Q(m+1); Q.SetCount(m+1);
483622 for (int i = 0; i <= n; ++i){
--- trunk/ONGEO.h (revision 59)
+++ trunk/ONGEO.h (revision 60)
@@ -450,7 +450,8 @@
450450 enum ONGEO_NI_ParameterMethod { // 曲線パラメータ [0 - 1] の計算方法
451451 ONGEO_NI_EquallySpaced, // 等間隔に配置
452452 ONGEO_NI_ChordLength, // 制御点列の長さに合わせて配置、最も典型的な方法
453- ONGEO_NI_Centripetal // 制御点列の長さの平方根に合わせて配置、急激な曲がりを含む場合はこちらの方が有利
453+ ONGEO_NI_Centripetal, // 制御点列の長さの平方根に合わせて配置、急激な曲がりを含む場合はこちらの方が有利
454+ ONGEO_NI_Manual // 呼び出し側で生成したパラメータを使用する。prm 配列に予めパラメータ値を入れて関数を呼び出すこと。 prm を null_ptr の場合は関数は失敗する。
454455 };
455456 enum ONGEO_NI_Solver{
456457 ONGEO_NI_Solver_ON_Matrix_Inverse
@@ -462,9 +463,10 @@
462463 /// @param [in] order Nurbs曲線の階数
463464 /// @param [in] pmethod 曲線パラメータの計算方法
464465 /// @param [out] nc 作成した Nurbs曲線
466+/// @param [in/out] prm 各点を曲線に投影した際のパラメータ。呼び出し元で pt_cnt 分の領域を確保すること。pmethod が ONGEO_NI_Manual の時はさらにパラメータ値を入れて呼び出すこと。null_ptr の時は格納しない。
465467 /// @param [in] solver 連立一次方程式をどの方法で解くか?
466468 /// @return true:成功、false:失敗
467-ONGEO_DECL bool ONGEO_FitNurbsCurveToPointArray(const ON_3dPoint *pts, int pt_cnt, int order, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, ONGEO_NI_Solver solver = ONGEO_NI_Solver_ON_Matrix_Inverse);
469+ONGEO_DECL bool ONGEO_FitNurbsCurveToPointArray(const ON_3dPoint *pts, int pt_cnt, int order, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, double *prm = 0, ONGEO_NI_Solver solver = ONGEO_NI_Solver_ON_Matrix_Inverse);
468470
469471 /// 与えられた点列から最小二乗法を用いて、指定した制御点数、階数の Nurbs曲線を作成する。
470472 /// @param [in] pts 点列
@@ -473,24 +475,25 @@
473475 /// @param [in] cpt_cnt 制御点数
474476 /// @param [in] pmethod 曲線パラメータの計算方法
475477 /// @param [out] nc 作成した Nurbs曲線
476-/// @param [out] prm 各点を曲線に投影した際のパラメータ。呼び出し元で pt_cnt 分の領域を確保すること。null_ptr の時は格納しない。
478+/// @param [in/out] prm 各点を曲線に投影した際のパラメータ。呼び出し元で pt_cnt 分の領域を確保すること。pmethod が ONGEO_NI_Manual の時はさらにパラメータ値を入れて呼び出すこと。null_ptr の時は格納しない。
477479 /// @param [out] err 各点と曲線との誤差。呼び出し元で pt_cnt 分の領域を確保すること。null_ptr の時は格納しない。
478480 /// @param [in] solver 連立一次方程式をどの方法で解くか?
479481 /// @return 誤差の最大値
480482 ONGEO_DECL double ONGEO_FitNurbsCurveToPointArray_LeastSquare(const ON_3dPoint *pts, int pt_cnt, int order, int cpt_cnt, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, double *prm = 0, double *err = 0, ONGEO_NI_Solver solver_id = ONGEO_NI_Solver_ON_Matrix_Inverse);
481483
482-#if 0
483-/// 与えられた点列から、許容誤差 tolerance 以内を通るNurbs曲線を作成する。
484-/// @param [in] pts 点列
485-/// @param [in] pt_cnt 点列の個数
486-/// @param [in] order Nurbs曲線の階数
487-/// @param [in] tolerance 許容誤差
488-/// @param [in] pmethod 曲線パラメータの計算方法
489-/// @param [out] nc 作成した Nurbs曲線
484+/// 与えられた点列から、与えられた点のうちのいくつかを通過する、許容誤差 tolerance 以内となる Nurbs 曲線を作成する。
485+/// @param [in] pts 点列
486+/// @param [in] pt_cnt 点列の個数
487+/// @param [in] order_want Nurbs曲線の階数。ただし、選択された点数が order_want+1 より少ないときは、選択された点数に応じた階数で作成する。
488+/// @param [in] tolerance 許容誤差
489+/// @param [in] pmethod 曲線パラメータの計算方法
490+/// @param [out] nc 作成した Nurbs曲線
491+/// @param [in/out] prm 各点を曲線に投影した際のパラメータ。呼び出し元で pt_cnt 分の領域を確保すること。pmethod が ONGEO_NI_Manual の時はさらにパラメータ値を入れて呼び出すこと。null_ptr の時は格納しない。
492+/// @param [out] err 各点と曲線との誤差。呼び出し元で pt_cnt 分の領域を確保すること。null_ptr の時は格納しない。
493+/// @param [out] selected_indices 選択された点列インデックスを格納する配列
490494 /// @param [in] solver 連立一次方程式をどの方法で解くか?
491495 /// @return true:成功、false:失敗
492-ONGEO_DECL bool ONGEO_FitNurbsCurveToPointArray(const ON_3dPoint *pts, int pt_cnt, int order, double tolerance, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, ONGEO_NI_Solver solver = ONGEO_NI_Solver_ON_Matrix_Inverse);
493-#endif
496+ONGEO_DECL double ONGEO_FitNurbsCurveToPointArray_AdaptiveKnotSelection(const ON_3dPoint *pts, int pt_cnt, int order_want, double tolerance, ONGEO_NI_ParameterMethod pmethod, ON_NurbsCurve &nc, double *prm = 0, double *err = 0, ON_SimpleArray<int> *selected_indices = 0, ONGEO_NI_Solver solver_id = ONGEO_NI_Solver_ON_Matrix_Inverse);
494497
495498 /// 与えられた点グリッドを通るNurbs曲面を作成する。
496499 /// @param [in] pts 点グリッド (P_u0v0、 P_u1v0、P_u2v0、...、P_u0v1、P_u1v1、... の順に並べる)
Show on old repository browser