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: sh1783.c,v 1.2 2001-03-19 15:59:05 afr Exp $
45 *
46 */
47
48
49 #define SH1783
50
51 #include "sislP.h"
52
53 #if defined(SISLNEEDPROTOTYPES)
54 typedef void (*fevalProc)(SISLCurve *,int,double,int *,double[],int *);
55 #else
56 typedef void (*fevalProc)();
57 #endif
58
59 #if defined(SISLNEEDPROTOTYPES)
60 static void sh1783_s9relax (fevalProc fevalc1,fevalProc fevalc2,
61 SISLCurve *,SISLCurve *,int,double,double,int *,
62 double [],double,double *,int *,double [],int *);
63 #else
64 static void sh1783_s9relax ();
65 #endif
66
67 #if defined(SISLNEEDPROTOTYPES)
68 void
sh1783(SISLCurve * pc1,SISLCurve * pc2,double aepsge,double epar[],int idir1,int idir2,double elast[],double enext[],int * jstat)69 sh1783 (SISLCurve * pc1, SISLCurve * pc2, double aepsge, double epar[],
70 int idir1, int idir2, double elast[], double enext[], int *jstat)
71 #else
72 void
73 sh1783 (pc1, pc2, aepsge, epar, idir1, idir2, elast, enext, jstat)
74 SISLCurve *pc1;
75 SISLCurve *pc2;
76 double aepsge;
77 double epar[];
78 int idir1;
79 int idir2;
80 double elast[];
81 double enext[];
82 int *jstat;
83 #endif
84 /*
85 *********************************************************************
86 *
87 *********************************************************************
88 *
89 * PURPOSE : March along two curves as long as the coincide. Then return
90 * the last points found that are closer to each other than
91 * the tolerance, and the first points that are more distant.
92 * Start the marching in a given intersection point.
93 *
94 *
95 * INPUT : pc1 - Pointer to the first curve.
96 * pc2 - Pointer to the second curve..
97 * aepsge - Geometry resolution.
98 * epar[2] - Parameter values for the intersection point.
99 * idir1 - Direction to march first curve.
100 * = 1 : March in the direction of the parametrization.
101 * = -1 : March in the opposite direction.
102 * idir2 - Direction to march second curve.
103 *
104 *
105 * OUTPUT : elast[2] - Parameter values of the last point within the
106 * interval of coincidence.
107 * enext[2] - Parameter values of the first point found outside
108 * this interval.
109 * jstat - status messages
110 * = 3 : Coincidence along both curves.
111 * = 2 : Coincidence along the entire second curve.
112 * = 1 : Coincidence along the entire first curve.
113 * = 0 : Ok. End of interval found.
114 * < 0 : Error.
115 *
116 *
117 * METHOD : March along one curve, and for each step iterate to
118 * the closest point of the other curve at the midpoint and
119 * the endpoint of the step. The geometry and knot vectors of
120 * both curves are considered when making the step, and we
121 * march along the curve that has set the step.
122 *
123 * CALLS : s1221 - Evaluate B-spline curve.
124 * s1227 - Evaluate B-spline curve.
125 * s1307 - Compute unit tangent and radius of curvature.
126 * s1311 - Find current step length.
127 * sh1992 - Find box-size of object.
128 * s6lenght - Length of vector.
129 *
130 *
131 * REFERENCES :
132 *
133 * WRITTEN BY : Vibeke Skytt, SI, Oslo, Norway. 04.91.
134 *
135 *********************************************************************
136 */
137 {
138 int kstat; /* Status variable */
139 int ki; /* Counter. */
140 int kleftc1 = 0; /* Left indicator for point calculation of curve 1.*/
141 int kleftc2 = 0; /* Left indicator for point calculation of curve 2.*/
142 int kk1, kk2, kn1, kn2; /* Orders and number of vertices of curves */
143 int kdim; /* The dimension of the space in which the curves lie. */
144 int kpos = 0; /* Position of error */
145 int kderc = 2; /* Number of derivatives to be claculated on the curves */
146 int kdum; /* Temporary variable */
147 int kchange; /* Indicates which curve that is marched along.
148 = 0 : First curve.
149 = 1 : Second curve. */
150 double s3dinf1[20]; /* Pointer to storage for point info of curve 1
151 (10 dobules pr point when idim=3, 7 when idim=3) */
152 double s3dinf2[20]; /* Pointer to storage for point info of curve 2
153 (10 dobules pr point when idim=3, 7 when idim=3) */
154 double *st1; /* Knot vector of first curve */
155 double *st2; /* Knot vector of second curve */
156 double tfirst1, tfirst2; /* First parameter value on curves */
157 double tend1, tend2; /* Last parameter on curves */
158 double sderc1[20]; /* Position, first and second derivatives on curve 1 */
159 double sderc2[20]; /* Position, first and second derivatives on curve 2 */
160 double tx, tx1, tx2; /* Parameter values of first curve. */
161 double ty, ty1, ty2; /* Parameter value of second curve. */
162 double tstep; /* Final step length */
163 double txstep, tystep; /* Step length */
164 double txmaxinc, tymaxinc; /* Maximal increment in parameter value along curve*/
165 double txlengthend, tylengthend; /* Length of 1st derivative at start of segment */
166 double txincre, tyincre; /* Parameter value increment */
167 double txmax, tymax; /* Local maximal step length */
168 double tdist = DZERO; /* Distance */
169 double tpos; /* New iteration point on curve pc2 */
170
171 /* Pointer to curve evaluator routines */
172
173 fevalProc fevalc1;
174 fevalProc fevalc2;
175
176 /* Make maximal step length based on box-size of curve 1 */
177
178 sh1992cu (pc1, 0, aepsge, &kstat);
179 if (kstat < 0)
180 goto error;
181
182 txmax = MAX (pc1->pbox->e2max[0][0] - pc1->pbox->e2min[0][0],
183 pc1->pbox->e2max[0][1] - pc1->pbox->e2min[0][1]);
184 txmax = MAX (txmax, pc1->pbox->e2max[0][2] - pc1->pbox->e2min[0][2]);
185
186 /* Make maximal step length based on box-size of curve 2 */
187
188 sh1992cu (pc2, 0, aepsge, &kstat);
189 if (kstat < 0)
190 goto error;
191
192 tymax = MAX (pc2->pbox->e2max[0][0] - pc2->pbox->e2min[0][0],
193 pc2->pbox->e2max[0][1] - pc2->pbox->e2min[0][1]);
194 tymax = MAX (tymax, pc2->pbox->e2max[0][2] - pc2->pbox->e2min[0][2]);
195
196 /* Copy curve pc1 attributes to local parameters. */
197
198 kdim = pc1->idim;
199 kk1 = pc1->ik;
200 kn1 = pc1->in;
201 st1 = pc1->et;
202
203 /* Copy curve pc2 attributes to local parameters. */
204
205 kk2 = pc2->ik;
206 kn2 = pc2->in;
207 st2 = pc2->et;
208
209 /* Check that dimensions are equal */
210
211 if (kdim != pc2->idim || kdim > 3)
212 goto err105;
213
214 /* Copy interval description into local variables */
215
216 tfirst1 = epar[0];
217 tfirst2 = epar[1];
218 tend1 = (idir1 == 1) ? st1[kn1] : st1[kk1 - 1];
219 tend2 = (idir2 == 1) ? st2[kn2] : st2[kk2 - 1];
220
221 /* To make sure we do not start outside or end outside the curve we
222 truncate tfirst1 to the knot interval of the curve */
223
224 tfirst1 = (idir1 == 1) ? MAX (tfirst1, st1[kk1 - 1]) : MIN (tfirst1, st1[kn1]);
225
226 /* To make sure we do not start outside or end outside the curve we
227 truncate tstart2 and tend2 to the knot interval of the curve */
228
229 tfirst2 = (idir2 == 1) ? MAX (tfirst2, st2[kk2 - 1]) : MIN (tfirst2, st2[kn2]);
230
231 /* Set curve evaluator of 1. curve. */
232
233 fevalc1 = (idir1 == 1) ? s1221 : s1227;
234
235 /* Set curve evaluator of 2. curve. */
236
237 fevalc2 = (idir2 == 1) ? s1221 : s1227;
238
239 /* Store knot values at start of curve */
240
241 tx1 = tfirst1;
242 kdum = MAX (kk1, kk2);
243 txmaxinc = fabs (tend1 - tfirst1) / (kdum * kdum);
244
245 /* Make start point and intital step length based on first curve */
246
247 fevalc1 (pc1, kderc, tx1, &kleftc1, sderc1, &kstat);
248 if (kstat < 0) goto error;
249
250 ty1 = tfirst2;
251 tymaxinc = fabs (tend2 - tfirst2) / (kdum * kdum);
252
253 /* Make start point and intital step length based on second curve */
254
255 fevalc2 (pc2, kderc, ty1, &kleftc2, sderc2, &kstat);
256 if (kstat < 0) goto error;
257
258 /* While end not reached */
259
260 while (idir1 * tx1 < idir1 * tend1 && idir2 * ty1 < idir2 * tend2)
261 {
262
263 /* Calculate unit tangent and radius of curvature of first curve. */
264
265 s1307 (sderc1, kdim, s3dinf1, &kstat);
266 if (kstat < 0)
267 goto error;
268
269 /* Calculate step length based on curvature of first curve. */
270
271 txstep = s1311 (s3dinf1[3 * kdim], aepsge, tymax, &kstat);
272 if (kstat < 0)
273 goto error;
274
275 /* Remember length of start tangent, end of zero segment */
276
277 txlengthend = s6length (sderc1 + kdim, kdim, &kstat);
278 if (kstat < 0)
279 goto error;
280
281 /* Calculate unit tangent and radius of curvature of second curve. */
282
283 s1307 (sderc2, kdim, s3dinf2, &kstat);
284 if (kstat < 0)
285 goto error;
286
287 /* Calculate step length based on curvature */
288
289 tystep = s1311 (s3dinf2[3 * kdim], aepsge, txmax, &kstat);
290 if (kstat < 0)
291 goto error;
292
293 /* Remember length of start tangent, end of zero segment */
294
295 tylengthend = s6length (sderc2 + kdim, kdim, &kstat);
296 if (kstat < 0)
297 goto error;
298
299 /* Find minimum step length. */
300
301 tstep = MIN (txstep, tystep);
302 kchange = (txstep <= tystep) ? 0 : 1;
303
304 /* Find candidate end point, make sure that no breaks in tangent or
305 curvature exists between start and endpoints of the segment */
306 /* Compute increment in the parameter values. Use REL_PAR_RES if the
307 tangent has zero length. */
308
309 if (DEQUAL (txlengthend, DZERO))
310 txincre = REL_PAR_RES;
311 else
312 txincre = MIN (tstep / txlengthend, txmaxinc);
313
314 if (DEQUAL (tylengthend, DZERO))
315 tyincre = REL_PAR_RES;
316 else
317 tyincre = MIN (tstep / tylengthend, tymaxinc);
318
319 /* Make sure that we don't pass any knots of curve 1. */
320
321 /* VSK. 01-93. Is it possible that several knots might be passed. */
322
323 if (idir1 > 0 && (tx1 + txincre) > (st1[kleftc1 + 1] + REL_PAR_RES) &&
324 !(tx1 > (st1[kleftc1 + 1] - REL_PAR_RES)))
325 {
326 txincre = st1[kleftc1 + 1] - tx1;
327 tstep = txincre * txlengthend;
328 tyincre = (tylengthend > DZERO) ? tstep / tylengthend : REL_PAR_RES;
329 kchange = 0;
330 }
331
332 /*
333 guen if (idir1 < 0 && (tx1 - txincre < st1[kleftc1] - REL_PAR_RES))
334 guen fixed to:
335 */
336 /* VSK. 01-93. Is it possible that several knots might be passed. */
337
338 if (idir1 < 0 && (tx1 - txincre) < (st1[kleftc1] - REL_PAR_RES) &&
339 !(tx1 < (st1[kleftc1] + REL_PAR_RES)))
340 {
341 txincre = idir1 * (st1[kleftc1] - tx1);
342 tstep = txincre * txlengthend;
343 tyincre = (tylengthend > DZERO) ? tstep / tylengthend : REL_PAR_RES;
344 kchange = 0;
345 }
346
347 /* Avoid passing next knot of curve 2. */
348
349 /* VSK. 01-93. Is it possible that several knots might be passed. */
350
351 if (idir2 > 0 && (ty1 + tyincre) > (st2[kleftc2 + 1] + REL_PAR_RES) &&
352 !(ty1 > (st2[kleftc2 + 1] - REL_PAR_RES)))
353 {
354 tyincre = st2[kleftc2 + 1] - ty1;
355 tstep = tyincre * tylengthend;
356 txincre = (txlengthend > DZERO) ? tstep / txlengthend : REL_PAR_RES;
357 kchange = 1;
358 }
359
360 /* Avoid passing previous knot of curve 2. */
361
362 /*
363 guen if (idir2 < 0 && (ty1 - tyincre < st2[kleftc2] - REL_PAR_RES))
364 guen fixed to:
365 */
366 /* VSK. 01-93. Is it possible that several knots might be passed. */
367
368 if (idir2 < 0 && (ty1 - tyincre) < (st2[kleftc2] - REL_PAR_RES) &&
369 !(ty1 > (st2[kleftc2] + REL_PAR_RES)))
370 {
371 tyincre = idir2 * (st2[kleftc2] - ty1);
372 tstep = tyincre * tylengthend;
373 txincre = (txlengthend > DZERO) ? tstep / txlengthend : REL_PAR_RES;
374 kchange = 1;
375 }
376
377
378 /* Set endpoints of step. */
379
380 tx2 = tx1 + idir1 * txincre;
381 ty2 = ty1 + idir2 * tyincre;
382
383 for (tx = (tx1 + tx2) / (double) 2.0, ty = (ty1 + ty2) / (double) 2.0, ki = 0;
384 ki < 2; ki++, tx = tx2, ty = ty2)
385 {
386 if (kchange == 0)
387 {
388 if (idir1 * tx >= idir1 * tend1)
389 break;
390
391 /* March along first curve. Iterate down to the second. */
392
393 sh1783_s9relax (fevalc1, fevalc2, pc1, pc2, kderc, aepsge, tx, &kleftc1, sderc1, ty,
394 &tpos, &kleftc2, sderc2, jstat);
395 if (kstat < 0)
396 goto error;
397 }
398 else
399 {
400 if (idir2 * ty >= idir2 * tend2)
401 break;
402
403 /* March along second curve. Iterate down to the first. */
404
405 sh1783_s9relax (fevalc2, fevalc1, pc2, pc1, kderc, aepsge, ty, &kleftc2, sderc2, tx,
406 &tpos, &kleftc1, sderc1, jstat);
407 if (kstat < 0)
408 goto error;
409 }
410
411 /* Check if point on curve and surface are within positional and
412 angular tolerances */
413
414 tdist = s6dist (sderc1, sderc2, kdim);
415
416 if (tdist > aepsge)
417 break; /* Points not within tolerances */
418 }
419
420 if (tdist > aepsge)
421 break; /* Points not within tolerances */
422
423 /* Update start parameter value of segment. */
424
425 if (kchange == 0)
426 {
427 tx1 = tx2;
428 ty1 = (idir2 > 0) ? MAX(ty1,tpos) : MIN(ty1,tpos);
429 }
430 else
431 {
432 tx1 = (idir1 > 0) ? MAX(tx1,tpos) : MIN(tx1,tpos);
433 ty1 = ty2;
434 }
435 }
436
437 elast[0] = tx1;
438 elast[1] = ty1;
439 if (tdist > aepsge)
440 {
441 enext[0] = (kchange == 0) ? tx : tpos;
442 enext[1] = (kchange == 1) ? ty : tpos;
443 *jstat = 0;
444 }
445 else if (idir1 * tx1 >= idir1 * tend1 && idir2 * ty1 >= idir2 * tend2)
446 *jstat = 3;
447 else if (idir2 * ty1 >= idir2 * tend2)
448 *jstat = 2;
449 else
450 *jstat = 1;
451
452 goto out;
453
454 /* Error in input, dimension not equal to 2 or 3 */
455
456 err105:*jstat = -105;
457 s6err ("sh1783", *jstat, kpos);
458 goto out;
459
460 /* Error in lower level function */
461
462 error:*jstat = kstat;
463 s6err ("sh1783", *jstat, kpos);
464 goto out;
465
466 out:
467 return;
468 }
469
470
471 #if defined(SISLNEEDPROTOTYPES)
472 static void
sh1783_s9relax(fevalProc fevalc1,fevalProc fevalc2,SISLCurve * pc1,SISLCurve * pc2,int ider,double aepsge,double ax1,int * jleft1,double eder1[],double anext,double * cx2,int * jleft2,double eder2[],int * jstat)473 sh1783_s9relax(fevalProc fevalc1,fevalProc fevalc2,
474 SISLCurve * pc1, SISLCurve * pc2,int ider, double aepsge,
475 double ax1, int *jleft1, double eder1[],double anext,
476 double *cx2, int *jleft2, double eder2[], int *jstat)
477 #else
478 static void
479 sh1783_s9relax (fevalc1, fevalc2, pc1, pc2, ider, aepsge, ax1, jleft1, eder1,
480 anext,cx2, jleft2, eder2, jstat)
481 fevalProc fevalc1;
482 fevalProc fevalc2;
483 SISLCurve *pc1;
484 SISLCurve *pc2;
485 int ider;
486 double aepsge;
487 double ax1;
488 int *jleft1;
489 double eder1[];
490 double anext;
491 double *cx2;
492 int *jleft2;
493 double eder2[];
494 int *jstat;
495 #endif
496 /*
497 *********************************************************************
498 *
499 *********************************************************************
500 *
501 * PURPOSE : Evaluate the first curve in a given parameter value and
502 * iterate down to the closest point on the second curve
503 * to the position on the first curve.
504 *
505 *
506 * INPUT : fevalc1 - Curve evaluator corresponding to first curve.
507 * fevalc1 - Curve evaluator corresponding to second curve.
508 * pc1 - Pointer to the first curve.
509 * pc2 - Pointer to the second curve.
510 * ider - Number of derivatives to compute. 0 <= ider <= 2.
511 * aepsge - Geometry resolution.
512 * ax1 - Parameter value at which to evaluate curve 1.
513 * anext - Start parameter to the iteration on curve 2.
514 *
515 *
516 * INPUT/OUTPUT : jleft1 - Parameter used to set knot interval of curve1.
517 * Used in s1221.
518 * jleft2 - Parameter used to set knot interval of curve2.
519 *
520 *
521 * OUTPUT : eder1 - 0-ider'th derivative of curve 1 evaluated in ax1.
522 * Dimension is (ider+1)*pc1->idim.
523 * cx2 - Parameter value of the point on curve 2 closest
524 * to the point given by eder1.
525 * eder2 - 0-ider'th derivative of curve 2 evaluated in *cx2.
526 * Dimension is (ider+1)*pc2->idim.
527 * jstat - status messages
528 * > 0 : Warning.
529 * = 0 : Ok.
530 * < 0 : Error.
531 *
532 *
533 * METHOD :
534 *
535 *
536 * CALLS : s1221 - Evaluate curve.
537 * s1771 - Closest point between a curve and a point.
538 * newPoint - Create new point object.
539 * freePoint - Free point object.
540 *
541 *
542 * REFERENCES :
543 *
544 * WRITTEN BY : Vibeke Skytt, SI, Oslo, Norway. Oct. 1990
545 *
546 *********************************************************************
547 */
548 {
549 int kstat = 0; /* Status variable. */
550 double tstart; /* Start parameter value of curve 2. */
551 double tend; /* End parameter value of curve 2. */
552 SISLPoint *qpoint = SISL_NULL; /* SISLPoint instance used to represent point on curve 1. */
553
554 /* Find endpoints of the parameter interval of curve 2. */
555
556 tstart = *(pc2->et + pc2->ik - 1);
557 tend = *(pc2->et + pc2->in);
558
559 /* Make point sderc at curve at ax1 */
560
561 fevalc1 (pc1, ider, ax1, jleft1, eder1, &kstat);
562 if (kstat < 0) goto error;
563
564 /* Find closest point on curve 2 to eder1 */
565
566 qpoint = newPoint (eder1, pc1->idim, 0);
567 if (qpoint == SISL_NULL) goto err101;
568
569 s1771 (qpoint, pc2, aepsge, tstart, tend, anext, cx2, &kstat);
570 if (kstat < 0)
571 goto error;
572
573 /* Calculate point and derivatives in second curve */
574
575 fevalc2 (pc2, ider, *cx2, jleft2, eder2, &kstat);
576 if (kstat < 0) goto error;
577
578 *jstat = 0;
579 goto out;
580
581 /* Error in space allocation. */
582
583 err101:
584 *jstat = -101;
585 goto out;
586
587 /* Error in lower level routine. */
588
589 error:
590 *jstat = kstat;
591 goto out;
592
593 out:
594 if (qpoint != SISL_NULL)
595 freePoint (qpoint);
596
597 return;
598 }
599
600