1 /*
2 * Copyright (C) 1998, 2000-2007, 2010, 2011, 2012, 2013 SINTEF ICT,
3 * Applied Mathematics, Norway.
4 *
5 * Contact information: E-mail: tor.dokken@sintef.no
6 * SINTEF ICT, Department of Applied Mathematics,
7 * P.O. Box 124 Blindern,
8 * 0314 Oslo, Norway.
9 *
10 * This file is part of SISL.
11 *
12 * SISL is free software: you can redistribute it and/or modify
13 * it under the terms of the GNU Affero General Public License as
14 * published by the Free Software Foundation, either version 3 of the
15 * License, or (at your option) any later version.
16 *
17 * SISL is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Affero General Public License for more details.
21 *
22 * You should have received a copy of the GNU Affero General Public
23 * License along with SISL. If not, see
24 * <http://www.gnu.org/licenses/>.
25 *
26 * In accordance with Section 7(b) of the GNU Affero General Public
27 * License, a covered work must retain the producer line in every data
28 * file that is created or manipulated using SISL.
29 *
30 * Other Usage
31 * You can be released from the requirements of the license by purchasing
32 * a commercial license. Buying such a license is mandatory as soon as you
33 * develop commercial activities involving the SISL library without
34 * disclosing the source code of your own applications.
35 *
36 * This file may be used in accordance with the terms contained in a
37 * written agreement between you and SINTEF ICT.
38 */
39
40 #include "sisl-copyright.h"
41
42 /*
43 *
44 * $Id: s9iterimp.c,v 1.3 2005-02-28 09:04:50 afr Exp $
45 *
46 */
47 #define S9ITERIMP
48
49 #include "sislP.h"
50
51 #if defined(SISLNEEDPROTOTYPES)
52 void
s9iterimp(double epoint[],double epnt1[],double epar1[],SISLSurf * psurf1,double eimpli[],int ideg,double astep,double aepsge,double gpnt1[],double gpar1[],int * jstat)53 s9iterimp(double epoint[],double epnt1[],double epar1[],SISLSurf *psurf1,
54 double eimpli[],int ideg,double astep,double aepsge,
55 double gpnt1[],double gpar1[],int *jstat)
56 #else
57 void s9iterimp(epoint,epnt1,epar1,psurf1,eimpli,ideg,astep,aepsge,
58 gpnt1,gpar1,jstat)
59 double epoint[];
60 double epnt1[];
61 double epar1[];
62 SISLSurf *psurf1;
63 double eimpli[];
64 int ideg;
65 double astep;
66 double aepsge;
67 double gpnt1[];
68 double gpar1[];
69 int *jstat;
70 #endif
71 /*
72 *********************************************************************
73 *
74 * PURPOSE : To iterate to an intersection point between a B-spline
75 * surfaces, an implicit surface and a plane.
76 *
77 *
78 * INPUT : epoint - Array containing parts of plane description.
79 * epoint[0:2] contains a position value.
80 * epoint[3:5] contains the normal to the plane
81 * A point in the plane is defined by
82 * epoint[0:2] + astep*epoint[3:5]
83 * epnt1 - 0-2 Derivatives + normal of start point for
84 * iteration in B-spline surface
85 * epar1 - Parameter pair of start point in B-spline surface
86 * psurf1 - Description of B-spline surface
87 * eimpli - Description of implicit surface
88 * ideg - Degree of implicit surface
89 * ideg=1: Plane
90 * ideg=2; Quadric surface
91 * ideg=1001: Torus surface
92 * ideg=1003: Silhouette line parallel projection
93 * ideg=1004: Silhouette line perspective projection
94 * ideg=1005: Silhouette line circular projection
95 * astep - Step length
96 * aepsge - Absolute tolerance
97 *
98 *
99 * OUTPUT : gpnt1 - 0-2 Derivatives + normal of result of iteration
100 * in B-spline surface
101 * gpar1 - Parameter pair of result of iteration in B-spline
102 * surface
103 * jstat - status messages
104 * = 2 : Iteration diverged or to many iterations
105 * = 1 : iteration converged, singular point found
106 * = 0 : ok, iteration converged
107 * < 0 : error
108 *
109 *
110 * METHOD : We want to find the intersection point between the three
111 * surfaces.
112 *
113 * Ideg=1:
114 * P(s,t)
115 * AX = 0 The implicit represented plane given by econic
116 * BX = 0 The implicit represented plane giving the step
117 *
118 *
119 * Ideg=2:
120 * P(s,t)
121 * XAX = 0 The implicit second degree surface
122 * BX = 0 The implicit represented plane giving the step
123 *
124 *
125 * Ideg=1001; Torus surface
126 * P(s,t)
127 * Torus described by center, normal, big and small radius
128 *
129 *
130 * By making a Newton iteration on the functions we get when
131 * P(s,t) is put into the implicit equations we can iterate to
132 * an intersection point.
133 *
134 * USE : The function is only working in 3-D
135 *
136 * REFERENCES :
137 *
138 *-
139 * CALLS :
140 *
141 * WRITTEN BY : Tor Dokken, SI, Oslo, Norway, 4-July-1988
142 * Revised by : Tor Dokken, SI, Oslo, Norway, 24-Feb-1989
143 * Detects degenerate points
144 * Revised by : Tor Dokken, SI, Oslo, Norway, March 1989
145 * Test for almost zero determinant introduced.
146 * Revised by : Mike Floater, SI, 1991-01
147 * Add perspective and circular silhouettes (ideg=1004,ideg=1005)
148 *
149 *********************************************************************
150 */
151 {
152 int ki; /* Variable used in loop */
153 int kcont; /* Indicator telling if iteration is not finished */
154 int kder = 1; /* Derivative indicator */
155 int klfu=0; /* Pointer into knot vector */
156 int klfv=0; /* Pointer into knot vector */
157 int kstat; /* Status variable */
158 int knbit; /* Counter for number of iterations */
159 int kdim = 3; /* Set dimension to 3 */
160 int kmaxit = 100; /* Maximal number of iterations allowed */
161 int kpos=1; /* Position indicator ofr errors */
162 int ksing; /* Singularity indicator */
163 int ksize; /* Number of doubles for storage of derivateves
164 and normal vector */
165 int ksizem3; /* ksize - 3 */
166 double spoint[3]; /* SISLPoint in intersection plane */
167 double *snorm; /* Pointer to normal vector of intersection plane */
168 double sbinorm[3]; /* Vector normal tu curve tangent */
169 double *sp,*spu,*spv,*spn; /* Pointers into gpnt1 */
170 double sprev[3]; /* Coordinates of previous point in iteration */
171 double ta11,ta12,ta21; /* Variables used in equation systems */
172 double ta22,tb1,tb2; /* Variables used in equation systems */
173 double sdiff[3]; /* Difference between two vectors */
174 double tdum,tdum1; /* Dummy variables */
175 double tdist; /* Error so fare in iteration */
176 double tcurdst; /* Error at current step in the iteration */
177 double sder[3]; /* Derivatives of comb. of impl. surf and par.surf*/
178 double sproj[3]; /* Projection direction */
179 double tlnorm; /* Length of normal vector of step plane */
180 double tdiststep; /* Distance from step plane */
181 double titer; /* Iteration criteria */
182
183 /* If ideg=1,2 or 1001 then only derivatives up to second order
184 are calculated, then 18 doubles for derivatives and 3 for the
185 normal vector are to be used for calculation of points in the
186 spline surface. For ideg=1003,1004,1005 we have a silhouette curve and
187 derivatives up to the third are to be calculated,
188 thus 30 +3 a total of 33 doubles are to be calculated */
189
190 if (ideg==1003 || ideg==1004 || ideg==1005)
191 {
192 kder = 3;
193 ksize = 33;
194 }
195 else
196 {
197 ksize = 21;
198 kder =2;
199 }
200 ksizem3 = ksize -3;
201
202 /* Make description of intersection plane */
203
204 tlnorm = s6length(epoint+3,3,&kstat);
205 if (kstat<0) goto error;
206 if (DEQUAL(tlnorm,DZERO)) tlnorm = (double)1.0;
207
208 for (ki=0;ki<3;ki++)
209 {
210 spoint[ki] = epoint[ki] + astep*epoint[ki+3]/tlnorm;
211 }
212
213 snorm = epoint + 3;
214
215 /* Copy input variables to output variables */
216
217 memcopy(gpnt1,epnt1,ksize,DOUBLE);
218 memcopy(gpar1,epar1,2,DOUBLE);
219
220 /* At the start of the iteration the point gpnt1 is put into both implicit
221 equations */
222
223 /* Set a number of local pointers that are used often */
224 sp = gpnt1;
225 spu = gpnt1 + 3;
226 spv = gpnt1 + 6;
227 spn = gpnt1 + ksizem3;
228
229 kcont = 1;
230 knbit = 0;
231
232 while (kcont)
233
234 {
235
236 /* For all degrees we have to put the B-spline surface into the plane
237 determining the step length before we branch degree on the degree
238 of the implicit surface. */
239
240 ta21 = s6scpr(spu,snorm,kdim);
241 ta22 = s6scpr(spv,snorm,kdim);
242 s6diff(spoint,sp,kdim,sdiff);
243 tb2 = s6scpr(sdiff,snorm,kdim);
244 tdum = max(fabs(ta21),fabs(ta22));
245 tdum = max(tdum,fabs(tb2));
246 if (DEQUAL(tdum,DZERO)) tdum = (double)1.0;
247 ta21 /= tdum;
248 ta22 /= tdum;
249 tb2 /= tdum;
250
251
252 /* Calculate value and derivatives of the parametric surface put into
253 the equation of the implicit surface */
254
255 s1331(gpnt1,eimpli,ideg,1,sder,sproj,&kstat);
256
257 ta11 = sder[1];
258 ta12 = sder[2];
259 tb1 = -sder[0];
260
261 tdum = max(fabs(ta11),fabs(ta12));
262 tdum = max(tdum,fabs(tb1));
263 if (DEQUAL(tdum,DZERO)) tdum = (double)1.0;
264 ta11 /= tdum;
265 ta12 /= tdum;
266 tb1 /= tdum;
267
268
269 /* Calculate determinant of equation system */
270
271 tdum1 = ta11*ta22 - ta12*ta21;
272 tdum = MAX(fabs(ta11),fabs(ta22));
273 tdum = MAX(fabs(ta12),tdum);
274 tdum = MAX(fabs(ta21),tdum);
275
276 if (DEQUAL((tdum+tdum1),tdum)) tdum1 =DZERO;
277
278
279 /* If tdum1 = 0.0, then the equation system is singular, try an
280 alternative setup of the equation system */
281
282 if (tdum1 == DZERO && ideg < 1003)
283 {
284 s6crss(sproj,snorm,sbinorm);
285 ta11 = s6scpr(spu,sbinorm,kdim);
286 ta12 = s6scpr(spv,sbinorm,kdim);
287 tb1 = s6scpr(sdiff,sbinorm,kdim);
288
289 tdum = max(fabs(ta11),fabs(ta12));
290 tdum = max(tdum,fabs(tb1));
291 if (DEQUAL(tdum,DZERO)) tdum = (double)1.0;
292 ta11 /= tdum;
293 ta12 /= tdum;
294 tb1 /= tdum;
295
296 /* Calculate determinant of equation system */
297
298 tdum1 = ta11*ta22 - ta12*ta21;
299 tdum = MAX(fabs(ta11),fabs(ta22));
300 tdum = MAX(fabs(ta12),tdum);
301 tdum = MAX(fabs(ta21),tdum);
302
303 if (DEQUAL((tdum+tdum1),tdum)) tdum1 =DZERO;
304 }
305
306 if (DNEQUAL(tdum1,DZERO))
307 {
308 gpar1[0] += (tb1*ta22-tb2*ta12)/tdum1;
309 gpar1[1] += (ta11*tb2-ta21*tb1)/tdum1;
310 }
311
312 /* Calculate value of new points */
313
314 s1421(psurf1,kder,gpar1,&klfu,&klfv,gpnt1,gpnt1+ksizem3,&kstat);
315 if (kstat<0) goto error;
316
317 /* If the surface normal has zero length no use in continuing */
318
319 if (kstat == 2) goto war02;
320
321
322 tcurdst = s1309(gpnt1,sproj,eimpli,ideg,&kstat);
323 if (kstat < 0) goto error;
324 if (kstat ==2) goto war02;
325
326 tcurdst = fabs(tcurdst);
327
328 /* Calculate distance from step plane */
329
330 s6diff(spoint,gpnt1,kdim,sdiff);
331 tdiststep = fabs(s6scpr(sdiff,snorm,kdim)/tlnorm);
332
333
334 /* tcurdst now contains the distance between the point in the parametric
335 surface and the projection along sproj of this point onto the implicit
336 surface if ideg== 1,2 or 1001. In the case ideg==1003,1004,1005 we have a
337 silhouette line and tcurdst contains the angle PI minus the angle
338 between the view direction and the normal of the surface */
339
340 /* We continue iteration so long as the error titer is not decreasing */
341
342
343 if (ideg==1003 || ideg==1004 || ideg==1005)
344 {
345 /* tcurdst contains an angle and is compared with ANGULAR_TOLERANCE,
346 while tdiststep contains a distance and is compared with aepsge.
347 To make a measure of these that is consistent they have to have
348 the same unit measure thus we make. */
349
350 titer = tcurdst*aepsge + tdiststep*ANGULAR_TOLERANCE;
351 }
352 else
353 titer = tcurdst + tdiststep;
354
355 if (DEQUAL(tcurdst,DZERO) && DEQUAL(tdiststep,DZERO))
356 {
357 /* Length is zero iteration has converged */
358 kcont = 0;
359 }
360
361 if (knbit==0)
362 {
363 /* First iteration intitate distance variable, if the equation
364 systems were not singular */
365
366 if (DEQUAL(tdum1,DZERO)) goto war02;
367 tdist = titer;
368 knbit = 1;
369 }
370 else
371 {
372 /* More than one iteration done, stop if distance is not decreasing.
373 Then decide if we converge distance between the points is within
374 the tolerance and the last step had singular or none singular
375 equation systems. */
376
377 knbit = knbit + 1;
378 if (titer>=tdist)
379 {
380 /* Distance is not decreasing */
381
382 if (fabs(s6dist(sprev,gpnt1,kdim)) <= aepsge)
383 {
384 /* Distance within tolerance */
385
386 /* Check if singularity reached.
387 This is the case if tdum1=0.0
388 or if the relative distance between
389 the input point and
390 the output point is within the
391 relative computer resolution */
392
393 ksing = 1;
394 for (ki=0 ; ki<3; ki++)
395 {
396 tdum = MAX(fabs(epnt1[ki]),fabs(gpnt1[ki]));
397 if (DEQUAL(tdum,DZERO)) tdum = (double)1.0;
398 if(fabs(epnt1[ki]-gpnt1[ki])/tdum > REL_COMP_RES)
399 ksing = 0;
400 }
401 if (DEQUAL(tdum1,DZERO) || ksing == 1)
402 {
403 /* Singular equation system */
404 goto war01;
405 }
406 else
407 {
408 /* Nonsingular equation system */
409 goto war00;
410 }
411 }
412 else
413 {
414 /* Distance is not within tolerance, divergence */
415 goto war02;
416 }
417 }
418 /* Distance still decreasing */
419
420 tdist = titer;
421 }
422
423 /* Make sure that not to many iteration are being done */
424 if (knbit > kmaxit) goto war02;
425 /* Remember this point */
426 memcopy(sprev,gpnt1,3,DOUBLE);
427 }
428
429 /* Iteration converged, calculate also second derivatives */
430 war00:
431
432 kder = 2;
433 s1421(psurf1,kder,gpar1,&klfu,&klfv,gpnt1,gpnt1+ksizem3,&kstat);
434 if (kstat<0) goto error;
435
436 *jstat = 0;
437 goto out;
438
439 /* Iteration converged, singular point found */
440 war01:
441 *jstat = 1;
442 goto out;
443
444 /* To many iterations or iteration diverging */
445 war02:
446 *jstat = 2;
447 goto out;
448
449 /* Error in lower level routine. */
450
451 error :
452 *jstat = kstat;
453 s6err("s9iterimp",*jstat,kpos);
454 goto out;
455
456 out:
457 return;
458 }
459