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: s1173.c,v 1.2 2001-03-19 15:58:42 afr Exp $
45 *
46 */
47
48
49 #define S1173
50
51 #include "sislP.h"
52
53 /*
54 * Forward declarations.
55 * ---------------------
56 */
57 #if defined(SISLNEEDPROTOTYPES)
58 static void s1173_s9dir(double *,double *,double *,double [],double [],
59 double [],double);
60 static void s1173_s9corr(double [],double,double,double,double,double,double);
61 static double s1173_s9del(double *,double *,double *,int);
62 #else
63 static void s1173_s9dir();
64 static void s1173_s9corr();
65 static double s1173_s9del();
66 #endif
67
68 #if defined(SISLNEEDPROTOTYPES)
69 void
s1173(SISLPoint * ppoint,SISLSurf * psurf,double aepsge,double estart[],double eend[],double enext[],double gpos[],int * jstat)70 s1173(SISLPoint *ppoint, SISLSurf *psurf, double aepsge,double estart[],
71 double eend[], double enext[], double gpos[],int *jstat)
72 #else
73 void s1173(ppoint,psurf,aepsge,estart,eend,enext,gpos,jstat)
74 SISLPoint *ppoint;
75 SISLSurf *psurf;
76 double aepsge;
77 double estart[];
78 double eend[];
79 double enext[];
80 double gpos[];
81 int *jstat;
82 #endif
83 /*
84 *********************************************************************
85 *
86 *********************************************************************
87 *
88 * PURPOSE : Newton iteration on the distance function between
89 * a onedimensional surface and a level value (point).
90 * The function finds a closest point(maximum) or an
91 * intersection point.
92 *
93 *
94 * INPUT : ppoint - Pointer to the point(level value) in.
95 * psurface - Pointer to the surface.
96 * aepsge - Geometry resolution.
97 * estart - Start values of parameter intervalls.
98 * eend - End value of parameter intervalls.
99 *
100 *
101 *
102 * OUTPUT : gpos - Parameter values of the surface in intersection
103 * or closest point( maximum).
104 * jstat - status messages
105 * = 2 : A minimum distanse found.
106 * = 1 : Intersection found.
107 * < 0 : error.
108 *
109 *
110 * METHOD : Newton iteration in two parameter direction.
111 *
112 *
113 * REFERENCES :
114 *
115 *
116 * WRITTEN BY : Ulf J. Krystad, SI, JUNE 1989
117 *
118 *********************************************************************
119 */
120 {
121 int kstat = 0; /* Local status variable. */
122 int kpos = 0; /* Position of error. */
123 int kleft1=0; /* Variables used in the evaluator. */
124 int kleft2=0; /* Variables used in the evaluator. */
125 int kder=2; /* Order of derivatives to be calulated */
126 int kdim=1; /* Dimension of space the surface lies in */
127 int knbit; /* Number of iterations */
128 int kdir; /* Changing direction. */
129 double tdelta[2]; /* Parameter intervals of the surface. */
130 double tdist; /* Distance between position and origo. */
131 double td[2],t1[2],tdn[2];/* Distances between old and new parameter
132 value in the tree parameter directions. */
133 double tprev; /* Previous difference between the curves. */
134 double *sval =SISL_NULL; /* Value ,first and second derivatiev of surf. */
135 double *sdiff; /* Difference between the point and the surf. */
136 double *snorm; /* Normal vector of the surface, dummy. */
137 double snext[2]; /* Parameter values */
138
139 /* Test input. */
140
141 if (ppoint->idim != psurf->idim) goto err106;
142 if (ppoint->idim != kdim) goto err106;
143
144 /* Fetch endpoints and the intervals of parameter interval of curves. */
145
146 tdelta[0] = psurf->et1[psurf->in1] - psurf->et1[psurf->ik1 - 1];
147 tdelta[1] = psurf->et2[psurf->in2] - psurf->et2[psurf->ik2 - 1];
148
149
150 /* Allocate local used memory */
151
152 sval = newarray(8*kdim,double);
153 if (sval == SISL_NULL) goto err101;
154
155 sdiff = sval + 6*kdim;
156 snorm = sdiff + kdim;
157
158 /* Initiate variables. */
159
160 tprev = (double)HUGE;
161
162
163 /* Evaluate 0-1.st derivatives of surface */
164
165 s1421(psurf,kder,enext,&kleft1,&kleft2,sval,snorm,&kstat);
166 if (kstat < 0) goto error;
167
168 /* Compute the distanse vector and value and the new step. */
169
170 s1173_s9dir(&tdist,td,td+1,sdiff,ppoint->ecoef,sval,aepsge);
171
172
173 /* Correct if we are not inside the parameter intervall. */
174
175
176 t1[0] = td[0];
177 t1[1] = td[1];
178 s1173_s9corr(t1,enext[0],enext[1],estart[0],eend[0],estart[1],eend[1]);
179
180
181 /* Iterate to find the intersection point. */
182
183 for (knbit = 0; knbit < 50; knbit++)
184 {
185 /* Evaluate 0-1.st derivatives of surface */
186
187 snext[0] = enext[0] + t1[0];
188 snext[1] = enext[1] + t1[1];
189
190 s1421(psurf,kder,snext,&kleft1,&kleft2,sval,snorm,&kstat);
191 if (kstat < 0) goto error;
192
193
194 /* Compute the distanse vector and value and the new step. */
195
196 s1173_s9dir(&tdist,tdn,tdn+1,sdiff,ppoint->ecoef,sval,aepsge);
197
198
199 /* Check if the direction of the step have change. */
200
201 kdir = (s6scpr(td,tdn,2) >= DZERO); /* 0 if changed. */
202
203
204 /* Ordinary converging. */
205
206 if (tdist <= tprev || kdir)
207 {
208 enext[0] += t1[0];
209 enext[1] += t1[1];
210
211 td[0] = t1[0] = tdn[0];
212 td[1] = t1[1] = tdn[1];
213
214 /* Correct if we are not inside the parameter intervall. */
215
216 s1173_s9corr(t1,enext[0],enext[1],estart[0],eend[0],estart[1],eend[1]);
217
218
219 if ( (fabs(t1[0]/tdelta[0]) <= REL_COMP_RES) &&
220 (fabs(t1[1]/tdelta[1]) <= REL_COMP_RES)) break;
221
222 tprev = tdist;
223 }
224
225 /* Not converging, corrigate and try again. */
226
227 else
228 {
229 t1[0] /= (double)2;
230 t1[1] /= (double)2;
231 }
232 }
233
234 /* Iteration stopped, test if point is within resolution */
235
236 if (tdist <= aepsge)
237 *jstat = 1;
238 else
239 *jstat = 2;
240
241 /* Test if the iteration is close to a knot */
242 if (DEQUAL(enext[0],psurf->et1[kleft1]))
243 gpos[0] = psurf->et1[kleft1];
244 else if (DEQUAL(enext[0],psurf->et1[kleft1+1]))
245 gpos[0] = psurf->et1[kleft1+1];
246 else
247 gpos[0] = enext[0];
248
249 if (DEQUAL(enext[1],psurf->et2[kleft2]))
250 gpos[1] = psurf->et2[kleft2];
251 else if (DEQUAL(enext[1],psurf->et2[kleft2+1]))
252 gpos[1] = psurf->et2[kleft2+1];
253 else
254 gpos[1] = enext[1];
255
256
257 /* Iteration completed. */
258
259
260 goto out;
261
262
263 /* Error in allocation */
264
265 err101: *jstat = -101;
266 s6err("s1173",*jstat,kpos);
267 goto out;
268
269 /* Error in input. Conflicting dimensions. */
270
271 err106: *jstat = -106;
272 s6err("s1173",*jstat,kpos);
273 goto out;
274
275 /* Error in lower level routine. */
276
277 error : *jstat = kstat;
278 s6err("s1173",*jstat,kpos);
279 goto out;
280
281 out: if (sval != SISL_NULL) freearray(sval);
282 }
283
284 #if defined(SISLNEEDPROTOTYPES)
285 static void
s1173_s9corr(double gd[],double acoef1,double acoef2,double astart1,double aend1,double astart2,double aend2)286 s1173_s9corr(double gd[], double acoef1,double acoef2,double astart1,
287 double aend1,double astart2, double aend2)
288 #else
289 static void s1173_s9corr(gd,acoef1,acoef2,astart1,aend1,astart2,aend2)
290 double gd[];
291 double acoef1;
292 double acoef2;
293 double astart1;
294 double aend1;
295 double astart2;
296 double aend2;
297 #endif
298 /*
299 *********************************************************************
300 *
301 *********************************************************************
302 *
303 * PURPOSE : To be sure that we are inside the boorder of the
304 * parameter plan. If we are outside clipping is used
305 * to corrigate the step value.
306 *
307 *
308 * INPUT : acoef1 - Coeffisient in the first direction.
309 * acoef2 - Coeffisient in the second direction.
310 * astart1 - The lower boorder in first direction.
311 * aend1 - The higher boorder in first direction.
312 * estart2 - The lower boorder in second direction.
313 * eend2 - The higher boorder in second direction.
314 *
315 *
316 *
317 * INPUT/OUTPUT : gd - Old and new step value.
318 *
319 *
320 * METHOD : We are cutting a line inside a rectangle.
321 * In this case we always know that the startpoint of
322 * the line is inside the rectangel, and we may therfor
323 * use a simple kind of clipping.
324 *
325 *
326 * REFERENCES :
327 *
328 *
329 * WRITTEN BY : Arne Laksaa, SI, Feb 1989
330 *
331 *********************************************************************
332 */
333 {
334 if (acoef1 + gd[0] < astart1) gd[0] = astart1 - acoef1;
335 else if (acoef1 + gd[0] > aend1) gd[0] = aend1 - acoef1;
336
337 if (acoef2 + gd[1] < astart2) gd[1] = astart2 - acoef2;
338 else if (acoef2 + gd[1] > aend2) gd[1] = aend2 - acoef2;
339 }
340
341 #if defined(SISLNEEDPROTOTYPES)
342 static void
s1173_s9dir(double * cdist,double * cdiff1,double * cdiff2,double gdiff[],double evalp[],double evals[],double aepsge)343 s1173_s9dir(double *cdist, double *cdiff1, double *cdiff2,
344 double gdiff[], double evalp[], double evals[], double aepsge)
345 #else
346 static void s1173_s9dir(cdist,cdiff1,cdiff2,gdiff,evalp,evals,aepsge)
347 double *cdist;
348 double *cdiff1;
349 double *cdiff2;
350 double gdiff[];
351 double evalp[];
352 double evals[];
353 double aepsge;
354 #endif
355 /*
356 *********************************************************************
357 *
358 *********************************************************************
359 *
360 * PURPOSE : To compute the distance vector and value beetween
361 * a point and a point on a surface.
362 * And to compute a next step on both parameter direction
363 *
364 *
365 * INPUT : evalp - Value in point.
366 * evals - Value and derivatives in point on surface.
367 * aepsge - The geometry resolution.
368 *
369 * OUTPUT : gdiff - Array to use when computing the differens vector.
370 * cdiff1 - Relative parameter value in intersection
371 * point in first direction.
372 * cdiff2 - Relative parameter value in intersection
373 * point in second direction.
374 * cdist - The value to the point in the distance surface.
375 *
376 *
377 * METHOD : This is a one dimensional case. We want to minimize
378 *
379 * min <(point - S(x,y),point - S(x,y)>
380 * x,y
381 *
382 * This is equivalent to
383 *
384 * x,y : (point-S(x,y))Sx(x,y) = 0
385 * (point-S(x,y))Sy(x,y) = 0
386 *
387 * Using Taylor we get:
388 *
389 * x,y : (point-S-DxSx-DySy)(Sx+DxSxx+DySxy) =0
390 * (point-S-DxSx-DySy)(Sy+DySyy+DxSxy) =0
391 *
392 * If we neglect the Dx**2,Dy**2 and Dx*Dy parts, we get:
393 *
394 * x,y: [(point-S)Sxx - Sx**2]Dx + [(point-S)Sxy - Sx*Sy]Dy = - (point-S)Sx
395 * [(point-S)Syy - Sy**2]Dy + [(point-S)Sxy - Sx*Sy]Dx = - (point-S)Sy
396 *
397 * The solution of this matrix equation is the
398 * following function.
399 * If, however, the matrix is singular,
400 * we estimate Dx and Dy separately
401 * by Newton iteration in one variable.
402 * We combine these two increments
403 * by convexcombination to get the minimum distance to move(!=0)
404 *
405 * REFERENCES :
406 *
407 *
408 * WRITTEN BY : Ulf J. Krystad, SI, June 1989
409 *
410 *********************************************************************
411 */
412 {
413 int kstat=0; /* Local status variable. */
414 double tdiv; /* Determinant */
415 double ta11,ta12,ta21,ta22; /* The matrix */
416 double tmax; /* The largest value in matrix */
417 double tb1,tb2; /* The right hand side. */
418 double tval,tderx,tderxx; /* Function and deriv.
419 values in one-dimentional case */
420 double tdery,tderyy;
421 double tderxy;
422 double tdeltax,tdeltay; /* Locals for the step value to be determined. */
423 double ttemp; /* Temporary value. */
424
425 if (aepsge < 0) kstat=1;
426
427 /* Computing the different vector */
428 s6diff(evalp,evals,1,gdiff);
429
430 /* Computing the length of the different vector. */
431 *cdist = s6length(gdiff,1,&kstat);
432
433 /* Init */
434 tval = evals[0];
435 tderx = evals[1];
436 tdery = evals[2];
437 tderxx = evals[3];
438 tderxy = evals[4];
439 tderyy = evals[5];
440 tdeltax = DZERO;
441 tdeltay = DZERO;
442 *cdiff1 = DZERO;
443 *cdiff2 = DZERO;
444
445
446 /* Building the matrix. */
447
448 ta11 = (gdiff[0]*tderxx - tderx*tderx);
449 ta12 = (gdiff[0]*tderxy - tderx*tdery);
450 ta21 = (gdiff[0]*tderxy - tderx*tdery);
451 ta22 = (gdiff[0]*tderyy - tdery*tdery);
452 tb1 = -gdiff[0]*tderx;
453 tb2 = -gdiff[0]*tdery;
454
455 if (DEQUAL(tb1,DZERO) && DEQUAL(tb2,DZERO))
456 {
457 /* Finished, we have found a max. */
458 }
459 else
460 {
461 tdiv = ta11*ta22 - ta21*ta12;
462 tmax = max(fabs(ta11),max(fabs(ta12),max(fabs(ta21),fabs(ta22))));
463
464 if (fabs(tdiv) > tmax*REL_COMP_RES)
465 {
466 /* The matrix is ok, solve the system using Cramers rule. */
467 tdeltax = tb1*ta22 - tb2*ta12;
468 tdeltay = ta11*tb2 - ta21*tb1;
469 tdeltax /= tdiv;
470 tdeltay /= tdiv;
471 }
472 else
473 {
474 /* The matrix is nearly singular,
475 use Newton on each parameter direction*/
476 tdeltax = s1173_s9del(gdiff,&tderx,&tderxx,1);
477 tdeltay = s1173_s9del(gdiff,&tdery,&tderyy,1);
478
479
480 if (fabs(tdeltax) < REL_COMP_RES || fabs(tdeltay) < REL_COMP_RES )
481 /* If one is very small, we use them as they are. */
482 ;
483 else
484 {
485 /* Use the shortest step; min (1-k)Dx + kDy */
486 ttemp = tdeltay*tdeltax/(tdeltax*tdeltax + tdeltay*tdeltay);
487 tdeltax = tdeltay*ttemp;
488 tdeltay = tdeltax*ttemp;
489
490 }
491
492 }
493 }
494
495 *cdiff1 = tdeltax;
496 *cdiff2 = tdeltay;
497
498 }
499
500 #if defined(SISLNEEDPROTOTYPES)
501 static double
s1173_s9del(double * eco,double * eco1,double * eco2,int idim)502 s1173_s9del(double *eco, double *eco1, double *eco2, int idim)
503 #else
504 static double s1173_s9del(eco,eco1,eco2,idim)
505 double *eco;
506 double *eco1;
507 double *eco2;
508 int idim;
509 #endif
510 /*
511 *********************************************************************
512 *
513 *********************************************************************
514 *
515 * PURPOSE : To compute the distance on the parameter line to a point
516 * on the curve on which the tangent is orthogonal to the
517 * difference vector from this point on the curve to the
518 * point in the space.
519 *
520 *
521 * INPUT : eco - The differens vector beetween the point and the
522 * current posision on the curve.
523 * eco1 - The first derevative vector in the current posision
524 * on the curve.
525 * eco2 - The second derevative vector in the current posision
526 * on the curve.
527 * idim - Dimension of space the vectors lie in.
528 *
529 *
530 * OUTPUT : s1173_s9del - The computed parameter distance.
531 *
532 *
533 * METHOD : We have to find the parameter distance "dt" from
534 * the equation:
535 * <ecoef-dt*ecoef1,ecoef1+dt*ecoef2> = 0.
536 * This may be written:
537 * <ecoef,ecoef1> + <ecoef,ecoef2>*dt
538 * - <ecoef1,ecoef1>*dt + <ecoef1,ecoef2>*dt*dt = 0
539 * The following function is the solution of this second
540 * degree equation. We are always using the solution
541 * with the smallest absolute value.
542 *
543 *
544 * REFERENCES :
545 *
546 *
547 * WRITTEN BY : Arne Laksaa, SI, Mar 1989
548 *
549 *********************************************************************
550 */
551 {
552 double t1,t2,t3,t4,t5,t6; /* Constants in equation. */
553
554 t1 = s6scpr(eco,eco1,idim);
555 t3 = s6scpr(eco1,eco1,idim);
556 t2 = t3 - s6scpr(eco,eco2,idim);
557 t4 = -(double)2 * s6scpr(eco1,eco2,idim);
558
559
560
561 if (DEQUAL(t4,DZERO)) /* The second degree part is degenerated. */
562 {
563 if (DEQUAL(t2,DZERO))
564 {
565 if (DEQUAL(t3,DZERO)) return DZERO;
566 else return (t1/t3);
567 }
568 else return (t1/t2);
569 }
570 else /* An ordinary second degree equation. */
571 {
572 t5 = t2*t2 - (double)2*t4*t1;
573 if (t5 < DZERO) return (t1/t3);
574 else
575 {
576 t6 = sqrt(t5);
577 t5 = (t2 + t6)/t4;
578 t6 = (t2 - t6)/t4;
579 t1 *= t3;
580
581
582 /* We have two solutions and we want to use the one
583 with the same sign as we get while using an other
584 metode t1/t3. If both solutions have the same
585 sign we use the one with smallest value. */
586
587 if (t1 < DZERO)
588 {
589 if (t5 <= DZERO && t6 <= DZERO)
590 {
591 if (t5 > t6) return t5;
592 else return t6;
593 }
594 else if (t5 <= DZERO) return t5;
595 else if (t6 <= DZERO) return t6;
596 else return min(t5,t6);
597 }
598 else if (t1 > DZERO)
599 {
600 if (t5 >= DZERO && t6 >= DZERO)
601 {
602 if (t5 < t6) return t5;
603 else return t6;
604 }
605 else if (t5 >= DZERO) return t5;
606 else if (t6 >= DZERO) return t6;
607 else return max(t5,t6);
608 }
609 else return min(fabs(t5),fabs(t6));
610 }
611 }
612 }
613
614
615
616
617
618
619
620
621
622
623
624
625