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: s1770.c,v 1.2 2001-03-19 15:58:53 afr Exp $
45 *
46 */
47 #define S1770
48
49 #include "sislP.h"
50
51 /*
52 * Forward declarations.
53 * ---------------------
54 */
55
56 #if defined(SISLNEEDPROTOTYPES)
57 static
58 void
59 s1770_s9corr(double [],double,double,double,double,double,double);
60 static
61 void
62 s1770_s9dir(double *,double *,double *,double [],double [],double [],int);
63 #else
64 static void s1770_s9corr();
65 static void s1770_s9dir();
66 #endif
67
68 #if defined(SISLNEEDPROTOTYPES)
69 void
s1770(SISLCurve * pcurve1,SISLCurve * pcurve2,double aepsge,double astart1,double astart2,double aend1,double aend2,double anext1,double anext2,double * cpos1,double * cpos2,int * jstat)70 s1770(SISLCurve *pcurve1,SISLCurve *pcurve2,double aepsge,
71 double astart1,double astart2,double aend1,double aend2,
72 double anext1,double anext2,double *cpos1,double *cpos2,int *jstat)
73 #else
74 void s1770(pcurve1,pcurve2,aepsge,astart1,astart2,
75 aend1,aend2,anext1,anext2,cpos1,cpos2,jstat)
76 SISLCurve *pcurve1;
77 SISLCurve *pcurve2;
78 double aepsge;
79 double astart1;
80 double astart2;
81 double aend1;
82 double aend2;
83 double anext1;
84 double anext2;
85 double *cpos1;
86 double *cpos2;
87 int *jstat;
88 #endif
89 /*
90 *********************************************************************
91 *
92 *********************************************************************
93 *
94 * PURPOSE : Newton iteration on the distance function between
95 * two curves to find a closest point or an intersection point.
96 *
97 *
98 * INPUT : pcurve1 - Pointer to the first curve in the intersection.
99 * pcurve2 - Pointer to the second curve in the intersection.
100 * aepsge - Geometry resolution.
101 * astart1 - Start parameter value of the first curve.
102 * astart2 - Start parameter value of the second curve.
103 * aend1 - End parameter value of the first curve.
104 * aend2 - End parameter value of the second curve.
105 * anext1 - Start parameter value of the iteration on
106 * the first curve.
107 * anext2 - Start parameter value of the iteration on
108 * the second curve.
109 *
110 *
111 *
112 * OUTPUT : cpos1 - Parameter value of of first curve in intersection
113 * point.
114 * cpos2 - Parameter value of of second curve in intersection
115 * point.
116 * jstat - status messages
117 * = 2 : A minimum distanse found.
118 * = 1 : Intersection found.
119 * < 0 : error.
120 *
121 *
122 * METHOD : Newton iteration in two parameter directions.
123 *
124 *
125 * REFERENCES :
126 *
127 *
128 * WRITTEN BY : Arne Laksaa, SI, Feb 1989
129 *
130 *********************************************************************
131 */
132 {
133 int kstat = 0; /* Local status variable. */
134 int kpos = 0; /* Position of error. */
135 int kleft1=0,kleft2=0; /* Variables used in the evaluator. */
136 int kder=1; /* Order of derivatives to be calulated */
137 int kdim; /* Dimension of space the curves lie in */
138 int knbit; /* Number of iterations */
139 int kdir; /* Changing direction. */
140 double tdelta1,tdelta2; /* Parameter interval of the curves. */
141 double tdist; /* Distance between position and origo. */
142 double td[2],t1[2],tdn[2];/* Distances between old and new parameter
143 value in the two parameter directions. */
144 double tprev; /* Previous difference between the curves. */
145 double *sval1=SISL_NULL; /* Value ,first and second derivatie on curve 1*/
146 double *sval2; /* Value ,first and second derivatie on curve 1*/
147 double *sdiff; /* Difference between the curves */
148
149 /* Test input. */
150
151 if (pcurve1->idim != pcurve2->idim) goto err106;
152
153 kdim = pcurve1 -> idim;
154 if (kdim == 2)
155 {
156 s1770_2D(pcurve1,pcurve2,aepsge,astart1,astart2,
157 aend1,aend2,anext1,anext2,cpos1,cpos2,jstat);
158 goto out;
159 }
160
161 /* Fetch endpoints and the intervals of parameter interval of curves. */
162
163 tdelta1 = pcurve1->et[pcurve1->in] - pcurve1->et[pcurve1->ik - 1];
164 tdelta2 = pcurve2->et[pcurve2->in] - pcurve2->et[pcurve2->ik - 1];
165
166 /* Allocate local used memory */
167
168 sval1 = newarray((2*kder+5)*kdim,double);
169 if (sval1 == SISL_NULL) goto err101;
170
171 sval2 = sval1 + (kder+2)*kdim;
172 sdiff = sval2 + (kder+2)*kdim;
173
174 /* Initiate variables. */
175
176 tprev = (double)HUGE;
177
178 /* Evaluate 0-1.st derivatives of both curves */
179
180 s1221(pcurve1,kder,anext1,&kleft1,sval1,&kstat);
181 if (kstat < 0) goto error;
182
183 s1221(pcurve2,kder,anext2,&kleft2,sval2,&kstat);
184 if (kstat < 0) goto error;
185
186 /* Compute the distanse vector and value and the new step. */
187
188 s1770_s9dir(&tdist,td,td+1,sdiff,sval1,sval2,kdim);
189
190 /* Correct if we are not inside the parameter intervall. */
191
192 t1[0] = td[0];
193 t1[1] = td[1];
194 s1770_s9corr(t1,anext1,anext2,astart1,aend1,astart2,aend2);
195
196 /* Iterate to find the intersection point. */
197
198 for (knbit = 0; knbit < 30; knbit++)
199 {
200 /* Evaluate 0-1.st derivatives of both curves */
201
202 s1221(pcurve1,kder,anext1+t1[0],&kleft1,sval1,&kstat);
203 if (kstat < 0) goto error;
204
205 s1221(pcurve2,kder,anext2+t1[1],&kleft2,sval2,&kstat);
206 if (kstat < 0) goto error;
207
208
209 /* Compute the distanse vector and value and the new step. */
210
211 s1770_s9dir(&tdist,tdn,tdn+1,sdiff,sval1,sval2,kdim);
212
213 /* Check if the direction of the step have change. */
214
215 kdir = (s6scpr(td,tdn,2) >= DZERO); /* 0 if changed. */
216
217 /* Ordinary converging. */
218
219 if (tdist < tprev*(double)0.9 || kdir)
220 {
221 anext1 += t1[0];
222 anext2 += t1[1];
223
224 td[0] = tdn[0];
225 td[1] = tdn[1];
226
227 /* Correct if we are not inside the parameter intervall. */
228
229 t1[0] = td[0];
230 t1[1] = td[1];
231 s1770_s9corr(t1,anext1,anext2,astart1,aend1,astart2,aend2);
232
233 if ( (fabs(t1[0]/tdelta1) <= REL_COMP_RES) &&
234 (fabs(t1[1]/tdelta2) <= REL_COMP_RES) ) break;
235
236 tprev = tdist;
237 }
238
239 /* Not converging, corrigate and try again. */
240
241 else
242 {
243 t1[0] /= (double)2;
244 t1[1] /= (double)2;
245 /* knbit--; */
246 }
247 }
248
249 /* Iteration stopped, test if point founds found is within resolution */
250
251 if (tdist <= aepsge)
252 *jstat = 1;
253 else
254 *jstat = 2;
255
256 *cpos1 = anext1;
257 *cpos2 = anext2;
258
259 /* Iteration completed. */
260
261
262 goto out;
263
264 /* Error in allocation */
265
266 err101: *jstat = -101;
267 s6err("s1770",*jstat,kpos);
268 goto out;
269
270 /* Error in input. Conflicting dimensions. */
271
272 err106: *jstat = -106;
273 s6err("s1770",*jstat,kpos);
274 goto out;
275
276 /* Error in lower level routine. */
277
278 error : *jstat = kstat;
279 s6err("s1770",*jstat,kpos);
280 goto out;
281
282 out: if (sval1 != SISL_NULL) freearray(sval1);
283 }
284
285 #if defined(SISLNEEDPROTOTYPES)
286 static
287 void
s1770_s9corr(double gdn[],double acoef1,double acoef2,double astart1,double aend1,double astart2,double aend2)288 s1770_s9corr(double gdn[],double acoef1,double acoef2,
289 double astart1,double aend1,double astart2,double aend2)
290 #else
291 static void s1770_s9corr(gdn,acoef1,acoef2,astart1,aend1,astart2,aend2)
292 double gdn[];
293 double acoef1;
294 double acoef2;
295 double astart1;
296 double aend1;
297 double astart2;
298 double aend2;
299 #endif
300 /*
301 *********************************************************************
302 *
303 *********************************************************************
304 *
305 * PURPOSE : To be sure that we are inside the boorder of the
306 * parameter plan. If we are outside clipping is used
307 * to corrigate the step value.
308 *
309 *
310 * INPUT : acoef1 - Coeffisient in the first direction.
311 * acoef2 - Coeffisient in the second direction.
312 * astart1 - The lower boorder in first direction.
313 * aend1 - The higher boorder in first direction.
314 * astart2 - The lower boorder in second direction.
315 * aend2 - The higher boorder in second direction.
316 *
317 *
318 *
319 * INPUT/OUTPUT : gdn - Old and new step value.
320 *
321 *
322 * METHOD :
323 *
324 *
325 * REFERENCES :
326 *
327 *
328 * WRITTEN BY : Arne Laksaa, SI, Feb 1989
329 *
330 *********************************************************************
331 */
332 {
333 if (acoef1 + gdn[0] < astart1)
334 gdn[0] = astart1 - acoef1;
335 else if (acoef1 + gdn[0] > aend1)
336 gdn[0] = aend1 - acoef1;
337
338 if (acoef2 + gdn[1] < astart2)
339 gdn[1] = astart2 - acoef2;
340 else if (acoef2 + gdn[1] > aend2)
341 gdn[1] = aend2 - acoef2;
342 }
343
344 #if defined(SISLNEEDPROTOTYPES)
345 static
346 void
s1770_s9dir(double * cdist,double * cdiff1,double * cdiff2,double gdiff[],double eval1[],double eval2[],int idim)347 s1770_s9dir(double *cdist,double *cdiff1,double *cdiff2,
348 double gdiff[],double eval1[],double eval2[],int idim)
349 #else
350 static void s1770_s9dir(cdist,cdiff1,cdiff2,gdiff,eval1,eval2,idim)
351 double *cdist;
352 double *cdiff1;
353 double *cdiff2;
354 double eval1[];
355 double eval2[];
356 double gdiff[];
357 int idim;
358 #endif
359 /*
360 *********************************************************************
361 *
362 *********************************************************************
363 *
364 * PURPOSE : To compute the distance vector and value beetween
365 * a points on the first curve and a point on the second
366 * curve. And to compute a next step on both curves.
367 * This is equivalent to the nearest way to the
368 * parameter plan in the tangent plan from a point in the
369 * distance surface between two curves.
370 *
371 *
372 * INPUT : eval1 - Value and derevative in first parameterdirection.
373 * eval2 - Value and derevative in second parameterdirection.
374 * idim - Dimension of space the curves lie in.
375 *
376 *
377 * OUTPUT : gdiff - Array to use when computing the differens vector.
378 * cdiff1 - Relative parameter value in intersection
379 * point in first direction.
380 * cdiff2 - Relative parameter value in intersection
381 * point in second direction.
382 * cdist - The value to the point in the distance surface.
383 *
384 *
385 * METHOD : The method is to compute the parameter distance to the points
386 * on both tangents which is closest to each other.
387 * The differens vector beetween these points are orthogonal
388 * to both tangents. If the distance vector beetween the two
389 * points on the curve is "diff" and the two derevativ vectors
390 * are "der1" and "der2", and the two wanted parameter distance
391 * are "dt1" and "dt2", then we get the following system of
392 * equations:
393 * <dt1*der1+dist-dt2*der2,der2> = 0
394 * <dt1*der1+dist-dt2*der2,der1> = 0
395 * This is futher:
396 *
397 * | -<der1,der2> <der2,der2> | | dt1 | | <dist,der2> |
398 * | | | | = | |
399 * | -<der1,der1> <der1,der2> | | dt2 | | <dist,der1> |
400 *
401 * The solution of this matrix equation is the
402 * following function.
403 *
404 *
405 * REFERENCES :
406 *
407 *
408 * WRITTEN BY : Arne Laksaa, SI, Feb 1989
409 *
410 *********************************************************************
411 */
412 {
413 int kstat; /* Local status variable. */
414 register double tdet; /* Determinant */
415 register double t1,t2,t3,t4,t5; /* Variables in equation system */
416
417 /* Computing the different vector */
418
419 s6diff(eval1,eval2,idim,gdiff);
420
421 /* Computing the length of the different vector. */
422
423 *cdist = s6length(gdiff,idim,&kstat);
424
425 t1 = s6scpr(eval1+idim,eval1+idim,idim);
426 t2 = s6scpr(eval1+idim,eval2+idim,idim);
427 t3 = s6scpr(eval2+idim,eval2+idim,idim);
428 t4 = s6scpr(gdiff,eval1+idim,idim);
429 t5 = s6scpr(gdiff,eval2+idim,idim);
430
431 /* Computing the determinant. */
432
433 tdet = t2*t2 - t1*t3;
434
435 if (DEQUAL(tdet,DZERO))
436 {
437 *cdiff1 = DZERO;
438 *cdiff2 = DZERO;
439 }
440 else
441 {
442 /* Using Cramer's rule to find the solution of the system. */
443
444 *cdiff1 = (t4*t3 - t5*t2)/tdet;
445 *cdiff2 = (t2*t4 - t1*t5)/tdet;
446 }
447 }
448