1 /*=====================================================================
2 File: nurbs.cpp
3 Purpose:
4 Revision: $Id: nurbs.cpp,v 1.3 2002/05/24 17:27:24 philosophil Exp $
5 Author: Philippe Lavoie (3 Oct, 1996)
6 Modified by:
7
8 Copyright notice:
9 Copyright (C) 1996-1997 Philippe Lavoie
10
11 This library is free software; you can redistribute it and/or
12 modify it under the terms of the GNU Library General Public
13 License as published by the Free Software Foundation; either
14 version 2 of the License, or (at your option) any later version.
15
16 This library is distributed in the hope that it will be useful,
17 but WITHOUT ANY WARRANTY; without even the implied warranty of
18 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 Library General Public License for more details.
20
21 You should have received a copy of the GNU Library General Public
22 License along with this library; if not, write to the Free
23 Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 =====================================================================*/
25 #include <nurbs.h>
26 #include <fstream>
27 #include <string.h>
28 #include <nurbsS.h>
29 #include "integrate.h"
30
31 #include <stdlib.h>
32
33 /*!
34 */
35 namespace PLib {
36
37 /*!
38 \brief default constructor
39 \author Philippe Lavoie
40 \date 24 January 1997
41 */
42 template <class T, int N>
NurbsCurve()43 NurbsCurve<T,N>::NurbsCurve(): P(1),U(1),deg_(0)
44 {
45 }
46
47 /*!
48 \brief A copy constructor.
49
50 \param nurb the NURBS curve to copy
51
52 \author Philippe Lavoie
53 \date 24 January 1997
54 */
55 template <class T, int N>
NurbsCurve(const NurbsCurve<T,N> & nurb)56 NurbsCurve<T,N>::NurbsCurve(const NurbsCurve<T,N>& nurb):
57 ParaCurve<T,N>(), P(nurb.P),U(nurb.U),deg_(nurb.deg_)
58 {
59 }
60
61 /*!
62 \brief Resets a NURBS curve to new values
63
64 \param P1 the new values for the control points
65 \param U1 the new values for the knot vector
66 \param Degree the new degree of the curve
67
68 \warning The size of P1,U1 and Degree must agree: P.n()+degree+1=U.n()
69 \author Philippe Lavoie
70 \date 24 January 1997
71 */
72 template <class T, int N>
reset(const Vector<HPoint_nD<T,N>> & P1,const Vector<T> & U1,int Degree)73 void NurbsCurve<T,N>::reset(const Vector< HPoint_nD<T,N> >& P1, const Vector<T> &U1, int Degree) {
74 int nSize = P1.n() ;
75 int mSize = U1.n() ;
76 deg_ = Degree ;
77 if(nSize != mSize-deg_-1){
78 #ifdef USE_EXCEPTION
79 throw NurbsSizeError(P1.n(),U1.n(),Degree) ;
80 #else
81 Error err("reset");
82 err << "Invalid input size for the control points and the knot vector when reseting a Nurbs Curve.\n";
83 err << nSize << " control points and " << mSize << " knots\n" ;
84 err.fatal() ;
85 #endif
86 }
87 P.resize(P1.n()) ;
88 U.resize(U1.n()) ;
89 P = P1 ;
90 U = U1 ;
91 }
92
93 /*!
94 \brief Constructor with control points in 4D
95
96 \param P1 the control points
97 \param U1 the knot vector
98 \param Degree the degree of the curve
99
100 \warning The size of P1,U1 and Degree must agree: P.n()+degree+1=U.n()
101
102 \author Philippe Lavoie
103 \date 24 January 1997
104 */
105 template <class T, int N>
NurbsCurve(const Vector<HPoint_nD<T,N>> & P1,const Vector<T> & U1,int Degree)106 NurbsCurve<T,N>::NurbsCurve(const Vector< HPoint_nD<T,N> >& P1, const Vector<T> &U1, int Degree): P(P1), U(U1), deg_(Degree)
107 {
108
109 if(P.n() != U.n()-deg_-1){
110 #ifdef USE_EXCEPTION
111 throw NurbsSizeError(P.n(),U.n(),deg_) ;
112 #else
113 Error err("NurbsCurve(P1,U1,Degree)");
114 err << "Invalid input size for the control points and the knot vector.\n";
115 err << P.n() << " control points and " << U.n() << " knots\n" ;
116 err.fatal() ;
117 #endif
118 }
119 }
120
121 /*!
122 \brief Constructor with control points in 3D
123
124 \param P1 --> the control point vector
125 \param W --> the weight for each control points
126 \param U1 --> the knot vector
127 \param Degree --> the degree of the curve
128
129 \warning The size of P1,U1 and Degree must agree: P.n()+degree+1=U.n()
130
131 \author Philippe Lavoie
132 \date 24 January 1997
133 */
134 template <class T, int N>
NurbsCurve(const Vector<Point_nD<T,N>> & P1,const Vector<T> & W,const Vector<T> & U1,int Degree)135 NurbsCurve<T,N>::NurbsCurve(const Vector< Point_nD<T,N> >& P1, const Vector<T>& W, const Vector<T>& U1, int Degree): P(P1.n()), U(U1), deg_(Degree)
136 {
137 int nSize = P1.n() ;
138 int mSize = U1.n() ;
139
140 if(nSize != mSize-deg_-1){
141 #ifdef USE_EXCEPTION
142 throw NurbsSizeError(P.n(),U.n(),deg_) ;
143 #else
144 Error err("NurbsCurve(P1,W,U1,Degree)") ;
145 err << "Invalid input size for the control points and the knot vector.\n" ;
146 err << nSize << " control points and " << mSize << " knots\n" ;
147 err.fatal() ;
148 #endif
149 }
150 if(nSize != W.n()){
151 #ifdef USE_EXCEPTION
152 throw NurbsInputError(nSize,W.n()) ;
153 #else
154 Error err("NurbsCurve(P1,W,U1,Degree)") ;
155 err << "Size mismatched between the control points and the weights\n" ;
156 err << "ControlPoints size = " << nSize << ", Weight size = " << W.n() << endl ;
157 err.fatal() ;
158 #endif
159 }
160
161 for(int i = 0 ;i<nSize;i++){
162 const Point_nD<T,N>& pt = P1[i] ; // This makes the SGI compiler happy
163 for(int j=0;j<N;j++)
164 P[i].data[j] = pt.data[j] * W[i] ;
165 P[i].w() = W[i] ;
166 }
167 }
168
169 /*!
170 \brief The assignment operator for a NURBS curve
171
172 \param curve the NURBS curve to copy
173 \return A reference to itself
174
175 \warning The curve being copied must be valid, otherwise strange
176 results might occur.
177 \author Philippe Lavoie
178 \date 24 January 1997
179 */
180 template <class T, int N>
operator =(const NurbsCurve<T,N> & curve)181 NurbsCurve<T,N>& NurbsCurve<T,N>::operator=(const NurbsCurve<T,N>& curve) {
182 if(curve.U.n() != curve.P.n()+curve.deg_+1){
183 #ifdef USE_EXCEPTION
184 throw NurbsSizeError(curve.P.n(),curve.U.n(),curve.deg_) ;
185 #else
186 Error err("operator=") ;
187 err << "Invalid assignment... the curve being assigned to isn't valid\n" ;
188 err.fatal() ;
189 #endif
190 }
191 deg_ = curve.deg_ ;
192 U = curve.U ;
193 P = curve.P ;
194 if(U.n()!=P.n()+deg_+1){
195 #ifdef USE_EXCEPTION
196 throw NurbsSizeError(P.n(),U.n(),deg_) ;
197 #else
198 Error err("operator=") ;
199 err << "Error in assignment... couldn't assign properly the vectors\n" ;
200 err.fatal() ;
201 #endif
202 }
203 return *this ;
204 }
205
206
207 /*!
208 \brief draws a NURBS curve on an image
209
210 This will draw very primitively the NURBS curve on an image.
211 The drawing assumes the line is only in the xy plane (the z
212 is not used for now).
213
214 The algorithm finds the points on the curve at a \a step
215 parametric intervall between them and join them by a line.
216 No fancy stuff.
217
218 \param Img <-- draws the nurbs curve to this Image
219 \param color --> the line is drawn in this color
220 \param step --> the parametric distance between two computed points.
221
222 \author Philippe Lavoie
223 \date 24 January 1997
224 */
225 template <class T, int N>
drawImg(Image_UBYTE & Img,unsigned char color,T step)226 void NurbsCurve<T,N>::drawImg(Image_UBYTE& Img,unsigned char color,T step){
227 Point_nD<T,N> a1,a2 ;
228 T u_max = U[U.n()-1-deg_] ;
229 if(step<=0)
230 step = 0.01 ;
231 a1 = this->pointAt(U[deg_]) ;
232 T u ;
233 int i1,j1,i2,j2 ;
234 getCoordinates(a1,i1,j1,Img.rows(),Img.cols()) ;
235 for(u=U[deg_]+step ; u < u_max+(step/2.0) ; u+=step){ // the <= u_max doesn't work
236 a2 = this->pointAt(u) ;
237 if(!getCoordinates(a2,i2,j2,Img.rows(),Img.cols()))
238 continue ;
239 Img.drawLine(i1,j1,i2,j2,color) ;
240 i1 = i2 ;
241 j1 = j2 ;
242 }
243 a2 = this->pointAt(U[P.n()]) ;
244 if(getCoordinates(a2,i2,j2,Img.rows(),Img.cols()))
245 Img.drawLine(i1,j1,i2,j2,color) ;
246 }
247
248 /*!
249 \brief Draws a NURBS curve on an image
250
251 This will draw very primitively the NURBS curve on an image.
252 The drawing assumes the line is only in the xy plane (the z
253 is not used for now).
254
255 The algorithm finds the points on the curve at a \a step
256 parametric intervall between them and join them by a line.
257 No fancy stuff.
258
259 \param Img draws the nurbs curve to this Image
260 \param color the line is drawn in this color
261 \param step the parametric distance between two computed points.
262
263 \author Philippe Lavoie
264 \date 24 January 1997
265 */
266 template <class T, int N>
drawImg(Image_Color & Img,const Color & color,T step)267 void NurbsCurve<T,N>::drawImg(Image_Color& Img,const Color& color,T step){
268 Point_nD<T,N> a1,a2 ;
269 T u_max = U[U.n()-1-deg_] ;
270 if(step<=0)
271 step = 0.01 ;
272 a1 = this->pointAt(U[deg_]) ;
273 int i1,j1,i2,j2 ;
274 getCoordinates(a1,i1,j1,Img.rows(),Img.cols()) ;
275 T u ;
276 for(u=U[deg_]+step ; u < u_max+(step/2.0) ; u+=step){ // the <= u_max doesn't work
277 a2 = this->pointAt(u) ;
278 if(!getCoordinates(a2,i2,j2,Img.rows(),Img.cols()))
279 continue ;
280 Img.drawLine(i1,j1,i2,j2,color) ;
281 i1 = i2 ;
282 j1 = j2 ;
283 }
284 a2 = this->pointAt(U[P.n()]) ;
285 if(getCoordinates(a2,i2,j2,Img.rows(),Img.cols()))
286 Img.drawLine(i1,j1,i2,j2,color) ;
287 }
288
289 /*!
290 \brief Draws an anti-aliased NURBS curve on an image
291
292 This will draw the NURBS by using a circular brush profile. The
293 drawing is performed by averaging the intensity of the
294 profile at the pixels.
295
296 \param Img draws the nurbs curve to this Image
297 \param color the line is drawn in this color
298 \param precision this number influences the number of points used for
299 averaging purposes.
300 \param alpha a flag indicating if the profile is used as an alpha
301 chanel. If so, the line doesn't overwrite, it blends
302 the line with the image already present in Img.
303
304 \warning This routine is very \e slow; use normal drawing for speed.
305
306 \author Philippe Lavoie
307 \date 25 July 1997
308 */
309 template <class T, int N>
drawAaImg(Image_Color & Img,const Color & color,int precision,int alpha)310 void NurbsCurve<T,N>::drawAaImg(Image_Color& Img, const Color& color, int precision, int alpha){
311 NurbsCurve<T,3> profile ;
312
313 profile.makeCircle(Point_nD<T,3>(0,0,0),Point_nD<T,3>(1,0,0),Point_nD<T,3>(0,0,1),1.0,0,M_PI) ;
314 drawAaImg(Img,color,profile,precision,alpha) ;
315 }
316
317 /*!
318 \brief draws an anti-aliased NURBS curve on an image
319
320 This will draw the NURBS by using a user-defined brush profile.
321 The drawing is performed by averaging the intensity of the
322 profile at the pixels.
323
324 \param Img draws the nurbs curve to this Image
325 \param color the line is drawn in this color
326 \param profile the profile of the NURBS curve to draw
327 \param precision this number influences the number of points used for
328 averaging purposes.
329 \param alpha a flag indicating if the profile is used as an alpha
330 chanel. If so, the line doesn't overwrite, it blends
331 the line with the image already present in Img.
332
333 \warning This routine is very \e slow; use normal drawing for speed.
334
335 \author Philippe Lavoie
336 \date 22 August 1997
337 */
338 template <class T, int N>
drawAaImg(Image_Color & Img,const Color & color,const NurbsCurve<T,3> & profile,int precision,int alpha)339 void NurbsCurve<T,N>::drawAaImg(Image_Color& Img, const Color& color, const NurbsCurve<T,3>& profile, int precision, int alpha){
340 Vector< HPoint_nD<T,3> > sPts(2) ;
341 sPts[0] = sPts[1] = HPoint_nD<T,3>(1,1,1,1) ;
342 Vector<T> sKnot(4) ;
343 sKnot[0] = sKnot[1] = 0.0 ;
344 sKnot[2] = sKnot[3] = 1.0 ;
345
346 NurbsCurve<T,3> scaling(sPts,sKnot,1) ;
347
348 drawAaImg(Img,color,profile,scaling,precision,alpha) ;
349 }
350
351 /*!
352 \brief Draws an anti-aliased NURBS curve on an image
353
354 This will draw the NURBS by using a brush profile. The
355 drawing is performed by averaging the intensity of the
356 profile at the pixels.
357
358 This function generates a sweep surface by using the profile
359 given in its argument. The sweep is always performed by
360 following the y-axis of the profile. A scaling function
361 is also used when sweeping. This is used to vary the shape
362 of the profile while it's being swept (see the sweep member
363 function of NurbsSurface<T,N> for more details).
364
365 \param Img draws the nurbs curve to this Image
366 \param color the line is drawn in this color
367 \param profile the profile of the NURBS curve to draw
368 \param scaling the scaling to give the profile while drawing the curve
369 \param precision this number influences the number of points used for
370 averaging purposes.
371 \param alpha a flag indicating if the profile is used as an alpha
372 chanel. If so, the line doesn't overwrite, it blends
373 the line with the image already present in Img.
374
375 \warning This routine is very \e slow; use normal drawing for speed
376 or lower the precision factor.
377
378 \author Philippe Lavoie
379 \date 25 July 1997
380 */
381 template <class T, int N>
drawAaImg(Image_Color & Img,const Color & color,const NurbsCurve<T,3> & profile,const NurbsCurve<T,3> & scaling,int precision,int alpha)382 NurbsSurface<T,3> NurbsCurve<T,N>::drawAaImg(Image_Color& Img, const Color& color, const NurbsCurve<T,3>& profile, const NurbsCurve<T,3>& scaling, int precision, int alpha){
383 Matrix<T> addMatrix ;
384 Matrix_INT nMatrix ;
385
386 addMatrix.resize(Img.rows(),Img.cols()) ;
387 nMatrix.resize(Img.rows(),Img.cols()) ;
388
389 int i,j ;
390
391 T du,dv ;
392 // compute a coarse distance for the curve
393 Point_nD<T,N> a,b,c ;
394 a = this->pointAt(0.0) ;
395 b = this->pointAt(0.5) ;
396 c = this->pointAt(1.0) ;
397
398 T distance = norm(b-a) + norm(c-b) ;
399
400 dv = distance*T(precision) ;
401 dv = (U[U.n()-1]-U[0])/dv ;
402
403 // compute a coarse distance for the trajectory
404 Point_nD<T,3> a2,b2,c2 ;
405 a2 = profile.pointAt(0.0) ;
406 b2 = profile.pointAt(0.5) ;
407 c2 = profile.pointAt(1.0) ;
408 distance = norm(b2-a2) + norm(c2-b2) ;
409 du = distance*T(precision) ;
410 du = (profile.knot()[profile.knot().n()-1]-profile.knot()[0])/du ;
411
412 NurbsSurface<T,3> drawCurve ;
413 NurbsCurve<T,3> trajectory ;
414
415 to3D(*this,trajectory) ;
416
417 drawCurve.sweep(trajectory,profile,scaling,P.n()-1) ;
418
419 T u,v ;
420
421 for(u=U[0];u<U[U.n()-1];u+=du)
422 for(v=profile.knot()[0];v<profile.knot()[profile.knot().n()-1];v+=dv){
423 Point_nD<T,3> p ;
424 p = drawCurve.pointAt(u,v) ;
425 if(getCoordinates(p,i,j,Img.rows(),Img.cols())){
426 addMatrix(i,j) += p.z() ;
427 nMatrix(i,j) += 1 ;
428 }
429 }
430
431 T maxP = 1.0 ;
432 for(i=0;i<Img.rows();++i)
433 for(j=0;j<Img.cols();++j){
434 addMatrix(i,j) /= T(nMatrix(i,j)) ;
435 if(addMatrix(i,j)>maxP)
436 maxP = addMatrix(i,j) ;
437 }
438
439 for(i=0;i<Img.rows();++i)
440 for(j=0;j<Img.cols();++j){
441 if(nMatrix(i,j)){
442 double mean = double(addMatrix(i,j))/double(maxP) ;
443 if(alpha){
444 Img(i,j).r = (unsigned char)(mean*double(color.r)+(1.0-mean)*double(Img(i,j).r)) ;
445 Img(i,j).g = (unsigned char)(mean*double(color.g)+(1.0-mean)*double(Img(i,j).g)) ;
446 Img(i,j).b = (unsigned char)(mean*double(color.b)+(1.0-mean)*double(Img(i,j).b)) ;
447 }
448 else{
449 Img(i,j) = mean*color ;
450 }
451 }
452 }
453 return drawCurve ;
454 }
455
456 /*!
457 \brief Performs geometrical modifications
458
459 Each control points will be modified by a rotation-translation
460 matrix.
461
462 \param A the rotation-translation matrix
463
464 \author Philippe Lavoie
465 \date 22 August 1997
466 */
467 template <class T, int N>
transform(const MatrixRT<T> & A)468 void NurbsCurve<T,N>::transform(const MatrixRT<T>& A){
469 for(int i=P.n()-1;i>=0;--i)
470 P[i] = A*P[i] ;
471 }
472
473 /*!
474 \brief Evaluates the curve in 4D at parameter \a u
475
476 \latexonly
477 It evaluates the NURBS curve in 4D at the parametric point
478 $u$. Using the following equation
479 \begin{equation}
480 C(u) = \sum_{i=0}^n N_{i,p} P_i \hspace{0.5in} a \leq u \leq b
481 \end{equation}
482 where $P_i$ are the control points and $N_{i,p}$ are the
483 $p$th degree B-spline basis functions.
484 \endlatexonly
485
486 For more details on the algorithm, see A4.1 on page 124 of
487 the Nurbs Book.
488
489 \param u the parametric value at which the curve is evaluated
490
491 \return the 4D point at \a C(u)
492
493 \warning the parametric value must be in a valid range
494
495 \author Philippe Lavoie
496 \date 24 January, 1997
497 */
498 template <class T, int N>
operator ()(T u) const499 HPoint_nD<T,N> NurbsCurve<T,N>::operator()(T u) const{
500 static Vector<T> Nb ;
501 int span = findSpan(u) ;
502
503 basisFuns(u,span,Nb) ;
504
505 HPoint_nD<T,N> p(0) ;
506 for(int i=deg_;i>=0;--i) {
507 p += Nb[i] * P[span-deg_+i] ;
508 }
509 return p ;
510 }
511
512 /*!
513 \brief Evaluates the curve in homogenous space at parameter \a u
514
515 \latexonly
516 It evaluates the NURBS curve in 4D at the parametric point
517 $u$. Using the following equation
518 \begin{equation}
519 C(u) = \sum_{i=0}^n N_{i,p} P_i \hspace{0.5in} a \leq u \leq b
520 \end{equation}
521 where $P_i$ are the control points and $N_{i,p}$ are the
522 $p$th degree B-spline basis functions.
523 \endlatexonly
524
525 For more details on the algorithm, see A4.1 on page 124 of
526 the Nurbs Book.
527
528 \param u the parametric value at which the curve is evaluated
529 \param span the span of u
530
531 \return the 4D point at \a C(u)
532
533 \warning the parametric value must be in a valid range
534 \author Philippe Lavoie
535 \date 24 January, 1997
536 */
537 template <class T, int N>
hpointAt(T u,int span) const538 HPoint_nD<T,N> NurbsCurve<T,N>::hpointAt(T u, int span) const{
539 static Vector<T> Nb ;
540
541 basisFuns(u,span,Nb) ;
542
543 HPoint_nD<T,N> p(0,0,0,0) ;
544 for(int i=deg_;i>=0;--i) {
545 p += Nb[i] * P[span-deg_+i] ;
546 }
547 return p ;
548 }
549
550 /*!
551 \brief Computes the derivative of degree \a d of the curve at
552 parameter \a u
553
554 For more information on the algorithm used, see A3.2 on p 93
555 of the NurbsBook.
556
557 \param u the parametric value to evaluate at
558 \param d the degree of the derivative
559
560 \return The derivative \a d in norma space at the parameter \a u
561
562 \warning \a u and \a d must be in a valid range.
563 \author Philippe Lavoie
564 \date 24 January, 1997
565 */
566 template <class T, int N>
derive3D(T u,int d) const567 Point_nD<T,N> NurbsCurve<T,N>::derive3D(T u, int d) const {
568 Vector< Point_nD<T,N> > ders ;
569 deriveAt(u,d,ders) ;
570 return ders[d] ;
571 }
572
573 /*!
574 \brief Computes the derivative of degree \a of the curve at parameter \a u
575
576 For more information on the algorithm used, see A3.2 on p 93
577 of the NurbsBook.
578
579 \param u the parametric value to evaluate at
580 \param d the degree of the derivative
581
582 \return The derivative \a d in 4D at the parameter \a u
583
584 \warning \a u and \a d must be in a valid range.
585
586 \author Philippe Lavoie
587 \date 24 January, 1997
588 */
589 template <class T, int N>
derive(T u,int d) const590 HPoint_nD<T,N> NurbsCurve<T,N>::derive(T u, int d) const {
591 Vector< HPoint_nD<T,N> > ders ;
592 deriveAtH(u,d,ders) ;
593 return ders[d] ;
594 }
595
596 /*!
597 \brief Computes the derivative of degree \a d of the
598 curve at parameter \a u in the homonegeous domain
599
600 For more information on the algorithm used, see A3.2 on p 93
601 of the NurbsBook.
602
603 \param u the parametric value to evaluate at
604 \param d the degree of the derivative
605 \param ders a vector containing the derivatives of the curve at \a u.
606
607 \warning \a u and \a d must be in a valid range.
608
609 \author Philippe Lavoie
610 \date 24 January, 1997
611 */
612 template <class T, int N>
deriveAtH(T u,int d,Vector<HPoint_nD<T,N>> & ders) const613 void NurbsCurve<T,N>::deriveAtH(T u,int d, Vector< HPoint_nD<T,N> >& ders) const{
614 int du = minimum(d,deg_) ;
615 int span ;
616 Matrix<T> derF(du+1,deg_+1) ;
617 ders.resize(d+1) ;
618
619 span = findSpan(u) ;
620 dersBasisFuns(du,u,span,derF) ;
621 for(int k=du;k>=0;--k){
622 ders[k] = 0 ;
623 for(int j=deg_;j>=0;--j){
624 ders[k] += derF(k,j)*P[span-deg_+j] ;
625 }
626 }
627 }
628
629 /*!
630 \brief Computes the derivative of degree \a d of the curve at parameter \a u
631
632 For more information on the algorithm used, see A3.2 on p 93
633 of the NurbsBook.
634
635 \param u the parametric value to evaluate at
636 \param d the degree of the derivative
637 \param span the span of \a u
638 \param ders a vector containing the derivatives of the curve at \a u.
639
640 \warning \a u and \a d must be in a valid range.
641 \author Philippe Lavoie
642 \date 9 October, 1998
643 */
644 template <class T, int N>
deriveAtH(T u,int d,int span,Vector<HPoint_nD<T,N>> & ders) const645 void NurbsCurve<T,N>::deriveAtH(T u, int d, int span, Vector< HPoint_nD<T,N> >& ders) const{
646 int du = minimum(d,deg_) ;
647 Matrix<T> derF(du+1,deg_+1) ;
648 ders.resize(d+1) ;
649
650 dersBasisFuns(du,u,span,derF) ;
651 for(int k=du;k>=0;--k){
652 ders[k] = 0 ;
653 for(int j=deg_;j>=0;--j){
654 ders[k] += derF(k,j)*P[span-deg_+j] ;
655 }
656 }
657 }
658
659
660 // Setup the binomial coefficients into th matrix Bin
661 // Bin(i,j) = (i j)
662 // The binomical coefficients are defined as follow
663 // (n) n!
664 // (k) = k!(n-k)! 0<=k<=n
665 // and the following relationship applies
666 // (n+1) (n) ( n )
667 // ( k ) = (k) + (k-1)
668 /*!
669 \brief Setup a matrix containing binomial coefficients
670
671 Setup the binomial coefficients into th matrix Bin
672 \htmlonly
673 \[ Bin(i,j) = \left( \begin{array}{c}i \\ j\end{array} \right)\]
674 The binomical coefficients are defined as follow
675 \[ \left(\begin{array}{c} n \\ k \end{array} \right)= \frac{ n!}{k!(n-k)!} \mbox{for $0\leq k \leq n$} \]
676 and the following relationship applies
677 \[ \left(\begin{array}{c} n+1 \\ k \end{array} \right) =
678 \left(\begin{array}{c} n \\ k \end{array} \right) +
679 \left(\begin{array}{c} n \\ k-1 \end{array} \right) \]
680 \endhtmlonly
681
682 \param Bin the binomial matrix
683 \author Philippe Lavoie
684 \date 24 January, 1997
685 */
686 template <class T>
binomialCoef(Matrix<T> & Bin)687 void binomialCoef(Matrix<T>& Bin){
688 int n,k ;
689 // Setup the first line
690 Bin(0,0) = 1.0 ;
691 for(k=Bin.cols()-1;k>0;--k)
692 Bin(0,k) = 0.0 ;
693 // Setup the other lines
694 for(n=0;n<Bin.rows()-1;n++){
695 Bin(n+1,0) = 1.0 ;
696 for(k=1;k<Bin.cols();k++)
697 if(n+1<k)
698 Bin(n,k) = 0.0 ;
699 else
700 Bin(n+1,k) = Bin(n,k) + Bin(n,k-1) ;
701 }
702 }
703
704 /*!
705 \brief Computes the derivative at the parameter \a u
706
707 \param u the parameter at which the derivative is computed
708 \param d the degree of derivation
709 \param ders the vector containing the derivatives of the point at \a u.
710
711 \wanring \a u and \a d must be in a valid range.
712 \author Philippe Lavoie
713 \date 24 January, 1997
714 */
715 template <class T, int N>
deriveAt(T u,int d,Vector<Point_nD<T,N>> & ders) const716 void NurbsCurve<T,N>::deriveAt(T u, int d, Vector< Point_nD<T,N> >& ders) const{
717 Vector< HPoint_nD<T,N> > dersW ;
718 deriveAtH(u,d,dersW) ;
719 Point_nD<T,N> v ;
720 int k,i ;
721 ders.resize(d+1) ;
722
723 static Matrix<T> Bin(1,1) ;
724 if(Bin.rows() != d+1){
725 Bin.resize(d+1,d+1) ;
726 binomialCoef(Bin) ;
727 }
728
729 // Compute the derivative at the parmeter u
730
731 for(k=0;k<=d;k++){
732 v.x() = dersW[k].x() ;
733 v.y() = dersW[k].y() ;
734 v.z() = dersW[k].z() ;
735 for(i=k ;i>0 ;--i){
736 v -= (Bin(k,i)*dersW[i].w())*ders[k-i] ;
737 }
738 ders[k] = v ;
739 ders[k] /= dersW[0].w() ;
740 }
741 }
742
743 /*!
744 \brief Computes the derivative of the curve at the parameter \a u
745
746 \param u the parameter at which the derivative is computed
747 \param d the degree of derivation
748 \param span the span of \a u.
749 \param ders the vector containing the derivatives of the point at \a u.
750
751 \warning \a u and $d$ must be in a valid range.
752 \author Philippe Lavoie
753 \date 9 October 1998
754 */
755 template <class T, int N>
deriveAt(T u,int d,int span,Vector<Point_nD<T,N>> & ders) const756 void NurbsCurve<T,N>::deriveAt(T u, int d, int span, Vector< Point_nD<T,N> >& ders) const{
757 Vector< HPoint_nD<T,N> > dersW ;
758 deriveAtH(u,d,span,dersW) ;
759 Point_nD<T,N> v ;
760 int k,i ;
761 ders.resize(d+1) ;
762
763 static Matrix<T> Bin(1,1) ;
764 if(Bin.rows() != d+1){
765 Bin.resize(d+1,d+1) ;
766 binomialCoef(Bin) ;
767 }
768
769 // Compute the derivative at the parmeter u
770
771 for(k=0;k<=d;k++){
772 v.x() = dersW[k].x() ;
773 v.y() = dersW[k].y() ;
774 v.z() = dersW[k].z() ;
775 for(i=k ;i>0 ;--i){
776 v -= (Bin(k,i)*dersW[i].w())*ders[k-i];
777 }
778 ders[k] = v ;
779 ders[k] /= dersW[0].w() ;
780 }
781 }
782
783 /*!
784 \brief Computes the normal of the curve at \a u from a vector.
785
786 Computes the normal of the curve at \a u from a vector.
787 If the curve lies only in the xy-plane, then calling the
788 function with the vector v = (0,0,1) (the z$axis) will
789 yield a proper normal for this curve.
790
791 \param u the parameter at which the normal is computed
792 \param v the vector to compute the normal with
793
794 \return the normal vector in 3D.
795 \warning \a u must be in a valid range.
796 \author Philippe Lavoie
797 \date 2 September, 1997
798 */
799 template <class T, int N>
normal(T u,const Point_nD<T,N> & v) const800 Point_nD<T,N> NurbsCurve<T,N>::normal(T u, const Point_nD<T,N>& v) const{
801 return crossProduct(firstDn(u),v) ;
802 }
803
804 /*!
805 \brief Computes the basis function of the curve
806
807 Computes the \a i basis function of degree \a p of the curve at
808 parameter \a u.
809
810 \latexonly
811 The basis function is noted as $N_{ip}(u)$.
812
813 The B-spline basis function of $p$-degree is defined as
814 \begin{eqnarray}
815 N_{i,0}(u) & = & \left\{ \begin{array}{ll} 1 & \mbox{if $u_i \leq u < u_{i+1}$} \\ 0 & \mbox{otherwise}\end{array}\right. \nonumber \\
816 N_{i,p}(u) & = & \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u) \nonumber
817 \end{eqnarray}
818
819 where the $u_i$ define the knot vector $U = \{u_0,\ldots,u_m\}$
820 as a nondecreasing sequence of real numbers, {\em i.e.},
821 $u_i \leq u_{i+1}$ for $i=0,\ldots,m-1$. And $m$ is related
822 to the number of control points $n$ and the degree of the curve
823 $p$ with the relation $m = n + p + 1$. The knot vector has
824 the form
825
826 \begin{equation}
827 U=\{\underbrace{a,\ldots,a}_{p+1},u_{p+1},\ldots,u_{m-p-1},\underbrace{b,\ldots,b}_{p+1} \}
828 \end{equation}
829
830 \endlatexonly
831 \htmlonly
832 You can have more information about this function in the LaTeX version.
833 \endhtmlonly
834
835 \param u the parametric variable
836 \param i specifies which basis function to compute
837 \param p the degree to which the basis function is computed
838
839 \return the value of \a N_{ip}(u)
840
841 \author Philippe Lavoie
842 \date 24 January 1997
843 */
844 template <class T, int D>
845 T NurbsCurve<T,D>::basisFun(T u, int i, int p) const{
846 T Nip ;
847 T saved,Uleft,Uright,temp ;
848
849 if(p<1)
850 p = deg_ ;
851
852 if((i==0 && u == U[0]) ||
853 (i == U.n()-p-2 && u==U[U.n()-1])){
854 Nip = 1.0 ;
855 return Nip ;
856 }
857 if(u<U[i] || u>=U[i+p+1]){
858 Nip = 0.0 ;
859 return Nip;
860 }
861 T* N = (T*) alloca((p+1)*sizeof(T)) ; // Vector<T> N(p+1) ;
862
863
864 int j ;
865 for(j=p;j>=0;--j){
866 if(u>=U[i+j] && u<U[i+j+1])
867 N[j] = 1.0 ;
868 else
869 N[j] = 0.0 ;
870 }
871 for(int k=1; k<=p ; k++){
872 if(N[0] == 0.0)
873 saved = 0.0 ;
874 else
875 saved = ( (u-U[i])*N[0])/(U[i+k]-U[i]) ;
876 for(j=0;j<p-k+1;j++){
877 Uleft = U[i+j+1] ;
878 Uright = U[i+j+k+1] ;
879 if(N[j+1]==0.0){
880 N[j] = saved ;
881 saved = 0.0 ;
882 }
883 else {
884 temp = N[j+1]/(Uright-Uleft) ;
885 N[j] = saved+(Uright-u)*temp ;
886 saved = (u-Uleft)*temp ;
887 }
888 }
889 }
890 Nip = N[0] ;
891
892 return Nip ;
893 }
894
895 // it returns the matrix ders, where ders(n+1,deg+1) and the C'(u) = ders(1,span-deg+j) ;
896
897 /*!
898 \brief Compute the derivatives functions at \a u of the NURBS curve
899
900 For information on the algorithm, see A2.3 on p72 of the NURBS
901 book.
902
903 The result is stored in the ders matrix, where ders is of
904 size \a (n+1,deg+1) and the derivative
905 N'_i(u) = ders(1,i=span-deg+j) where j=0...deg+1.
906
907 \param n the degree of the derivation
908 \param u the parametric value
909 \param span the span for the basis functions
910 \param ders A matrix containing the derivatives of the curve.
911
912 \warning \a n, \a u and \a span must be valid values.
913 \author Philippe Lavoie
914 \date 24 January 1997
915 */
916 template <class T, int N>
dersBasisFuns(int n,T u,int span,Matrix<T> & ders) const917 void NurbsCurve<T,N>::dersBasisFuns(int n,T u, int span, Matrix<T>& ders) const {
918 T* left = (T*) alloca(2*(deg_+1)*sizeof(T)) ;
919 T* right = &left[deg_+1] ;
920
921 Matrix<T> ndu(deg_+1,deg_+1) ;
922 T saved,temp ;
923 int j,r ;
924
925 ders.resize(n+1,deg_+1) ;
926
927 ndu(0,0) = 1.0 ;
928 for(j=1; j<= deg_ ;j++){
929 left[j] = u-U[span+1-j] ;
930 right[j] = U[span+j]-u ;
931 saved = 0.0 ;
932
933 for(r=0;r<j ; r++){
934 // Lower triangle
935 ndu(j,r) = right[r+1]+left[j-r] ;
936 temp = ndu(r,j-1)/ndu(j,r) ;
937 // Upper triangle
938 ndu(r,j) = saved+right[r+1] * temp ;
939 saved = left[j-r] * temp ;
940 }
941
942 ndu(j,j) = saved ;
943 }
944
945 for(j=deg_;j>=0;--j)
946 ders(0,j) = ndu(j,deg_) ;
947
948 // Compute the derivatives
949 Matrix<T> a(deg_+1,deg_+1) ;
950 for(r=0;r<=deg_;r++){
951 int s1,s2 ;
952 s1 = 0 ; s2 = 1 ; // alternate rows in array a
953 a(0,0) = 1.0 ;
954 // Compute the kth derivative
955 for(int k=1;k<=n;k++){
956 T d ;
957 int rk,pk,j1,j2 ;
958 d = 0.0 ;
959 rk = r-k ; pk = deg_-k ;
960
961 if(r>=k){
962 a(s2,0) = a(s1,0)/ndu(pk+1,rk) ;
963 d = a(s2,0)*ndu(rk,pk) ;
964 }
965
966 if(rk>=-1){
967 j1 = 1 ;
968 }
969 else{
970 j1 = -rk ;
971 }
972
973 if(r-1 <= pk){
974 j2 = k-1 ;
975 }
976 else{
977 j2 = deg_-r ;
978 }
979
980 for(j=j1;j<=j2;j++){
981 a(s2,j) = (a(s1,j)-a(s1,j-1))/ndu(pk+1,rk+j) ;
982 d += a(s2,j)*ndu(rk+j,pk) ;
983 }
984
985 if(r<=pk){
986 a(s2,k) = -a(s1,k-1)/ndu(pk+1,r) ;
987 d += a(s2,k)*ndu(r,pk) ;
988 }
989 ders(k,r) = d ;
990 j = s1 ; s1 = s2 ; s2 = j ; // Switch rows
991 }
992 }
993
994 // Multiply through by the correct factors
995 r = deg_ ;
996 for(int k=1;k<=n;k++){
997 for(j=deg_;j>=0;--j)
998 ders(k,j) *= r ;
999 r *= deg_-k ;
1000 }
1001
1002 }
1003
1004 // Computes the non-zero basis functions into N of size deg+1
1005 // The following relationship applies N[i] <= N[span-deg+i] for i = 0..deg
1006 // A2.1 on p68 of the Nurbs Book
1007 /*!
1008 \brief computes the non-zero basis functions of the curve
1009
1010 Computes the non-zero basis functions and puts the result
1011 into \a N. \a N has a size of deg+1. To relate \a N to the basis
1012 functions, Basis[span -deg +i] = N[i] for i=0...deg.
1013
1014 \latexonly
1015 The B-spline basis function of $p$-degree is defined as
1016 \begin{eqnarray}
1017 N_{i,0}(u) & = & \left\{ \begin{array}{ll} 1 & \mbox{if $u_i \leq u < u_{i+1}$} \\ 0 & \mbox{otherwise}\end{array}\right. \nonumber \\
1018 N_{i,p}(u) & = & \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u) \nonumber
1019 \end{eqnarray}
1020
1021 where the $u_i$ define the knot vector $U = \{u_0,\ldots,u_m\}$
1022 as a nondecreasing sequence of real numbers, {\em i.e.},
1023 $u_i \leq u_{i+1}$ for $i=0,\ldots,m-1$. And $m$ is related
1024 to the number of control points $n$ and the degree of the curve
1025 $p$ with the relation $m = n + p + 1$. The knot vector has
1026 the form
1027
1028 \begin{equation}
1029 U=\{\underbrace{a,\ldots,a}_{p+1},u_{p+1},\ldots,u_{m-p-1},\underbrace{b,\ldots,b}_{p+1} \}
1030 \end{equation}
1031
1032 The B-spline basis function are non-zero for at most $p+1$ of
1033 the $N_{i,p}$. The relationship between the non-zero basis
1034 functions in N and $N_{i,p}$ is as follows,
1035 $N_{span - deg + j, p} = N[j]$ for $j=0,\ldots,deg$. Where
1036 span is the non-zero span of the basis functions. This
1037 non-zero span for \a u can be found by calling
1038 {\tt findSpan(u)}.
1039
1040 \endlatexonly
1041 \htmlonly
1042 You can find more information in the LaTeX version.
1043 \endhtmlonly
1044
1045 \param u the parametric value
1046 \param i the non-zero span of the basis functions
1047 \param N the non-zero basis functions
1048
1049 \warning \a u and \a i must be valid values
1050 \author Philippe Lavoie
1051 \date 24 January 1997
1052 */
1053 template <class T, int D>
basisFuns(T u,int i,Vector<T> & N) const1054 void NurbsCurve<T,D>::basisFuns(T u, int i, Vector<T>& N) const{
1055 T* left = (T*) alloca(2*(deg_+1)*sizeof(T)) ;
1056 T* right = &left[deg_+1] ;
1057
1058 T temp,saved ;
1059
1060 N.resize(deg_+1) ;
1061
1062 N[0] = 1.0 ;
1063 for(int j=1; j<= deg_ ; j++){
1064 left[j] = u-U[i+1-j] ;
1065 right[j] = U[i+j]-u ;
1066 saved = 0.0 ;
1067 for(int r=0 ; r<j; r++){
1068 temp = N[r]/(right[r+1]+left[j-r]) ;
1069 N[r] = saved+right[r+1] * temp ;
1070 saved = left[j-r] * temp ;
1071 }
1072 N[j] = saved ;
1073 }
1074
1075 }
1076
1077 /*!
1078 \brief Determines the knot span index
1079
1080 Determines the knot span for which their exists non-zero basis
1081 functions. The span is the index \a k for which the parameter
1082 \a u is valid in the [u_k,u_{k+1}] range.
1083
1084 \param u the parametric value
1085 \return the span index at \a u.
1086 \warning \a u must be in a valid range
1087 \author Philippe Lavoie
1088 \date 24 January 1997
1089 \modified 20 January, 1999 (Alejandro Frangi)
1090 */
1091 template <class T, int N>
findSpan(T u) const1092 int NurbsCurve<T,N>::findSpan(T u) const{
1093 if(u>=U[P.n()])
1094 return P.n()-1 ;
1095 if(u<=U[deg_])
1096 return deg_ ;
1097
1098 int low = 0 ;
1099 int high = P.n()+1 ;
1100 int mid = (low+high)/2 ;
1101
1102 while(u<U[mid] || u>= U[mid+1]){
1103 if(u<U[mid])
1104 high = mid ;
1105 else
1106 low = mid ;
1107 mid = (low+high)/2 ;
1108 }
1109 return mid ;
1110
1111 }
1112
1113 /*!
1114 \brief Finds the knot \a k for which \a u is in the range [u_k,u_{k+1})
1115
1116 \param u parametric value
1117
1118 \return the index \a k
1119
1120 \warning \a u must be in a valid range.
1121
1122 \author Philippe Lavoie
1123 \date 24 January, 1997
1124 */
1125 template <class T, int N>
findKnot(T u) const1126 int NurbsCurve<T,N>::findKnot(T u) const{
1127 if(u==U[P.n()])
1128 return P.n() ;
1129 for(int i=deg_+1; i<P.n() ; i++)
1130 if(U[i]>u){
1131 return i-1 ;
1132 }
1133 return -1 ;
1134 }
1135
1136
1137 /*!
1138 \brief Finds the multiplicity of a knot
1139 \param r the knot to observe
1140 \return the multiplicity of the knot
1141
1142 \warning \a r must be a valid knot index
1143 \author Philippe Lavoie
1144 \date 24 January, 1997
1145 */
1146 template <class T, int N>
findMult(int r) const1147 int NurbsCurve<T,N>::findMult(int r) const {
1148 int s=1 ;
1149 for(int i=r;i>deg_+1;--i)
1150 if(U[i]<=U[i-1])
1151 s++ ;
1152 else
1153 return s ;
1154 return s ;
1155 }
1156
1157 /*!
1158 \brief Finds the multiplicity of a knot at a parametric value
1159
1160 Finds the index of the knot at parametric value \a u and
1161 returns its multiplicity.
1162
1163 \param u the parametric value
1164 \param r the knot of interest
1165 \param s the multiplicity of this knot
1166
1167 \warning \a u must be in a valid range.
1168
1169 \author Philippe Lavoie
1170 \date 24 January, 1997
1171 */
1172 template <class T, int N>
findMultSpan(T u,int & r,int & s) const1173 void NurbsCurve<T,N>::findMultSpan(T u, int& r, int& s) const {
1174 r = findKnot(u) ;
1175 if(u==U[r]){
1176 s = findMult(r) ;
1177 }
1178 else
1179 s = 0 ;
1180 }
1181
1182 /*!
1183 \brief Resizes a NURBS curve
1184
1185 Resizes a NURBS curve. The old values are lost and new ones
1186 have to be created.
1187
1188 \param n the new number of control points for the curve
1189 \param Deg the new degree for the curve
1190
1191 \author Philippe Lavoie
1192 \date 24 January 1997
1193 */
1194 template <class T, int N>
resize(int n,int Deg)1195 void NurbsCurve<T,N>::resize(int n, int Deg){
1196 deg_ = Deg ;
1197 P.resize(n) ;
1198 U.resize(n+deg_+1) ;
1199 }
1200
1201 /*!
1202 \brief A least squares curve approximation
1203
1204 \latexonly
1205 This routine solves the following problem: find the NURBS curve
1206 $C$ satisfying
1207 \begin{itemize}
1208 \item $Q_0 = C(0)$ and $Q_m = C(1)$
1209 \item the remaining $Q_k$ are approximated in the least squares
1210 sense, {\em i.e.}
1211 \[ \sum_{k=1}^{m-1} | Q_k-C(\bar{u}_k)|^2 \]
1212 in a minimum with respect to the $n$ variable $P_i$; the
1213 $\bar{u}$ are the parameter values computed with the
1214 chord length method.
1215 \end{itemize}
1216
1217 The resulting curve will generally not pass through $Q_k$ and
1218 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
1219 \endlatexonly
1220 \htmlonly
1221 This routines generates a curve that approrimates the points in the
1222 least square sense, you can find more details in the LaTeX version.
1223 \endhtmlonly
1224
1225 For more details, see section 9.4.1 on page 491 of the NURBS
1226 book.
1227
1228 \param Q the vector of 3D points
1229 \param degC the degree of the curve
1230 \param n the number of control points in the new curve.
1231
1232 \warning \a deg must be smaller than Q.n().
1233 \author Philippe Lavoie
1234 \date 24 January, 1997
1235 */
1236 template <class T, int N>
leastSquares(const Vector<Point_nD<T,N>> & Q,int degC,int n)1237 int NurbsCurve<T,N>::leastSquares(const Vector< Point_nD<T,N> >& Q, int degC, int n){
1238 Vector<T> ub(Q.n()) ;
1239
1240 chordLengthParam(Q,ub) ;
1241
1242 return leastSquares(Q,degC,n,ub) ;
1243 }
1244
1245 /*!
1246 \brief A least squares curve approximation
1247
1248 \latexonly
1249 This routine solves the following problem: find the NURBS curve
1250 $C$ satisfying
1251 \begin{itemize}
1252 \item $Q_0 = C(0)$ and $Q_m = C(1)$
1253 \item the remaining $Q_k$ are approximated in the least squares
1254 sense, {\em i.e.}
1255 \[ \sum_{k=1}^{m-1} | Q_k-C(\bar{u}_k)|^2 \]
1256 in a minimum with respect to the $n$ variable $P_i$; the
1257 $\bar{u}$ are the precomputed parameter values.
1258 \end{itemize}
1259
1260 The resulting curve will generally not pass through $Q_k$ and
1261 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
1262 \endlatexonly
1263 \htmlonly
1264 This routines generates a curve that approrimates the points in the
1265 least square sense, you can find more details in the LaTeX version.
1266 \endhtmlonly
1267
1268 For more details, see section 9.4.1 on page 491 of the NURBS
1269 book.
1270
1271 \param Q the vector of 3D points
1272 \param degC the degree of the curve
1273 \param n the number of control points in the new curve
1274 \param ub the knot coefficients
1275
1276 \warning the variable curve \b must contain a valid knot vector.
1277 \author Philippe Lavoie
1278 \date 24 January 1997
1279 */
1280 template <class T, int N>
leastSquares(const Vector<Point_nD<T,N>> & Q,int degC,int n,const Vector<T> & ub)1281 int NurbsCurve<T,N>::leastSquares(const Vector< Point_nD<T,N> >& Q, int degC, int n, const Vector<T>& ub){
1282 int i,j;
1283 T d,a ;
1284
1285 if(ub.n() != Q.n()){
1286 #ifdef USE_EXCEPTION
1287 throw NurbsInputError(ub.n(),Q.n()) ;
1288 #else
1289 Error err("leastSquares");
1290 err << "leastSquaresCurve\n" ;
1291 err << "ub size is different than Q's\n" ;
1292 err.fatal() ;
1293 #endif
1294 }
1295
1296 deg_ = degC ;
1297 U.resize(n+deg_+1) ;
1298
1299 // Changing the method to generate a U compare to the one
1300 // described by Piegl and Tiller in the NURBS book (eq 9.69)
1301
1302 U.reset(1.0) ;
1303 d = (T)(Q.n())/(T)(n) ;
1304 for(j=0;j<=deg_;++j)
1305 U[j] = 0 ;
1306
1307 for(j=1;j<n-deg_;++j){
1308 U[deg_+j] = 0.0 ;
1309 for(int k=j;k<j+deg_;++k){
1310 i = (int)(k*d) ;
1311 a = T(k*d)-T(i) ;
1312 int i2 = (int)((k-1)*d) ;
1313 U[deg_+j] += a*ub[i2]+(1-a)*ub[i] ;
1314 }
1315 U[deg_+j] /= deg_ ;
1316 }
1317
1318 return leastSquares(Q, degC, n, ub, U) ;
1319 }
1320
1321 /*!
1322 \brief A least squares curve approximation
1323
1324 \latexonly
1325 This routine solves the following problem: find the NURBS curve
1326 $C$ satisfying
1327 \begin{itemize}
1328 \item $Q_0 = C(0)$ and $Q_m = C(1)$
1329 \item the remaining $Q_k$ are approximated in the least squares
1330 sense, {\em i.e.}
1331 \[ \sum_{k=1}^{m-1} | Q_k-C(\bar{u}_k)|^2 \]
1332 in a minimum with respect to the $n$ variable $P_i$; the
1333 $\bar{u}$ are the precomputed parameter values.
1334 \end{itemize}
1335
1336 The resulting curve will generally not pass through $Q_k$ and
1337 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
1338 \endlatexonly
1339 \htmlonly
1340 This routines generates a curve that approrimates the points in the
1341 least square sense, you can find more details in the LaTeX version.
1342 \endhtmlonly
1343
1344 For more details, see section 9.4.1 on page 491 of the NURBS
1345 book.
1346
1347 \param Q the vector of 4D points
1348 \param degC the degree of the curve
1349 \param n the number of control points in the new curve
1350 \param ub the knot coefficients
1351
1352 \warning the variable curve \b must contain a valid knot vector.
1353
1354 \author Philippe Lavoie
1355 \date 24 January 1997
1356 */
1357 template <class T, int N>
leastSquaresH(const Vector<HPoint_nD<T,N>> & Q,int degC,int n,const Vector<T> & ub)1358 int NurbsCurve<T,N>::leastSquaresH(const Vector< HPoint_nD<T,N> >& Q, int degC, int n, const Vector<T>& ub){
1359 int i,j;
1360 T d,a ;
1361
1362 if(ub.n() != Q.n()){
1363 #ifdef USE_EXCEPTION
1364 throw NurbsInputError(ub.n(),Q.n()) ;
1365 #else
1366 Error err("leastSquares");
1367 err << "leastSquaresCurve\n" ;
1368 err << "ub size is different than Q's\n" ;
1369 err.fatal() ;
1370 #endif
1371 }
1372
1373 deg_ = degC ;
1374 U.resize(n+deg_+1) ;
1375
1376 // Changing the method to generate a U compare to the one
1377 // described by Piegl and Tiller in the NURBS book (eq 9.69)
1378
1379 U.reset(1.0) ;
1380 d = (T)(Q.n())/(T)(n) ;
1381 for(j=0;j<=deg_;++j)
1382 U[j] = 0 ;
1383
1384 for(j=1;j<n-deg_;++j){
1385 U[deg_+j] = 0.0 ;
1386 for(int k=j;k<j+deg_;++k){
1387 i = (int)(k*d) ;
1388 a = T(k*d)-T(i) ;
1389 int i2 = (int)((k-1)*d) ;
1390 U[deg_+j] += a*ub[i2]+(1-a)*ub[i] ;
1391 }
1392 U[deg_+j] /= deg_ ;
1393 }
1394
1395 return leastSquaresH(Q, degC, n, ub, U) ;
1396 }
1397
1398 /*!
1399 \brief A least squares curve approximation
1400
1401 \latexonly
1402 This routine solves the following problem: find the NURBS curve
1403 $C$ satisfying
1404 \begin{itemize}
1405 \item $Q_0 = C(0)$ and $Q_m = C(1)$
1406 \item the remaining $Q_k$ are approximated in the least squares
1407 sense, {\em i.e.}
1408 \[ \sum_{k=1}^{m-1} | Q_k-C(\bar{u}_k)|^2 \]
1409 in a minimum with respect to the $n$ variable $P_i$; the
1410 $\bar{u}$ are the precomputed parameter values.
1411 \end{itemize}
1412
1413 The resulting curve will generally not pass through $Q_k$ and
1414 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
1415
1416 \endlatexonly
1417 \htmlonly
1418 This routines generates a curve that approrimates the points in the
1419 least square sense, you can find more details in the LaTeX version.
1420 \endhtmlonly
1421
1422 For more details, see section 9.4.1 on page 491 of the NURBS
1423 book.
1424
1425 \param Q the vector of 3D points
1426 \param degC the degree of the curve
1427 \param n the number of control points in the new curve
1428 \param ub the knot coefficients
1429 \param knot the knot vector to use for the curve
1430
1431 \return 1 if succesfull, 0 it the number of points to approximate
1432 the curve with is too big compared to the number of points.
1433 \warning the variable curve \b must contain a valid knot vector.
1434 \author Philippe Lavoie
1435 \date 24 January 1997
1436 */
1437 template <class T, int D>
leastSquares(const Vector<Point_nD<T,D>> & Q,int degC,int n,const Vector<T> & ub,const Vector<T> & knot)1438 int NurbsCurve<T,D>::leastSquares(const Vector< Point_nD<T,D> >& Q, int degC, int n, const Vector<T>& ub, const Vector<T>& knot){
1439 int i,j,span;
1440 const int& m=Q.n() ;
1441
1442 if(ub.n() != Q.n()){
1443 #ifdef USE_EXCEPTION
1444 throw NurbsInputError(ub.n(),Q.n()) ;
1445 #else
1446 Error err("leastSquares");
1447 err << "leastSquaresCurve\n" ;
1448 err << "ub size is different than Q's\n" ;
1449 err.fatal();
1450 #endif
1451 }
1452
1453 if(knot.n() != n+degC+1){
1454 #ifdef USE_EXCEPTION
1455 throw NurbsSizeError(n,knot.n(),degC) ;
1456 #else
1457 Error err("leastSquares");
1458 err << "The knot vector supplied doesn't have the proper size.\n" ;
1459 err << "It should be n+degC+1 = " << n+degC+1 << " and it is " << knot.n() << endl ;
1460 err.fatal() ;
1461 #endif
1462 }
1463
1464 deg_ = degC ;
1465
1466 U = knot ;
1467
1468 P.resize(n) ;
1469
1470 Vector< Point_nD<T,D> > R(n),rk(m) ;
1471 Vector<T> funs(deg_+1) ;
1472 Matrix_DOUBLE N(m,n) ;
1473 R[0] = Q[0] ;
1474 R[n-1] = Q[m-1] ;
1475 N(0,0) = 1.0 ;
1476 N(m-1,n-1) = 1.0 ;
1477
1478 // Set up N
1479 N(0,0) = 1.0 ;
1480 N(m-1,n-1) = 1.0 ;
1481
1482 // for(i=1;i<m-1;i++){
1483 for(i=0;i<m;i++){
1484 span = findSpan(ub[i]) ;
1485 basisFuns(ub[i],span,funs);
1486 for(j=0;j<=deg_;++j){ // BOOO
1487 //if(span-deg_+j>0)
1488 N(i,span-deg_+j) = (double)funs[j] ;
1489 }
1490 rk[i] = Q[i]-N(i,0)*Q[0]-N(i,n-1)*Q[m-1] ;
1491
1492 }
1493
1494 // Set up R
1495 // for(i=1;i<n-1;i++){
1496 for(i=0;i<n;i++){
1497 R[i] = 0.0 ;
1498 // for(j=1;j<m-1;j++)
1499 for(j=0;j<m;j++)
1500 R[i] += N(j,i)*rk[j] ;
1501 if(R[i].x()*R[i].x()<1e-10 &&
1502 R[i].y()*R[i].y()<1e-10 &&
1503 R[i].z()*R[i].z()<1e-10)
1504 return 0 ;
1505 }
1506
1507 // Solve N^T*N*P = R
1508
1509 // must check for the case where we want a curve of degree 1 having
1510 // only 2 points.
1511 if(n-2>0){
1512 Matrix_DOUBLE X(n-2,D),B(n-2,D),Ns(m-2,n-2) ;
1513 for(i=0;i<B.rows();i++){
1514 for(j=0;j<D;j++)
1515 B(i,j) = (double)R[i+1].data[j] ;
1516 }
1517 Ns = N.get(1,1,m-2,n-2) ;
1518
1519 solve(transpose(Ns)*Ns,B,X) ;
1520
1521 for(i=0;i<X.rows();i++){
1522 for(j=0;j<X.cols();j++)
1523 P[i+1].data[j] = (T)X(i,j) ;
1524 P[i+1].w() = 1.0 ;
1525 }
1526 }
1527 P[0] = Q[0] ;
1528 P[n-1] = Q[m-1] ;
1529 return 1 ;
1530 }
1531
1532 /*!
1533 \brief A least squares curve approximation
1534
1535 \latexonly
1536 This routine solves the following problem: find the NURBS curve
1537 $C$ satisfying
1538 \begin{itemize}
1539 \item $Q_0 = C(0)$ and $Q_m = C(1)$
1540 \item the remaining $Q_k$ are approximated in the least squares
1541 sense, {\em i.e.}
1542 \[ \sum_{k=1}^{m-1} | Q_k-C(\bar{u}_k)|^2 \]
1543 in a minimum with respect to the $n$ variable $P_i$; the
1544 $\bar{u}$ are the precomputed parameter values.
1545 \end{itemize}
1546
1547 The resulting curve will generally not pass through $Q_k$ and
1548 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
1549
1550 \endlatexonly
1551 \htmlonly
1552 This routines generates a curve that approrimates the points in the
1553 least square sense, you can find more details in the LaTeX version.
1554 \endhtmlonly
1555
1556 For more details, see section 9.4.1 on page 491 of the NURBS
1557 book.
1558
1559 \param Q the vector of 4D points
1560 \param degC the degree of the curve
1561 \param n the number of control points in the new curve
1562 \param ub the knot coefficients
1563 \param knot the knot vector to use for the curve
1564
1565 \return 1 if succesfull, 0 it the number of points to approximate
1566 the curve with is too big compared to the number of points.
1567
1568 \warning the variable curve \b must contain a valid knot vector.
1569 \author Philippe Lavoie
1570 \date 24 January 1997
1571 */
1572 template <class T, int D>
leastSquaresH(const Vector<HPoint_nD<T,D>> & Q,int degC,int n,const Vector<T> & ub,const Vector<T> & knot)1573 int NurbsCurve<T,D>::leastSquaresH(const Vector< HPoint_nD<T,D> >& Q, int degC, int n, const Vector<T>& ub, const Vector<T>& knot){
1574 int i,j,span,m ;
1575
1576 m = Q.n() ;
1577
1578 if(ub.n() != Q.n()){
1579 #ifdef USE_EXCEPTION
1580 throw NurbsInputError(ub.n(),Q.n()) ;
1581 #else
1582 Error err("leastSquares");
1583 err << "leastSquaresCurve\n" ;
1584 err << "ub size is different than Q's\n" ;
1585 err.fatal();
1586 #endif
1587 }
1588
1589 if(knot.n() != n+degC+1){
1590 #ifdef USE_EXCEPTION
1591 throw NurbsSizeError(n,knot.n(),degC) ;
1592 #else
1593 Error err("leastSquares");
1594 err << "The knot vector supplied doesn't have the proper size.\n" ;
1595 err << "It should be n+degC+1 = " << n+degC+1 << " and it is " << knot.n() << endl ;
1596 err.fatal() ;
1597 #endif
1598 }
1599
1600 deg_ = degC ;
1601
1602 U = knot ;
1603
1604 P.resize(n) ;
1605
1606 Vector< HPoint_nD<T,D> > R(n),rk(m) ;
1607 Vector<T> funs(deg_+1) ;
1608 Matrix_DOUBLE N(m,n) ;
1609 R[0] = Q[0] ;
1610 R[n-1] = Q[m-1] ;
1611 N(0,0) = 1.0 ;
1612 N(m-1,n-1) = 1.0 ;
1613
1614 // Set up N
1615 N(0,0) = 1.0 ;
1616 N(m-1,n-1) = 1.0 ;
1617
1618 // for(i=1;i<m-1;i++){
1619 for(i=0;i<m;i++){
1620 span = findSpan(ub[i]) ;
1621 basisFuns(ub[i],span,funs);
1622 for(j=0;j<=deg_;j++){
1623 //if(span-deg_+j>0)
1624 N(i,span-deg_+j) = (double)funs[j] ;
1625 }
1626 rk[i] = Q[i]-N(i,0)*Q[0]-N(i,n-1)*Q[m-1] ;
1627
1628 }
1629
1630 // Set up R
1631 // for(i=1;i<n-1;i++){
1632 for(i=0;i<n;i++){
1633 R[i] = 0.0 ;
1634 // for(j=1;j<m-1;j++)
1635 for(j=0;j<m;j++)
1636 R[i] += N(j,i)*rk[j] ;
1637 if(R[i].x()*R[i].x()<1e-10 &&
1638 R[i].y()*R[i].y()<1e-10 &&
1639 R[i].z()*R[i].z()<1e-10)
1640 return 0 ;
1641 }
1642
1643 // Solve N^T*N*P = R
1644
1645 // must check for the case where we want a curve of degree 1 having
1646 // only 2 points.
1647 if(n-2>0){
1648 Matrix_DOUBLE X(n-2,D+1),B(n-2,D+1),Ns(m-2,n-2) ;
1649 for(i=0;i<B.rows();i++){
1650 for(j=0;j<D+1;j++)
1651 B(i,j) = (double)R[i+1].data[j] ;
1652 }
1653 Ns = N.get(1,1,m-2,n-2) ;
1654
1655 solve(transpose(Ns)*Ns,B,X) ;
1656
1657 for(i=0;i<X.rows();i++){
1658 for(j=0;j<X.cols();j++)
1659 P[i+1].data[j] = (T)X(i,j) ;
1660 P[i+1].w() = 1.0 ;
1661 }
1662 }
1663 P[0] = Q[0] ;
1664 P[n-1] = Q[m-1] ;
1665 return 1 ;
1666 }
1667
1668 /*!
1669 \brief Get the knot removal error bound for an internal knot
1670
1671 Get the knot removal error bound for an internal knot r
1672 (non-rational). For more information on the algorithm, see
1673 A9.8 from the Nurbs book on page 428.
1674
1675
1676 \param curve a NURBS curve
1677 \param r the index of the internal knot to check
1678 \param s the multiplicity of that knot
1679
1680 \return The maximum distance between the new curve and the old one
1681
1682 \author Philippe Lavoie
1683 \date 24 January, 1997
1684 */
1685 template <class T, int N>
1686 T NurbsCurve<T,N>::getRemovalBnd(int r, int s ) const{
1687 Vector< HPoint_nD<T,N> > temp(U.rows()) ;
1688 int ord = deg_+1 ;
1689 int last = r-s ;
1690 int first = r-deg_ ;
1691 int off ;
1692 int i,j,ii,jj ;
1693 T alfi,alfj ;
1694 T u ;
1695
1696 u = U[r] ;
1697
1698 off = first-1;
1699 temp[0] = P[off] ;
1700 temp[last+1-off] = P[last+1] ;
1701
1702 i=first ; j=last ;
1703 ii=1 ; jj=last-off ;
1704
1705 while(j-i>0){
1706 alfi = (u-U[i])/(U[i+ord]-U[i]) ;
1707 alfj = (u-U[j])/(U[j+ord]-U[j]) ;
1708 temp[ii] = (P[i]-(1.0-alfi)*temp[ii-1])/alfi ;
1709 temp[jj] = (P[j]-alfj*temp[jj+1])/(1.0-alfj) ;
1710 ++i ; ++ii ;
1711 --j ; --jj ;
1712 }
1713 if(j-i<0){
1714 return distance3D(temp[ii-1],temp[jj+1]) ;
1715 }
1716 else{
1717 alfi=(u-U[i])/(U[i+ord]-U[i]) ;
1718 return distance3D(P[i],alfi*temp[ii+1]+(1.0-alfi)*temp[ii-1]) ;
1719 }
1720
1721 }
1722
1723 /*!
1724 \brief Removes an internal knot from a curve.
1725 This is A5.8 on p185 from the NURB book modified to not check for
1726 tolerance before removing the knot.
1727
1728 \param r the knot to remove
1729 \param s the multiplicity of the knot
1730 \param num the number of times to try to remove the knot
1731
1732 \warning r \b must be an internal knot.
1733
1734 \author Philippe Lavoie
1735 \date 24 January 1997
1736 */
1737 template <class T, int N>
removeKnot(int r,int s,int num)1738 void NurbsCurve<T,N>::removeKnot(int r, int s, int num)
1739 {
1740 int m = U.n() ;
1741 int ord = deg_+1 ;
1742 int fout = (2*r-s-deg_)/2 ;
1743 int last = r-s ;
1744 int first = r-deg_ ;
1745 T alfi, alfj ;
1746 int i,j,k,ii,jj,off ;
1747 T u ;
1748
1749 Vector< HPoint_nD<T,N> > temp( 2*deg_+1 ) ;
1750
1751 u = U[r] ;
1752
1753 if(num<1){
1754 #ifdef USE_EXCEPTION
1755 throw NurbsInputError() ;
1756 #else
1757 Error err("removeKnot");
1758 err << "A knot can only be removed a positive number of times!\n" ;
1759 err << "num = " << num << endl ;
1760 err.fatal() ;
1761 #endif
1762 }
1763
1764 int t;
1765 for(t=0;t<num;++t){
1766 off = first-1 ;
1767 temp[0] = P[off] ;
1768 temp[last+1-off] = P[last+1] ;
1769 i = first; j = last ;
1770 ii = 1 ; jj = last-off ;
1771 while(j-i > t){
1772 alfi = (u-U[i])/(U[i+ord+t]-U[i]) ;
1773 alfj = (u-U[j-t])/(U[j+ord]-U[j-t]) ;
1774 temp[ii] = (P[i]-(1.0-alfi)*temp[ii-1])/alfi ;
1775 temp[jj] = (P[j]-alfj*temp[jj+1])/(1.0-alfj) ;
1776 ++i ; ++ii ;
1777 --j ; --jj ;
1778 }
1779 i = first ; j = last ;
1780 while(j-i>t){
1781 P[i] = temp[i-off] ;
1782 P[j] = temp[j-off] ;
1783 ++i; --j ;
1784 }
1785 --first ; ++last ;
1786 }
1787 if(t==0) {
1788 #ifdef USE_EXCEPTION
1789 throw NurbsError();
1790 #endif
1791 cerr << "Major error happening... t==0\n" ;
1792 return ;
1793 }
1794
1795 for(k=r+1; k<m ; ++k)
1796 U[k-t] = U[k] ;
1797 j = fout ; i=j ; // Pj thru Pi will be overwritten
1798 for(k=1; k<t; k++)
1799 if( (k%2) == 1)
1800 ++i ;
1801 else
1802 --j ;
1803 for(k=i+1; k<P.n() ; k++) {// Shift
1804 P[j++] = P[k] ;
1805 }
1806
1807 resize(P.n()-t,deg_) ;
1808
1809 return ;
1810 }
1811
1812
1813 /*!
1814 \brief Remove knots from a curve without exceeding an error bound
1815
1816 For more information about the algorithm, see A9.9 on p429 of the NURB book.
1817
1818 \param ub the knot coefficients
1819 \param ek the error after removing
1820
1821 \author Philippe Lavoie
1822 \date 24 January 1997
1823 */
1824 template <class T, int N>
removeKnotsBound(const Vector<T> & ub,Vector<T> & ek,T E)1825 void NurbsCurve<T,N>::removeKnotsBound(const Vector<T>& ub,
1826 Vector<T>& ek, T E){
1827 Vector<T> Br(U.n()) ;
1828 Vector_INT S(U.n()) ;
1829 Vector_INT Nl(U.n());
1830 Vector_INT Nr(U.n());
1831 Vector<T> NewError(ub.n()) ;
1832 Vector<T> temp(ub.n()) ;
1833 int i,BrMinI ;
1834 int r,s,Rstart,Rend,k ;
1835 T BrMin,u,Infinity=1e20 ;
1836
1837 if(ek.n() != ub.n()){
1838 #ifdef USE_EXCEPTION
1839 throw NurbsInputError(ek.n(),ub.n());
1840 #else
1841 Error err("removeKnotsBound");
1842 err << "Error in removeKnotsBoundCurve\n" ;
1843 err << "The size of ub and ek mismatch\n" ;
1844 err.fatal() ;
1845 #endif
1846 }
1847
1848 Br.reset(Infinity) ;
1849 S.reset(0) ;
1850 s = 1 ;
1851 for(i=deg_+1;i<P.n();i++){
1852 if(U[i]<U[i+1]){
1853 Br[i] = getRemovalBnd(i,s) ;
1854 S[i] = s ;
1855 s = 1 ;
1856 }
1857 else {
1858 Br[i] = Infinity ;
1859 S[i] = 1 ;
1860 s++ ;
1861 }
1862 }
1863
1864
1865 // This might NOT be ok
1866 Nl[0] = 0 ;
1867 Nr.reset(ub.n()-1) ;
1868 for(i=0;i<ub.n();i++){
1869 int span = findSpan(ub[i]);
1870 if(!Nl[span]){
1871 Nl[span] = i ;
1872 }
1873 if(i+1<ub.n())
1874 Nr[span] = i+1 ;
1875 }
1876
1877 // set up N
1878 while(1) {
1879 BrMinI = Br.minIndex() ;
1880 BrMin = Br[BrMinI] ;
1881
1882 if(BrMin == Infinity)
1883 break ;
1884
1885 s = S[BrMinI] ;
1886 r = BrMinI ;
1887
1888 // Verify the Error for the affected range
1889 Rstart = maximum(r-deg_,deg_+1) ;
1890 Rend = minimum(r+deg_-S[r+deg_]+1,P.n()-1) ;
1891 Rstart = Nl[Rstart] ;
1892 Rend = Nr[Rend] ;
1893
1894 int removable ;
1895 removable = 1 ;
1896 for(i=Rstart;i<=Rend ; i++){
1897 // Using eqn 9.81, 9.83 and A2.4 to compute NewError[i]
1898 T a ;
1899 s = S[r] ;
1900 if((deg_+s)%2){
1901 u = ub[i] ;
1902 k = (deg_+s+1)/2 ;
1903 a = U[r]-U[r-k+1] ;
1904 a /= U[r-k+deg_+2]-U[r-k+1] ;
1905 NewError[i] = (1.0-a)*Br[r] * basisFun(u,r-k+1);
1906 }
1907 else{
1908 u = ub[i] ;
1909 k = (deg_+s)/2 ;
1910 NewError[i] = Br[r] * basisFun(u,r-k);
1911 }
1912 temp[i] = NewError[i] + ek[i];
1913 if(temp[i]>E){
1914 removable = 0 ;
1915 Br[r] = Infinity ;
1916 break ;
1917 }
1918 }
1919 if(removable){
1920 // Remove the knot
1921 removeKnot(r,S[r],1) ;
1922
1923 // update the error
1924 for(i=Rstart; i<=Rend ; i++)
1925 ek[i] = temp[i] ;
1926
1927 // Break if there is no more interior knot
1928 if(P.n()<=deg_+1){
1929 break ;
1930 }
1931
1932 // Update the new index range for some of the knots
1933 Rstart = Nl[r-deg_-1] ;
1934 Rend = Nr[r-S[r]] ;
1935 int span, oldspan ;
1936 oldspan = -1 ;
1937 for(k=Rstart;k<=Rend;k++){
1938 span = findSpan(ub[k]);
1939 if(span != oldspan){
1940 Nl[span] = k ;
1941 }
1942 if(k+1<ub.n())
1943 Nr[span] = k+1 ;
1944 oldspan = span ;
1945 }
1946 for(k=r-S[r]+1;k<Nl.n()-1;k++){
1947 Nl[k] = Nl[k+1] ;
1948 Nr[k] = Nr[k+1] ;
1949 }
1950 Nl.resize(Nl.n()-1) ; // Shrink Nl
1951 Nr.resize(Nr.n()-1) ; // Shrink Nr
1952 // Update the new error bounds for some of the knots
1953 Rstart = maximum(r-deg_,deg_+1) ;
1954 Rend = minimum(r+deg_-S[r]+1,P.n()) ;
1955 s = S[Rstart] ;
1956 for(i=Rstart;i<=Rend;i++){
1957 if(U[i]<U[i+1]){
1958 Br[i] = getRemovalBnd(i,s) ;
1959 S[i] = s ;
1960 s = 1;
1961 }
1962 else {
1963 Br[i] = Infinity ;
1964 S[i] = 1 ;
1965 s++ ;
1966 }
1967 }
1968 for(i=Rend+1;i<Br.n()-1;i++){
1969 Br[i] = Br[i+1] ;
1970 S[i] = S[i+1] ;
1971 }
1972 Br.resize(Br.n()-1) ; // Shrink Br
1973 }
1974 else{
1975 Br[r] = Infinity ;
1976 }
1977
1978 }
1979
1980 }
1981
1982 /*!
1983 \relates NurbsCurve
1984 \brief chord length parameterization
1985
1986 Performs chord length parameterization from a vector of points.
1987 \latexonly
1988 The chord length parameterization works as follows:
1989 \begin{itemize}
1990 \item The total chord length is defined as
1991 \begin{equation} d = \sum_{k=1}^{n-1} | Q_k-Q_{k-1}|
1992 \end{equation}
1993 \item $\bar{u}_0 = 0$ and $\bar{u}_{n-1} = 1$
1994 \item \begin{equation}
1995 \bar{u}_k = \bar{u}_{k-1}+\frac{| Q_k-Q_{k-1}| }{d}
1996 \hspace{0.5in} k=1,\ldots,n-2
1997 \end{equation}
1998 \end{itemize}
1999 Where $n$ is the size of the vector $Q$.
2000 \endlatexonly
2001 \htmlonly
2002 There are more details about this function in the LaTeX version.
2003 \endhtmlonly
2004
2005 \param Q a vector of 3D points
2006 \param ub the result of chord length parameterization
2007 \return the total chord length of the points.
2008
2009 \author Philippe Lavoie
2010 \date 24 January, 1997
2011 */
2012 template <class T, int N>
2013 T chordLengthParam(const Vector< Point_nD<T,N> >& Q, Vector<T> &ub){
2014 int i ;
2015 T d = T(0);
2016
2017 ub.resize(Q.n()) ;
2018 ub[0] = 0 ;
2019 for(i=1;i<ub.n();i++){
2020 d += norm(Q[i]-Q[i-1]) ;
2021 }
2022 if(d>0){
2023 for(i=1;i<ub.n()-1;++i){
2024 ub[i] = ub[i-1] + norm(Q[i]-Q[i-1])/d ;
2025 }
2026 ub[ub.n()-1] = 1.0 ; // In case there is some addition round-off
2027 }
2028 else{
2029 for(i=1;i<ub.n()-1;++i)
2030 ub[i] = T(i)/T(ub.n()-1) ;
2031 ub[ub.n()-1] = 1.0 ;
2032 }
2033 return d ;
2034 }
2035
2036 /*!
2037 \brief chord length parameterization
2038 \relates NurbsCurve
2039
2040 Performs chord length parameterization from a vector of points.
2041 \latexonly
2042 The chord length parameterization works as follows:
2043 \begin{itemize}
2044 \item The total chord length is defined as
2045 \begin{equation} d = \sum_{k=1}^{n-1} | Q_k-Q_{k-1}|
2046 \end{equation}
2047 \item $\bar{u}_0 = 0$ and $\bar{u}_{n-1} = 1$
2048 \item \begin{equation}
2049 \bar{u}_k = \bar{u}_{k-1}+\frac{| Q_k-Q_{k-1}| }{d}
2050 \hspace{0.5in} k=1,\ldots,n-2
2051 \end{equation}
2052 \end{itemize}
2053 Where $n$ is the size of the vector $Q$.
2054 \endlatexonly
2055 \htmlonly
2056 There is more information about this function in the LaTeX version.
2057 \endhtmlonly
2058
2059 \param Q a vector of 4D points
2060 \param ub the result of chord length parameterization
2061
2062 \return the total chord length of the points.
2063 \author Philippe Lavoie
2064 \date 24 January, 1997
2065 */
2066 template <class T, int N>
2067 T chordLengthParamH(const Vector< HPoint_nD<T,N> >& Q, Vector<T> &ub){
2068 int i ;
2069 T d = 0.0 ;
2070
2071 ub.resize(Q.n()) ;
2072 ub[0] = 0 ;
2073 for(i=1;i<ub.n();i++){
2074 d += norm(Q[i]-Q[i-1]) ;
2075 }
2076 for(i=1;i<ub.n()-1;i++){
2077 ub[i] = ub[i-1] + norm(Q[i]-Q[i-1])/d ;
2078 }
2079 ub[ub.n()-1] = 1.0 ; // In case there is some addition round-off
2080 return d ;
2081 }
2082
2083 /*!
2084 \brief Approximation of a curve bounded to a certain error
2085
2086 It is a type II approximation: it starts with a lot of control
2087 points then tries to eliminate as much as it can as long as
2088 the curve stays within a certain error bound.
2089
2090 The method uses least squares fitting along with knot
2091 removal techniques. It is the algorithm A9.10 on p 431 of
2092 the NURBS book.
2093
2094 \param Q the points to approximate
2095 \param degree the degree of the approximation curve
2096 \param E the maximum error allowed
2097
2098 \author Philippe Lavoie
2099 \date 24 January 1997
2100 */
2101 template <class T, int N>
globalApproxErrBnd(Vector<Point_nD<T,N>> & Q,int degC,T E)2102 void NurbsCurve<T,N>::globalApproxErrBnd(Vector< Point_nD<T,N> >& Q, int degC, T E){
2103 Vector<T> ub(Q.n()) ;
2104 chordLengthParam(Q,ub) ;
2105
2106 globalApproxErrBnd(Q,ub,degC,E) ;
2107 }
2108
2109 /*!
2110 \brief Approximation of a curve bounded to a certain error
2111
2112 It is a type II approximation: it starts with a lot of control
2113 points then tries to eliminate as much as it can as long as
2114 the curve stays within a certain error bound.
2115
2116 The method uses least squares fitting along with knot
2117 removal techniques. It is the algorithm A9.10 on p 431 of
2118 the NURBS book.
2119
2120 \param Q the points to approximate
2121 \param ub the vector of parameters where the points are located
2122 \param degree the degree of the approximation curve
2123 \param E the maximum error allowed
2124
2125 \warning ub and Q must be of the same size
2126
2127 \author Philippe Lavoie
2128 \code 24 January 1997
2129 */
2130 template <class T, int N>
globalApproxErrBnd(Vector<Point_nD<T,N>> & Q,Vector<T> & ub,int degC,T E)2131 void NurbsCurve<T,N>::globalApproxErrBnd(Vector< Point_nD<T,N> >& Q, Vector<T>& ub, int degC, T E){
2132 Vector<T> ek(Q.n()) ;
2133 Vector<T> Uh(Q.n()) ;
2134 NurbsCurve<T,N> tcurve ;
2135 int i,j,degL ;
2136
2137 if(ub.n() != Q.n()){
2138 #ifdef USE_EXCEPTION
2139 throw NurbsInputError(ub.n(),Q.n()) ;
2140 #else
2141 Error err("globalApproxErrBnd");
2142 err << "The data vector and the parameter vectors are not of the same size!\n" ;
2143 err << "Q.n() = " << Q.n() << ", ub.n() = " << ub.n() << endl ;
2144 err.fatal() ;
2145 #endif
2146 }
2147
2148 resize(Q.n(),1) ;
2149
2150 // Initialize U
2151 deg_ = 1 ;
2152 for(i=0;i<ub.n();i++){
2153 U[i+deg_] = ub[i] ;
2154 }
2155 U[0] = 0 ;
2156 U[U.n()-1] = 1.0 ;
2157 // Initialize P
2158 for(i=0;i<P.n();i++)
2159 P[i] = Q[i] ;
2160
2161 for(degL=1; degL<=degC+1 ; degL++){
2162 removeKnotsBound(ub,ek,E) ;
2163
2164 if(degL==degC)
2165 break ;
2166
2167 if(degL<degC){
2168
2169 // Find the degree elevated knot vector
2170 Uh.resize(U.n()*2) ;
2171
2172 Uh[0] = U[0] ;
2173 j = 1 ;
2174 for(i=1;i<U.n();++i){
2175 if(U[i]>U[i-1])
2176 Uh[j++] = U[i-1] ;
2177 Uh[j++] = U[i] ;
2178 }
2179 Uh[j++] = U[U.n()-1] ;
2180 Uh.resize(j) ;
2181 tcurve = *this ;
2182 if(!leastSquares(Q,degL+1,Uh.n()-degL-1-1,ub,Uh)){
2183 *this = tcurve ;
2184 degreeElevate(1);
2185 }
2186 }
2187 else{
2188 tcurve = *this ;
2189 if(!leastSquares(Q,degL,P.n(),ub,U)){
2190 *this = tcurve ;
2191 }
2192 }
2193
2194
2195 // Project the points from curve to Q and update ek and ub
2196 // for(i=0;i<Q.n();i++){
2197 for(i=0;i<Q.n();i++){
2198 T u_i ;
2199 Point_nD<T,N> r_i ;
2200 projectTo(Q[i],ub[i],u_i,r_i) ;
2201 ek[i] = norm(r_i-Q[i]) ;
2202 ub[i] = u_i ;
2203 }
2204 }
2205 }
2206
2207 /*!
2208 \brief Approximation of a curve bounded to a certain error
2209
2210 The algorithm is quite simplistic but in some cases gives
2211 better result than globalApproxErrBnd (when the error allowed
2212 is low and the data is very close to each other).
2213
2214 This algorithm generates a first degree interpolation of the
2215 data points, degree elevates the curve by 1 degree recomputes
2216 the error around each points, removes the knots which are
2217 within a certain error range then repeats the process until
2218 the desired degree is reached.
2219
2220 \param Q the points to approximate
2221 \param degC the degree of the approximation curve
2222 \param E the maximum error allowed
2223
2224 \author Philippe Lavoie
2225 \date 24 January 1997
2226 */
2227 template <class T, int N>
globalApproxErrBnd2(Vector<Point_nD<T,N>> & Q,int degC,T E)2228 void NurbsCurve<T,N>::globalApproxErrBnd2(Vector< Point_nD<T,N> >& Q,
2229 int degC,
2230 T E){
2231 Vector<T> ub(Q.n()) ;
2232 Vector<T> ek(Q.n()) ;
2233 Vector<T> Uh(Q.n()) ;
2234 NurbsCurve<T,N> tcurve ;
2235 int i,degL ;
2236
2237 resize(Q.n(),1) ;
2238
2239 chordLengthParam(Q,ub) ;
2240
2241 // Initialize U
2242 deg_ = 1 ;
2243 for(i=0;i<ub.n();i++){
2244 U[i+deg_] = ub[i] ;
2245 }
2246 U[0] = 0 ;
2247 U[U.n()-1] = 1.0 ;
2248 // Initialize P
2249 for(i=0;i<P.n();i++)
2250 P[i] = Q[i] ;
2251
2252 for(degL=1; degL<degC ; degL++){
2253 degreeElevate(1);
2254
2255 // Project the points from curve to Q and update ek and ub
2256 // for(i=0;i<Q.n();i++){
2257 for(i=0;i<Q.n();i++){
2258 T u_i ;
2259 Point_nD<T,N> r_i ;
2260 projectTo(Q[i],ub[i],u_i,r_i) ;
2261 ek[i] = norm(r_i-Q[i]) ;
2262 ub[i] = u_i ;
2263 }
2264 removeKnotsBound(ub,ek,E) ;
2265 }
2266 }
2267
2268 /*!
2269 \brief Approximation of a curve bounded to a certain error
2270
2271 The algorithm is quite simplistic but in some cases gives
2272 better result than globalApproxErrBnd (when the error allowed
2273 is low and the data is very close to each other).
2274
2275 This algorithm generates a first degree interpolation of the
2276 data points, degree elevates it to the degree requested then
2277 removes all the control points which are within the error
2278 bound.
2279
2280 \param Q the points to approximate
2281 \param degC the degree of the approximation curve
2282 \param E the maximum error allowed
2283
2284 \author Philippe Lavoie
2285 \date 24 January 1997
2286 */
2287 template <class T, int N>
globalApproxErrBnd3(Vector<Point_nD<T,N>> & Q,int degC,T E)2288 void NurbsCurve<T,N>::globalApproxErrBnd3(Vector< Point_nD<T,N> >& Q,int degC,T E){
2289 //NurbsCurve<T,N> tCurve(1) ;
2290 Vector<T> ub(Q.n()) ;
2291 Vector<T> ek(Q.n()) ;
2292 int i ;
2293
2294 resize(Q.n(),1) ;
2295
2296 chordLengthParam(Q,ub) ;
2297
2298 // Initialize U
2299 deg_ = 1 ;
2300 for(i=0;i<ub.n();i++){
2301 U[i+deg_] = ub[i] ;
2302 }
2303 U[0] = 0 ;
2304 U[U.n()-1] = 1.0 ;
2305
2306 // Initialize P
2307 for(i=0;i<P.n();i++)
2308 P[i] = Q[i] ;
2309
2310 //removeKnotsBoundCurve(curve,ub,ek,E/10.0,curve) ;
2311 degreeElevate(degC-1) ;
2312 removeKnotsBound(ub,ek,E) ;
2313 }
2314
2315
2316 /*!
2317 \brief Approximation of a curve bounded to a certain error
2318
2319 The algorithm is quite simplistic but in some cases gives
2320 better result than globalApproxErrBnd (when the error allowed
2321 is low and the data is very close to each other).
2322
2323 This algorithm generates a first degree interpolation of the
2324 data points, degree elevates it to the degree requested then
2325 removes all the control points which are within the error
2326 bound.
2327
2328 \param Q the points to approximate
2329 \param ub the parameter vector
2330 \param degC the degree of the approximation curve
2331 \param E the maximum error allowed
2332
2333 \author Philippe Lavoie
2334 \date 24 January 1997
2335 */
2336 template <class T, int N>
globalApproxErrBnd3(Vector<Point_nD<T,N>> & Q,const Vector<T> & ub,int degC,T E)2337 void NurbsCurve<T,N>::globalApproxErrBnd3(Vector< Point_nD<T,N> >& Q,
2338 const Vector<T> &ub,
2339 int degC,
2340 T E){
2341 Vector<T> ek(Q.n()) ;
2342 int i ;
2343
2344 resize(Q.n(),1) ;
2345
2346 // Initialize U
2347 deg_ = 1 ;
2348 for(i=0;i<ub.n();i++){
2349 U[i+deg_] = ub[i] ;
2350 }
2351 U[0] = 0 ;
2352 U[U.n()-1] = 1.0 ;
2353
2354 // Initialize P
2355 for(i=0;i<P.n();i++)
2356 P[i] = Q[i] ;
2357
2358 degreeElevate(degC-1) ;
2359 removeKnotsBound(ub,ek,E) ;
2360 }
2361
2362
2363 /*!
2364 \brief project a point onto the curve
2365
2366 It finds the closest point in the curve to a point $p$.
2367 For more information, see A6.4 and A6.5 on p 231 of the
2368 NURBS book
2369
2370 \param p the point \a p being projected
2371 \param guess initial guess
2372 \param u the parametric value for which \a C(u) is closest to \a p.
2373 \param r the point at \a C(u)
2374 \param e1 the minimal error
2375 \param e2 the maximal error
2376 \param maxTry the maximum number of times to try before returning from the function
2377
2378 \author Philippe Lavoie
2379 \date 24 January 1997
2380 */
2381 template <class T, int N>
projectTo(const Point_nD<T,N> & p,T guess,T & u,Point_nD<T,N> & r,T e1,T e2,int maxTry) const2382 void NurbsCurve<T,N>::projectTo(const Point_nD<T,N>& p, T guess, T& u, Point_nD<T,N>& r, T e1, T e2,int maxTry) const{
2383 T un ;
2384 T c1, c2;
2385 Vector< Point_nD<T,N> > Cd ;
2386 Point_nD<T,N> c, cd,cdd ;
2387 int t = 0 ;
2388 u = guess ;
2389
2390 if(u<U[0]) u = U[0] ;
2391 if(u>U[U.n()-1]) u = U[U.n()-1] ;
2392
2393 while(1) {
2394 ++t ;
2395 if(t>maxTry){
2396 r = c ;
2397 return ;
2398 }
2399 c = this->pointAt(u) ;
2400 deriveAt(u,2,Cd) ;
2401 cd = Cd[1] ;
2402 cdd = Cd[2] ;
2403 c1 = norm2(c-p) ;
2404 if(c1<e1*e1){
2405 r = c ;
2406 return ;
2407 }
2408 c2 = norm((Point_nD<T,N>)(cd*(c-p))) ;
2409 //c2 *= c2 ;
2410 c2 /= norm(cd)*norm(c-p) ;
2411 //if(c2<e2*e2){
2412 if(c2<e2){
2413 r =c ;
2414 return ;
2415 }
2416
2417 un = u- cd*(c-p)/(cdd*(c-p)+norm2(cd)) ;
2418
2419 if(un<U[0]) un = U[0] ;
2420 if(un>U[U.n()-1]) un = U[U.n()-1] ;
2421
2422 if(norm2((un-u)*cd)<e1*e1){
2423 r = c ;
2424 return ;
2425 }
2426 u = un ;
2427 }
2428 }
2429
2430 /*!
2431 \brief degree elevate a curve a number of times
2432
2433 For more information, see A5.9 on p 206 of the NURBS book
2434
2435 \param t the number of times to increase the degree of the curve
2436
2437 \author Philippe Lavoie
2438 \date 24 January, 1997
2439 */
2440 template <class T, int N>
degreeElevate(int t)2441 void NurbsCurve<T,N>::degreeElevate(int t){
2442 if(t<=0){
2443 return ;
2444 }
2445
2446 NurbsCurve<T,N> c(*this) ;
2447
2448 int i,j,k ;
2449 int n = c.ctrlPnts().n()-1;
2450 int p = c.deg_ ;
2451 int m = n+p+1;
2452 int ph = p+t ;
2453 int ph2 = ph/2 ;
2454 Matrix<T> bezalfs(p+t+1,p+1) ; // coefficients for degree elevating the Bezier segment
2455 Vector< HPoint_nD<T,N> > bpts(p+1) ; // pth-degree Bezier control points of the current segment
2456 Vector< HPoint_nD<T,N> > ebpts(p+t+1) ; // (p+t)th-degree Bezier control points of the current segment
2457 Vector< HPoint_nD<T,N> > Nextbpts(p-1) ; // leftmost control points of the next Bezier segment
2458 Vector<T> alphas(p-1) ; // knot instertion alphas.
2459
2460 // Compute the binomial coefficients
2461 Matrix<T> Bin(ph+1,ph2+1) ;
2462 binomialCoef(Bin) ;
2463
2464 // Compute Bezier degree elevation coefficients
2465 T inv,mpi ;
2466 bezalfs(0,0) = bezalfs(ph,p) = 1.0 ;
2467 for(i=1;i<=ph2;i++){
2468 inv= 1.0/Bin(ph,i) ;
2469 mpi = minimum(p,i) ;
2470 for(j=maximum(0,i-t); j<=mpi; j++){
2471 bezalfs(i,j) = inv*Bin(p,j)*Bin(t,i-j) ;
2472 }
2473 }
2474
2475 for(i=ph2+1;i<ph ; i++){
2476 mpi = minimum(p,i) ;
2477 for(j=maximum(0,i-t); j<=mpi ; j++)
2478 bezalfs(i,j) = bezalfs(ph-i,p-j) ;
2479 }
2480
2481 resize(c.P.n()+c.P.n()*t,ph) ; // Allocate more control points than necessary
2482
2483 int mh = ph ;
2484 int kind = ph+1 ;
2485 T ua = c.U[0] ;
2486 T ub = 0.0 ;
2487 int r=-1 ;
2488 int oldr ;
2489 int a = p ;
2490 int b = p+1 ;
2491 int cind = 1 ;
2492 int rbz,lbz = 1 ;
2493 int mul,save,s;
2494 T alf;
2495 int first, last, kj ;
2496 T den,bet,gam,numer ;
2497
2498 P[0] = c.P[0] ;
2499 for(i=0; i <= ph ; i++){
2500 U[i] = ua ;
2501 }
2502
2503 // Initialize the first Bezier segment
2504
2505 for(i=0;i<=p ;i++)
2506 bpts[i] = c.P[i] ;
2507
2508 while(b<m){ // Big loop thru knot vector
2509 i=b ;
2510 while(b<m && c.U[b] >= c.U[b+1]) // for some odd reasons... == doesn't work
2511 b++ ;
2512 mul = b-i+1 ;
2513 mh += mul+t ;
2514 ub = c.U[b] ;
2515 oldr = r ;
2516 r = p-mul ;
2517 if(oldr>0)
2518 lbz = (oldr+2)/2 ;
2519 else
2520 lbz = 1 ;
2521 if(r>0)
2522 rbz = ph-(r+1)/2 ;
2523 else
2524 rbz = ph ;
2525 if(r>0){ // Insert knot to get Bezier segment
2526 numer = ub-ua ;
2527 for(k=p;k>mul;k--){
2528 alphas[k-mul-1] = numer/(c.U[a+k]-ua) ;
2529 }
2530 for(j=1;j<=r;j++){
2531 save = r-j ; s = mul+j ;
2532 for(k=p;k>=s;k--){
2533 bpts[k] = alphas[k-s] * bpts[k]+(1.0-alphas[k-s])*bpts[k-1] ;
2534 }
2535 Nextbpts[save] = bpts[p] ;
2536 }
2537 }
2538
2539 for(i=lbz;i<=ph;i++){ // Degree elevate Bezier, only the points lbz,...,ph are used
2540 ebpts[i] = 0.0 ;
2541 mpi = minimum(p,i) ;
2542 for(j=maximum(0,i-t); j<=mpi ; j++)
2543 ebpts[i] += bezalfs(i,j)*bpts[j] ;
2544 }
2545
2546 if(oldr>1){ // Must remove knot u=c.U[a] oldr times
2547 // if(oldr>2) // Alphas on the right do not change
2548 // alfj = (ua-U[kind-1])/(ub-U[kind-1]) ;
2549 first = kind-2 ; last = kind ;
2550 den = ub-ua ;
2551 bet = (ub-U[kind-1])/den ;
2552 for(int tr=1; tr<oldr; tr++){ // Knot removal loop
2553 i = first ; j = last ;
2554 kj = j-kind+1 ;
2555 while(j-i>tr){ // Loop and compute the new control points for one removal step
2556 if(i<cind){
2557 alf=(ub-U[i])/(ua-U[i]) ;
2558 P[i] = alf*P[i] + (1.0-alf)*P[i-1] ;
2559 }
2560 if( j>= lbz){
2561 if(j-tr <= kind-ph+oldr){
2562 gam = (ub-U[j-tr])/den ;
2563 ebpts[kj] = gam*ebpts[kj] + (1.0-gam)*ebpts[kj+1] ;
2564 }
2565 else{
2566 ebpts[kj] = bet*ebpts[kj]+(1.0-bet)*ebpts[kj+1] ;
2567 }
2568 }
2569 ++i ; --j; --kj ;
2570 }
2571 --first ; ++last ;
2572 }
2573 }
2574
2575 if(a!=p) // load the knot u=c.U[a]
2576 for(i=0;i<ph-oldr; i++){
2577 U[kind++] = ua ;
2578 }
2579 for(j=lbz; j<=rbz ; j++) { // load control points onto the curve
2580 P[cind++] = ebpts[j] ;
2581 }
2582
2583 if(b<m){ // Set up for next pass thru loop
2584 for(j=0;j<r;j++)
2585 bpts[j] = Nextbpts[j] ;
2586 for(j=r;j<=p;j++)
2587 bpts[j] = c.P[b-p+j] ;
2588 a=b ;
2589 b++ ;
2590 ua = ub ;
2591 }
2592 else{
2593 for(i=0;i<=ph;i++)
2594 U[kind+i] = ub ;
2595 }
2596 }
2597 resize(mh-ph,ph) ; // Resize to the proper number of control points
2598 }
2599
2600 /*!
2601 \brief It inserts a knot a number of times
2602
2603 It inserts the knot u, r times and generates the curve nc.
2604 For more information, see A5.1 on page 151 of the NURBS book
2605
2606 \param u the knot to insert
2607 \param r the number of times to insert \a u.
2608 \param nc the resulting NURBS curve
2609
2610 \return the number of knots inserted (0 if none)
2611
2612 \warning the routine throws a NurbsError if the u value is not inside
2613 the clamped range.
2614
2615 \author Philippe Lavoie
2616 \date 24 January 1997
2617 */
2618 template <class T, int N>
knotInsertion(T u,int r,NurbsCurve<T,N> & nc)2619 int NurbsCurve<T,N>::knotInsertion(T u, int r,NurbsCurve<T,N>& nc){
2620 // Compute k and s u = [ u_k , u_k+1) with u_k having multiplicity s
2621 int k=0,s=0 ;
2622 int i,j ;
2623 int p = deg_ ;
2624
2625 if(u<U[deg_] || u>U[P.n()]){
2626 #ifdef USE_EXCEPTION
2627 throw NurbsError();
2628 #else
2629 Error err("knotInsertion");
2630 err << "The parametric value isn't inside a valid range." ;
2631 err << "The valid range is between " << U[deg_] << " and " << U[P.n()] << endl ;
2632 err.fatal();
2633 #endif
2634 }
2635
2636 for(i=0;i<U.n();i++){
2637 if(U[i]>u) {
2638 k = i-1 ;
2639 break ;
2640 }
2641 }
2642
2643 if(u<=U[k]){
2644 s = 1 ;
2645 for(i=k;i>deg_;i--){
2646 if(U[i]<=U[i-1])
2647 s++ ;
2648 else
2649 break ;
2650 }
2651 }
2652 else{
2653 s=0 ;
2654 }
2655
2656 if((r+s)>p+1)
2657 r = p+1-s ;
2658
2659 if(r<=0)
2660 return 0 ;
2661
2662 nc.resize(P.n()+r,deg_) ;
2663
2664 // Load new knot vector
2665 for(i=0;i<=k;i++)
2666 nc.U[i] = U[i] ;
2667 for(i=1;i<=r;i++)
2668 nc.U[k+i] = u ;
2669 for(i=k+1;i<U.n(); i++)
2670 nc.U[i+r] = U[i] ;
2671
2672 // Save unaltered control points
2673 Vector< HPoint_nD<T,N> > R(p+1) ;
2674
2675 for(i=0; i<=k-p ; i++)
2676 nc.P[i] = P[i] ;
2677 for(i=k-s ; i< P.n() ; i++)
2678 nc.P[i+r] = P[i] ;
2679 for(i=0; i<=p-s; i++)
2680 R[i] = P[k-p+i] ;
2681
2682 // Insert the knot r times
2683 int L=0 ;
2684 T alpha ;
2685 for(j=1; j<=r ; j++){
2686 L = k-p+j ;
2687 for(i=0;i<=p-j-s;i++){
2688 alpha = (u-U[L+i])/(U[i+k+1]-U[L+i]) ;
2689 R[i] = alpha*R[i+1] + (1.0-alpha)*R[i] ;
2690 }
2691 nc.P[L] = R[0] ;
2692 if(p-j-s > 0)
2693 nc.P[k+r-j-s] = R[p-j-s] ;
2694 }
2695
2696 // Load remaining control points
2697 for(i=L+1; i<k-s ; i++){
2698 nc.P[i] = R[i-L] ;
2699 }
2700 return r ;
2701 }
2702
2703 /*!
2704 \brief Refine the curve knot vector
2705
2706 For more information, see A5.4 on page 164 of the NURBS book
2707
2708 \param X the new knots to insert in the knot vector
2709
2710 \author Philippe Lavoie
2711 \date 24 January, 1997
2712 */
2713 template <class T, int N>
refineKnotVector(const Vector<T> & X)2714 void NurbsCurve<T,N>::refineKnotVector(const Vector<T>& X){
2715 int n = P.n()-1 ;
2716 int p = deg_ ;
2717 int m = n+p+1 ;
2718 int a,b ;
2719 int r = X.n()-1 ;
2720
2721 NurbsCurve<T,N> c(*this) ;
2722
2723 resize(r+1+n+1,p) ;
2724
2725 a = c.findSpan(X[0]) ;
2726 b = c.findSpan(X[r]) ;
2727 ++b ;
2728 int j ;
2729 for(j=0; j<=a-p ; j++)
2730 P[j] = c.P[j] ;
2731 for(j = b-1 ; j<=n ; j++)
2732 P[j+r+1] = c.P[j] ;
2733 for(j=0; j<=a ; j++)
2734 U[j] = c.U[j] ;
2735 for(j=b+p ; j<=m ; j++)
2736 U[j+r+1] = c.U[j] ;
2737 int i = b+p-1 ;
2738 int k = b+p+r ;
2739 for(j=r; j>=0 ; j--){
2740 while(X[j] <= c.U[i] && i>a){
2741 P[k-p-1] = c.P[i-p-1] ;
2742 U[k] = c.U[i] ;
2743 --k ;
2744 --i ;
2745 }
2746 P[k-p-1] = P[k-p] ;
2747 for(int l=1; l<=p ; l++){
2748 int ind = k-p+l ;
2749 T alpha = U[k+l] - X[j] ;
2750 if(alpha==0.0)
2751 P[ind-1] = P[ind] ;
2752 else
2753 alpha /= U[k+l]-c.U[i-p+l] ;
2754 P[ind-1] = alpha*P[ind-1] + (1.0-alpha)*P[ind] ;
2755 }
2756 U[k] = X[j] ;
2757 --k ;
2758 }
2759 }
2760
2761 /*!
2762 \brief global curve interpolation with points in 3D
2763
2764 \param Q the 3D points to interpolate
2765 \param d the degree of the interpolation
2766
2767 \warning The number of points to interpolate must be greater than
2768 the degree specified for the curve.
2769 \author Philippe Lavoie
2770 \date 24 January, 1997
2771 */
2772 template <class T, int N>
globalInterp(const Vector<Point_nD<T,N>> & Q,int d)2773 void NurbsCurve<T,N>::globalInterp(const Vector< Point_nD<T,N> >& Q, int d){
2774 Vector<T> ub ;
2775 chordLengthParam(Q,ub) ;
2776 globalInterp(Q,ub,d) ;
2777 }
2778
2779 /*!
2780 \brief global curve interpolation with points in 3D
2781
2782 Global curve interpolation with points in 3D and with the parametric values
2783 specified.
2784
2785 \param Q the 3D points to interpolate
2786 \param ub the parametric values
2787 \param d the degree of the interpolation
2788
2789 \warning The number of points to interpolate must be greater than
2790 the degree specified for the curve.
2791 \author Philippe Lavoie
2792 \date 24 January, 1997
2793 */
2794 template <class T, int D>
globalInterp(const Vector<Point_nD<T,D>> & Q,const Vector<T> & ub,int d)2795 void NurbsCurve<T,D>::globalInterp(const Vector< Point_nD<T,D> >& Q, const Vector<T>& ub, int d){
2796 int i,j ;
2797
2798 if(d<=0){
2799 #ifdef USE_EXCEPTION
2800 throw NurbsInputError() ;
2801 #else
2802 Error err("globalInterp");
2803 err << "The degree specified is equal or smaller than 0\n" ;
2804 err << "deg = " << deg_ << endl ;
2805 err.fatal() ;
2806 #endif
2807 }
2808 if(d>=Q.n()){
2809 #ifdef USE_EXCEPTION
2810 throw NurbsInputError() ;
2811 #else
2812 Error err("globalInterp");
2813 err << "The degree specified is greater then Q.n()+1\n" ;
2814 err << "Q.n() = " << Q.n() << ", and deg = " << deg_ << endl ;
2815 err.warning() ;
2816 d = Q.n()-1 ;
2817 #endif
2818 }
2819
2820
2821 resize(Q.n(),d) ;
2822 Matrix_DOUBLE A(Q.n(),Q.n()) ;
2823
2824 knotAveraging(ub,d,U) ;
2825
2826 // Initialize the basis matrix A
2827 Vector<T> N(deg_+1) ;
2828
2829 for(i=1;i<Q.n()-1;i++){
2830 int span = findSpan(ub[i]);
2831 basisFuns(ub[i],span,N) ;
2832 for(j=0;j<=deg_;j++)
2833 A(i,span-deg_+j) = (double)N[j] ;
2834 }
2835 A(0,0) = 1.0 ;
2836 A(Q.n()-1,Q.n()-1) = 1.0 ;
2837
2838 // Init matrix for LSE
2839 Matrix_DOUBLE qq(Q.n(),D) ;
2840 Matrix_DOUBLE xx(Q.n(),D) ;
2841 for(i=0;i<Q.n();i++){
2842 const Point_nD<T,D>& qp = Q[i] ; // this makes the SGI compiler happy
2843 for(j=0; j<D;j++)
2844 qq(i,j) = (double)qp.data[j] ;
2845 }
2846
2847 solve(A,qq,xx) ;
2848
2849 // Store the data
2850 for(i=0;i<xx.rows();i++){
2851 for(j=0;j<D;j++)
2852 P[i].data[j] = (T)xx(i,j) ;
2853 P[i].w() = 1.0 ;
2854 }
2855
2856 }
2857
2858 /*!
2859 \brief global curve interpolation with the 1st derivatives of the points specified.
2860
2861 Global curve interpolation with the 1st degree derivative
2862 specified for each of the points to interpolate.
2863 This will generate a number of control points 2 times greater
2864 than the number of interpolation points.
2865
2866 If the derivative specified is of unit length, \e i.e. the
2867 tangent vectors are given, then the derivative at each point
2868 will be multiplied by the chord length. A second multiplicative
2869 factor can be specified to make the derivative even greater.
2870 This second number should be close to 1.0.
2871
2872 For more information about the algorithm, refer to section
2873 9.2.4 on page 373 of the NURBS book.
2874
2875 \param Q the points to interpolate
2876 \param D the first derivative at these points
2877 \param d the degree of the curve
2878 \param unitD a flag specifying if the derivatives are unit vectors
2879 \param a a multiplicative factor for the tangent (must be greater
2880 than 0 or the function aborts).
2881
2882 \warning The number of points to interpolate must be greater than
2883 the degree specified for the curve.
2884 \author Philippe Lavoie
2885 \date 3 September, 1997
2886 */
2887 template <class T, int nD>
globalInterpD(const Vector<Point_nD<T,nD>> & Q,const Vector<Point_nD<T,nD>> & D,int d,int unitD,T a)2888 void NurbsCurve<T,nD>::globalInterpD(const Vector< Point_nD<T,nD> >& Q, const Vector< Point_nD<T,nD> >& D, int d, int unitD, T a){
2889 int i,j,n ;
2890
2891 if(d<=1){
2892 #ifdef USE_EXCEPTION
2893 throw NurbsInputError() ;
2894 #else
2895 Error err("globalInterpD");
2896 err << "The degree specified is equal or smaller than 1\n" ;
2897 err << "deg = " << deg_ << endl ;
2898 err.fatal() ;
2899 #endif
2900 }
2901 if(d>=Q.n()){
2902 #ifdef USE_EXCEPTION
2903 throw NurbsInputError() ;
2904 #else
2905 Error err("globalInterpD");
2906 err << "The degree specified is greater then Q.n()+1\n" ;
2907 err << "Q.n() = " << Q.n() << ", and deg = " << deg_ << endl ;
2908 err.warning() ;
2909 d = Q.n()-1 ;
2910 #endif
2911 }
2912 if(a<=0){
2913 #ifdef USE_EXCEPTION
2914 throw NurbsInputError() ;
2915 #else
2916 Error err("globalInterpD");
2917 err << "The a value must be greater than 0\n" ;
2918 err << "It is presently " << a << endl ;
2919 err.fatal() ;
2920 #endif
2921 }
2922 if(Q.n() != D.n()){
2923 #ifdef USE_EXCEPTION
2924 throw NurbsInputError(Q.n(),D.n()) ;
2925 #else
2926 Error err("globalInterpD") ;
2927 err << "The number of points to interpolate is different than\n the number of derivative points.\n" ;
2928 err << "Q.n() = " << Q.n() << ", D.n() = " << D.n() << endl ;
2929 err.fatal() ;
2930 #endif
2931 }
2932
2933 deg_ = d ;
2934 n = 2*Q.n() ;
2935
2936 resize(n,deg_) ;
2937
2938 Vector<T> ub(Q.n()) ;
2939
2940 T chordLength ;
2941
2942 chordLength = chordLengthParam(Q,ub) ;
2943
2944 if(unitD)
2945 chordLength *= a ;
2946
2947 // Setup up knot vector
2948 switch(deg_){
2949 case 2:
2950 {
2951 for(i=0;i<=deg_;++i){
2952 U[i] = T(0) ;
2953 U[U.n()-1-i] = T(1) ;
2954 }
2955 for(i=0;i<ub.n()-1;++i){
2956 U[2*i+deg_] = ub[i] ;
2957 U[2*i+deg_+1] = (ub[i]+ub[i+1])/T(2) ;
2958 }
2959 break ;
2960 }
2961 case 3:
2962 {
2963 for(i=0;i<=deg_;++i){
2964 U[i] = T(0) ;
2965 U[U.n()-1-i] = T(1) ;
2966 }
2967 for(i=1;i<ub.n()-1;++i){
2968 U[deg_+2*i] = (2*ub[i]+ub[i+1])/T(3) ;
2969 U[deg_+2*i+1] = (ub[i]+2*ub[i+1])/T(3) ;
2970 }
2971 U[4] = ub[1]/T(2);
2972 U[U.n()-deg_-2] = (ub[ub.n()-1]+T(1))/T(2) ;
2973 }
2974 default:
2975 {
2976 Vector<T> ub2(2*Q.n()) ;
2977 for(i=0;i<ub.n()-1;++i){
2978 ub2[2*i] = ub[i] ;
2979 ub2[2*i+1] = (ub[i]+ub[i+1])/2.0 ;
2980 }
2981
2982 ub2[ub2.n()-2] = (ub2[ub2.n()-1]+ub2[ub2.n()-3])/2.0 ;
2983 knotAveraging(ub2,deg_,U) ;
2984 }
2985 }
2986
2987 // Initialize the basis matrix A
2988 Matrix_DOUBLE A(n,n) ;
2989 Vector<T> N(deg_+1) ;
2990 Matrix<T> Nd(1,1) ;
2991
2992 for(i=1;i<Q.n()-1;i++){
2993 int span = findSpan(ub[i]);
2994 basisFuns(ub[i],span,N) ;
2995 dersBasisFuns(1,ub[i],span,Nd) ;
2996 for(j=0;j<=deg_;j++) {
2997 A(2*i,span-deg_+j) = (double)N[j] ;
2998 A(2*i+1,span-deg_+j) = (double)Nd(1,j) ;
2999 }
3000 }
3001 A(0,0) = 1.0 ;
3002 A(1,0) = -1.0 ;
3003 A(1,1) = 1.0 ;
3004 A(A.rows()-2,A.cols()-2) = -1.0 ;
3005 A(A.rows()-2,A.cols()-1) = 1.0 ;
3006 A(A.rows()-1,A.cols()-1) = 1.0 ;
3007
3008 // Init matrix for LSE
3009 Matrix_DOUBLE qq(n,nD) ;
3010 Matrix_DOUBLE xx(n,nD) ;
3011 for(i=0;i<Q.n();i++){
3012 const Point_nD<T,nD>& qp = Q[i] ; // this makes the SGI compiler happy
3013 const Point_nD<T,nD>& dp = D[i] ;
3014 for(j=0; j<nD;j++){
3015 qq(2*i,j) = (double)qp.data[j] ;
3016 qq(2*i+1,j) = (double)dp.data[j] ;
3017 if(unitD)
3018 qq(2*i+1,j) *= chordLength ;
3019 }
3020 }
3021
3022 T d0Factor = U[deg_+1]/deg_ ;
3023 T dnFactor = (1-U[U.n()-deg_-2])/deg_ ;
3024
3025 // the following three assignment must be made to make the SGI compiler
3026 // a happy compiler
3027 const Point_nD<T,nD>& dp0 = D[0] ;
3028 const Point_nD<T,nD>& dpn = D[D.n()-1] ;
3029 const Point_nD<T,nD>& qpn = Q[Q.n()-1] ;
3030 for(j=0;j<nD;++j){
3031 qq(1,j) = d0Factor*double(dp0.data[j]) ;
3032 qq(A.cols()-2,j) = dnFactor*double(dpn.data[j]) ;
3033 if(unitD){
3034 qq(1,j) *= chordLength ;
3035 qq(A.cols()-2,j) *= chordLength ;
3036 }
3037 qq(A.cols()-1,j) = double(qpn.data[j]) ;
3038 }
3039
3040 if(!solve(A,qq,xx)){
3041 #ifdef USE_EXCEPTION
3042 throw NurbsError();
3043 #else
3044 Error err("globablInterpD");
3045 err << "The matrix couldn't not be solved properly. There is no valid"
3046 " solutions for the input parameters you gave OR there is a "
3047 " computational error (using double float might solve the problem).";
3048 err.warning();
3049 #endif
3050 }
3051
3052 // Store the data
3053 for(i=0;i<xx.rows();i++){
3054 for(j=0;j<nD;j++)
3055 P[i].data[j] = (T)xx(i,j) ;
3056 P[i].w() = 1.0 ;
3057 }
3058
3059 P[0] = Q[0] ;
3060 P[P.n()-1] = Q[Q.n()-1] ;
3061 }
3062
3063 /*!
3064 \brief global curve interpolation with points in 4D
3065
3066 Global curve interpolation with points in 4D
3067
3068 \param Q the points in 4D to interpolate
3069 \param d the degree of the curve interpolation
3070
3071 \warning The number of points to interpolate must be greater than
3072 the degree specified for the curve.
3073 \author Philippe Lavoie
3074 \date 24 January, 1997
3075 */
3076 template <class T, int D>
globalInterpH(const Vector<HPoint_nD<T,D>> & Q,int d)3077 void NurbsCurve<T,D>::globalInterpH(const Vector< HPoint_nD<T,D> >& Q, int d){
3078 int i,j ;
3079
3080 resize(Q.n(),d) ;
3081 Matrix_DOUBLE A(Q.n(),Q.n()) ;
3082 Vector<T> ub(Q.n()) ;
3083
3084 chordLengthParamH(Q,ub) ;
3085
3086 // Setup the Knot Vector for the curve
3087 for(i=0;i<=deg_;i++)
3088 U[i] = 0 ;
3089 for(i=P.n(); i<U.n(); i++)
3090 U[i] = 1.0 ;
3091 for(j=1; j<Q.n()-deg_ ; j++){
3092 T t=0 ;
3093 for(i=j; i< j+deg_ ; i++)
3094 t += ub[i] ;
3095 U[j+deg_] = t/(T)deg_ ;
3096 }
3097
3098 // Initialize the basis matrix A
3099 Vector<T> N(deg_+1) ;
3100
3101 for(i=1;i<Q.n()-1;i++){
3102 int span = findSpan(ub[i]);
3103 basisFuns(ub[i],span,N) ;
3104 for(j=0;j<=deg_;j++)
3105 A(i,span-deg_+j) = (double)N[j] ;
3106 }
3107 A(0,0) = 1.0 ;
3108 A(Q.n()-1,Q.n()-1) = 1.0 ;
3109
3110 // Init matrix for LSE
3111 Matrix_DOUBLE qq(Q.n(),D+1) ;
3112 Matrix_DOUBLE xx(Q.n(),D+1) ;
3113 for(i=0;i<Q.n();i++)
3114 for(j=0; j<D+1;j++)
3115 qq(i,j) = (double)Q[i].data[j] ;
3116
3117 solve(A,qq,xx) ;
3118
3119 // Store the data
3120 for(i=0;i<xx.rows();i++){
3121 for(j=0;j<D+1;j++)
3122 P[i].data[j] = (T)xx(i,j) ;
3123 }
3124
3125 }
3126
3127 /*!
3128 \brief global curve interpolation with 4D points and a knot vector defined.
3129
3130 Global curve interpolation with 4D points and a knot vector
3131 defined.
3132
3133 \param Q the 3D points to interpolate
3134 \param Uc the knot vector to set the curve to
3135 \param d the degree of the interpolation
3136
3137 \warning The number of points to interpolate must be greater than
3138 the degree specified for the curve.
3139 \author Philippe Lavoie
3140 \date 24 January, 1997
3141 */
3142 template <class T, int D>
globalInterpH(const Vector<HPoint_nD<T,D>> & Q,const Vector<T> & Uc,int d)3143 void NurbsCurve<T,D>::globalInterpH(const Vector< HPoint_nD<T,D> >& Q, const Vector<T>& Uc, int d){
3144 int i,j ;
3145
3146 resize(Q.n(),d) ;
3147 Matrix_DOUBLE A(Q.n(),Q.n()) ;
3148 Vector<T> ub(Q.n()) ;
3149
3150 if(Uc.n() != U.n()){
3151 #ifdef USE_EXCEPTION
3152 throw NurbsInputError(Uc.n(),U.n()) ;
3153 #else
3154 Error err("globalInterp");
3155 err << "Invalid dimension for the given Knot vector.\n" ;
3156 err << "U required = " << U.n() << ", U given = " << Uc.n() << endl ;
3157 err.fatal() ;
3158 #endif
3159 }
3160 U = Uc ;
3161 chordLengthParamH(Q,ub) ;
3162
3163 // Initialize the basis matrix A
3164 Vector<T> N(deg_+1) ;
3165
3166 for(i=1;i<Q.n()-1;i++){
3167 int span = findSpan(ub[i]);
3168 basisFuns(ub[i],span,N) ;
3169 for(j=0;j<=deg_;j++)
3170 A(i,span-deg_+j) = (double)N[j] ;
3171 }
3172 A(0,0) = 1.0 ;
3173 A(Q.n()-1,Q.n()-1) = 1.0 ;
3174
3175 // Init matrix for LSE
3176 Matrix_DOUBLE qq(Q.n(),D+1) ;
3177 Matrix_DOUBLE xx(Q.n(),D+1) ;
3178 for(i=0;i<Q.n();i++)
3179 for(j=0; j<D+1;j++)
3180 qq(i,j) = (double)Q[i].data[j] ;
3181
3182 solve(A,qq,xx) ;
3183
3184 // Store the data
3185 for(i=0;i<xx.rows();i++){
3186 for(j=0;j<D+1;j++)
3187 P[i].data[j] = (T)xx(i,j) ;
3188 }
3189
3190 }
3191
3192 /*!
3193 \brief global curve interpolation with 4D points,
3194 a knot vector defined and the parametric value
3195 vector defined.
3196
3197 Global curve interpolation with 4D points, a knot vector
3198 defined and the parametric value vector defined.
3199
3200 \param Q the 3D points to interpolate
3201 \param ub the parametric values vector
3202 \param Uc the knot vector to set the curve to
3203 \param d the degree of the interpolation
3204
3205 \warning The number of points to interpolate must be greater than
3206 the degree specified for the curve. Uc must be compatible with
3207 the values given for Q.n(), ub.n() and d.
3208 \author Philippe Lavoie
3209 \date 24 January, 1997
3210 */
3211 template <class T, int D>
globalInterpH(const Vector<HPoint_nD<T,D>> & Q,const Vector<T> & ub,const Vector<T> & Uc,int d)3212 void NurbsCurve<T,D>::globalInterpH(const Vector< HPoint_nD<T,D> >& Q, const Vector<T>& ub, const Vector<T>& Uc, int d){
3213 int i,j ;
3214
3215 resize(Q.n(),d) ;
3216 Matrix_DOUBLE A(Q.n(),Q.n()) ;
3217
3218 if(Uc.n() != U.n()){
3219 #ifdef USE_EXCEPTION
3220 throw NurbsInputError(Uc.n(),U.n()) ;
3221 #else
3222 Error err("globalInterp");
3223 err << "Invalid dimension for the given Knot vector.\n" ;
3224 err << "U required = " << U.n() << ", U given = " << Uc.n() << endl ;
3225 err.fatal() ;
3226 #endif
3227 }
3228 U = Uc ;
3229
3230 // Initialize the basis matrix A
3231 Vector<T> N(deg_+1) ;
3232
3233 for(i=1;i<Q.n()-1;i++){
3234 int span = findSpan(ub[i]);
3235 basisFuns(ub[i],span,N) ;
3236 for(j=0;j<=deg_;j++)
3237 A(i,span-deg_+j) = (double)N[j] ;
3238 }
3239 A(0,0) = 1.0 ;
3240 A(Q.n()-1,Q.n()-1) = 1.0 ;
3241
3242 // Init matrix for LSE
3243 Matrix_DOUBLE qq(Q.n(),D+1) ;
3244 Matrix_DOUBLE xx(Q.n(),D+1) ;
3245 for(i=0;i<Q.n();i++)
3246 for(j=0; j<D+1;j++)
3247 qq(i,j) = (double)Q[i].data[j] ;
3248
3249 solve(A,qq,xx) ;
3250
3251 // Store the data
3252 for(i=0;i<xx.rows();i++){
3253 for(j=0;j<D+1;j++)
3254 P[i].data[j] = (T)xx(i,j) ;
3255 }
3256
3257 }
3258
3259 template <class T, int N>
pow2(T a)3260 inline T pow2(T a){
3261 return a*a ;
3262 }
3263
3264 /*!
3265 \brief Find the intersection point of two lines
3266
3267 This routines finds the intersection point of two lines.
3268 The algorithm generates a plane from one of the lines
3269 and finds the intersection point between this plane and the
3270 other line.
3271
3272 \param p1 a point in the first line
3273 \param t1 the tangent at p0t along the line 2
3274 \param p2 a point in the second line
3275 \param t2 the tangent at p1 along the line 2
3276 \param p the intersection point
3277
3278 \return 1 if the lines intersect, 0 if they don't.
3279
3280 \author Philippe Lavoie
3281 \date 25 July, 1997
3282 */
3283 template <class T, int N>
intersectLine(const Point_nD<T,N> & p1,const Point_nD<T,N> & t1,const Point_nD<T,N> & p2,const Point_nD<T,N> & t2,Point_nD<T,N> & p)3284 int intersectLine(const Point_nD<T,N>& p1, const Point_nD<T,N>& t1, const Point_nD<T,N>& p2, const Point_nD<T,N>& t2, Point_nD<T,N>& p){
3285 // a line is written like
3286 // L(t) = Q + u*t
3287 // u is the parametric value P is a point in the line and T is the tangent
3288 // a plane is of the form
3289 // (X-P).v = 0
3290 // where P is a point where the plane goes through and v is the normal to it
3291 // and X is (x,y,z)
3292 // solving for u
3293
3294 Point_nD<T,N> v,px ;
3295
3296 //px = crossProduct(t1,p1-p2) ;
3297 //v = crossProduct(px,t1) ;
3298 px = crossProduct(t1,t2) ;
3299 v = crossProduct(px,t1) ;
3300
3301 T t = (p1-p2)*v ;
3302 T vw = v*t2 ;
3303 if(to2power(vw)<1e-7)
3304 return 0 ;
3305 t /= vw ;
3306 p = p2+(((p1-p2)*v)/vw)*t2 ;
3307 return 1 ;
3308 }
3309
3310 #ifdef HAVE_TEMPLATE_OF_TEMPLATE
3311 template <class T>
intersectLine(const Point_nD<T,2> & p1,const Point_nD<T,2> & t1,const Point_nD<T,2> & p2,const Point_nD<T,2> & t2,Point_nD<T,2> & p)3312 int intersectLine(const Point_nD<T,2>& p1, const Point_nD<T,2>& t1, const Point_nD<T,2>& p2, const Point_nD<T,2>& t2, Point_nD<T,2>& p){
3313 cout << "PLEASE, DEFINE THIS FUNCTION\n" ;
3314
3315 return 1 ;
3316 }
3317 #else
3318
3319 #ifdef TEMPLATE_SPECIALIZATION
3320
3321 template <>
intersectLine(const Point_nD<float,2> & p1,const Point_nD<float,2> & t1,const Point_nD<float,2> & p2,const Point_nD<float,2> & t2,Point_nD<float,2> & p)3322 int intersectLine(const Point_nD<float,2>& p1, const Point_nD<float,2>& t1, const Point_nD<float,2>& p2, const Point_nD<float,2>& t2, Point_nD<float,2>& p){
3323 cout << "PLEASE, DEFINE THIS FUNCTION\n" ;
3324
3325 return 1 ;
3326 }
3327
3328 template <>
intersectLine(const Point_nD<double,2> & p1,const Point_nD<double,2> & t1,const Point_nD<double,2> & p2,const Point_nD<double,2> & t2,Point_nD<double,2> & p)3329 int intersectLine(const Point_nD<double,2>& p1, const Point_nD<double,2>& t1, const Point_nD<double,2>& p2, const Point_nD<double,2>& t2, Point_nD<double,2>& p){
3330 cout << "PLEASE, DEFINE THIS FUNCTION\n" ;
3331
3332 return 1 ;
3333 }
3334 #endif //TEMPLATE_SPECIALIZATION
3335
3336 #endif
3337
3338 /*!
3339 \brief generates a circular curve
3340
3341 Generates parts of a circle, starting at angle \a as and
3342 finishing at \a ae with a radius \a r and having the origin
3343 located at \a O. The \a X and \a Y vector describe the local
3344 x-axis and the local y-axis of the circle.
3345
3346 The degrees are specified in radians.
3347
3348 \param O the center of the circle
3349 \param X unit length vector lying in the plane of the circle
3350 \param Y unit length vector lying in the plane of the circle
3351 \param r the radius of the circle
3352 \param as start angle in radians measured with respect to $X$
3353 \param ae end angle in radians measured with respect to $X$
3354
3355 \author Philippe Lavoie
3356 \date 25 July, 1997
3357 */
3358 template <class T, int N>
makeCircle(const Point_nD<T,N> & O,const Point_nD<T,N> & X,const Point_nD<T,N> & Y,T r,double as,double ae)3359 void NurbsCurve<T,N>::makeCircle(const Point_nD<T,N>& O, const Point_nD<T,N>& X, const Point_nD<T,N>& Y, T r, double as, double ae){
3360 double theta,angle,dtheta ;
3361 int narcs ;
3362 while(ae<as)
3363 ae += 2*M_PI ;
3364 theta = ae-as ;
3365 if(theta<=M_PI/2.0)
3366 narcs = 1 ;
3367 else{
3368 if(theta<=M_PI)
3369 narcs = 2 ;
3370 else{
3371 if(theta<=1.5*M_PI)
3372 narcs = 3 ;
3373 else
3374 narcs = 4 ;
3375 }
3376 }
3377 dtheta = theta/(double)narcs ;
3378 int n = 2*narcs+1 ; // n control points ;
3379 double w1 = cos(dtheta/2.0) ; // dtheta/2.0 is base angle
3380
3381 Point_nD<T,N> P0,T0,P2,T2,P1 ;
3382 P0 = O + r*cos(as)*X + r*sin(as)*Y ;
3383 T0 = -sin(as)*X + cos(as)*Y ; // initialize start values
3384 resize(n,2) ;
3385
3386 P[0] = P0 ;
3387 int i ;
3388 int index = 0 ;
3389 angle = as ;
3390 for(i=1;i<=narcs;++i){
3391 angle += dtheta ;
3392 P2 = O+ r*cos(angle)*X + r*sin(angle)*Y ;
3393 P[index+2] = P2 ;
3394 T2 = -sin(angle)*X + cos(angle)*Y ;
3395 intersectLine(P0,T0,P2,T2,P1) ;
3396 P[index+1] = P1 ;
3397 P[index+1] *= w1 ;
3398 index += 2 ;
3399 if(i<narcs){
3400 P0 = P2 ;
3401 T0 = T2 ;
3402 }
3403 }
3404 int j = 2*narcs+1 ; // load the knot vector
3405 for(i=0;i<3;++i){
3406 U[i] = 0.0 ;
3407 U[i+j] = 1.0 ;
3408 }
3409 switch(narcs){
3410 case 1: break ;
3411 case 2:
3412 U[3] = U[4] = 0.5 ;
3413 break ;
3414 case 3:
3415 U[3] = U[4] = 1.0/3.0 ;
3416 U[5] = U[6] = 2.0/3.0 ;
3417 break ;
3418 case 4:
3419 U[3] = U[4] = 0.25 ;
3420 U[5] = U[6] = 0.50 ;
3421 U[7] = U[8] = 0.75 ;
3422 break ;
3423 }
3424 }
3425
3426 /*!
3427 \brief generates a circular curve
3428
3429 Generates a circular curve of radius $r$ at origin $O$. The
3430 curve is drawn in the $xy$-axis.
3431
3432 \param O the center of the circle
3433 \param r the radius of the circle
3434 \param as start angle measured with respect to the x-axis
3435 \param ae end angle measured with respect to the y-axis
3436
3437 \author Philippe Lavoie
3438 \date 25 July, 1997
3439 */
3440 template <class T, int N>
makeCircle(const Point_nD<T,N> & O,T r,double as,double ae)3441 void NurbsCurve<T,N>::makeCircle(const Point_nD<T,N>& O, T r, double as, double ae){
3442 makeCircle(O,Point_nD<T,N>(1,0,0),Point_nD<T,N>(0,1,0),r,as,ae) ;
3443 }
3444
3445 /*!
3446 \brief generates a circular curve
3447
3448 Generates a circle of radius $r$ at origin $O$. The
3449 curve is drawn in the $xy$-axis.
3450
3451 \param O the center of the circle
3452 \param r the radius of the circle
3453
3454 \author Philippe Lavoie
3455 \date 3 May, 1999
3456 */
3457 template <class T, int N>
makeCircle(const Point_nD<T,N> & O,T r)3458 void NurbsCurve<T,N>::makeCircle(const Point_nD<T,N>& O, T r){
3459 resize(9,2);
3460 U[0] = U[1] = U[2] = 0 ;
3461 U[3] = U[4] = 0.25 ;
3462 U[5] = U[6] = 0.50 ;
3463 U[7] = U[8] = 0.75 ;
3464 U[9] = U[10] = U[11] = 1 ;
3465
3466
3467 const T wm = T(0.707106781185) ; // sqrt(2)/2
3468
3469 P[0] = HPoint_nD<T,N>(r,0,0,1) ;
3470 P[1] = HPoint_nD<T,N>(r*wm,r*wm,0,wm) ;
3471 P[2] = HPoint_nD<T,N>(0,r,0,1) ;
3472 P[3] = HPoint_nD<T,N>(-r*wm,r*wm,0,wm) ;
3473 P[4] = HPoint_nD<T,N>(-r,0,0,1) ;
3474 P[5] = HPoint_nD<T,N>(-r*wm,-r*wm,0,wm) ;
3475 P[6] = HPoint_nD<T,N>(0,-r,0,1) ;
3476 P[7] = HPoint_nD<T,N>(r*wm,-r*wm,0,wm) ;
3477 P[8] = HPoint_nD<T,N>(r,0,0,1) ;
3478
3479 for(int i=8;i>=0;--i){
3480 P[i].x() += O.x() ;
3481 P[i].y() += O.y() ;
3482 P[i].z() += O.z() ;
3483 }
3484
3485
3486
3487 }
3488
3489 /*!
3490 \brief Finds the union of two knot vectors
3491 \relates NurbsCurve
3492
3493 Finds the union between two knot vectors
3494
3495 \param Ua knot vector A
3496 \param Ub knot vector B
3497
3498 \return the union of Ua and Ub
3499
3500 \warning The result is useless unless the knot vectors being compared are
3501 from a NURBS curve of a same degree
3502 \author Philippe Lavoie
3503 \date 24 January, 1997
3504 */
3505 template <class T>
knotUnion(const Vector<T> & Ua,const Vector<T> & Ub)3506 Vector<T> knotUnion(const Vector<T>& Ua, const Vector<T>& Ub) {
3507 Vector<T> U(Ua.n()+Ub.n()) ;
3508 int done = 0 ;
3509 int i,ia,ib ;
3510 T t ;
3511
3512 i = ia = ib = 0 ;
3513 while(!done){
3514 if(Ua[ia] == Ub[ib]){
3515 t = Ua[ia] ;
3516 ++ia ; ++ib ;
3517 }
3518 else{
3519 if(Ua[ia]<Ub[ib]){
3520 t = Ua[ia] ;
3521 ++ia ;
3522 }
3523 else{
3524 t = Ub[ib] ;
3525 ++ib ;
3526 }
3527 }
3528 U[i++] = t ;
3529 done = (ia>=Ua.n() || ib>=Ub.n()) ;
3530 }
3531 U.resize(i) ;
3532 return U ;
3533 }
3534
3535 /*!
3536 \brief Merges the knot vector of a curve with another knot vector
3537
3538 Will merge the Knot vector U with the one from the curve
3539 and it will refine the curve appropriately.
3540
3541 \param Um the knot vector to merge with
3542
3543 \warning the knot U must be common with the one from the curve c
3544 \author Philippe Lavoie
3545 \date 24 January, 1997
3546 */
3547 template <class T, int N>
mergeKnotVector(const Vector<T> & Um)3548 void NurbsCurve<T,N>::mergeKnotVector(const Vector<T> &Um){
3549 int i,ia,ib ;
3550 // Find the knots to insert
3551 Vector<T> I(Um.n()) ;
3552 int done = 0 ;
3553 i = ia = ib = 0 ;
3554 while(!done) {
3555 if(Um[ib] == U[ia]){
3556 ++ib ; ++ia ;
3557 }
3558 else{
3559 I[i++] = Um[ib] ;
3560 ib++ ;
3561 }
3562 done = (ia>=U.n() || ib >= Um.n()) ;
3563 }
3564 I.resize(i) ;
3565
3566 if(I.n()>0){
3567 // Refine the curve
3568 refineKnotVector(I) ;
3569 }
3570 }
3571
3572
3573 /*!
3574 \brief Generate compatible curves from an array of curves
3575 \relates NurbsCurveArray
3576
3577 This routine will put to the same degree all the curves in
3578 the array and it will ensure that they have the same knot
3579 vector.
3580
3581 \param ca the array of Nurbs curves
3582 \warning the knot vector of all the curves must be in the range [0,1]
3583 \author Philippe Lavoie
3584 \date 24 January, 1997
3585 */
3586 template <class T, int N>
generateCompatibleCurves(NurbsCurveArray<T,N> & ca)3587 void generateCompatibleCurves(NurbsCurveArray<T,N> &ca){
3588 int i;
3589 NurbsCurve<T,N> tc ;
3590
3591 if(ca.n()<=1) // Nothing to do... only 1 curve in the array
3592 return ;
3593
3594 // Increase all the curves to the highest degree
3595 int p = 1 ;
3596 for(i=0;i<ca.n();i++)
3597 if(p<ca[i].deg_)
3598 p = ca[i].deg_ ;
3599 for(i=0;i<ca.n();i++){
3600 ca[i].degreeElevate(p-ca[i].deg_);
3601 }
3602
3603 // Find a common Knot vector
3604 Vector<T> Uc(ca[0].U) ;
3605 for(i=1;i<ca.n();i++){
3606 Uc = knotUnion(Uc,ca[i].U) ;
3607 }
3608
3609 // Refine the knot vectors
3610 for(i=0;i<ca.n();i++)
3611 ca[i].mergeKnotVector(Uc) ;
3612 }
3613
3614 /*!
3615 \brief reads a NurbsCurve<T,N> from a file
3616
3617 \param fin an input file stream
3618
3619 \return 0 if an error occurs, 1 otherwise
3620
3621 \author Philippe Lavoie
3622 \date 24 January 1997
3623 */
3624 template <class T, int N>
read(ifstream & fin)3625 int NurbsCurve<T,N>::read(ifstream &fin){
3626 if(!fin) {
3627 return 0 ;
3628 }
3629 int np,d;
3630 char *type ;
3631 type = new char[3] ;
3632 if(!fin.read(type,sizeof(char)*3)) { delete []type ; return 0 ;}
3633 int r1 = strncmp(type,"nc3",3) ;
3634 int r2 = strncmp(type,"nc4",3) ;
3635 if(!(r1==0 || r2==0)) {
3636 delete []type ;
3637 return 0 ;
3638 }
3639 int st ;
3640 char stc ;
3641 if(!fin.read((char*)&stc,sizeof(char))) { delete []type ; return 0 ;}
3642 if(!fin.read((char*)&np,sizeof(int))) { delete []type ; return 0 ;}
3643 if(!fin.read((char*)&d,sizeof(int))) { delete []type ; return 0 ;}
3644
3645 st = stc - '0' ;
3646 if(st != sizeof(T)){ // not of the same type size
3647 delete []type ;
3648 return 0 ;
3649 }
3650
3651 resize(np,d) ;
3652
3653 if(!fin.read((char*)U.memory(),sizeof(T)*U.n())) { delete []type ; return 0 ;}
3654
3655
3656 T *p,*p2 ;
3657 if(!r1){
3658 p = new T[3*np] ;
3659 if(!fin.read((char*)p,sizeof(T)*3*np)) { delete []type ; return 0 ;}
3660 p2 = p ;
3661 for(int i=0;i<np;i++){
3662 P[i].x() = *(p++) ;
3663 P[i].y() = *(p++) ;
3664 P[i].z() = *(p++) ;
3665 P[i].w() = 1.0 ;
3666 }
3667 delete []p2 ;
3668 }
3669 else{
3670 p = new T[4*np] ;
3671 if(!fin.read((char*)p,sizeof(T)*4*np)) { delete []type ; return 0 ;}
3672 p2 = p ;
3673 for(int i=0;i<np;i++){
3674 P[i].x() = *(p++) ;
3675 P[i].y() = *(p++) ;
3676 P[i].z() = *(p++) ;
3677 P[i].w() = *(p++) ;
3678 }
3679 delete []p2 ;
3680 }
3681
3682 delete []type ;
3683 return 1 ;
3684 }
3685
3686 /*!
3687 \brief Reads a NurbsCurve<T,N> from a file.
3688
3689 \param filename the filename to read the curve from
3690 \return 0 if an error occurs, 1 otherwise
3691
3692 \author Philippe Lavoie
3693 \date 24 January 1997
3694 */
3695 template <class T, int N>
read(const char * filename)3696 int NurbsCurve<T,N>::read(const char* filename){
3697 ifstream fin(filename) ;
3698 if(!fin) {
3699 return 0 ;
3700 }
3701 return read(fin) ;
3702 }
3703
3704 /*!
3705 \brief Writes a NurbsCurve<T,N> to a file.
3706
3707 \param filename the filename to write to.
3708 \return 0 if an error occurs, 1 otherwise
3709
3710 \author Philippe Lavoie
3711 \date 24 January 1997
3712 */
3713 template <class T, int N>
write(const char * filename) const3714 int NurbsCurve<T,N>::write(const char* filename) const {
3715 ofstream fout(filename) ;
3716
3717 if(!fout)
3718 return 0 ;
3719 return write(fout) ;
3720 }
3721
3722 /*!
3723 \brief Writes a NurbsCurve<T,N> to an output stream.
3724
3725 \param fout the output stream
3726 \return 0 if an error occurs, 1 otherwise
3727
3728 \author Philippe Lavoie
3729 \date 24 January 1997
3730 */
3731 template <class T, int N>
write(ofstream & fout) const3732 int NurbsCurve<T,N>::write(ofstream &fout) const {
3733 if(!fout)
3734 return 0 ;
3735 int pn = P.n() ;
3736 char st = '0' + sizeof(T) ;
3737 if(!fout.write((char*)&"nc4",sizeof(char)*3)) return 0 ;
3738 if(!fout.write((char*)&st,sizeof(char))) return 0 ;
3739 if(!fout.write((char*)&pn,sizeof(int))) return 0 ;
3740 if(!fout.write((char*)°_,sizeof(int))) return 0 ;
3741 if(!fout.write((char*)U.memory(),sizeof(T)*U.n())) return 0 ;
3742
3743 T *p,*p2 ;
3744 p = new T[P.n()*4] ;
3745 p2 = p ;
3746 for(int i=0;i<P.n();i++) {
3747 *p = P[i].x() ; p++ ;
3748 *p = P[i].y() ; p++ ;
3749 *p = P[i].z() ; p++ ;
3750 *p = P[i].w() ; p++ ;
3751 }
3752 if(!fout.write((char*)p2,sizeof(T)*P.n()*4)) return 0 ;
3753 delete []p2 ;
3754 return 1 ;
3755 }
3756
3757 /*!
3758 \brief Computes the non-zero basis functions of the curve
3759 \relates NurbsCurve
3760
3761 Computes the non-zero basis functions and puts the result
3762 into N. N has a size of deg+1. To relate N to the basis
3763 functions, N_{all}[span -deg +i] = N[i] for i=0... deg.
3764
3765 \param u the parametric value
3766 \param span the non-zero span of the basis functions
3767 \param deg the degree of the curve
3768 \param U the knot vector on which the Basis functions are computed
3769 \param N the non-zero basis functions
3770
3771 \author Philippe Lavoie
3772 \date 24 January 1997
3773 */
3774 template <class T>
nurbsBasisFuns(T u,int span,int deg,const Vector<T> & U,Vector<T> & N)3775 void nurbsBasisFuns(T u, int span, int deg, const Vector<T>& U, Vector<T>& N) {
3776 //Vector<T> left(deg+1), right(deg+1) ;
3777 T* left = (T*) alloca(2*(deg+1)*sizeof(T)) ;
3778 T* right = &left[deg+1] ;
3779
3780 T temp,saved ;
3781
3782 N.resize(deg+1) ;
3783
3784 N[0] = 1.0 ;
3785 for(int j=1; j<= deg ; j++){
3786 left[j] = u-U[span+1-j] ;
3787 right[j] = U[span+j]-u ;
3788 saved = 0.0 ;
3789 for(int r=0 ; r<j; r++){
3790 temp = N[r]/(right[r+1]+left[j-r]) ;
3791 N[r] = saved+right[r+1] * temp ;
3792 saved = left[j-r] * temp ;
3793 }
3794 N[j] = saved ;
3795 }
3796
3797 }
3798
3799 /*!
3800 \brief Compute the derivatives functions at \a u of the NURBS curve
3801 \relates NurbsCurve
3802
3803 For information on the algorithm, see A2.3 on p72 of
3804 the NURBS book. The result is stored in the ders matrix, where
3805 ders is of size (n+1,deg+1) and the derivative
3806 C'(u) = ders(1,span-deg+j) where j=0...deg+1.
3807
3808 \param n the degree of the derivation
3809 \param u the parametric value
3810 \param span the span for the basis functions
3811 \param deg the degree of the curve
3812 \param U the knot vector on which the Basis functions must be computed.
3813 \param ders A matrix containing the derivatives of the curve.
3814
3815 \warning \a n, \a u and \a span must be in a valid range.
3816 \author Philippe Lavoie
3817 \date 24 January 1997
3818 */
3819 template <class T>
nurbsDersBasisFuns(int n,T u,int span,int deg,const Vector<T> & U,Matrix<T> & ders)3820 void nurbsDersBasisFuns(int n,T u, int span, int deg, const Vector<T>& U, Matrix<T>& ders) {
3821 //Vector<T> left(deg+1),right(deg+1) ;
3822 T* left = (T*) alloca(2*(deg+1)*sizeof(T)) ;
3823 T* right = &left[deg+1] ;
3824
3825 Matrix<T> ndu(deg+1,deg+1) ;
3826 T saved,temp ;
3827 int j,r ;
3828
3829 ders.resize(n+1,deg+1) ;
3830
3831 ndu(0,0) = 1.0 ;
3832 for(j=1; j<= deg ;j++){
3833 left[j] = u-U[span+1-j] ;
3834 right[j] = U[span+j]-u ;
3835 saved = 0.0 ;
3836
3837 for(r=0;r<j ; r++){
3838 // Lower triangle
3839 ndu(j,r) = right[r+1]+left[j-r] ;
3840 temp = ndu(r,j-1)/ndu(j,r) ;
3841 // Upper triangle
3842 ndu(r,j) = saved+right[r+1] * temp ;
3843 saved = left[j-r] * temp ;
3844 }
3845
3846 ndu(j,j) = saved ;
3847 }
3848
3849 for(j=0;j<=deg;j++)
3850 ders(0,j) = ndu(j,deg) ;
3851
3852 // Compute the derivatives
3853 Matrix<T> a(deg+1,deg+1) ;
3854 for(r=0;r<=deg;r++){
3855 int s1,s2 ;
3856 s1 = 0 ; s2 = 1 ; // alternate rows in array a
3857 a(0,0) = 1.0 ;
3858 // Compute the kth derivative
3859 for(int k=1;k<=n;k++){
3860 T d ;
3861 int rk,pk,j1,j2 ;
3862 d = 0.0 ;
3863 rk = r-k ; pk = deg-k ;
3864
3865 if(r>=k){
3866 a(s2,0) = a(s1,0)/ndu(pk+1,rk) ;
3867 d = a(s2,0)*ndu(rk,pk) ;
3868 }
3869
3870 if(rk>=-1){
3871 j1 = 1 ;
3872 }
3873 else{
3874 j1 = -rk ;
3875 }
3876
3877 if(r-1 <= pk){
3878 j2 = k-1 ;
3879 }
3880 else{
3881 j2 = deg-r ;
3882 }
3883
3884 for(j=j1;j<=j2;j++){
3885 a(s2,j) = (a(s1,j)-a(s1,j-1))/ndu(pk+1,rk+j) ;
3886 d += a(s2,j)*ndu(rk+j,pk) ;
3887 }
3888
3889 if(r<=pk){
3890 a(s2,k) = -a(s1,k-1)/ndu(pk+1,r) ;
3891 d += a(s2,k)*ndu(r,pk) ;
3892 }
3893 ders(k,r) = d ;
3894 j = s1 ; s1 = s2 ; s2 = j ; // Switch rows
3895 }
3896 }
3897
3898 // Multiply through by the correct factors
3899 r = deg ;
3900 for(int k=1;k<=n;k++){
3901 for(j=0;j<=deg;j++)
3902 ders(k,j) *= r ;
3903 r *= deg-k ;
3904 }
3905
3906 }
3907
3908
3909 /*!
3910 \brief Decompose the curve into B�zier segments
3911
3912 This function decomposes the curve into an array of 4D B�zier
3913 segments.
3914
3915 \param c an array of B�zier segments
3916
3917 \warning The end B�zier segments will not be valid if the NURBS curve
3918 is not clamped.
3919
3920 \author Philippe Lavoie
3921 \date 16 February 1997
3922 */
3923 template <class T, int N>
decompose(NurbsCurveArray<T,N> & c) const3924 void NurbsCurve<T,N>::decompose(NurbsCurveArray<T,N>& c) const {
3925 int i,m,a,b,nb,mult,j,r,save,s,k ;
3926 T numer,alpha ;
3927 T* alphas = (T*) alloca((deg_+1)*sizeof(T)) ; //Vector<T> alphas(deg+1) ;
3928
3929 // all the curves will have the same vector
3930 Vector<T> nU ;
3931 nU.resize(2*(deg_+1)) ;
3932 for(i=0;i<nU.n()/2;i++)
3933 nU[i] = 0 ;
3934 for(i=nU.n()/2;i<nU.n();i++)
3935 nU[i] = 1 ;
3936
3937 c.resize(P.n()-deg_) ;
3938 for(i=0;i<c.n();i++){
3939 c[i].resize(deg_+1,deg_) ;
3940 c[i].U = nU ;
3941 }
3942
3943 m = P.n()+deg_ ;
3944 a = deg_ ;
3945 b = deg_+1 ;
3946 nb = 0 ;
3947
3948 for(i=0;i<=deg_;i++)
3949 c[nb].P[i] = P[i] ;
3950 while(b<m){
3951 i = b ;
3952 while(b<m && U[b+1] <= U[b]) b++ ;
3953 mult = b-i+1 ;
3954 if(mult<deg_){
3955 numer = U[b]-U[a] ; // th enumerator of the alphas
3956 for(j=deg_;j>mult;j--) // compute and store the alphas
3957 alphas[j-mult-1] = numer/(U[a+j]-U[a]) ;
3958 r = deg_-mult ; // insert knot r times
3959 for(j=1;j<=r;j++){
3960 save=r-j;
3961 s=mult+j; // this many new points
3962 for(k=deg_;k>=s;k--){
3963 alpha = alphas[k-s] ;
3964 c[nb].P[k] = alpha*c[nb].P[k] + (1.0-alpha)*c[nb].P[k-1] ;
3965 }
3966 if(b<m) // control point of
3967 c[nb+1].P[save] = c[nb].P[deg_]; // next segment
3968 }
3969 }
3970 nb++ ;
3971 if(b<m){ // initialize for next segment
3972 for(i=deg_-mult; i<= deg_ ; i++)
3973 c[nb].P[i] = P[b-deg_+i];
3974 a=b ;
3975 b++ ;
3976 }
3977 }
3978 c.resize(nb) ;
3979
3980 }
3981
3982 template <class T, int N>
project2D(const HPoint_nD<T,N> & p)3983 inline Point_nD<T,N> project2D(const HPoint_nD<T,N>& p){
3984 Point_nD<T,N> pnt ;
3985 //if(p.z()==0.0){
3986 pnt.x() = p.x()/p.w() ;
3987 pnt.y() = p.y()/p.w() ;
3988 //}
3989 //else{
3990 //pnt.x() = (p.x()/p.w())/absolute(p.z()) ;
3991 //pnt.y() = (p.y()/p.w())/absolute(p.z()) ;
3992 //}
3993 return pnt ;
3994 }
3995
3996 const float offX = 50 ;
3997 const float offY = 70 ;
3998
3999 template <class T, int N>
movePsP(Point_nD<T,N> & p,T magFact)4000 inline void movePsP(Point_nD<T,N> &p, T magFact){
4001 p *= magFact ;
4002 p += Point_nD<T,N>(offX,offY,0) ;
4003 //p = p*magFact+Point_nD<T,N>(offX,offY,0) ;
4004 }
4005
4006 #ifdef HAVE_TEMPLATE_OF_TEMPLATE
4007 template <class T>
movePsP(Point_nD<T,2> & p,T magFact)4008 inline void movePsP(Point_nD<T,2> &p, T magFact){
4009 p *= magFact ;
4010 p += Point_nD<T,2>(offX,offY) ;
4011 //p = p*magFact+Point_nD<T,N>(offX,offY,0) ;
4012 }
4013 #else
4014
4015 template <>
movePsP(Point_nD<float,2> & p,float magFact)4016 inline void movePsP(Point_nD<float,2> &p, float magFact){
4017 p *= magFact ;
4018 p += Point_nD<float,2>(offX,offY) ;
4019 //p = p*magFact+Point_nD<T,N>(offX,offY,0) ;
4020 }
4021
4022 template <>
movePsP(Point_nD<double,2> & p,double magFact)4023 inline void movePsP(Point_nD<double,2> &p, double magFact){
4024 p *= magFact ;
4025 p += Point_nD<double,2>(offX,offY) ;
4026 //p = p*magFact+Point_nD<double,N>(offX,offY,0) ;
4027 }
4028
4029 #endif
4030
4031 /*!
4032 \brief Writes the curve in the postscript format to a file.
4033
4034 \param filename the file to write the postscript file to
4035 \param cp a flag indicating if the control points should be
4036 drawn, 0 = no and 1 = yes
4037 \param magFact a magnification factor, the 2D point of the control
4038 points will be magnified by this value. The size is
4039 measured in postscript points. If the magFact is
4040 set to a value smaller or equal to 0, than the
4041 program will try to guess a magnification factor
4042 such that the curve is large enough to fill the
4043 page.
4044 \param dash the size of the dash in postscript points . A size
4045 smaller or equal to 0 indicates that
4046 the line joining the control points is plain.
4047
4048 \return 0 if an error occurs, 1 otherwise
4049
4050 \warning If the weights of the curve are not all at 1, the result might
4051 not be representative of the true NURBS curve.
4052 \author Philippe Lavoie
4053 \date 16 February 1997
4054 */
4055 template <class T, int N>
writePS(const char * filename,int cp,T magFact,T dash,bool bOpen) const4056 int NurbsCurve<T,N>::writePS(const char* filename,int cp,T magFact, T dash, bool bOpen) const {
4057
4058 ofstream fout(filename) ;
4059
4060 if(!fout)
4061 return 0 ;
4062
4063 if(deg_<3){
4064 NurbsCurve<T,N> c3(*this) ;
4065 c3.degreeElevate(3-deg_) ;
4066 return c3.writePS(filename,cp,magFact,dash,bOpen) ;
4067 }
4068
4069 NurbsCurveArray<T,N> Ca ;
4070 if (bOpen)
4071 decompose(Ca) ;
4072 else
4073 decomposeClosed(Ca) ;
4074
4075 int guess =0 ;
4076 if(magFact<= T() ){
4077 magFact = T(1) ;
4078 guess = 1 ;
4079 }
4080
4081 Matrix< Point_nD<T,N> > pnts(Ca.n(),deg_+1) ;
4082 int i,j ;
4083
4084 //HPoint_nD<T,N> tp ;
4085
4086 for(i=0;i<Ca.n();i++){
4087 for(j=0;j<deg_+1;j++){
4088 pnts(i,j) = project2D(Ca[i].P[j]) ;
4089 }
4090 }
4091
4092 // find bounding box parameters
4093 T mx,my,Mx,My ;
4094 mx = Mx = pnts(0,0).x() ;
4095 my = My = pnts(0,0).y() ;
4096
4097 for(i=0;i<Ca.n();i++){
4098 Point_nD<T,N> p ;
4099 int step ;
4100 step = 8 ;
4101 for(j=0;j<=step;j++){
4102 T u ;
4103 u = (T)j/(T)step ;
4104 p = project2D(Ca[i](u)) ;
4105 if(p.x() < mx)
4106 mx = p.x() ;
4107 if(p.x() > Mx)
4108 Mx = p.x() ;
4109 if(p.y() < my)
4110 my = p.y() ;
4111 if(p.y() > My)
4112 My = p.y() ;
4113 }
4114 }
4115
4116 if(guess){
4117 //magFact = minimum((T)500/(T)(Mx-mx),(T)700/(T)(My-my)) ;
4118 }
4119
4120 mx = mx*magFact+offX;
4121 my = my*magFact+offY;
4122 Mx = Mx*magFact+offX;
4123 My = My*magFact+offY;
4124 for(i=0;i<Ca.n();i++)
4125 for(j=0;j<deg_+1;j++)
4126 movePsP(pnts(i,j),magFact) ;
4127
4128 fout << "%!PS-Adobe-2.1\n%%Title: " << filename << endl ;
4129 fout << "%%Creator: NurbsCurve<T,N>::writePS\n" ;
4130 fout << "%%BoundingBox: " << mx << ' ' << my << ' ' << Mx << ' ' << My << endl ;
4131 fout << "%%Pages: 0" << endl ;
4132 fout << "%%EndComments" << endl ;
4133 fout << "0 setlinewidth\n" ;
4134 fout << "0 setgray\n" ;
4135 fout << endl ;
4136
4137 fout << "newpath\n" ;
4138 fout << pnts(0,0).x() << ' ' << pnts(0,0).y() << " moveto\n" ;
4139 for(i=0;i<Ca.n();i++){
4140 for(j=1;j<deg_+1;j++){
4141 fout << pnts(i,j).x() << ' ' << pnts(i,j).y() << ' ' ;
4142 }
4143 fout << "curveto\n" ;
4144 }
4145 fout << "stroke\n" ;
4146
4147 if(cp>0){ // draw the control points of the original curve
4148 Vector< Point_nD<T,N> > pts(P.n()) ;
4149 for(i=0;i<P.n();i++){
4150 pts[i] = project2D(P[i]) ;
4151 movePsP(pts[i],magFact) ;
4152 fout << "newpath\n" ;
4153 fout << pts[i].x() << ' ' << pts[i].y() << " 3 0 360 arc\nfill\n" ;
4154 }
4155 if(dash>0)
4156 fout << "[" << dash << "] " << dash << " setdash\n" ;
4157 fout << "newpath\n" ;
4158
4159 fout << pts[0].x() << ' ' << pts[0].y() << " moveto\n" ;
4160 for(i=1;i<P.n();i++)
4161 fout << pts[i].x() << ' ' << pts[i].y() << " lineto\n" ;
4162 fout << "stroke\n" ;
4163 }
4164 else{
4165 if(cp<0){
4166 Vector< Point_nD<T,N> > pts(P.n()*Ca.n()) ;
4167 int k=0 ;
4168 for(i=0;i<Ca.n();i++)
4169 for(j=0;j<deg_+1;j++){
4170 pts[k] = project2D(Ca[i].P[j]) ;
4171 movePsP(pts[k],magFact) ;
4172 fout << "newpath\n" ;
4173 fout << pts[k].x() << ' ' << pts[k].y() << " 3 0 360 arc\nfill\n" ;
4174 k++ ;
4175 }
4176 if(dash>0)
4177 fout << "[" << dash << "] " << dash << " setdash\n" ;
4178 fout << "newpath\n" ;
4179
4180 fout << pts[0].x() << ' ' << pts[0].y() << " moveto\n" ;
4181 for(i=1;i<k;i++)
4182 fout << pts[i].x() << ' ' << pts[i].y() << " lineto\n" ;
4183 fout << "stroke\n" ;
4184 }
4185 }
4186
4187 fout << "showpage\n%%EOF\n" ;
4188 return 1 ;
4189 }
4190
4191 /*!
4192 \brief Writes a post-script file representing the curve
4193
4194 Writes the curve in the postscript format to a file, it also
4195 draws the points defined in $points$ with their associated
4196 vectors if $vector$ is used.
4197
4198 \param filename the file to write the postscript file to
4199 \param points draws these additional points as empty circles
4200 \param vectors specify a vector associated with the points
4201 (this can be an empty vector)
4202 \param cp a flag indicating if the control points should be
4203 drawn, 0 = no and 1 = yes
4204 \param magFact a magnification factor, the 2D point of the control
4205 points will be magnified by this value. The size
4206 is measured in postscript points. If the magFact
4207 is set to a value smaller or equal to 0, than the
4208 program will try to guess a magnification factor
4209 such that the curve is large enough to fill the
4210 page.
4211 \param dash the size of the dash in postscript points . A size
4212 smaller or equal to 0 indicates that
4213 the line joining the control points is plain.
4214
4215 \return 0 if an error occurs, 1 otherwise
4216
4217 \warning If the weights of the curve are not all at 1, the result might
4218 not be representative of the true NURBS curve. If vectors is
4219 used, then it must be of the same size as points. If a vector
4220 element is (0,0,0) it will not be drawn.
4221 \author Philippe Lavoie
4222 \date 16 February 1997
4223 */
4224 template <class T, int N>
writePSp(const char * filename,const Vector<Point_nD<T,N>> & points,const Vector<Point_nD<T,N>> & vectors,int cp,T magFact,T dash,bool bOpen) const4225 int NurbsCurve<T,N>::writePSp(const char* filename,const Vector< Point_nD<T,N> >& points, const Vector< Point_nD<T,N> >& vectors, int cp, T magFact, T dash, bool bOpen) const {
4226 ofstream fout(filename) ;
4227
4228 if(!fout)
4229 return 0 ;
4230
4231 if(deg_<3){
4232 NurbsCurve<T,N> c3(*this) ;
4233 c3.degreeElevate(3-deg_) ;
4234 return c3.writePSp(filename,points,vectors,cp,magFact,dash,bOpen) ;
4235 }
4236
4237 // extract the Bezier segments
4238 NurbsCurveArray<T,N> Ca ;
4239 if (bOpen)
4240 decompose(Ca) ;
4241 else
4242 decomposeClosed(Ca) ;
4243
4244 int guess =0 ;
4245 if(magFact<=0){
4246 magFact = 1.0 ;
4247 guess = 1 ;
4248 }
4249
4250 Matrix< Point_nD<T,N> > pnts(Ca.n(),deg_+1) ;
4251 int i,j ;
4252 for(i=0;i<Ca.n();i++){
4253 for(j=0;j<deg_+1;j++){
4254 pnts(i,j) = project2D(Ca[i].P[j]) ;
4255 }
4256 }
4257
4258
4259 // find bounding box parameters
4260 T mx,my,Mx,My ;
4261 mx = Mx = pnts(0,0).x() ;
4262 my = My = pnts(0,0).y() ;
4263
4264 for(i=0;i<Ca.n();i++){
4265 Point_nD<T,N> p ;
4266 int step ;
4267 step = 8 ;
4268 for(j=0;j<=step;j++){
4269 T u ;
4270 u = (T)j/(T)step ;
4271 p = project2D(Ca[i](u)) ;
4272 if(p.x() < mx)
4273 mx = p.x() ;
4274 if(p.x() > Mx)
4275 Mx = p.x() ;
4276 if(p.y() < my)
4277 my = p.y() ;
4278 if(p.y() > My)
4279 My = p.y() ;
4280 }
4281 }
4282
4283 if(guess){
4284 magFact = minimum((T)500/(T)(Mx-mx),(T)700/(T)(My-my)) ;
4285 }
4286
4287 mx = mx*magFact+offX;
4288 my = my*magFact+offY;
4289 Mx = Mx*magFact+offX;
4290 My = My*magFact+offY;
4291 for(i=0;i<Ca.n();i++)
4292 for(j=0;j<deg_+1;j++)
4293 movePsP(pnts(i,j),magFact) ;
4294
4295 fout << "%!PS-Adobe-2.1\n%%Title: " << filename << endl ;
4296 fout << "%%Creator: NurbsCurve<T,N>::writePS\n" ;
4297 fout << "%%BoundingBox: " << mx << ' ' << my << ' ' << Mx << ' ' << My << endl ;
4298 fout << "%%Pages: 0" << endl ;
4299 fout << "%%EndComments" << endl ;
4300 fout << "0 setlinewidth\n" ;
4301 fout << "0 setgray\n" ;
4302 fout << endl ;
4303
4304 fout << "newpath\n" ;
4305 fout << pnts(0,0).x() << ' ' << pnts(0,0).y() << " moveto\n" ;
4306 for(i=0;i<Ca.n();i++){
4307 for(j=1;j<deg_+1;j++){
4308 fout << pnts(i,j).x() << ' ' << pnts(i,j).y() << ' ' ;
4309 }
4310 fout << "curveto\n" ;
4311 }
4312 fout << "stroke\n" ;
4313
4314 if(cp>0){ // draw the control points of the original curve
4315 Vector< Point_nD<T,N> > pts(P.n()) ;
4316 for(i=0;i<P.n();i++){
4317 pts[i] = project2D(P[i]) ;
4318 movePsP(pts[i],magFact) ;
4319 fout << "newpath\n" ;
4320 fout << pts[i].x() << ' ' << pts[i].y() << " 3 0 360 arc\nfill\n" ;
4321 }
4322 if(dash>0)
4323 fout << "[" << dash << "] " << dash << " setdash\n" ;
4324 fout << "newpath\n" ;
4325
4326 fout << pts[0].x() << ' ' << pts[0].y() << " moveto\n" ;
4327 for(i=1;i<P.n();i++)
4328 fout << pts[i].x() << ' ' << pts[i].y() << " lineto\n" ;
4329 fout << "stroke\n" ;
4330 }
4331 else{
4332 if(cp<0){
4333 Vector< Point_nD<T,N> > pts(P.n()*Ca.n()) ;
4334 int k=0 ;
4335 for(i=0;i<Ca.n();i++)
4336 for(j=0;j<deg_+1;j++){
4337 pts[k] = project2D(Ca[i].P[j]) ;
4338 movePsP(pts[k],magFact) ;
4339 fout << "newpath\n" ;
4340 fout << pts[k].x() << ' ' << pts[k].y() << " 3 0 360 arc\nfill\n" ;
4341 k++ ;
4342 }
4343 if(dash>0)
4344 fout << "[" << dash << "] " << dash << " setdash\n" ;
4345 fout << "newpath\n" ;
4346
4347 fout << pts[0].x() << ' ' << pts[0].y() << " moveto\n" ;
4348 for(i=1;i<k;i++)
4349 fout << pts[i].x() << ' ' << pts[i].y() << " lineto\n" ;
4350 fout << "stroke\n" ;
4351 }
4352 }
4353
4354 for(i=0;i<points.n();++i){
4355 Point_nD<T,N> p ;
4356 p = points[i] ;
4357 movePsP(p,magFact) ;
4358 fout << "newpath\n" ;
4359 fout << p.x() << ' ' << p.y() << " 3 0 360 arc\nfill\n" ;
4360 }
4361
4362 if(vectors.n()==points.n()){
4363 for(i=0;i<points.n();++i){
4364 Point_nD<T,N> p,p2 ;
4365 p = points[i] ;
4366 p2 = points[i] + vectors[i] ;
4367 movePsP(p,magFact) ;
4368 movePsP(p2,magFact) ;
4369 fout << "newpath\n" ;
4370 fout << p.x() << ' ' << p.y() << " moveto\n" ;
4371 if(dash>0)
4372 fout << "[" << dash/2.0 << "] " << dash/2.0 << " setdash\n" ;
4373 fout << p2.x() << ' ' << p2.y() << " lineto\n" ;
4374 fout << "stroke\n" ;
4375 }
4376 }
4377
4378 fout << "showpage\n%%EOF\n" ;
4379 return 1 ;
4380 }
4381
4382 /*!
4383 \brief Writes the curve to a VRML file
4384
4385 A circle is swept around the trajectory made by the curve.
4386 The resulting surface is saved as a VRML file.
4387
4388 \param filename the name of the VRML file to save to
4389 \param radius the radius of the line
4390 \param K the minimum number of interpolation
4391 \param color the color of the line
4392 \param Nu the number of points for the circle
4393 \param Nv the number of points along the path
4394 \param u_s the starting parametric value for \a u
4395 \param u_e the end parametric value for \a u
4396
4397 \return returns 1 on success, 0 otherwise.
4398
4399 \author Philippe Lavoie
4400 \date 25 July 1997
4401 */
4402 template <class T, int N>
writeVRML(const char * filename,T radius,int K,const Color & color,int Nu,int Nv,T u_s,T u_e) const4403 int NurbsCurve<T,N>::writeVRML(const char* filename,T radius,int K, const Color& color,int Nu,int Nv, T u_s, T u_e) const{
4404 NurbsSurface<T,N> S ;
4405 NurbsCurve<T,N> C ;
4406
4407 C.makeCircle(Point_nD<T,N>(0,0,0),Point_nD<T,N>(1,0,0),Point_nD<T,N>(0,0,1),radius,0,2*M_PI);
4408 S.sweep(*this,C,K) ;
4409 return S.writeVRML(filename,color,Nu,Nv,0,1,u_s,u_e) ;
4410 }
4411
4412 /*!
4413 \brief Writes the curve to a VRML file
4414
4415 A circle is swept around the trajectory made by the curve.
4416 The resulting surface is saved as a VRML file.
4417
4418 \param filename the name of the VRML file to save to
4419 \param radius the radius of the line
4420 \param K the minimum number of interpolation
4421 \param color the color of the line
4422 \param Nu the number of points for the circle
4423 \param Nv the number of points along the path
4424 \param u_s the starting parametric value for \a u
4425 \param u_e the end parametric value for \a u
4426
4427 \return returns 1 on success, 0 otherwise.
4428
4429 \author Philippe Lavoie
4430 \date 4 May 1999
4431 */
4432 template <class T, int N>
writeVRML97(const char * filename,T radius,int K,const Color & color,int Nu,int Nv,T u_s,T u_e) const4433 int NurbsCurve<T,N>::writeVRML97(const char* filename,T radius,int K, const Color& color,int Nu,int Nv, T u_s, T u_e) const{
4434 NurbsSurface<T,N> S ;
4435 NurbsCurve<T,N> C ;
4436
4437 C.makeCircle(Point_nD<T,N>(0,0,0),Point_nD<T,N>(1,0,0),Point_nD<T,N>(0,0,1),radius,0,2*M_PI);
4438 S.sweep(*this,C,K) ;
4439 return S.writeVRML97(filename,color,Nu,Nv,0,1,u_s,u_e) ;
4440 }
4441
4442 /*!
4443 \brief Writes the curve to a VRML file
4444
4445 A circle is swept around the trajectory made by the curve.
4446 The resulting surface is saved as a VRML file.
4447
4448 \param filename the name of the ostream to write to
4449 \param radius the radius of the line
4450 \param K the minimum number of interpolation
4451 \param color the color of the line
4452 \param Nu the number of points for the circle
4453 \param Nv the number of points along the path
4454 \param u_s the starting parametric value for \a u
4455 \param u_e the end parametric value for \a u
4456
4457 \return returns 1 on success, 0 otherwise.
4458
4459 \author Philippe Lavoie
4460 \date 25 July 1997
4461 */
4462 template <class T, int N>
writeVRML(ostream & fout,T radius,int K,const Color & color,int Nu,int Nv,T u_s,T u_e) const4463 int NurbsCurve<T,N>::writeVRML(ostream &fout,T radius,int K, const Color& color,int Nu,int Nv, T u_s, T u_e) const{
4464 NurbsSurface<T,N> S ;
4465 NurbsCurve<T,N> C ;
4466
4467 C.makeCircle(Point_nD<T,N>(0,0,0),Point_nD<T,N>(1,0,0),Point_nD<T,N>(0,0,1),radius,0,2*M_PI);
4468 S.sweep(*this,C,K) ;
4469 return S.writeVRML(fout,color,Nu,Nv,0,1,u_s,u_e) ;
4470 }
4471
4472 /*!
4473 \brief Writes the curve to a VRML file
4474
4475 A circle is swept around the trajectory made by the curve.
4476 The resulting surface is saved as a VRML file.
4477
4478 \param filename the name of the ostream to write to
4479 \param radius the radius of the line
4480 \param K the minimum number of interpolation
4481 \param color the color of the line
4482 \param Nu the number of points for the circle
4483 \param Nv the number of points along the path
4484 \param u_s the starting parametric value for \a u
4485 \param u_e the end parametric value for \a u
4486
4487 \return returns 1 on success, 0 otherwise.
4488
4489 \author Philippe Lavoie
4490 \date 4 May 1999
4491 */
4492 template <class T, int N>
writeVRML97(ostream & fout,T radius,int K,const Color & color,int Nu,int Nv,T u_s,T u_e) const4493 int NurbsCurve<T,N>::writeVRML97(ostream &fout,T radius,int K, const Color& color,int Nu,int Nv, T u_s, T u_e) const{
4494 NurbsSurface<T,N> S ;
4495 NurbsCurve<T,N> C ;
4496
4497 C.makeCircle(Point_nD<T,N>(0,0,0),Point_nD<T,N>(1,0,0),Point_nD<T,N>(0,0,1),radius,0,2*M_PI);
4498 S.sweep(*this,C,K) ;
4499 return S.writeVRML97(fout,color,Nu,Nv,0,1,u_s,u_e) ;
4500 }
4501
4502 /*!
4503 \brief Transforms a 2D curve into a 3D curve
4504 \relates NurbsCurve
4505
4506 \param c2d the curve in 2D
4507 \param c3d the curve in 3D
4508
4509 \warning The curve must be valid
4510
4511 \author Philippe Lavoie
4512 \date 16 October 1998
4513 */
4514 template <class T>
to3D(const NurbsCurve<T,2> & c2d,NurbsCurve<T,3> & c3d)4515 void to3D(const NurbsCurve<T,2>& c2d, NurbsCurve<T,3>& c3d){
4516 c3d.resize(c2d.ctrlPnts().n(),c2d.degree()) ;
4517
4518 c3d.modKnot(c2d.knot()) ;
4519 HPoint_nD<T,3> p(0) ;
4520 for(int i=c2d.ctrlPnts().n()-1;i>=0;--i){
4521 p.x() = c2d.ctrlPnts()[i].x() ;
4522 p.y() = c2d.ctrlPnts()[i].y() ;
4523 p.w() = c2d.ctrlPnts()[i].w() ;
4524 c3d.modCP(i,p) ;
4525 }
4526 }
4527
4528 template <class T>
to3D(const NurbsCurve<T,3> & c2d,NurbsCurve<T,3> & c3d)4529 void to3D(const NurbsCurve<T,3>& c2d, NurbsCurve<T,3>& c3d){
4530 c3d = c2d ;
4531 }
4532
4533 /*!
4534 \brief Transforms a 3D curve into a 2D curve
4535 \relates NurbsCurve
4536
4537 Actually it just puts the x,y and w value into a 2D curve. It doesn't handle
4538 the z value, if you want some perspective transformation,
4539 you should perform the needed transformation before hand
4540 on the 3D curve.
4541
4542 \param c3d the curve in 3D
4543 \param c2d the curve in 2D
4544
4545 \warning The curve must be valid
4546
4547 \author Philippe Lavoie
4548 \date 20 October 1998
4549 */
4550 template <class T>
to2D(const NurbsCurve<T,3> & c3d,NurbsCurve<T,2> & c2d)4551 void to2D(const NurbsCurve<T,3>& c3d, NurbsCurve<T,2>& c2d){
4552 c2d.resize(c3d.ctrlPnts().n(),c3d.degree()) ;
4553
4554 c2d.modKnot(c3d.knot()) ;
4555 HPoint_nD<T,2> p(0) ;
4556 for(int i=c3d.ctrlPnts().n()-1;i>=0;--i){
4557 p.x() = c3d.ctrlPnts()[i].x() ;
4558 p.y() = c3d.ctrlPnts()[i].y() ;
4559 p.w() = c3d.ctrlPnts()[i].w() ;
4560 c2d.modCP(i,p) ;
4561 }
4562 }
4563
4564 /*!
4565 \brief Generates a knot vector using the averaging technique
4566 \relates NurbsCurve
4567
4568 \latexonly
4569 The technique is as follows:
4570 \begin{itemize}
4571 \item $u_0 = \cdots = u_{deg} = 0$
4572 \item $u_{m-deg} = \cdots = u_{m-1} = 1$
4573 \item \begin{equation}
4574 u_{j+deg} = \frac{1}{deg}\sum_{i=j}^{j+deg+1}\bar{u}_i
4575 \hspace{0.5in} j= 1,\ldots,n-deg-1
4576 \end{equation}
4577 \end{itemize}
4578 where $n$ is the size of the $\bar{u}$ knot coefficient vector,
4579 $m=n+deg+1$ is the size of the knot vector and $deg$ is the
4580 degree of the curve.
4581 \endlatexonly
4582 \htmlonly
4583 There is more information about this routine in the LaTeX version.
4584 \endhtmlonly
4585
4586 \param uk the knot coefficients
4587 \param deg the degree of the curve associated with the knot vector
4588 \param U an average knot vector
4589
4590 \author Philippe Lavoie
4591 \date 24 January, 1997
4592 */
4593 template <class T>
knotAveraging(const Vector<T> & uk,int deg,Vector<T> & U)4594 void knotAveraging(const Vector<T>& uk, int deg, Vector<T>& U){
4595 U.resize(uk.n()+deg+1) ;
4596
4597 int j ;
4598 for(j=1;j<uk.n()-deg;++j){
4599 U[j+deg] = 0.0 ;
4600 for(int i=j;i<j+deg;++i)
4601 U[j+deg] += uk[i] ;
4602 U[j+deg] /= (T)deg ;
4603 }
4604 for(j=0;j<=deg;++j)
4605 U[j] = 0.0 ;
4606 for(j=U.n()-deg-1;j<U.n();++j)
4607 U[j] = 1.0 ;
4608 }
4609
4610
4611
4612 /*!
4613 \brief Compute the parameters by averaging the knots
4614 \relates NurbsCurve
4615
4616 Generates a parameter vector by averaging the knots from the
4617 knot vector.
4618
4619 \param U the knot vector
4620 \param deg the degree of the curve associated with the knot vector
4621 \param uk the parameter vector
4622
4623 \author Philippe Lavoie
4624 \date 26 August 1997
4625 */
4626 template <class T>
averagingKnots(const Vector<T> & U,int deg,Vector<T> & uk)4627 void averagingKnots(const Vector<T>& U, int deg, Vector<T>& uk){
4628 uk.resize(U.n()-deg-1) ;
4629
4630 int i,k ;
4631
4632 uk[0] = U[0] ;
4633 uk[uk.n()-1] = U[U.n()-1] ;
4634
4635 for(k=1;k<uk.n()-1;++k){
4636 uk[k] = 0.0 ;
4637 for(i=k+1;i<k+deg+1;++i)
4638 uk[k] += U[i] ;
4639 uk[k] /= deg ;
4640 }
4641 }
4642
4643 /*!
4644 \brief Moves a point on the NURBS curve
4645
4646 This modifies the curve such that the point $C(u)$ is moved
4647 by delta.
4648
4649 See section 11.5.1 of the NURBS book for an explanation of
4650 the algorithm.
4651
4652 \param u the point to move
4653 \param delta the movement in 3D of the point at $C(u)$
4654
4655 \author Philippe Lavoie
4656 \date 24 January 1997
4657 */
4658 template <class T, int N>
movePoint(T u,const Point_nD<T,N> & delta)4659 int NurbsCurve<T,N>::movePoint(T u, const Point_nD<T,N>& delta) {
4660 BasicArray< Point_nD<T,N> > d(1) ;
4661 d[0] = delta ;
4662 return movePoint(u,d) ;
4663 }
4664
4665 /*!
4666 \brief Moves a point in the NURBS curve
4667
4668 This modifies the curve such that the point \a C(u) is moved
4669 by delta. Delta is a vector containing the movement as
4670 D^{(k)} where (k) specifies the derivative. Thus
4671 at D[0], this specifies the 0th derivative movement, at
4672 D[1] it specifies the 1st derivative movement of the point.
4673 \e i.e. Suppose that C(u) = (10,20,3) then a
4674 D[0] = (10,10,10) will move the point to C(u) = (20,30,13)
4675
4676 See section 11.5.1 of the NURBS book for an explanation of
4677 the algorithm.
4678
4679 \param u the point to move
4680 \param delta the vector of movement
4681
4682 \author Philippe Lavoie
4683 \date 24 January 1997
4684 */
4685 template <class T, int N>
movePoint(T u,const BasicArray<Point_nD<T,N>> & delta)4686 int NurbsCurve<T,N>::movePoint(T u, const BasicArray< Point_nD<T,N> >& delta) {
4687 int i,j ;
4688
4689 // setup B
4690 Matrix_DOUBLE B ;
4691
4692 int n,m ; // n is the number of rows, m the number of columns
4693
4694 m = deg_ + 1 ;
4695 n = delta.n() ;
4696
4697 B.resize(n,m) ;
4698
4699 int span = findSpan(u) ;
4700
4701 n = 0 ;
4702
4703 Matrix<T> R ;
4704
4705 dersBasisFuns(delta.n()-1,u,span,R) ;
4706
4707
4708 for(i=0;i<delta.n();++i){
4709 if(delta[i].x()==0.0 && delta[i].y()==0.0 && delta[i].z()==0.0)
4710 continue ;
4711 for(j=0;j<m;++j){
4712 B(n,j) = (double)R(i,j) ;
4713 }
4714 ++n ;
4715 }
4716
4717 Matrix_DOUBLE A ;
4718 Matrix_DOUBLE Bt(transpose(B)) ;
4719 Matrix_DOUBLE BBt ;
4720
4721 BBt = inverse(B*Bt) ;
4722 A = Bt*BBt ;
4723
4724 Matrix_DOUBLE dD ;
4725
4726 dD.resize(delta.n(),N) ;
4727 for(i=0;i<delta.n();++i){
4728 const Point_nD<T,N>& d = delta[i] ; // this makes the SGI compiler happy
4729 for(j=0;j<N;++j)
4730 dD(i,j) = (double)d.data[j] ;
4731 }
4732
4733 Matrix_DOUBLE dP ;
4734
4735 dP = A*dD ;
4736
4737 for(i=0;i<m;++i){
4738 P[span-deg_+i].x() += dP(i,0)*P[span-deg_+i].w() ;
4739 P[span-deg_+i].y() += dP(i,1)*P[span-deg_+i].w() ;
4740 P[span-deg_+i].z() += dP(i,2)*P[span-deg_+i].w() ;
4741 }
4742
4743 return 1 ;
4744 }
4745
4746 /*!
4747 \brief Moves a point with some constraint
4748
4749 This will modify the NURBS curve by respecting a certain
4750 number of constraints. $u_r$ specifies the parameters on which
4751 the constraints should be applied.
4752 The constraint are defined by $D$ which specifies the
4753 vector by which the points should move.
4754
4755 For example, if you want to move the point C(0.5) by
4756 (10,0,10) and fix the point C(0.6) on the current curve
4757 (a move of (0,0,0)).
4758 u_r = 0.5, 0.6 and D = (10,0,10), (0,0,0)
4759
4760 The \a u_r vector should be in an increasing order.
4761
4762 See section 11.5.1 of the NURBS book for an explanation of
4763 the algorithm.
4764
4765 \param ur the vector of parameters on which a constraint is applied
4766 \param D a vector of the value of \a D_r^{(0)}
4767
4768 \return 1 if the operation is possible, 0 if the problem is ill defined
4769 \e i.e. there isn't enough information to find a unique
4770 solution (the system is overdetermined) or that the system
4771 has non-independant components.
4772
4773 \warning ur and D must have the same size and the values inside ur should
4774 not repeat and they should be ordered
4775 \author Philippe Lavoie
4776 \date 24 January 1997
4777 */
4778 template <class T, int N>
movePoint(const BasicArray<T> & ur,const BasicArray<Point_nD<T,N>> & D)4779 int NurbsCurve<T,N>::movePoint(const BasicArray<T>& ur, const BasicArray< Point_nD<T,N> >& D) {
4780 BasicArray<int> fixCP(0) ;
4781 BasicArray<int> Dr(D.n()) ;
4782 BasicArray<int> Dk(D.n()) ;
4783
4784 if(ur.n() != D.n()){
4785 #ifdef USE_EXCEPTION
4786 throw NurbsInputError(ur.n(),D.n()) ;
4787 #else
4788 Error err("movePoint(ur,D)");
4789 err << "The two input vectors are not of the same size\n" ;
4790 err << "ur.n()= " << ur.n() << ", D.n() = " << D.n() << endl ;
4791 err.fatal() ;
4792 #endif
4793 }
4794
4795 for(int i=0;i<Dr.n();++i){
4796 Dr[i] = i ;
4797 }
4798 Dk.reset(0) ;
4799 return movePoint(ur,D,Dr,Dk,fixCP) ;
4800 }
4801
4802 /*!
4803 \brief Moves a point with some constraint
4804
4805 This will modify the NURBS curve by respecting a certain
4806 number of constraints. u_r specifies the parameters on which
4807 the constraints should be applied.
4808 The constraint are defined by $D_r^{(k)}$ which requires 3
4809 vectors to fully qualify. \a D specifies the value
4810 of the constraint and D_r and D_k are used to specify
4811 on which parameter the constraint is applied and of what degree.
4812
4813 For example, if you want to move the point C(0.5) by
4814 (10,0,10) and fix the point C(0.6) on the current curve
4815 (a move of (0,0,0)) but change its 1st derivative by (0,20,0).
4816 Then the following values must be inputed to the routine.
4817 u_r = [0.5,0.6], D = [(10,0,10), (0,0,0), (0,20,0)],
4818 D_r = [0, 1, 1] and D_k = [0, 0, 1].
4819
4820 The values in D should be ordered in respect with r and k.
4821 {\em i.e.} for D_i=D_{r_i}^{(k_i)}, then i < j implies
4822 that r_i < r_j and that either r_i < r_j or k_i < k_j.
4823
4824 See section 11.5.1 of the NURBS book for an explanation of
4825 the algorithm.
4826
4827 \param ur the vector of parameters on which a constraint is applied
4828 \param D a vector of the value of D_r^{(k)}
4829 \param Dr a vector specifying the value of r for D
4830 \param Dk a vector specifying the value of k for D
4831
4832 \return 1 if the operation is possible, 0 if the problem is ill defined
4833 \e i.e. there isn't enough information to find a unique
4834 solution (the system is overdetermined) or that the system
4835 has non-independant components.
4836
4837 \warning The values inside ur should \e not repeat. D,Dr and Dk must
4838 be of the same size.
4839 \author Philippe Lavoie
4840 \date 24 January 1997
4841 */
4842 template <class T, int N>
movePoint(const BasicArray<T> & ur,const BasicArray<Point_nD<T,N>> & D,const BasicArray_INT & Dr,const BasicArray_INT & Dk)4843 int NurbsCurve<T,N>::movePoint(const BasicArray<T>& ur, const BasicArray< Point_nD<T,N> >& D, const BasicArray_INT& Dr, const BasicArray_INT& Dk) {
4844 BasicArray_INT fixCP(0) ;
4845
4846 if(D.n() != Dr.n() ){
4847 #ifdef USE_EXCEPTION
4848 throw NurbsInputError(D.n(),Dr.n()) ;
4849 #else
4850 Error err("movePoint(ur,D,Dr,Dk)");
4851 err << "The D,Dr,Dk vectors are not of the same size\n" ;
4852 err << "D.n()= " << D.n() << ", Dr.n() = " << Dr.n()
4853 << ", Dk.n() = " << Dk.n() << endl ;
4854 err.fatal() ;
4855 #endif
4856 }
4857
4858 if( D.n() !=Dk.n()){
4859 #ifdef USE_EXCEPTION
4860 throw NurbsInputError(D.n(),Dk.n()) ;
4861 #else
4862 Error err("movePoint(ur,D,Dr,Dk)");
4863 err << "The D,Dr,Dk vectors are not of the same size\n" ;
4864 err << "D.n()= " << D.n() << ", Dr.n() = " << Dr.n()
4865 << ", Dk.n() = " << Dk.n() << endl ;
4866 err.fatal() ;
4867 #endif
4868 }
4869
4870 return movePoint(ur,D,Dr,Dk,fixCP) ;
4871 }
4872
4873 /*!
4874 \brief Moves a point with some constraint
4875
4876 This will modify the NURBS curve by respecting a certain
4877 number of constraints. \a u_r specifies the parameters on which
4878 the constraints should be applied.
4879 The constraint are defined by D_r^{(k)} which requires 3
4880 vectors to fully qualify. \a D specifies the value
4881 of the constraint and D_r and D_k are used to specify
4882 on which parameter the constraint is applied and of what degree.
4883
4884 A second constraint \a fixCP consists of specifying which
4885 control points can not be moved by the routine.
4886
4887 For example, if you want to move the point C(0.5) by
4888 (10,0,10) and fix the point C(0.6) on the current curve
4889 (a move of (0,0,0)) but change its 1st derivative by (0,20,0).
4890 Doing this without modifying control point 4 .
4891 Then the following values must be inputed to the routine.
4892 u_r = [0.5, 0.6], D = [(10,0,10), (0,0,0), (0,20,0)],
4893 D_r = [0, 1, 1], D_k = [0, 0, 1] and fixCP= 4.
4894
4895 The values in D should be ordered in respect with r and k.
4896 \e i.e. for D_i=D_{r_i}^{(k_i)}, then i < j implies
4897 that r_i < r_j and that either r_i < r_j or k_i < k_j.
4898
4899 See section 11.5.1 of the NURBS book for an explanation of
4900 the algorithm.
4901
4902 \param ur the vector of parameters on which a constraint is applied
4903 \param D a vector of the value of D_r^{(k)}
4904 \param Dr a vector specifying the value of r for D
4905 \param Dk a vector specifying the value of k for D
4906 \param fixCP a vector specifying which control points {\em can not} be
4907 modified.
4908 \return 1 if the operation is possible, 0 if the problem is ill defined
4909 \e i.e. there isn't enough information to find a unique
4910 solution (the system is overdetermined) or that the system
4911 has non-independant components.
4912
4913 \warning The values of ur should \e not repeat. D,Dr and Dk must
4914 be of the same size.
4915 \author Philippe Lavoie
4916 \date 24 January 1997
4917 */
4918 template <class T, int N>
movePoint(const BasicArray<T> & ur,const BasicArray<Point_nD<T,N>> & D,const BasicArray_INT & Dr,const BasicArray_INT & Dk,const BasicArray_INT & fixCP)4919 int NurbsCurve<T,N>::movePoint(const BasicArray<T>& ur, const BasicArray< Point_nD<T,N> >& D, const BasicArray_INT& Dr, const BasicArray_INT& Dk, const BasicArray_INT& fixCP) {
4920 int i,j,n ;
4921
4922 if(D.n() != Dr.n() ){
4923 #ifdef USE_EXCEPTION
4924 throw NurbsInputError(D.n(),Dr.n()) ;
4925 #else
4926 Error err("movePoint(ur,D,Dr,Dk)");
4927 err << "The D,Dr,Dk vectors are not of the same size\n" ;
4928 err << "D.n()= " << D.n() << ", Dr.n() = " << Dr.n()
4929 << ", Dk.n() = " << Dk.n() << endl ;
4930 err.fatal() ;
4931 #endif
4932 }
4933
4934 if( D.n() !=Dk.n()){
4935 #ifdef USE_EXCEPTION
4936 throw NurbsInputError(D.n(),Dk.n()) ;
4937 #else
4938 Error err("movePoint(ur,D,Dr,Dk)");
4939 err << "The D,Dr,Dk vectors are not of the same size\n" ;
4940 err << "D.n()= " << D.n() << ", Dr.n() = " << Dr.n()
4941 << ", Dk.n() = " << Dk.n() << endl ;
4942 err.fatal() ;
4943 #endif
4944 }
4945
4946 // setup B
4947 Matrix_DOUBLE B ;
4948
4949 B.resize(D.n(),P.n()) ;
4950
4951 int span ;
4952
4953 Matrix<T> R ;
4954
4955 B.reset(0.0) ;
4956
4957 for(i=0;i<D.n();++i){
4958 span = findSpan(ur[Dr[i]]) ;
4959 dersBasisFuns(Dk[i],ur[Dr[i]],span,R) ;
4960 for(j=0;j<=deg_;++j){
4961 B(i,span-deg_+j) = (double)R(Dk[i],j) ;
4962 }
4963 }
4964
4965 // optimize B
4966 BasicArray_INT remove(B.cols()) ;
4967 BasicArray_INT map(B.cols()) ;
4968 remove.reset((int)1.0) ;
4969
4970 for(j=0;j<B.cols();++j){
4971 for(i=0;i<B.rows();++i)
4972 if((B(i,j)*B(i,j))>1e-10){
4973 remove[j] = 0 ;
4974 break ;
4975 }
4976 }
4977
4978 for(i=0;i<fixCP.n();++i){
4979 remove[fixCP[i]] = 1 ;
4980 }
4981
4982 n = 0 ;
4983 for(i=0;i<B.cols();++i){
4984 if(!remove[i]){
4985 map[n] = i ;
4986 ++n ;
4987 }
4988 }
4989
4990 map.resize(n) ;
4991
4992 Matrix_DOUBLE Bopt(B.rows(),n) ;
4993 for(j=0;j<n;++j){
4994 for(i=0;i<B.rows();++i)
4995 Bopt(i,j) = B(i,map[j]) ;
4996 }
4997
4998 Matrix_DOUBLE A ;
4999 Matrix_DOUBLE Bt(transpose(Bopt)) ;
5000 Matrix_DOUBLE BBt ;
5001
5002 BBt = inverse(Bopt*Bt) ;
5003
5004 A = Bt*BBt ;
5005
5006 Matrix_DOUBLE dD ;
5007
5008 dD.resize(D.n(),N) ;
5009 for(i=0;i<D.n();++i){
5010 const Point_nD<T,N>& d = D[i] ; // this makes the SGI compiler happy
5011 for(j=0;j<N;++j)
5012 dD(i,j) = (double)d.data[j] ;
5013 }
5014
5015 Matrix_DOUBLE dP ;
5016
5017 dP = A*dD ;
5018
5019 for(i=0;i<map.n();++i){
5020 P[map[i]].x() += dP(i,0)*P[map[i]].w() ;
5021 P[map[i]].y() += dP(i,1)*P[map[i]].w() ;
5022 P[map[i]].z() += dP(i,2)*P[map[i]].w() ;
5023 }
5024
5025 return 1 ;
5026 }
5027
5028 /*!
5029 \brief Splits the curve into two curves.
5030
5031 \param u splits at this parametric value
5032 \param cl the lower curve
5033 \param cu the upper curve
5034
5035 \return 1 if the operation, 0 otherwise.
5036
5037 \warning You \e must make sure that you split at a valid parametric
5038 value. You can't split a curve at its end points.
5039 \author Philippe Lavoie
5040 \date 16 October 1997
5041 */
5042 template <class T, int N>
splitAt(T u,NurbsCurve<T,N> & cl,NurbsCurve<T,N> & cu) const5043 int NurbsCurve<T,N>::splitAt(T u, NurbsCurve<T,N>& cl, NurbsCurve<T,N>& cu) const {
5044 if(u<= U[deg_])
5045 return 0 ;
5046 if(u>= U[U.n()-deg_-1])
5047 return 0 ;
5048
5049 // get the multiplicity at u
5050 int s,j,i ;
5051 int span = findSpan(u) ;
5052
5053 if(absolute(u-U[span])<1e-6)
5054 s = findMult(span) ;
5055 else
5056 s = 0 ;
5057
5058 BasicArray<T> X(deg_+1-s) ;
5059 X.reset(u) ;
5060
5061 cl = *this ;
5062 if(X.n()>0)
5063 cl.refineKnotVector(X) ;
5064
5065 span = cl.findSpan(u)-deg_ ;
5066 // span is the begining of the upper curve
5067 cu.resize(cl.P.n()-span,deg_) ;
5068 for(i=cl.P.n()-1,j=cu.P.n()-1;j>=0;--j,--i){
5069 cu.P[j] = cl.P[i] ;
5070 }
5071
5072 for(i=cl.U.n()-1,j=cu.U.n()-1;j>=0;--j,--i){
5073 cu.U[j] = cl.U[i] ;
5074 }
5075 cl.resize(span,deg_) ;
5076
5077 return 1 ;
5078 }
5079
5080 /*!
5081 \brief The curve is the result of mergin two curves
5082
5083 \param cl the lower curve
5084 \param cu the upper curve
5085
5086 \return 1 if the operation is succesfull, 0 otherwise.
5087
5088 \warning You must make sure the the knot vectors are compatible,
5089 \e i.e. that the end knots of the lower curve are the
5090 same as the first knots of the upper curve. The curves
5091 must also be of the same degree.
5092 \author Philippe Lavoie
5093 \date 16 October 1997
5094 */
5095 template <class T, int N>
mergeOf(const NurbsCurve<T,N> & cl,const NurbsCurve<T,N> & cu)5096 int NurbsCurve<T,N>::mergeOf(const NurbsCurve<T,N>& cl, const NurbsCurve<T,N> &cu){
5097 if(cl.deg_ != cu.deg_){
5098 #ifdef USE_EXCEPTION
5099 throw NurbsInputError();
5100 #else
5101 Error err("NurbsCurve<T,N>::mergeOf");
5102 err << " The two curves are not of the same degree\n" ;
5103 err.warning() ;
5104 return 0 ;
5105 #endif
5106 }
5107 if((cl.U[cl.U.n()-1]-cu.U[0])*(cl.U[cl.U.n()-1]-cu.U[0])>1e-8){
5108 #ifdef USE_EXCEPTION
5109 throw NurbsInputError();
5110 #else
5111 Error err("NurbsCurve<T,N>::mergeOf");
5112 err << " The two knot vectors are not compatible.\n" ;
5113 err << "The first one is " << cl.U << endl ;
5114 err << "The second is " << cu.U << endl ;
5115 err.warning() ;
5116 return 0 ;
5117 #endif
5118 }
5119 if(norm2(cl.P[cl.P.n()-1]-cu.P[0])>1e-8){
5120 #ifdef USE_EXCEPTION
5121 throw NurbsInputError();
5122 #else
5123 Error err("NurbsCurve<T,N>::mergeOf");
5124 err << " The end control points are NOT the same.\n" ;
5125 err << " cl.P[n-1] = " << cl.P[cl.P.n()-1] << endl ;
5126 err << " cu.P[0] = " << cu.P[0] << endl ;
5127 err.warning() ;
5128 return 0 ;
5129 #endif
5130 }
5131 resize(cl.P.n()+cu.P.n(),cl.deg_) ;
5132
5133 int i ;
5134 for(i=0;i<cl.P.n();++i)
5135 P[i] = cl.P[i] ;
5136 for(;i<P.n();++i)
5137 P[i] = cu.P[i-cl.P.n()] ;
5138
5139 for(i=0;i<cl.U.n();++i)
5140 U[i] = cl.U[i] ;
5141 for(;i<U.n();++i)
5142 U[i] = cu.U[i-cl.U.n()+deg_+1] ;
5143
5144 return 1 ;
5145 }
5146
5147 /*!
5148 \brief Determines the knot span index
5149 \relates NurbsCurve NurbsSurface
5150
5151 Determines the knot span for which their exists non-zero basis
5152 functions. The span is the index \a k for which the parameter
5153 \a u is valid in the [u_k,u_{k+1}] range.
5154
5155 \param u the parametric value
5156 \param U the knot vector
5157 \param deg the degree of the curve
5158
5159 \return the span index at \a u.
5160
5161 \warning \a u must be in a valid range
5162
5163 \author Philippe Lavoie
5164 \date 17 October 1997
5165 \modified 20 January, 1999 (Alejandro Frangi)
5166 */
5167 template <class T>
findSpan(T u,const Vector<T> & U,int deg)5168 int findSpan(T u, const Vector<T>& U, int deg) {
5169 if(u>=U[U.n()-deg-1])
5170 return U.n()-deg-1 ;
5171 if(u<=U[deg])
5172 return deg ;
5173 //AF
5174 int low = 0 ;
5175 int high = U.n()-deg ;
5176 int mid = (low+high)/2 ;
5177
5178 while(u<U[mid] || u>= U[mid+1]){
5179 if(u<U[mid])
5180 high = mid ;
5181 else
5182 low = mid ;
5183 mid = (low+high)/2 ;
5184 }
5185 return mid ;
5186
5187 }
5188
5189
5190 /*!
5191 \brief Generates a list of points from the curve
5192
5193 Generates a list of points from the curve. The list is
5194 generated within a user specified tolerance.
5195
5196 \param tolerance --> the tolerance for the tesselation.
5197
5198 \return The list of points.
5199
5200 \author Philippe Lavoie
5201 \date 17 October 1997
5202 */
5203 template <class T, int N>
tesselate(T tolerance,BasicList<T> * uk) const5204 BasicList<Point_nD<T,N> > NurbsCurve<T,N>::tesselate(T tolerance,BasicList<T> *uk) const {
5205 BasicList<Point_nD<T,N> > list, list2 ;
5206
5207 NurbsCurveArray<T,N> ca ;
5208 decompose(ca) ;
5209
5210 if(ca.n()==1){
5211 // get the number of steps
5212 T u = 0 ;
5213 Point_nD<T,N> maxD(0) ;
5214 Point_nD<T,N> prev ;
5215
5216 Vector< Point_nD<T,N> > ders(2) ;
5217
5218 deriveAt(u,1,ders) ;
5219
5220 prev = ders[1] ;
5221
5222 int i ;
5223 for(i=1;i<11;++i){
5224 u = T(i)/T(10) ;
5225 deriveAt(u,1,ders) ;
5226 Point_nD<T,N> delta = ders[1]-prev ;
5227 delta.x() = absolute(delta.x()) ;
5228 delta.y() = absolute(delta.y()) ;
5229 delta.z() = absolute(delta.z()) ;
5230 maxD = maximumRef(maxD,delta) ;
5231 prev = ders[1] ;
5232 }
5233
5234 const T sqr2 = T(1.414241527) ;
5235
5236 int n = (int)rint(sqr2*norm(maxD)/tolerance) ;
5237
5238 n+=2 ;
5239 if(n<3) n = 3 ;
5240
5241 for(i=0;i<n;++i){
5242 u = (U[U.n()-deg_-1]-U[deg_])*T(i)/T(n-1) + U[deg_] ;
5243 list.add(this->pointAt(u)) ;
5244 if(uk)
5245 uk->add(u) ;
5246 }
5247
5248 return list ;
5249 }
5250 else{
5251 for(int i=0;i<ca.n();++i){
5252 list2 = ca[i].tesselate(tolerance,uk).by_ref() ;
5253
5254 // remove the last point from the list to elliminate
5255 list.erase((BasicNode<Point_nD<T,N> >*)list.last()) ;
5256 list.addElements(list2) ;
5257 }
5258 }
5259 return list ;
5260 }
5261
5262 /*!
5263 \brief Finds the parametric value of maximal influence
5264 \relates NurbsCurve NurbsSurface
5265
5266 Finds the parametric value of maximal influence for a control
5267 point \a i. This finds the parametric value \a u were the basis
5268 function \a N_{i,p}(u) is maximal.
5269
5270 This routine only works for N_{i,p}(u) where p=1,2,3. Other
5271 values of \a p are not supported. The reason is that the routine
5272 uses pre-computed equations to find the proper values.
5273
5274 \param i the i-th control point
5275 \param U the knot vector
5276 \param p the degree of the basis function
5277 \param u the parametric value of maixmal influence
5278
5279 \return 1 if the operation was succesfull, 0 otherwise
5280 \warning The knot vector must be properly constructed,
5281 \latexonly i.e.
5282 \[
5283 U=\{\underbrace{a,\ldots,a}_{p+1},u_{p+1},\ldots,u_{m-p-1},\underbrace{b,\ldots,b}_{p+1} \}
5284 \]
5285 \endlatexonly
5286
5287 \author Philippe Lavoie
5288 \date 17 October 1997
5289 */
5290 template <class T>
maxInfluence(int i,const Vector<T> & U,int p,T & u)5291 int maxInfluence(int i, const Vector<T>& U, int p, T &u){
5292 if(i>U.n()-p-2)
5293 return 0 ;
5294 switch(p){
5295 case 1: u = U[i+1] ; return 1 ;
5296 case 2: {
5297 T A = U[i] + U[i+1] - U[i+2] - U[i+3] ;
5298 if(A>=0){
5299 u = U[i] ;
5300 return 1 ;
5301 }
5302 else{
5303 u = (U[i]*U[i+1] - U[i+2]*U[i+3])/A ;
5304 return 1 ;
5305 }
5306 }
5307 case 3:{
5308 double A = U[i]-U[i+3] ;
5309 if(A>=0){ // 4 knots at the same place from U[i] to U[i+3]
5310 u = U[i] ;
5311 return 1 ;
5312 }
5313 A = U[i+1]-U[i+3] ;
5314 if(A>=0){ // three knots are equal
5315 u = U[i+1] ;
5316 return 1 ;
5317 }
5318 // there are 4 points of possible interest
5319 // the 'good' one lie between U[i+1] and U[i+3]
5320 double a,b,c,d,e,X ;
5321 a = U[i] ;
5322 b = U[i+1] ;
5323 c = U[i+2] ;
5324 d = U[i+3] ;
5325 e = U[i+4] ;
5326
5327 double t1,t2,t3,t4,t5,t6,t7,t8,t9,t10,t11,t12,t13,t15,t16,t18;
5328 double t21,t22,t24,t25,t27,t28,t31,t32,t34,t35,t45,t46 ,t49 ,t52 ,t56;
5329 double t63 ,t66 ,t69 ,t88 ,t107,t110 ;
5330 double t115,t116,t117,t118,t119,t120,t121,t122,t124,t125,t127;
5331 double t133,t135,t136,t143,t151,t154 ;
5332 double t14,t17,t19,t20,t26,t29,t30,t33,t36,t37,t38,t39,t47,t55,t57 ;
5333 double t59,t62,t64,t65,t67,t70,t72,t73,t75,t76,t77,t78,t79,t80 ;
5334 double t81,t82,t83,t84,t85,t86,t87,t90,t91,t92,t93,t95,t96 ;
5335 double t97,t99,t101 ;
5336
5337 t1 = b*e;
5338 t2 = b*b;
5339 t3 = b*a;
5340 t4 = b*d;
5341 t5 = b*c;
5342 t6 = d*e;
5343 t7 = c*d;
5344 t8 = c*e;
5345 t9 = c*a;
5346 t10 = a*e;
5347 t11 = d*a;
5348 t12 = a*a;
5349 t13 = -t1+t2+t3-t4-t5+t6+t7+t8-t9-t10-t11+t12;
5350 t14 = 1/t13;
5351 t15 = t2*a;
5352 t16 = t3*e;
5353 t17 = t3*c;
5354 t18 = t7*e;
5355 t19 = t3*d;
5356 t20 = t12*b;
5357 t21 = t2*t12;
5358 t22 = e*e;
5359 t24 = c*c;
5360 t25 = t24*t22;
5361 t26 = t25*t11;
5362 t27 = t12*t22;
5363 t28 = t27*t5;
5364 t29 = t24*t12;
5365 t30 = t29*t6;
5366 t31 = t27*t7;
5367 t32 = d*d;
5368 t33 = t32*t12;
5369 t34 = t33*t5;
5370 t35 = t33*t8;
5371 t36 = t29*t4;
5372 t37 = t33*t1;
5373 t38 = t12*a;
5374 t39 = t38*b;
5375 t45 = t21*t22-t26-t28+t30+t31-t34+t35-t36-t37+t39*t7+
5376 t39*t8-t38*c*t6+t39*t6;
5377 t46 = t2*b;
5378 t47 = t46*a;
5379 t49 = t15*t18;
5380 t52 = t3*t22*c*d;
5381 t55 = t24*d*e*t3;
5382 t56 = t2*t22;
5383 t57 = t56*t7;
5384 t59 = c*t32*t16;
5385 t62 = t7*e*t12*b;
5386 t63 = t56*t9;
5387 t64 = t56*t11;
5388 t65 = t24*t32;
5389 t66 = t65*t3;
5390 t67 = t46*c;
5391 t69 = t2*t32;
5392 t70 = t69*t9;
5393 t72 = t69*t8;
5394 t73 = t47*t8-3.0*t49+2.0*t52+2.0*t55+t57+2.0*t59-3.0*t62-t63-t64+t66+
5395 t67*t11-t70+t47*t6+t72;
5396 t75 = t2*t24;
5397 t76 = t75*t10;
5398 t77 = t69*t10;
5399 t78 = t75*t11;
5400 t79 = t32*t22;
5401 t80 = t79*t9;
5402 t81 = t79*t3;
5403 t82 = t75*t6;
5404 t83 = t25*t3;
5405 t84 = t65*t10;
5406 t85 = t65*t1;
5407 t86 = t79*t5;
5408 t87 = t25*t4;
5409 t88 = t27*t4;
5410 t90 = -t76-t77-t78-t80+t81+t82+t83-t84-t85-t86-t87-t88-t67*t6;
5411 t91 = t29*t1;
5412 t92 = t21*t8;
5413 t93 = t21*t6;
5414 t95 = t21*t7;
5415 t96 = t21*t24;
5416 t97 = t46*t12;
5417 t99 = t2*t38;
5418 t101 = t65*t22;
5419 t107 = -t91+2.0*t92+2.0*t93+t46*t38+2.0*t95+t96-t97*c-t99*c+t101+t21*t32-
5420 t99*d-t99*e-t97*e-t97*d;
5421 t110 = sqrt(t45+t73+t90+t107);
5422
5423 X = t14*(2.0*t15-2.0*t16-2.0*t17+2.0*t18-2.0*t19+2.0*t20+2.0*t110)/2;
5424
5425 if(c-b > 0){
5426
5427 // X might be near U[i+2] but due to floating point error, it won't
5428 // be detected. It happens during tests, so it's possible...
5429 /*
5430 if(absolute(X-c)<0.0001*c){
5431 Error error("maxInfluence");
5432 error << "A numerical error in computing the point of maximal influence" ;
5433 error.warning() ;
5434 u = X ;
5435 return 1 ;
5436 }
5437 */
5438
5439 if(X> b && X<=c + 1e-6 ){ // adding 1e-6 because of float->double conversions
5440 u = (T)X ;
5441 return 1 ;
5442 }
5443
5444 X = t14*(2.0*t15-2.0*t16-2.0*t17+2.0*t18-2.0*t19+2.0*t20-2.0*t110)/2;
5445 if(X>b && X<=c){
5446 u = (T)X ;
5447 return 1 ;
5448 }
5449 }
5450
5451 t115 = -t9-t32-t6+t1-t3+t4+t10+t11+t8-t5-t22+t7;
5452 t116 = 1/t115;
5453 t117 = t6*a;
5454 t118 = t4*e;
5455 t119 = d*t22;
5456 t120 = t32*e;
5457 t121 = -t26+t28+t30-t31+t34-t35-t36-t37+2.0*t49-3.0*t52+2.0*t55-t57-3.0*
5458 t59;
5459 t122 = 2.0*t62+t63-t64+t66+t70-t72-t76-t77-t78+2.0*t80+2.0*t81+t82+t83-
5460 t84;
5461 t124 = t32*d;
5462 t125 = t124*b;
5463 t127 = t124*t22;
5464 t133 = t22*e;
5465 t135 = -t85+2.0*t86-t87-t88-t91-t125*t9-t127*c+t125*t8+t125*t10+t124*a*t8
5466 +t124*t133-t92+t93;
5467 t136 = t133*b;
5468 t143 = t32*t133;
5469 t151 = -t95+t136*t7-t136*t9+t133*c*t11+t136*t11+t69*t22+t96+t101-t143*c-b
5470 *t32*t133-t125*t22-t143*a-t127*a+t79*t12;
5471 t154 = sqrt(t121+t122+t135+t151);
5472
5473
5474
5475 X=t116*(2.0*t117+2.0*t118-2.0*t17-2.0*t119-2.0*t120+2.0*t18+2.0*t154)/2.0;
5476
5477 if(X>=c - 1e-6 && X<d){
5478 u = (T)X ;
5479 return 1 ;
5480 }
5481
5482 X=t116*(2.0*t117+2.0*t118-2.0*t17-2.0*t119-2.0*t120+2.0*t18-2.0*t154)/2.0;
5483
5484 if(X>=c && X<d){
5485 u = (T)X ;
5486 return 1 ;
5487 }
5488
5489 #ifdef USE_EXCEPTION
5490 throw NurbsComputationError();
5491 #else
5492 Error error("maxInfluence") ;
5493 error << "It seems the program couldn't find a proper maxInfluence point\n." ;
5494 error << "so far u is set to " << u << endl ;
5495 error << "and the input arguments were \n" ;
5496 error << "i = " << i << endl ;
5497 error << "U = " << U << endl ;
5498 error << "p = " << p << endl ;
5499 error.warning() ;
5500 return 0 ;
5501 #endif
5502 }
5503 default:{
5504 #ifdef USE_EXCEPTION
5505 throw NurbsInputError();
5506 #else
5507 Error error("maxInfluence");
5508 error << "The point of maximal influence is only computed for a degree of 3 or lower." ;
5509 error << "The degree given was " << p << endl ;
5510 error.warning() ;
5511 return 0 ;
5512 #endif
5513 }
5514 }
5515 return 0;
5516 }
5517
5518 /*!
5519 \brief Computes the basis function
5520 \relates NurbsCurve NurbsSurface
5521
5522 Computes the \a i basis function of degree \a p at
5523 parameter \a u.
5524
5525 \latexonly
5526 This is often noted as $N_{ip}(u)$.
5527
5528 The B-spline basis function of $p$-degree is defined as
5529 \begin{eqnarray}
5530 N_{i,0}(u) & = & \left\{ \begin{array}{ll} 1 & \mbox{if $u_i \leq u < u_{i+1}$} \\ 0 & \mbox{otherwise}\end{array}\right. \nonumber \\
5531 N_{i,p}(u) & = & \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u) \nonumber
5532 \end{eqnarray}
5533
5534 where the $u_i$ define the knot vector $U = \{u_0,\ldots,u_m\}$
5535 as a nondecreasing sequence of real numbers, {\em i.e.},
5536 $u_i \leq u_{i+1}$ for $i=0,\ldots,m-1$. And $m$ is related
5537 to the number of control points $n$ and the degree of the curve
5538 $p$ with the relation $m = n + p + 1$. The knot vector has
5539 the form
5540
5541 \begin{equation}
5542 U=\{\underbrace{a,\ldots,a}_{p+1},u_{p+1},\ldots,u_{m-p-1},\underbrace{b,\ldots,b}_{p+1} \}
5543 \end{equation}
5544
5545 \endlatexonly
5546 \htmlonly
5547 You can have more information in the LaTeX version.
5548 \endhtmlonly
5549
5550 \param u the parametric variable
5551 \param i specifies which basis function to compute
5552 \param p the degree to which the basis function is computed
5553 \param U the knot vector
5554
5555 \return the value of \a N_{ip}(u)
5556
5557 \author Philippe Lavoie
5558 \date 24 January 1997
5559 */
5560 template <class T>
nurbsBasisFun(T u,int i,int p,const Vector<T> & U)5561 T nurbsBasisFun(T u, int i, int p, const Vector<T>& U) {
5562 T Nip ;
5563 T saved,Uleft,Uright,temp ;
5564
5565 if(p<1){
5566 #ifdef USE_EXCEPTION
5567 throw NurbsInputError();
5568 #else
5569 Error error("nurbsBasisFun") ;
5570 error << "You need to specify a valid degree for the basis function!\n" ;
5571 error << "p = " << p << " but it requires a value >0.\n" ;
5572 error.fatal() ;
5573 #endif
5574 }
5575
5576 if((i==0 && u == U[p]) ||
5577 (i == U.n()-p-2 && u==U[U.n()-p-1])){
5578 Nip = 1.0 ;
5579 return Nip ;
5580 }
5581 if(u<U[i] || u>=U[i+p+1]){
5582 Nip = 0.0 ;
5583 return Nip;
5584 }
5585
5586 T* N = (T*) alloca((p+1)*sizeof(T)) ; // Vector<T> N(p+1) ;
5587
5588 int j ;
5589 for(j=0;j<=p;j++){
5590 if(u>=U[i+j] && u<U[i+j+1])
5591 N[j] = 1.0 ;
5592 else
5593 N[j] = 0.0 ;
5594 }
5595 for(int k=1; k<=p ; k++){
5596 if(N[0] == 0.0)
5597 saved = 0.0 ;
5598 else
5599 saved = ( (u-U[i])*N[0])/(U[i+k]-U[i]) ;
5600 for(j=0;j<p-k+1;j++){
5601 Uleft = U[i+j+1] ;
5602 Uright = U[i+j+k+1] ;
5603 if(N[j+1]==0.0){
5604 N[j] = saved ;
5605 saved = 0.0 ;
5606 }
5607 else {
5608 temp = N[j+1]/(Uright-Uleft) ;
5609 N[j] = saved+(Uright-u)*temp ;
5610 saved = (u-Uleft)*temp ;
5611 }
5612 }
5613 }
5614 Nip = N[0] ;
5615
5616 return Nip ;
5617 }
5618
5619 template <class T, int N>
5620 struct LengthData {
5621 int span ;
5622 const NurbsCurve<T,N>* c ;
LengthDataPLib::LengthData5623 LengthData(const NurbsCurve<T,N>* curve): c(curve) { }
5624 };
5625
5626 template <class T, int N>
5627 struct OpLengthFcn : public ClassPOvoid<T> {
operator ()PLib::OpLengthFcn5628 T operator()(T a, void* pnt){
5629 LengthData<T,N>* p = (LengthData<T,N>*)pnt ;
5630 return (p->c)->lengthF(a,p->span) ;
5631 }
5632 };
5633
5634
5635 /*!
5636 \brief Computes the length of the curve
5637
5638 Computes an approximation of the length of the curve
5639 using a numerical automatic integrator.
5640
5641 That integrator uses a Chebyshev Series Expansion
5642 to perform its approximation. This is why you can
5643 change the value $n$ which sets the number of
5644 elements in the series.
5645
5646 The method is simple, integrate between each span.
5647 This is necessary in case the tangant of a point
5648 at u_i is undefined. Add the result and return
5649 this as the approximation.
5650
5651 \param n the number of element in the Chebyshev series
5652 \param eps the accepted relative error
5653
5654 \return the length of the NURBS curve.
5655
5656 \author Philippe Lavoie
5657 \date 22 September 1998
5658 */
5659 template <class T, int N>
5660 T NurbsCurve<T,N>::length(T eps,int n) const {
5661 T l = T() ;
5662 T err ;
5663
5664 static Vector<T> bufFcn ;
5665
5666 if(bufFcn.n() != n){
5667 bufFcn.resize(n) ;
5668 intccini(bufFcn) ;
5669 }
5670
5671 LengthData<T,N> data(this) ;
5672 OpLengthFcn<T,N> op;
5673
5674 for(int i=deg_;i<P.n();++i){
5675 if(U[i] >= U[i+1])
5676 continue ;
5677 data.span = i ;
5678 l += intcc((ClassPOvoid<T>*)&op,(void*)&data,U[i],U[i+1],eps,bufFcn,err) ;
5679 }
5680 return l ;
5681 }
5682
5683 /*!
5684 \brief Computes the length of the curve inside [u_s,u_e]
5685
5686 Computes an approximation of the length of the curve
5687 using a numerical automatic integrator. The length
5688 is computed for the range [u_s,u_e]
5689
5690 That integrator uses a Chebyshev Series Expansion
5691 to perform its approximation. This is why you can
5692 change the value \a n which sets the number of
5693 elements in the series.
5694
5695 The method is similar to the one used by length
5696 excepted that it needs to check for the range.
5697
5698 \param us the starting range
5699 \param ue the end of the range
5700 \param n the number of element in the Chebyshev series
5701 \param eps the accepted relative error
5702
5703 \return the length of the NURBS curve.
5704
5705 \warning ue must be greater than us and both must be in a valid range.
5706
5707 \author Philippe Lavoie
5708 \date 22 September 1998
5709 */
5710 template <class T, int N>
5711 T NurbsCurve<T,N>::lengthIn(T us, T ue,T eps, int n) const {
5712 T l = T() ;
5713 T err ;
5714
5715 static Vector<T> bufFcn ;
5716
5717 if(bufFcn.n() != n){
5718 bufFcn.resize(n) ;
5719 intccini(bufFcn) ;
5720 }
5721
5722 LengthData<T,N> data(this) ;
5723 OpLengthFcn<T,N> op;
5724
5725 for(int i=deg_;i<P.n();++i){
5726 if(U[i] >= U[i+1])
5727 continue ;
5728 data.span = i ;
5729 if(i<findSpan(us))
5730 continue ;
5731 if(us>=U[i] && ue<=U[i+findMult(i)]){
5732 l = intcc((ClassPOvoid<T>*)&op,(void*)&data,us,ue,eps,bufFcn,err) ;
5733 break ;
5734 }
5735 if(us>=U[i]){
5736 l += intcc((ClassPOvoid<T>*)&op,(void*)&data,us,U[i+findMult(i)],eps,bufFcn,err) ;
5737 continue ;
5738 }
5739 if(ue<=U[i+findMult(i)]){
5740 l += intcc((ClassPOvoid<T>*)&op,(void*)&data,U[i],ue,eps,bufFcn,err) ;
5741 break ;
5742 }
5743 l += intcc((ClassPOvoid<T>*)&op,(void*)&data,U[i],U[i+findMult(i)],eps,bufFcn,err) ;
5744 }
5745 return l ;
5746 }
5747
5748 // the definitions are in f_nurbs.cpp and d_nurbs.cpp
5749
5750
5751 /*!
5752 \brief The function used by length
5753 \a length needs to integrate a function over an interval
5754 to determine the length of the NURBS curve.
5755 Well, this is the function.
5756
5757 \param u --> the parameter
5758
5759 \return square root of the square of the x,y and z value
5760
5761 \author Philippe Lavoie
5762 \date 22 September 1998
5763 */
5764 template <class T, int N>
5765 T NurbsCurve<T,N>::lengthF(T u) const {
5766 Point_nD<T,N> dd = firstDn(u) ;
5767 T tmp = sqrt(dd.x()*dd.x()+dd.y()*dd.y()+dd.z()*dd.z()) ;
5768 return tmp ;
5769 }
5770
5771 /*!
5772
5773 \a length needs to integrate a function over an interval
5774 to determine the length of the NURBS curve.
5775 Well, this is the function.
5776
5777 \param u the parameter
5778 \param span the span of the parameter
5779
5780 \return square root of the square of the x,y and z value
5781
5782 \author Philippe Lavoie
5783 \date 22 September 1998
5784 */
5785 template <class T, int N>
5786 T NurbsCurve<T,N>::lengthF(T u, int span) const {
5787 Point_nD<T,N> dd = firstDn(u,span) ;
5788 T tmp = sqrt(dd.x()*dd.x()+dd.y()*dd.y()+dd.z()*dd.z()) ;
5789 return tmp ;
5790 }
5791
5792
5793 /*!
5794 \brief Generate a straight line
5795
5796 Generate a straight line going from point P0 to point P1
5797 of degree \a d.
5798
5799 \param P0 the beginning of the line
5800 \param P1 the end of the line
5801 \param d the degree of the curve
5802
5803 \warning \a d must be greater or equal to 1
5804 \author Philippe Lavoie
5805 \date 22 September 1998
5806 */
5807 template <class T, int N>
makeLine(const Point_nD<T,N> & P0,const Point_nD<T,N> & P1,int d)5808 void NurbsCurve<T,N>::makeLine(const Point_nD<T,N>& P0, const Point_nD<T,N>& P1, int d) {
5809 if(d<2)
5810 d = 2 ;
5811 resize(2,1) ;
5812 P[0] = HPoint_nD<T,N>(P0) ;
5813 P[1] = HPoint_nD<T,N>(P1) ;
5814 U[0] = U[1] = 0 ;
5815 U[2] = U[3] = 1 ;
5816 degreeElevate(d-1) ;
5817 }
5818
5819 /*!
5820 \brief Computes the first derivative
5821 Computes the first derivative in the 4D homogenous space.
5822
5823 \param u --> compute the derivative at this parameter
5824
5825 \return The first derivative in homogenous space
5826
5827 \warning \a u must be in the valid range
5828
5829 \author Philippe Lavoie
5830 \date 13 October 1998
5831 */
5832 template <class T, int D>
firstD(T u) const5833 HPoint_nD<T,D> NurbsCurve<T,D>::firstD(T u) const {
5834 int span = findSpan(u) ;
5835 int i ;
5836
5837 static Vector<T> N ;
5838
5839 nurbsBasisFuns(u,span,deg_-1,U,N) ;
5840
5841 HPoint_nD<T,D> Cd(0,0,0,0) ;
5842 HPoint_nD<T,D> Qi ;
5843
5844 for(i=deg_-1;i>=0;--i){
5845 int j = span-deg_+i ;
5846 Qi = (P[j+1]-P[j]);
5847 Qi *= T(deg_)/(U[j+deg_+1]-U[j+1]) ;
5848 Cd += N[i]*Qi ;
5849 }
5850
5851 return Cd ;
5852 }
5853
5854 /*!
5855 \brief Computes the first derivative
5856
5857 Computes the first derivative in the honogenous space with the span given.
5858
5859 \param u compute the derivative at this parameter
5860 \param span the span of u
5861
5862 \return The first derivative of the point in the homogoneous space
5863
5864 \warning \a u and span must be in a valid range
5865
5866 \author Philippe Lavoie
5867 \date 13 October 1998
5868 */
5869 template <class T, int D>
firstD(T u,int span) const5870 HPoint_nD<T,D> NurbsCurve<T,D>::firstD(T u, int span) const {
5871 int i ;
5872
5873 static Vector<T> N ;
5874
5875 nurbsBasisFuns(u,span,deg_-1,U,N) ;
5876
5877 HPoint_nD<T,D> Cd(0,0,0,0) ;
5878 HPoint_nD<T,D> Qi ;
5879
5880 for(i=deg_-1;i>=0;--i){
5881 int j = span-deg_+i ;
5882 Qi = (P[j+1]-P[j]);
5883 Qi *= T(deg_)/(U[j+deg_+1]-U[j+1]) ;
5884 Cd += N[i]*Qi ;
5885 }
5886
5887 return Cd ;
5888 }
5889
5890
5891 /*!
5892 \brief Computes the first derivative
5893
5894 Computes the first derivative in the normal space.
5895
5896 \param u compute the derivative at this parameter
5897 \param span the span of u
5898
5899 \return The first derivative in normal space
5900
5901 \warning \a u and span must be in a valid range
5902 \author Philippe Lavoie
5903 \date 13 October 1998
5904 */
5905 template <class T, int N>
firstDn(T u) const5906 Point_nD<T,N> NurbsCurve<T,N>::firstDn(T u) const {
5907 int span = findSpan(u) ;
5908 Point_nD<T,N> Cp ;
5909 HPoint_nD<T,N> Cd ;
5910 Cd = firstD(u,span) ;
5911
5912 Point_nD<T,N> pd(Cd.projectW()) ;
5913 T w = Cd.w() ;
5914 Cd = hpointAt(u,span) ;
5915 pd -= w*project(Cd) ;
5916 pd /= Cd.w() ;
5917
5918 return pd ;
5919 }
5920
5921 /*!
5922 \brief Computes the first derivative
5923
5924 Computes the first derivative in the normal space (3D or 2D).
5925
5926 \param u --> compute the derivative at this parameter
5927 \param span --> the span of \a u
5928
5929 \warning \a u and \a span must be in a valid range
5930
5931 \author Philippe Lavoie
5932 \date 13 October 1998
5933 */
5934 template <class T, int N>
firstDn(T u,int span) const5935 Point_nD<T,N> NurbsCurve<T,N>::firstDn(T u, int span) const {
5936 int i ;
5937 Point_nD<T,N> Cp ;
5938 HPoint_nD<T,N> Cd ;
5939 Cd = firstD(u,span) ;
5940
5941 Point_nD<T,N> pd(Cd.projectW()) ;
5942 T w = Cd.w() ;
5943 Cd = hpointAt(u,span) ;
5944 pd -= w*project(Cd) ;
5945 pd /= Cd.w() ;
5946
5947 return pd ;
5948 }
5949
5950 /*!
5951 \brief A least squares curve approximation for closed curves
5952
5953 \latexonly
5954 This routine solves the following problem: find the NURBS curve
5955 $C$ satisfying
5956 \begin{itemize}
5957 \item the $Q_k$ are approximated in the least squares
5958 sense, {\em i.e.}
5959 \[ \sum_{k=0}^{m} | Q_k-C(\bar{u}_k)|^2 \]
5960 in a minimum with respect to the $n$ variable $P_i$; the
5961 $\bar{u}$ are the precomputed parameter values.
5962 \end{itemize}
5963
5964 The resulting curve will generally not pass through $Q_k$ and
5965 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
5966 \endlatexonly
5967 \htmlonly
5968 This routine finds a closed NURBS curve that satisfy a least
5969 square criteria. The resulting curve will generally not pass
5970 through the input points (except the first point).
5971 \endhtmlonly
5972
5973 For more details, see section 9.4.1 on page 491 of the NURBS
5974 book.
5975
5976 \param Qw the vector of 3D points (wrapped around)
5977 \param degC the degree of the curve
5978 \param nCP the number of (distinct) control points in the new curve
5979
5980 \author Alejandro Frangi
5981 \date 30 July 1998
5982 */
5983 template <class T, int N>
leastSquaresClosed(const Vector<Point_nD<T,N>> & Qw,int degC,int nCP)5984 int NurbsCurve<T,N>::leastSquaresClosed(const Vector< Point_nD<T,N> >& Qw, int degC, int nCP){
5985
5986 Vector<T> ub;
5987 Vector<T> Uk;
5988 chordLengthParamClosed(Qw,ub,degC) ;
5989 return leastSquaresClosed(Qw,degC,nCP,ub);
5990 }
5991
5992 /*!
5993
5994 \brief A least squares curve approximation for closed curves
5995
5996 \latexonly
5997 This routine solves the following problem: find the NURBS curve
5998 $C$ satisfying
5999 \begin{itemize}
6000 \item the $Q_k$ are approximated in the least squares
6001 sense, {\em i.e.}
6002 \[ \sum_{k=0}^{m} | Q_k-C(\bar{u}_k)|^2 \]
6003 in a minimum with respect to the $n$ variable $P_i$; the
6004 $\bar{u}$ are the precomputed parameter values.
6005 \end{itemize}
6006
6007 The resulting curve will generally not pass through $Q_k$ and
6008 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
6009 \endlatexonly
6010 \htmlonly
6011 This routine finds a closed NURBS curve that satisfy a least
6012 square criteria. The resulting curve will generally not pass
6013 through the input points (except the first point).
6014 \endhtmlonly
6015
6016 For more details, see section 9.4.1 on page 491 of the NURBS
6017 book.
6018
6019 \param Qw the vector of 3D points (wrapped around)
6020 \param degC the degree of the curve
6021 \param nCP the number of (distinct) control points in the new curve
6022 \param ub the parameter values of Qw obtained via chordLengthParamClosed()
6023
6024 \author Alejandro Frangi
6025 \date 30 July 1998
6026 */
6027 template <class T, int N>
leastSquaresClosed(const Vector<Point_nD<T,N>> & Qw,int degC,int nCP,const Vector<T> & ub)6028 int NurbsCurve<T,N>::leastSquaresClosed(const Vector< Point_nD<T,N> >& Qw, int degC, int nCP, const Vector<T>& ub){
6029 Vector<T> Uk;
6030 knotApproximationClosed(Uk,ub,nCP+degC-1,degC);
6031 return leastSquaresClosed(Qw,degC,nCP,ub,Uk);
6032 }
6033
6034 /*!
6035 \brief A least squares curve approximation for closed curves
6036
6037 \latexonly
6038 This routine solves the following problem: find the NURBS curve
6039 $C$ satisfying
6040 \begin{itemize}
6041 \item the $Q_k$ are approximated in the least squares
6042 sense, {\em i.e.}
6043 \[ \sum_{k=0}^{m} | Q_k-C(\bar{u}_k)|^2 \]
6044 in a minimum with respect to the $n$ variable $P_i$; the
6045 $\bar{u}$ are the precomputed parameter values.
6046 \end{itemize}
6047
6048 The resulting curve will generally not pass through $Q_k$ and
6049 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
6050 \endlatexonly
6051 \htmlonly
6052 This routine finds a closed NURBS curve that satisfy a least
6053 square criteria. The resulting curve will generally not pass
6054 through the input points (except the first point).
6055 \endhtmlonly
6056
6057 For more details, see section 9.4.1 on page 491 of the NURBS
6058 book.
6059
6060 \param Qw the vector of 4D points (wrapped around)
6061 \param degC the degree of the curve
6062 \param nCP the number of (distinct) control points in thenew curve
6063 \param ub the parameter values of Qw obtained via chordLengthParamClosed()
6064
6065 \author Alejandro Frangi
6066 \date 30 July 1998
6067 */
6068 template <class T, int N>
leastSquaresClosedH(const Vector<HPoint_nD<T,N>> & Qw,int degC,int nCP,const Vector<T> & ub)6069 int NurbsCurve<T,N>::leastSquaresClosedH(const Vector< HPoint_nD<T,N> >& Qw, int degC, int nCP, const Vector<T>& ub){
6070
6071 Vector<T> Uk;
6072 knotApproximationClosed(Uk,ub,nCP+degC-1,degC);
6073
6074 return leastSquaresClosedH(Qw,degC,nCP,ub,Uk);
6075 }
6076
6077 /*!
6078 \brief A least squares curve approximation for closed curves
6079
6080 \latexonly
6081 This routine solves the following problem: find the NURBS curve
6082 $C$ satisfying
6083 \begin{itemize}
6084 \item the $Q_k$ are approximated in the least squares
6085 sense, {\em i.e.}
6086 \[ \sum_{k=0}^{m} | Q_k-C(\bar{u}_k)|^2 \]
6087 in a minimum with respect to the $n$ variable $P_i$; the
6088 $\bar{u}$ are the precomputed parameter values.
6089 \end{itemize}
6090
6091 The resulting curve will generally not pass through $Q_k$ and
6092 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
6093 \endlatexonly
6094 \htmlonly
6095 This routine finds a closed NURBS curve that satisfy a least
6096 square criteria. The resulting curve will generally not pass
6097 through the input points (except the first point).
6098 \endhtmlonly
6099
6100 For more details, see section 9.4.1 on page 491 of the NURBS
6101 book.
6102
6103 \param Qw the vector of 4D points (wrapped around)
6104 \param degC the degree of the curve
6105 \param nCP the number of (distinct) control points in the new curve
6106 \param ub the parameter values of Qw obtained via chordLengthParamClosed()
6107 \param knots the knots for the control points obtained via knotApproximationClosed()
6108
6109 \author Alejandro Frangi
6110 \date 30 July 1998
6111 */
6112 template <class T, int D>
leastSquaresClosed(const Vector<Point_nD<T,D>> & Qw,int degC,int nCP,const Vector<T> & ub,const Vector<T> & knots)6113 int NurbsCurve<T,D>::leastSquaresClosed(const Vector< Point_nD<T,D> >& Qw, int degC, int nCP, const Vector<T>& ub, const Vector<T>& knots){
6114 resize(nCP+degC,degC) ;
6115
6116 int n = P.n()-1;
6117 int iN = nCP-1;
6118 int iM = Qw.n()-degC-1;
6119 int p = degC ;
6120
6121 if(ub.n() != Qw.n()){
6122 #ifdef USE_EXCEPTION
6123 throw NurbsInputError(ub.n(),Qw.n());
6124 #else
6125 Error err("leastSquaresClosed");
6126 err << "leastSquaresCurveC\n" ;
6127 err << "ub size is different than Qw's\n" ;
6128 err.fatal();
6129 #endif
6130 }
6131
6132 if( knots.n() != U.n()){
6133 #ifdef USE_EXCEPTION
6134 throw NurbsInputError(knots.n(),U.n());
6135 #else
6136 Error err("leastSquaresClosed");
6137 err << "The knot vector supplied doesn't have the proper size.\n" ;
6138 err << "It should be n+degC+1 = " << U.n() << " and it is " << knots.n() << endl ;
6139 err.fatal() ;
6140 #endif
6141 }
6142
6143 Matrix_DOUBLE N(iM+1,iN+1);
6144 Matrix_DOUBLE A(iN+1,iN+1);
6145 Matrix_DOUBLE R(iN+1,D);
6146 Matrix_DOUBLE Pi(iN+1,D);
6147
6148 // Load knot vector
6149 U = knots;
6150
6151 // Form matrix N (Eq. 9.66) p. 411
6152 N = 0;
6153 for (int i=0; i<=n; i++)
6154 for (int k=0; k<=iM; k++)
6155 N(k,i%(iN+1)) += basisFun(ub[k],i,p) ;
6156
6157 // Form R (Eq. 9.67)
6158 R.reset(0.0);
6159 for (int i=0; i<=iN; i++)
6160 for (int k=0; k<=iM; k++){
6161 const Point_nD<T,D>& qp = Qw[k] ; // this makes the SGI compiler happy
6162 const double& Nki = N(k,i);
6163 for (int j=0; j<D; j++)
6164 R(i,j) += Nki * qp.data[j] ;
6165 }
6166
6167 // The system to solve
6168 A = N.transpose() * N ;
6169 solve(A,R,Pi);
6170
6171 // Update control points
6172 for (int i=0; i<= n; i++)
6173 for (int k=0; k<D; k++){
6174 P[i].data[k] = Pi(i%(iN+1),k) ;
6175 P[i].w() = 1.0;
6176 }
6177 return 1;
6178 }
6179
6180
6181 /*!
6182 \brief A least squares curve approximation for closed curves
6183
6184 \latexonly
6185 This routine solves the following problem: find the NURBS curve
6186 $C$ satisfying
6187 \begin{itemize}
6188 \item the $Q_k$ are approximated in the least squares
6189 sense, {\em i.e.}
6190 \[ \sum_{k=0}^{m} | Q_k-C(\bar{u}_k)|^2 \]
6191 in a minimum with respect to the $n$ variable $P_i$; the
6192 $\bar{u}$ are the precomputed parameter values.
6193 \end{itemize}
6194
6195 The resulting curve will generally not pass through $Q_k$ and
6196 $C(\bar{u}_k)$ is not the closest point on $C(u)$ to $Q_k$.
6197 \endlatexonly
6198 \htmlonly
6199 This routine finds a closed NURBS curve that satisfy a least
6200 square criteria. The resulting curve will generally not pass
6201 through the input points (except the first point).
6202 \endhtmlonly
6203
6204 For more details, see section 9.4.1 on page 491 of the NURBS
6205 book.
6206
6207 \param Qw the vector of 4D points (wrapped around)
6208 \param degC the degree of the curve
6209 \param nCP the number of (distinct) control points in the new curve
6210 \param ub the parameter values of Qw obtained via chordLengthParamClosed()
6211 \param knots the knots for the control points obtained via knotApproximationC()
6212
6213 \author Alejandro Frangi
6214 \date 30 July 1998
6215 */
6216 template <class T, int D>
leastSquaresClosedH(const Vector<HPoint_nD<T,D>> & Qw,int degC,int nCP,const Vector<T> & ub,const Vector<T> & knots)6217 int NurbsCurve<T,D>::leastSquaresClosedH(const Vector< HPoint_nD<T,D> >& Qw, int degC, int nCP, const Vector<T>& ub, const Vector<T>& knots){
6218
6219 resize(nCP+degC,degC) ;
6220
6221 int n = P.n()-1;
6222 int iN = nCP-1;
6223 int iM = Qw.n()-degC-1;
6224 int p = degC ;
6225
6226
6227 if(ub.n() != Qw.n()){
6228 #ifdef USE_EXCEPTION
6229 throw NurbsInputError(ub.n(),Qw.n());
6230 #else
6231 Error err("leastSquaresClosedH");
6232 err << "leastSquaresCurveC\n" ;
6233 err << "ub size is different than Qw's\n" ;
6234 err.fatal();
6235 #endif
6236 }
6237
6238 if( knots.n() != U.n()){
6239 #ifdef USE_EXCEPTION
6240 throw NurbsInputError(knots.n(),U.n());
6241 #else
6242 Error err("leastSquaresClosed");
6243 err << "The knot vector supplied doesn't have the proper size.\n" ;
6244 err << "It should be n+degC+1 = " << U.n() << " and it is " << knots.n() << endl ;
6245 err.fatal() ;
6246 #endif
6247 }
6248
6249 Matrix_DOUBLE N(iM+1,iN+1);
6250 Matrix_DOUBLE A(iN+1,iN+1);
6251 Matrix_DOUBLE R(iN+1,D+1);
6252 Matrix_DOUBLE Pi(iN+1,D+1);
6253
6254 // Load knot vector
6255 U = knots;
6256
6257 // Form matrix N (Eq. 9.66) p. 411
6258 N = 0;
6259 for (int i=0; i<=n; i++)
6260 for (int k=0; k<=iM; k++)
6261 N(k,i%(iN+1)) += basisFun(ub[k],i,p) ;
6262
6263 // Form R (Eq. 9.67)
6264 R.reset(0.0);
6265 for (int i=0; i<=iN; i++)
6266 for (int k=0; k<=iM; k++){
6267 const HPoint_nD<T,D>& qp = Qw[k] ; // this makes the SGI compiler happy
6268 const double& Nki = N(k,i);
6269 for (int j=0; j<D+1; j++)
6270 R(i,j) += Nki * qp.data[j] ;
6271 }
6272
6273 // The system to solve
6274 A = N.transpose() * N ;
6275 solve(A,R,Pi);
6276
6277 // Update control points
6278 for (int i=0; i<= n; i++)
6279 for (int k=0; k<D+1; k++){
6280 P[i].data[k] = Pi(i%(iN+1),k) ;
6281 //P[i].w() = 1.0;
6282 }
6283 return 1;
6284 }
6285
6286
6287 /*!
6288 \brief chord length parameterization for a closed curve
6289
6290 Performs chord length parameterization from a vector of
6291 points that are supposed to form a closed curve.
6292
6293 \param Qw a vector of 3D points (already wrapped around)
6294 \param ub the result of chord length parameterization
6295 \param deg the degree of the curve
6296
6297 \return the total chord length of the points.
6298 \author Alejandro Frangi
6299 \date 30 July, 1998
6300 */
6301 template <class T, int N>
6302 T chordLengthParamClosed(const Vector< Point_nD<T,N> >& Qw, Vector<T> &ub,int deg){
6303
6304 int i ;
6305 T d = 0.0 ;
6306
6307 ub.resize(Qw.n()) ;
6308 ub[0] = 0 ;
6309 for(i=1;i<=ub.n()-deg;i++){
6310 d += norm(Qw[i]-Qw[i-1]) ;
6311 }
6312 if(d>0){
6313 for(i=1;i<ub.n();++i)
6314 ub[i] = ub[i-1] + norm(Qw[i]-Qw[i-1]);
6315 // Normalization
6316 for(i=0;i<ub.n();++i)
6317 ub[i]/= d;
6318 }
6319 else
6320 for(i=1;i<ub.n();++i)
6321 ub[i] = (T)(i)/(T)(ub.n()-2) ;
6322
6323 return d ;
6324 }
6325
6326 /*!
6327 \brief chord length parameterization for a closed curve
6328
6329 Performs chord length parameterization from a vector of
6330 points that are supposed to form a closed curve.
6331
6332 \param Q a vector of 3D points (wrapped around)
6333 \param ub the result of chord length parameterization
6334 \param deg the degree of the curve
6335
6336 \return the total chord length of the points.
6337 \author Alejandro Frangi
6338 \date 30 July, 1998
6339 */
6340 template <class T, int N>
6341 T chordLengthParamClosedH(const Vector< HPoint_nD<T,N> >& Qw, Vector<T> &ub,int deg){
6342
6343 int i ;
6344 T d = 0.0 ;
6345
6346 ub.resize(Qw.n()) ;
6347 ub[0] = 0 ;
6348 for(i=1;i<ub.n()-deg+1;i++){
6349 d += norm(Qw[i]-Qw[i-1]) ;
6350 }
6351 if(d>0){
6352 for(i=1;i<ub.n();++i)
6353 ub[i] = ub[i-1] + norm(Qw[i]-Qw[i-1]);
6354 // Normalization
6355 for(i=0;i<ub.n();++i)
6356 ub[i]/=ub[ub.n()-deg] ;
6357 }
6358 else
6359 for(i=1;i<ub.n();++i)
6360 ub[i] = (T)(i)/(T)(ub.n()-deg) ;
6361
6362 return d ;
6363 }
6364
6365
6366 /*!
6367 \brief refine the closed curve knot vector
6368
6369 Adapted from algorithm A5.4 on page 164 of the NURBS book.
6370
6371 \param X the knot vector to refine with
6372
6373 \author Alejandro F Frangi
6374 \date 17 July, 1998
6375 */
6376 template <class T, int N>
refineKnotVectorClosed(const Vector<T> & X)6377 void NurbsCurve<T,N>::refineKnotVectorClosed(const Vector<T>& X){
6378
6379 int n = P.n()-1 ;
6380 int p = deg_ ;
6381 int m = n+p+1 ;
6382 int a,b ;
6383 int r = X.n()-1 ;
6384
6385
6386 NurbsCurve<T,N> c(*this) ;
6387 resize(r+1+n+1,p) ;
6388 a = c.findSpan(X[0]) ;
6389 b = c.findSpan(X[r]) ;
6390 ++b ;
6391 int j ;
6392
6393 for(j=0; j<=a-p ; j++)
6394 P[j] = c.P[j] ;
6395 for(j = b-1 ; j<=n ; j++)
6396 P[j+r+1] = c.P[j] ;
6397 for(j=0; j<=a ; j++)
6398 U[j] = c.U[j] ;
6399 for(j=b+p ; j<=m ; j++)
6400 U[j+r+1] = c.U[j] ;
6401
6402 int i = b+p-1 ;
6403 int k = b+p+r ;
6404 for(j=r; j>=0 ; j--){
6405 while(X[j] <= c.U[i] && i>a){
6406 int ind = i-p-1;
6407 if (ind<0)
6408 ind += n + 1 ;
6409 P[k-p-1] = c.P[ind] ;
6410 U[k] = c.U[i] ;
6411 --k ;
6412 --i ;
6413 }
6414 P[k-p-1] = P[k-p] ;
6415 for(int l=1; l<=p ; l++){
6416 int ind = k-p+l ;
6417 T alpha = U[k+l] - X[j] ;
6418 if(alpha==0.0)
6419 P[ind-1] = P[ind] ;
6420 else {
6421 alpha /= U[k+l]-c.U[i-p+l] ;
6422 P[ind-1] = alpha*P[ind-1] + (1.0-alpha)*P[ind] ;
6423 }
6424 }
6425 U[k] = X[j] ;
6426 --k ;
6427 }
6428 }
6429
6430
6431 /*!
6432 \brief global closed curve interpolation with a list of points
6433
6434 Global curve interpolation with points in 3D. This
6435 function will generate a closed curve with C(d-1)
6436 continuity between the parameters u=0 and u=1
6437
6438 \param Q the 3D points to interpolate
6439 \param d the degree of the interpolation
6440
6441 \warning The number of points to interpolate must be greater than
6442 the degree specified for the curve.
6443 \author Alejandro Frangi
6444 \date 13 July, 1998
6445 */
6446 template <class T, int N>
globalInterpClosed(const Vector<Point_nD<T,N>> & Qw,int d)6447 void NurbsCurve<T,N>::globalInterpClosed(const Vector< Point_nD<T,N> >& Qw, int d){
6448 Vector<T> ub ;
6449 Vector<T> Uc;
6450
6451 chordLengthParamClosed(Qw,ub,d) ;
6452 knotAveragingClosed(ub,d,Uc);
6453
6454 globalInterpClosed(Qw,ub,Uc,d) ;
6455 }
6456
6457 /*!
6458 \brief global close curve interpolation with points in homogenous space
6459
6460
6461 Global curve interpolation with points in homogenouse space with C(d-1)
6462 continuity in the wrap-around point.
6463
6464 \param Qw the points in 4D to interpolate
6465 \param d the degree of the closed curve
6466
6467 \warning The number of points to interpolate must be greater than
6468 the degree specified for the curve. The interpolation
6469 degree is only 3. The first and last interpolation points
6470 should be equal.
6471
6472 \author Alejandro Frangi
6473 \date 13 July, 1998
6474 */
6475 template <class T, int D>
globalInterpClosedH(const Vector<HPoint_nD<T,D>> & Qw,int d)6476 void NurbsCurve<T,D>::globalInterpClosedH(const Vector< HPoint_nD<T,D> >& Qw, int d){
6477
6478 Vector<T> ub ;
6479 Vector<T> Uc;
6480
6481 chordLengthParamClosedH(Qw,ub,d) ;
6482 knotAveragingClosed(ub,d,Uc);
6483 globalInterpClosedH(Qw,ub,Uc,d);
6484
6485 }
6486
6487 /*!
6488 \brief global curve interpolation with homogenous points
6489
6490 Global curve interpolation with 4D points, a knot vector
6491 defined and the parametric value vector defined.
6492
6493 \param Q the 3D points to interpolate
6494 \param ub the parametric values vector
6495 \param d the degree of the closed curve
6496
6497 \warning The number of points to interpolate must be greater than
6498 the degree specified for the curve. Uc must be compatible with
6499 the values given for Q.n(), ub.n() and d.
6500
6501 \author Alejandro Frangi
6502 \date 13 July, 1998
6503 */
6504 template <class T, int D>
globalInterpClosed(const Vector<Point_nD<T,D>> & Qw,const Vector<T> & ub,int d)6505 void NurbsCurve<T,D>::globalInterpClosed(const Vector< Point_nD<T,D> >& Qw, const Vector<T>& ub, int d){
6506
6507 Vector<T> Uc;
6508 knotAveragingClosed(ub,d,Uc);
6509 globalInterpClosed(Qw,ub,Uc,d);
6510 }
6511
6512 /*!
6513 \brief global curve interpolation with homogenous points
6514
6515 Global curve interpolation with 4D points, a knot vector
6516 defined and the parametric value vector defined.
6517
6518 \param Q the 3D points to interpolate
6519 \param ub the parametric values vector
6520 \param d the degree of the closed curve
6521
6522 \warning The number of points to interpolate must be greater than
6523 the degree specified for the curve. Uc must be compatible with
6524 the values given for Q.n(), ub.n() and d.
6525
6526 \author Alejandro Frangi
6527 \date 13 July, 1998
6528 */
6529 template <class T, int D>
globalInterpClosedH(const Vector<HPoint_nD<T,D>> & Qw,const Vector<T> & ub,int d)6530 void NurbsCurve<T,D>::globalInterpClosedH(const Vector< HPoint_nD<T,D> >& Qw, const Vector<T>& ub, int d){
6531
6532 Vector<T> Uc;
6533 knotAveragingClosed(ub,d,Uc);
6534 globalInterpClosedH(Qw,ub,Uc,d);
6535 }
6536
6537 /*!
6538 \brief global curve interpolation with homogenous points
6539
6540 Global curve interpolation with 4D points, a knot vector
6541 defined and the parametric value vector defined.The curve will have C(d-1)
6542 continuity at the point u=0 and u=1.
6543
6544 \param Qw the 3D points to interpolate (wrapped around)
6545 \param ub the parametric values vector
6546 \param Uc the knot vector computed using knotAveragingC
6547 \param d the degree of the closed curve
6548
6549 \warning The number of points to interpolate must be greater than
6550 the degree specified for the curve. Uc must be compatible with
6551 the values given for Q.n(), ub.n().
6552 \author Alejandro Frangi
6553 \date 13 July, 1998
6554 */
6555 template <class T, int D>
globalInterpClosed(const Vector<Point_nD<T,D>> & Qw,const Vector<T> & ub,const Vector<T> & Uc,int d)6556 void NurbsCurve<T,D>::globalInterpClosed(const Vector< Point_nD<T,D> >& Qw,
6557 const Vector<T>& ub, const Vector<T>& Uc, int d){
6558 int i,j ;
6559
6560 resize(Qw.n(),d) ;
6561
6562 int iN = Qw.n() - d - 1;
6563 Matrix_DOUBLE A(iN+1,iN+1) ;
6564
6565 if(Uc.n() != U.n()){
6566 #ifdef USE_EXCEPTION
6567 throw NurbsInputError(Uc.n(),U.n());
6568 #else
6569 Error err("globalInterpClosedH");
6570 err << "Invalid dimension for the given Knot vector.\n" ;
6571 err << "U required = " << U.n() << ", U given = " << Uc.n() << endl ;
6572 err.fatal() ;
6573 #endif
6574 }
6575 U = Uc ;
6576
6577 // Initialize the basis matrix A
6578 Vector<T> N(d+1) ;
6579
6580 for(i=0;i<=iN;i++){
6581 int span = findSpan(ub[i]);
6582 basisFuns(ub[i],span,N) ;
6583 for(j=span-d;j<=span;j++)
6584 A(i,j%(iN+1)) = (double)N[j-span+d] ;
6585 }
6586
6587 // Init matrix for LSE
6588 Matrix_DOUBLE qq(iN+1,D) ;
6589 Matrix_DOUBLE xx(iN+1,D) ;
6590 for(i=0;i<=iN ;i++)
6591 for(j=0; j<D;j++)
6592 qq(i,j) = (double)Qw[i].data[j] ;
6593
6594 // AF: "solve" calls LU decomposition if A is square. I opted for
6595 // using the SVD routine which works better when the system of
6596 // equations is very large (more than 50 points). Probably since in
6597 // this cases the system matrix A is very sparse.
6598 SVDMatrix<double> svd(A) ;
6599 svd.solve(qq,xx) ;
6600
6601 // Store the data
6602 for(i=0;i<xx.rows();i++){
6603 for(j=0;j<D;j++)
6604 P[i].data[j] = (T)xx(i,j) ;
6605 P[i].w() = 1.0 ;
6606 }
6607
6608 // Wrap around of control points
6609 for(i=0;i<d;i++){
6610 for(j=0;j<D;j++)
6611 P[xx.rows()+i].data[j] = (T)xx(i,j) ;
6612 }
6613 }
6614
6615 /*!
6616 \brief global curve interpolation with homogenous points
6617
6618 Global curve interpolation with 4D points, a knot vector
6619 defined and the parametric value vector defined.The curve will have C(d-1)
6620 continuity at the point u=0 and u=1.
6621
6622 \param Qw the 3D points to interpolate (wrapped around)
6623 \param ub the parametric values vector
6624 \param Uc the knot vector computed using knotAveragingClosed
6625 \param d the degree of the closed curve
6626
6627 \warning The number of points to interpolate must be greater than
6628 the degree specified for the curve. Uc must be compatible with
6629 the values given for Q.n(), ub.n().
6630 \author Alejandro Frangi
6631 \date 13 July, 1998
6632 */
6633 template <class T, int D>
globalInterpClosedH(const Vector<HPoint_nD<T,D>> & Qw,const Vector<T> & ub,const Vector<T> & Uc,int d)6634 void NurbsCurve<T,D>::globalInterpClosedH(const Vector< HPoint_nD<T,D> >& Qw,
6635 const Vector<T>& ub, const Vector<T>& Uc, int d){
6636 int i,j ;
6637
6638 resize(Qw.n(),d) ;
6639
6640 int iN = Qw.n() - d - 1;
6641 Matrix_DOUBLE A(iN+1,iN+1) ;
6642
6643 if(Uc.n() != U.n()){
6644 #ifdef USE_EXCEPTION
6645 throw NurbsInputError(Uc.n(),U.n());
6646 #else
6647 Error err("globalInterpClosedH");
6648 err << "Invalid dimension for the given Knot vector.\n" ;
6649 err << "U required = " << U.n() << ", U given = " << Uc.n() << endl ;
6650 err.fatal() ;
6651 #endif
6652 }
6653 U = Uc ;
6654
6655 // Initialize the basis matrix A
6656 Vector<T> N(d+1) ;
6657
6658 for(i=0;i<=iN;i++){
6659 int span = findSpan(ub[i]);
6660 basisFuns(ub[i],span,N) ;
6661 for(j=span-d;j<=span;j++)
6662 A(i,j%(iN+1)) = (double)N[j-span+d] ;
6663 }
6664
6665 // Init matrix for LSE
6666 Matrix_DOUBLE qq(iN+1,D+1) ;
6667 Matrix_DOUBLE xx(iN+1,D+1) ;
6668 for(i=0;i<=iN ;i++)
6669 for(j=0; j<D+1;j++)
6670 qq(i,j) = (double)Qw[i].data[j] ;
6671
6672 // AF: "solve" calls LU decomposition if A is square. I opted for
6673 // using the SVD routine which works better when the system of
6674 // equations is very large (more than 50 points). Probably since in
6675 // this cases the system matrix A is very sparse.
6676 SVDMatrix<double> svd(A) ;
6677 svd.solve(qq,xx) ;
6678
6679 // Store the data
6680 for(i=0;i<xx.rows();i++){
6681 for(j=0;j<D+1;j++)
6682 P[i].data[j] = (T)xx(i,j) ;
6683 }
6684
6685 // Wrap around of control points
6686 for(i=0;i<d;i++){
6687 for(j=0;j<D+1;j++)
6688 P[xx.rows()+i].data[j] = (T)xx(i,j) ;
6689 }
6690 }
6691
6692 /*!
6693 \brief decompose the closed curve into B�zier segments
6694
6695 This function decomposes a closed curve into an array of B�zier
6696 segments.
6697
6698 \param c an array of B�zier segments
6699
6700 \author Alejandro Frangi
6701 \date 30 July 1998
6702 */
6703 template <class T, int D>
decomposeClosed(NurbsCurveArray<T,D> & c) const6704 void NurbsCurve<T,D>::decomposeClosed(NurbsCurveArray<T,D>& c) const {
6705
6706 int ix,b,nb,mult,j ;
6707 Vector<T> alphas(deg_+1) ;
6708 Vector<T> Uexpanded(U.n()+2*deg_) ;
6709 Vector< HPoint_nD<T,D> > Pexpanded(P.n()+2*deg_) ;
6710
6711 int N = P.n() - deg_ - 1;
6712 int i ;
6713
6714 // Left side
6715 for (i=0; i<deg_; i++){
6716 Pexpanded[i] = P[P.n()-2*deg_+i] ;
6717 Uexpanded[i] = U[N+1-deg_+i] - 1 ;
6718 }
6719
6720 // Copy
6721 for (i=0; i<P.n(); i++)
6722 Pexpanded[i+deg_] = P[i] ;
6723 for (i=0; i<U.n(); i++)
6724 Uexpanded[i+deg_] = U[i] ;
6725
6726 // Right side
6727 for (i=0; i<deg_; i++){
6728 Pexpanded[i+deg_+P.n()] = P[deg_+i] ;
6729 Uexpanded[i+deg_+U.n()] = 1 + U[2*deg_+1+i] ;
6730 }
6731 // Now do the decomposition
6732 Vector<T> nU ;
6733 nU.resize(2*(deg_+1)) ;
6734 for(i=0;i<nU.n()/2;i++)
6735 nU[i] = 0 ;
6736 for(i=nU.n()/2;i<nU.n();i++)
6737 nU[i] = 1 ;
6738
6739 c.resize(P.n()) ;
6740
6741 for(i=0;i<c.n();i++){
6742 c[i].resize(deg_+1,deg_) ;
6743 c[i].U = nU ;
6744 }
6745
6746 Vector<T> X(Uexpanded.n()*deg_) ;
6747
6748 // Construct the refinement vector
6749 ix= 0;
6750 i = 0;
6751 b = 2*deg_;
6752
6753 while ( b < U.n() ) {
6754 i = b;
6755 while( b < Uexpanded.n()-1 && Uexpanded[b+1] <= Uexpanded[b] ) b++ ;
6756 mult = b-i+1 ;
6757
6758 if(mult<deg_){
6759 for(j=deg_;j>=mult;j--) {
6760 X[ix] = Uexpanded[b] ;
6761 ix++ ;
6762 }
6763 }
6764 b++;
6765 }
6766
6767 X.resize(ix);
6768
6769 NurbsCurve<T,D> cl = NurbsCurve(Pexpanded,Uexpanded,deg_);
6770 cl.refineKnotVectorClosed(X) ;
6771
6772 // The number of Bezier segments coincides with the number of
6773 // distinct control points in a closed curve
6774 nb = P.n()-deg_;
6775
6776 c.resize(nb);
6777 for (i=0; i<c.n(); i++)
6778 for (int k=0; k<=deg_; k++)
6779 c[i].P[k] = cl.P[i*(deg_+1)+k+2*deg_];
6780 }
6781
6782 /*!
6783 \brief generates a knot vector using the averaging technique for interpolation with closed curve.
6784
6785 Generates a knot vector using the averaging technique for interpolation with closed curve. See eq 9.9 in the NURBS Book
6786
6787 \param uk the knot coefficients
6788 \param deg the degree of the curve associated with the knot vector
6789 \param U an average knot vector
6790
6791 \author Alejandro Frangi
6792 \date 13 July, 1998
6793 */
6794 template <class T>
knotAveragingClosed(const Vector<T> & uk,int deg,Vector<T> & U)6795 void knotAveragingClosed(const Vector<T>& uk, int deg, Vector<T>& U){
6796
6797 U.resize(uk.n()+deg+1) ;
6798
6799 int i, j ;
6800 int index;
6801 int iN = uk.n() - deg - 1;
6802 int n = uk.n() - 1;
6803 int m = U.n() - 1;
6804
6805 // Build temporary average sequence
6806 // Data stored in range U[deg+1 .. n]
6807 for (j=0; j<=iN; j++) {
6808 index = j+deg+1;
6809 U[index] = 0.0;
6810 for (i=j; i<=j+deg-1; i++)
6811 U[index] += uk[i];
6812 U[index] /= deg;
6813 }
6814
6815 // Now make the left and right periodic extensions
6816 // Left
6817 for (j=0; j<deg; j++) U[j] = U[j+iN+1] - 1;
6818 // Right
6819 for (j=n+1; j<=m; j++) U[j] = 1 + U[j-(iN+1)];
6820
6821 }
6822
6823 /*!
6824 \brief compute the knots for closed curve approximation
6825
6826 Generates a parameter vector by using the algorithm in
6827 eqs (9.68) and (9.69) of page 412 in the NURBS Book
6828
6829 \param U the knot vector
6830 \param ub the parameter vector of the approximated points (wrapped)
6831 \param n+1 the number of total control points (including wrapped ones!)
6832 \param p the degree of the curve associated with the knot vector
6833
6834 \author Alejandro Frangi
6835 \date 30 July 1998
6836 */
6837 template <class T>
knotApproximationClosed(Vector<T> & U,const Vector<T> & ub,int n,int p)6838 void knotApproximationClosed( Vector<T>& U, const Vector<T>& ub, int n, int p){
6839
6840 int i, j;
6841 int iN = n - p ;
6842 U.resize(n+p+2) ;
6843 T d = ub.n()/(T)(n-p+1) ;
6844 T alpha;
6845
6846 U = 0 ;
6847 // Initialize the internal knots
6848 for ( j=1; j<= n-p ; j++) {
6849 i = int(j*d);
6850 alpha = j*d - i;
6851 U[p+j] = (1-alpha)*ub[i-1] + alpha*ub[i] ;
6852 }
6853
6854 // Now make the left and right periodic extensions
6855 // Left
6856 for (j=0; j<p; j++) U[j] = U[j+iN+1] - 1;
6857 // Right
6858 for (j=n+1; j<U.n(); j++) U[j] = 1 + U[j-(iN+1)];
6859 }
6860
6861 /*!
6862 \brief wraps d points to the end of a point vector
6863
6864 Qw contains the same points that Q and wraps the end is
6865 padded with the first d points from Q
6866
6867 \input Q a vector of 3D points
6868 \input d number of wrapped points
6869 \input Qw a wrapped vector of 4D points
6870 \author Alejandro Frangi
6871 \date 14 July, 1998
6872 */
6873 template <class T, int N>
wrapPointVector(const Vector<Point_nD<T,N>> & Q,int d,Vector<Point_nD<T,N>> & Qw)6874 void wrapPointVector(const Vector<Point_nD<T,N> >& Q, int d, Vector<Point_nD<T,N> >& Qw){
6875 int i ;
6876
6877 Qw = Q;
6878 Qw.resize(Q.n()+d);
6879
6880 for (i=0; i<d; i++)
6881 Qw[i+Q.n()] = Q[i];
6882
6883 }
6884
6885 /*!
6886 \brief wraps d points to the end of a point vector
6887
6888 Qw contains the same points that Q and wraps the end is
6889 padded with the first d points from Q
6890
6891 \input Q a vector of 3D points
6892 \input d number of wrapped points
6893 \input Qw a wrapped vector of 4D points
6894 \author Alejandro Frangi
6895 \date 14 July, 1998
6896 */
6897 template <class T, int N>
wrapPointVectorH(const Vector<HPoint_nD<T,N>> & Q,int d,Vector<HPoint_nD<T,N>> & Qw)6898 void wrapPointVectorH(const Vector<HPoint_nD<T,N> >& Q, int d, Vector<HPoint_nD<T,N> >& Qw){
6899 int i ;
6900
6901 Qw = Q;
6902 Qw.resize(Q.n()+d);
6903
6904 for (i=0; i<d; i++)
6905 Qw[i+Q.n()] = Q[i];
6906
6907 }
6908
6909 /*!
6910 \brief Writes the curve to in Display format as a LINE object
6911
6912 This function writes a surface in LINE ascii format to
6913 interface with Display (Copyright 1993,1994,1995 David MacDonald,
6914 McConnell Brain Imaging Centre), Montreal Neurological Institute,
6915 McGill University.
6916
6917 \param filename the name of the file to save to
6918 \param iNu the number of straight segments in the line
6919 \param color the color of the line
6920 \param fA alpha blending parameter of the line
6921
6922 \return returns 1 on success, 0 otherwise.
6923
6924 \author Alejandro Frangi
6925 \date 21 January 1999
6926 */
6927 template <class T, int N>
writeDisplayLINE(const char * filename,int iNu,const Color & color,T fA) const6928 int NurbsCurve<T,N>::writeDisplayLINE(const char* filename, int iNu, const Color& color,T fA) const
6929 {
6930
6931 int i;
6932 // COnvert the curve to 3D if it is not
6933 NurbsCurve<T,3> curve3D;
6934 to3D(*this,curve3D);
6935 // Output it
6936 T fDu = 1/T(iNu);
6937 ofstream fout(filename) ;
6938 if(!fout)
6939 return 0 ;
6940 // Save the object type
6941 const char LINE = 'l'+ ('A' - 'a');
6942 fout << LINE << ' '; ;
6943 T fThickness = T(1.);
6944 // Save surface properties
6945 fout << fThickness << ' '
6946 << iNu << endl;
6947 // Fill points
6948 Point_nD<T,3> p;
6949 for (T u = 0; u<1-fDu/2; u+=fDu, i++){
6950 p = T(-1)*curve3D.pointAt(u);
6951 fout << p.x() << ' ' << p.z() << ' ' << p.y() << endl;
6952 }
6953 // New line
6954 fout << endl ;
6955 // Elements
6956 fout << 1 << endl ;
6957 // New line
6958 fout << endl ;
6959 // Surface color RGBA (one color for the whole surface)
6960 T fR= T(color.r)/255;
6961 T fG= T(color.g)/255;
6962 T fB= T(color.b)/255;
6963 // Colour flag = ONE_COLOUR
6964 fout << 0 << ' ' ;
6965 // The colour
6966 fout << fR << ' '
6967 << fG << ' '
6968 << fB << ' '
6969 << fA << endl;
6970 // New line
6971 fout << endl ;
6972 fout << iNu << endl;
6973 // New line
6974 fout << endl ;
6975 // Save the dummy integers
6976 for (i=0; i<iNu; i++)
6977 fout << i << ' ';
6978 fout << endl ;
6979 return 1;
6980 }
6981
6982 /*!
6983 \brief Writes the curve to a file in Display format as a line object
6984
6985 This function writes a surface in LINE ascii format to
6986 interface with Display (Copyright 1993,1994,1995 David MacDonald,
6987 McConnell Brain Imaging Centre), Montreal Neurological Institute,
6988 McGill University.
6989
6990 \param filename the name of the file to save to
6991 \param color the color of the curve
6992 \param Nu the number of points along the path
6993 \param u_s the starting parametric value for \a u
6994 \param u_e the end parametric value for \a u
6995
6996 \return returns 1 on success, 0 otherwise.
6997
6998 \author Alejandro Frangi
6999 \date 15 Juanuary 1999
7000 */
7001
7002 template <class T, int N>
writeDisplayLINE(const char * filename,const Color & color,int iNu,T u_s,T u_e) const7003 int NurbsCurve<T,N>::writeDisplayLINE(const char* filename,const Color& color, int iNu,T u_s, T u_e) const
7004 {
7005
7006 int i;
7007 T fDu = (u_e-u_s)/iNu;
7008 ofstream fout(filename) ;
7009 if(!fout)
7010 return 0 ;
7011
7012 // Save the object type
7013 const char LINE = 'l'+ ('A' - 'a');
7014 fout << LINE << ' '; ;
7015
7016 float fThickness = 1;
7017 // Save surface properties
7018 fout << fThickness << ' '
7019 << iNu << endl;
7020
7021 // Fill points
7022 NurbsCurve<T,3> Curve;
7023 to3D(*this,Curve);
7024 Point_nD<T,3> p;
7025 T u ;
7026 for ( u = u_s; u<u_e-fDu/2; u+=fDu, i++){
7027 // Requires sign inversion and swap of y-z
7028 p = -1.0*Curve.pointAt(u);
7029 fout << p.x() << ' ' << p.z() << ' ' << p.y() << endl;
7030 }
7031
7032 // New line
7033 fout << endl ;
7034 // Elements
7035 fout << 1 << endl ;
7036 // New line
7037 fout << endl ;
7038 // Surface color RGBA (one color for the whole line)
7039 float fR=color.r/255.0;
7040 float fG=color.g/255.0;
7041 float fB=color.b/255.0;
7042 float fA=1;
7043 /* Colour flag = ONE_COLOUR */
7044 fout << 0 << ' ' ;
7045 /* The colour */
7046 fout << fR << ' '
7047 << fG << ' '
7048 << fB << ' '
7049 << fA << endl;
7050
7051 // New line
7052 fout << endl ;
7053
7054 fout << iNu << endl;
7055
7056 // New line
7057 fout << endl ;
7058
7059 // Save the dummy integers
7060 for (i=0; i<iNu; i++)
7061 fout << i << ' ';
7062 fout << endl ;
7063
7064 return 1;
7065 }
7066
7067 /*!
7068 \brief set the tangent at a point
7069
7070 \param u the parametric value of the point
7071 \param T the tangent value to set to
7072
7073 \warning the tangent must be a unit length vector or an odd behavior
7074 might occur
7075
7076 \author Philippe Lavoie
7077 \date 2 March 1999
7078 */
7079 template <class T, int N>
setTangent(T u,const Point_nD<T,N> & T0)7080 void NurbsCurve<T,N>::setTangent(T u, const Point_nD<T,N>& T0) {
7081 Point_nD<T,N> ders = derive3D(u,1) ;
7082
7083 BasicArray<Point_nD<T,N> > D(2) ;
7084 BasicArray<int> dr(2) ;
7085 BasicArray<int> dk(2) ;
7086 BasicArray<T> ur(1) ;
7087
7088 ur[0] = u ;
7089 dr[0] = 0 ;
7090 dr[1] = 0 ;
7091 dk[0] = 0 ;
7092 dk[1] = 1 ;
7093 D[0] = Point_nD<T,N>(0) ;
7094
7095 T length = ders.norm() ;
7096
7097 D[1] = T0 - ders/length ;
7098 D[1] *= length ;
7099
7100 movePoint(ur,D,dr,dk) ;
7101
7102 }
7103
7104 /*!
7105 \brief set the tangent at the end points
7106
7107 \param T0 the tangent at the begining of the curve
7108 \param T1 the tangent at the end of the curve
7109
7110 \warning the tangent must be a unit length vector or and odd
7111 behavior might occur
7112
7113 \author Philippe Lavoie
7114 \date 2 March 1999
7115 */
7116 template <class T, int N>
setTangentAtEnd(const Point_nD<T,N> & T0,const Point_nD<T,N> & T1)7117 void NurbsCurve<T,N>::setTangentAtEnd(const Point_nD<T,N>& T0, const Point_nD<T,N>& T1) {
7118 Point_nD<T,N> ders0 = derive3D(U[deg_],1) ;
7119 Point_nD<T,N> ders1 = derive3D(U[P.n()],1) ;
7120
7121 BasicArray<Point_nD<T,N> > D(4) ;
7122 BasicArray<int> dr(4) ;
7123 BasicArray<int> dk(4) ;
7124 BasicArray<T> ur(2) ;
7125
7126 ur[0] = U[deg_] ;
7127 ur[1] = U[P.n()] ;
7128 D[0] = D[1] = Point_nD<T,N>(0) ;
7129 dr[0] = 0 ;
7130 dr[1] = 1 ;
7131 dr[2] = 0 ;
7132 dr[3] = 1 ;
7133 dk[0] = dk[1] = 0 ;
7134 dk[2] = dk[3] = 1 ;
7135
7136 T length = ders0.norm() ;
7137
7138 D[2] = T0 - ders0/length ;
7139 D[2] *= length ;
7140
7141 length = ders1.norm();
7142 D[3] = T1 - ders1/length ;
7143 D[3] *= length ;
7144
7145 movePoint(ur,D,dr,dk) ;
7146
7147 }
7148
7149
7150 /*!
7151 \brief clamp a NURBS curve
7152
7153 A clamped NURBS curve has degree+1 equal knots at both ends of the
7154 knot vector.
7155
7156 \author Philippe Lavoie
7157 \date 27 April 1999
7158 */
7159 template <class T, int N>
clamp()7160 void NurbsCurve<T,N>::clamp(){
7161 NurbsCurve<T,N> nc(*this) ;
7162
7163 int n1 = nc.knotInsertion(U[deg_],deg_,*this);
7164 int n2 = knotInsertion(U[P.n()],deg_,nc);
7165
7166 if(n1 || n2 ){
7167 U.resize(nc.U.n()-n1-n2) ;
7168 P.resize(U.n()-deg_-1) ;
7169 for(int i=U.n()-1;i>=0;--i){
7170 U[i] = nc.U[i+deg_] ;
7171 if(i<P.n())
7172 P[i] = nc.P[i+deg_] ;
7173 }
7174 }
7175
7176 }
7177
7178 /*!
7179 \brief unclamp a NURBS curve
7180
7181 An unclamped NURBS curve doesn't have any equal knots at both ends of
7182 the knot vector.
7183
7184 \author Philippe Lavoie
7185 \date 27 April 1999
7186 */
7187 template <class T, int N>
unclamp()7188 void NurbsCurve<T,N>::unclamp(){
7189 int n = P.n()-1 ;
7190 int i,j ;
7191 for(i=0;i<=deg_-2;++i){
7192 U[deg_-i-1] = U[deg_-i] - (U[n-i+1]-U[n-i]) ;
7193 int k=deg_-1 ;
7194 for(j=i;j>=0;--j){
7195 T alpha = (U[deg_]-U[k])/(U[deg_+j+1]-U[k]);
7196 P[j] = (P[j]-alpha*P[j+1])/(T(1)-alpha);
7197 --k ;
7198 }
7199 }
7200 U[0] = U[1] - (U[n-deg_+2]-U[n-deg_+1]) ; // set first knot
7201 for(i=0;i<=deg_-2;++i){
7202 U[n+i+2] = U[n+i+1] + (U[deg_+i+1]-U[deg_+i]);
7203 for(j=i;j>=0;--j){
7204 T alpha = (U[n+1]-U[n-j])/(U[n-j+i+2]-U[n-j]);
7205 P[n-j] = (P[n-j]-(1.0-alpha)*P[n-j-1])/alpha ;
7206 }
7207 }
7208 U[n+deg_+1] = U[n+deg_] + (U[2*deg_]-U[2*deg_-1]); // set last knot
7209 }
7210
7211 } // end namespace
7212