1 /*=============================================================================
2 File: hnurbs.cpp
3 Purpose:
4 Revision: $Id: hnurbs.cpp,v 1.2 2002/05/13 21:07:46 philosophil Exp $
5 Created by: 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 <hnurbs.h>
26
HNurbsCurveNode()27 HNurbsCurveNode::HNurbsCurveNode():u0(u0_),u1(u1_){
28 prev = 0 ;
29 next = 0 ;
30 curve = new NurbsCurve ;
31 u0 = 0 ;
32 u1 = 1 ;
33 uD = 1 ;
34 }
35
HNurbsCurveNode(const NurbsCurve & c,T uS,T uE)36 HNurbsCurveNode::HNurbsCurveNode(const NurbsCurve &c, T uS, T uE):u0(u0_),u1(u1_){
37 prev = 0 ;
38 next = 0 ;
39 curve = new NurbsCurve(c) ;
40 u0 = uS ;
41 u1 = uE ;
42 uD = uE-uS ;
43 }
44
operator ()(T u) const45 HPoint_nD<T,N> HNurbsCurveNode::operator()(T u) const {
46 if(u<u0 || u>u1)
47 return HPoint_nD<T,N>(0,0,0,0) ;
48
49 u -= u0_ ;
50 u /= uD ;
51 return (*curve)(u) ;
52 }
53
deriveAt(T u,int d,Vector<Point_nD<T,N>> & ders) const54 void HNurbsCurveNode::deriveAt(T u, int d, Vector< Point_nD<T,N> >& ders) const {
55 ders.resize(d+1) ;
56 ders.reset(0) ;
57 if(u<u0 || u>u1)
58 return ;
59
60 u -= u0_ ;
61 u /= uD ;
62 curve->deriveAt(u,d,ders) ;
63 }
64
deriveAt(T u,int d,Vector<HPoint_nD<T,N>> & ders) const65 void HNurbsCurveNode::deriveAt(T u, int d, Vector< HPoint_nD<T,N> >& ders) const {
66 ders.resize(d+1) ;
67 ders.reset(0) ;
68 if(u<u0 || u>u1)
69 return ;
70
71 u -= u0_ ;
72 u /= uD ;
73 curve->deriveAt(u,d,ders) ;
74 }
75
76
HNurbsCurve()77 HNurbsCurve::HNurbsCurve(){
78 first = 0 ;
79 last = 0 ;
80 }
81
add(const NurbsCurve & curve,T uS,T uE)82 void HNurbsCurve::add(const NurbsCurve& curve, T uS, T uE) {
83 HNurbsCurveNode *nC ;
84 nC = new HNurbsCurveNode(curve,uS,uE) ;
85 nC->prev = last ;
86 if(last)
87 last->next = nC ;
88 if(!first)
89 first = nC ;
90 last = nC ;
91 }
92
remove(void)93 void HNurbsCurve::remove(void){
94 HNurbsCurveNode *tC ;
95 if(last){
96 tC = last ;
97 last = last->prev;
98 last->next = 0 ;
99 delete tC ;
100 }
101 }
102
operator ()(T u) const103 HPoint_nD<T,N> HNurbsCurve::operator()(T u) const{
104 HNurbsCurveNode *c ;
105 HPoint_nD<T,N> result(0,0,0,0) ;
106 HPoint_nD<T,N> temp ;
107 Point_nD<T,N> temp2 ;
108 c = first ;
109 while(c){
110 temp = (*c)(u) ;
111 if(temp.w()!= 0){
112 temp2 = project(temp) ;
113 result.x() += temp2.x() ;
114 result.y() += temp2.y() ;
115 result.z() += temp2.z() ;
116 }
117 c = c->next ;
118 }
119 result.w() = 1.0 ;
120 return result ;
121 }
122
deriveAt(T u,int d,Vector<HPoint_nD<T,N>> & ders) const123 void HNurbsCurve::deriveAt(T u,int d, Vector< HPoint_nD<T,N> >& ders) const{
124 HNurbsCurveNode *c ;
125 Vector< HPoint_nD<T,N> > dTemp(d+1) ;
126
127 ders.resize(d+1) ;
128 ders.reset(0) ;
129 c = first ;
130 while(c){
131 c->deriveAt(u,d,dTemp) ;
132 ders += dTemp ;
133 c = c->next ;
134 }
135 }
deriveAt(T u,int d,Vector<Point_nD<T,N>> & ders) const136 void HNurbsCurve::deriveAt(T u,int d, Vector< Point_nD<T,N> >& ders) const{
137 HNurbsCurveNode *c ;
138 Vector< Point_nD<T,N> > dTemp(d+1) ;
139
140 ders.resize(d+1) ;
141 ders.reset(0) ;
142 c = first ;
143 while(c){
144 c->deriveAt(u,d,dTemp) ;
145 ders += dTemp ;
146 c = c->next ;
147 }
148 }
149
150
151
draw(Image_Color & img,const Color & col) const152 void HNurbsCurve::draw(Image_Color& img, const Color& col) const {
153 const int n=100 ;
154 T dU = 1.0/T(n) ;
155 T uP = 0 ;
156 HPoint_nD<T,N> p,c ;
157 Point_nD<T,N> p3,c3 ;
158
159 p = point4D(uP) ;
160 for(T u=dU; u<1.0; u += dU){
161 c = point4D(u) ;
162 p3 = project(p) ;
163 c3 = project(c) ;
164 img.drawLine((int)p3.y(),(int)p3.x(),(int)c3.y(),(int)c3.x(),col) ;
165 p = c ;
166 }
167 c = point4D(1.0) ;
168 p3 = project(p) ;
169 c3 = project(c) ;
170 img.drawLine((int)p3.y(),(int)p3.x(),(int)c3.y(),(int)c3.x(),col) ;
171 }
172
draw(Image_UBYTE & img,unsigned char col) const173 void HNurbsCurve::draw(Image_UBYTE &img, unsigned char col) const {
174 const int n=100 ;
175 T dU = 1.0/T(n) ;
176 HPoint_nD<T,N> p,c ;
177 Point_nD<T,N> p3,c3 ;
178
179 p3 = point3D(0.0) ;
180 for(T u=dU;u<1.0;u+=dU){
181 c3 = point3D(u) ;
182 img.drawLine((int)p3.y(),(int)p3.x(),(int)c3.y(),(int)c3.x(),col) ;
183 p3 = c3 ;
184 }
185 c3 = point3D(1.0);
186 img.drawLine((int)p3.y(),(int)p3.x(),(int)c3.y(),(int)c3.x(),col) ;
187 }
188
189 const int MaxRandom = 32768 ; // 2^15
190
191
reset()192 void HNurbsCurve::reset(){
193 HNurbsCurveNode* t ;
194 while(last){
195 t = last->prev ;
196 if(last->curve)
197 delete last->curve ;
198 delete last ;
199 last = t ;
200 }
201 }
202
203
204 // needs to be tested, and checked if this actually optimizes the method used in interpolate
setRandomVector(Vector_INT & rV,int rangeSize,int rangeOffset,int startAt=0,int finishAt=-1)205 void setRandomVector(Vector_INT& rV, int rangeSize, int rangeOffset, int startAt=0, int finishAt=-1){
206 if(finishAt<0)
207 finishAt= rV.n-1 ;
208 if(rV.n<=finishAt){
209 cerr << "You need to set the random vector's size to the proper value PRIOR to calling this function!\n" ;
210 while(1){;}
211 }
212
213 // write from 0 and offset it afterwards...
214 int i ;
215 for(i=startAt;i<=finishAt;++i){
216 double r = double(rand()) ;
217 int l ;
218 r /= double(MaxRandom) ; // r = [0,1]
219 r *= double(rangeSize) ; // r=[0,rangeSize] ;
220 --rangeSize ;
221 l = int(r) ;
222 for(int j=startAt;j<i;++j){
223 if(rV[j]<=l){
224 ++l ;
225 }
226 else{
227 int k=j ;
228 for(j=i;j>k;--j){
229 rV[j] = rV[j-1] ;
230 }
231 rV[k] = l ;
232 break ;
233 }
234 }
235 }
236
237 for(i=0;i<=finishAt-startAt;i++)
238 rV[i] += rangeOffset ;
239 }
240
interpolate(const Vector<Point_nD<T,N>> & Pts,int deg,T acceptError,int nSample,int maxTries,int nInitPoints,int nPoints)241 void HNurbsCurve::interpolate(const Vector< Point_nD<T,N> > &Pts, int deg, T acceptError, int nSample, int maxTries, int nInitPoints, int nPoints){
242 // Get a first interpolation of the data points
243 if(nInitPoints<0) nInitPoints = deg*2 ;
244 if(nPoints<0) nPoints = deg*2 ;
245
246 if(nPoints<deg+1 || nInitPoints < deg+1){
247 cerr << "Using a number of points smaller than deg+1!\n" ;
248 while(1){;}
249 }
250 if(Pts.n<deg+1){
251 cerr << "Not enough points to interpolate !\n" ;
252 while(1){;}
253 }
254
255 int i,j,k ;
256
257 NurbsCurve sampleC,minC ;
258 Vector< Point_nD<T,N> > sampleP ;
259 Vector_INT sampleI ;
260 Vector<T> error ;
261 T minError = -1.0 ;
262
263 sampleP.resize(nInitPoints) ;
264 sampleI.resize(nInitPoints) ;
265 error.resize(Pts.n) ;
266 srand(123) ;
267
268 for(k=0;k<nSample;++k){
269 sampleI[0] = 0 ;
270 sampleI[sampleP.n-1] = Pts.n-1 ;
271 for(i=1;i<sampleP.n-1;++i){
272 int ok = 0 ;
273 while(!ok){
274 ok = 1 ;
275 double r = double(rand()) ;
276 r /= double(MaxRandom) ; // r = [0,1]
277 r *= Pts.n-2 ; // r=[0,Pts.n-2] ;
278 r += 1 ; // r=[1,Pts.n-1]
279 sampleI[i] = int(r) ;
280 for(j=0;j<i;++j){
281 if(sampleI[j] == sampleI[i]){
282 ok = 0 ;
283 break ;
284 }
285 }
286 }
287 }
288 sampleI.qSort() ;
289 for(i=0;i<sampleP.n;++i)
290 sampleP[i] = Pts[sampleI[i]] ;
291 sampleC.globalInterp(sampleP,deg) ;
292 T u = 0 ;
293 for(i=0;i<Pts.n;++i){
294 T e = sampleC.minDist2(Pts[i],u) ;
295 error[i] = e ;
296 }
297 error.qSort() ;
298 if(minError>error[error.n/2] || minError<0){
299 minError = error[error.n/2] ;
300 minC = sampleC ;
301 }
302 }
303 reset() ;
304 add(minC,0,1) ;
305
306 // cerr << "Done first interpolation = " << minError << endl ;
307
308 // Finding the maximal error region and modifying the HNURBS until there is no more errors...
309 T maxError ;
310 Vector_INT regionI(Pts.n) ;
311 Vector<T> regionU(Pts.n) ;
312 sampleP.resize(nPoints) ;
313 sampleI.resize(nPoints) ;
314 Vector<T> errorM,errorT ;
315 int tryN = 0;
316 while(tryN < maxTries){
317 // Find max error between regions with no errors
318 T minU,maxU,tempError,u ;
319 int maxI = 0 ;
320
321 int index = 0 ;
322 regionI[0] = 0 ;
323 regionU[0] = 0.0 ;
324 maxError = -1.0 ;
325 u = 0 ;
326
327 for(i=1;i<Pts.n-1;++i){ // the end points have always an error of 0
328 tempError = minDist2(Pts[i],u) ;
329 if(tempError<acceptError){
330 ++index ;
331 regionI[index] = i ;
332 regionU[index] = u ;
333 continue ;
334 }
335 if(tempError>maxError){
336 maxError = tempError ;
337 maxI = index ; // The error is located between region maxI and maxI+1
338 }
339 }
340 ++index ;
341 regionI[index] = Pts.n-1 ;
342 regionU[index] = 1.0 ;
343 ++index ;
344 if(maxError<acceptError){
345 tryN = maxTries ;
346 break ;
347 }
348
349 // for the max region we add a curve fit
350 // 1st) find a region of sufficient size
351 int regionS = maxI ;
352 int regionE = maxI+1 ;
353
354 while((regionI[regionE]-regionI[regionS]) < 2*nPoints){ // must use a bigger region
355 if(regionS>0) --regionS ;
356 if(regionE<index-1) ++regionE ;
357 }
358
359 int regionSize = regionI[regionE]-regionI[regionS] ;
360 int regionStart = regionI[regionS] ;
361
362 error.resize(regionSize) ;
363 errorM.resize(regionSize) ;
364
365 minError = -1.0 ;
366 for(k=0;k<nSample;++k){
367 sampleI[0] = regionStart ;
368 sampleI[sampleP.n-1] = regionI[regionE] ;
369 if(regionSize==nPoints){
370 k=nSample ;
371 for(i=1;i<sampleP.n-1;++i)
372 sampleI[i] = regionStart+i ;
373 }
374 else{
375 for(i=1;i<sampleP.n-1;++i){
376 int ok = 0 ;
377 while(!ok){
378 ok = 1 ;
379 double r = double(rand()) ;
380 r /= double(MaxRandom) ; // r = [0,1]
381 r *= regionSize-2 ; // r=[0,regionSize-2] ;
382 r += 1 ; // r=[1,regionSize-1]
383 sampleI[i] = int(r)+regionStart ;
384 for(j=0;j<i;++j){
385 if(sampleI[j] == sampleI[i]){
386 ok = 0 ;
387 break ;
388 }
389 }
390 }
391 }
392 }
393 sampleI.qSort() ;
394 u = regionU[regionS] ;
395 for(i=1;i<sampleP.n-1;++i){
396 //T closest = minDist2(Pts[sampleI[i]],u) ;
397 sampleP[i] = Pts[sampleI[i]] - point3D(u) ;
398 }
399 sampleP[0] = Point_nD<T,N>(0,0,0) ;
400 sampleP[sampleP.n-1] = Point_nD<T,N>(0,0,0) ;
401 sampleC.globalInterp(sampleP,deg) ;
402 u = regionU[regionS] ;
403 for(i=0;i<error.n;++i){
404 //T closestE = minDist2(Pts[regionStart+i],u) ;
405 Point_nD<T,N> closest = point3D(u) ;
406 T u2 = u ;
407 u2 -= regionU[regionS] ;
408 u2 /= regionU[regionE]-regionU[regionS] ;
409 if(u2<0 || u2> 1.0) {
410 cerr << "Error in setting up u2!\n" ;
411 while(1) {; }
412 }
413 error[i] = abs2(closest+project(sampleC(u2))-Pts[regionStart+i]) ;
414 }
415 error.qSort() ;
416 if(minError>error[error.n/2] || minError<0){
417 minError = error[error.n/2] ;
418 minC = sampleC ;
419 minU = regionU[regionS] ;
420 maxU = regionU[regionE] ;
421 errorM = error ;
422 }
423 }
424 add(minC,regionU[regionS],regionU[regionE]) ;
425 //cerr << "Testing Error analysis\n" ;
426 errorT.resize(errorM.n) ;
427 u = regionU[regionS] ;
428 for(i=0;i<errorT.n;++i) {
429 T closestE = minDist2(Pts[i+regionStart],u) ;
430 errorT[i] = closestE ;
431 //cerr << "error at " << i << " = " << errorM[i] << " or " << errorT[i] << "for point " << Pts[i+regionStart] << " and " << point3D(u) << endl ;
432 }
433 errorT.qSort() ;
434
435
436 // cerr << "At try " << tryN << " maxError = " << maxError << " and now it is " << errorT[errorT.n-1] << " for region " << maxI << "[ " << regionStart << ", " << regionI[regionE] << "]" << index << endl ;
437 ++tryN ;
438 }
439
440 }
441