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: s1611.c,v 1.2 2001-03-19 15:58:51 afr Exp $
45 *
46 */
47
48
49 #define S1611
50
51 #include "sislP.h"
52
53 #if defined(SISLNEEDPROTOTYPES)
54 void
s1611(double epoint[],int inbpnt,int idim,double eptyp[],int iopen,int ik,double astpar,double aepsge,double * cendpar,SISLCurve ** rc,int * jstat)55 s1611 (double epoint[], int inbpnt, int idim, double eptyp[], int iopen,
56 int ik, double astpar, double aepsge,
57 double *cendpar, SISLCurve ** rc, int *jstat)
58 #else
59 void
60 s1611 (epoint, inbpnt, idim, eptyp, iopen, ik, astpar, aepsge,
61 cendpar, rc, jstat)
62 double epoint[];
63 int inbpnt;
64 int idim;
65 double eptyp[];
66 int iopen;
67 int ik;
68 double astpar;
69 double aepsge;
70 double *cendpar;
71 SISLCurve **rc;
72 int *jstat;
73 #endif
74 /*
75 *************************************************************************
76 *
77 * PURPOSE: To approximate a conic arc with a b-spline curve in the two or
78 * the three dimensional space. If two points are given a straight
79 * line is produced, if three a circular arc and if four or five
80 * a conic arc.
81 *
82 * INPUT:
83 * Epoint - Array (length idim*inbpnt) containing the points/
84 * tangents to be interpolated.
85 * Inbpnt - No. of points/tangents in the epoint array.
86 * Idim - The dimension of the space in which the points lie.
87 * Eptyp - Array (length inbpnt) containing type indicator for
88 * points/tangents :
89 * 1 - Ordinary point.
90 * 2 - Knuckle point. (Is treated as an ordinary point.)
91 * 3 - Tangent to next point.
92 * 4 - Tangent to prior point.
93 * Iopen - Flag telling if the curve should be open or closed:
94 * 0 : Closed curve.
95 * 1 : Open curve.
96 * NOT IN USE!
97 * Ik - The order of the B-spline curve to be produced.
98 * Astpar - Parameter-value to be used at the start of the curve.
99 * Aepsge - The geometry resolution.
100 *
101 * Output:
102 * Cendpar - Parameter-value used at the end of the curve.
103 * Rc - Pointer to output-curve.
104 * Jstat - Status variable:
105 * < 0 : Error.
106 * = 0 : Ok.
107 * > 0 : Warning.
108 *
109 * Method: First we find the conic curve on which the points lie.
110 * Then a b-spline curve describing the conic is produced.
111 *-
112 * Calls: s1614, s6crss, s6rotmat, s6inv4, s6mulvec, s1615, s1616, s1617,
113 * s1385, s1602, s6err.
114 *
115 * Written by: A.M. Ytrehus, SI Oslo Oct.91.
116 * After FORTRAN (P19506), written by: T. Dokken SI.
117 * REVISED BY : Christophe Birkeland, SI, July 1992 (eptyp is now DOUBLE)
118 * REVISED BY : Johannes Kaasa, SI, Aug. 1992 (transported status 105 from
119 * s1616 upwards)
120 * REVISED BY : Johannes Kaasa, SI, Aug. 1992 (Fixed bug in production of
121 * the rotational matrix, and made a seperate handling of
122 * circular arcs)
123 *****************************************************************
124 */
125 {
126 double *spoint = SISL_NULL;
127 int *sptyp = SISL_NULL;
128 double smatrix[16];
129 double sinv[16];
130 double *st = SISL_NULL;
131 double sbeg[3], slutt[3], svect1[3], svect2[3], snorm[3];
132 int knbpnt = 0;
133 int ktyp;
134 int kp, ki, kk, kn;
135 double tdum, tpar;
136 double tshape;
137 double spnt[3], sdum[3];
138 double start[3], stang[3], stop[3];
139 double strans[3];
140 double smul[3];
141 double sconic[6];
142 int kpos = 0;
143 int krem = 0;
144 int kstat = 0;
145 int *ieptyp = SISL_NULL; /* The point type descriptions (eptyp) in integer form. */
146 double centerpt[3], axis[3], sdir[3];
147 double angle, sdot, tlength1, tlength2, tcos;
148 int kstat1, kstat2;
149
150 *jstat = 0;
151
152
153 /* Copy eptyp to ieptyp. */
154
155 ieptyp = newarray(inbpnt,INT);
156 if (ieptyp == SISL_NULL)
157 goto err101;
158
159 for(ki=0; ki<inbpnt; ki++)
160 {
161 ieptyp[ki] = (int)eptyp[ki];
162 }
163
164
165 /* Test if legal input. */
166
167 if (idim < 2 || idim > 3 || ik < 3)
168 goto err104;
169
170
171 /* Allocate new arrays. */
172
173 spoint = newarray (inbpnt * idim, DOUBLE);
174 if (spoint == SISL_NULL)
175 goto err101;
176
177 sptyp = newarray (inbpnt, INT);
178 if (sptyp == SISL_NULL)
179 goto err101;
180
181
182 /* Check the types of the data/points. */
183
184 s1614 (epoint, inbpnt, idim, ieptyp,spoint, &knbpnt, sptyp, &kstat);
185 if (kstat < 0)
186 goto error;
187
188
189 /* Save the start- and end-points, in case of straight line. */
190
191 kk = idim * (knbpnt - 1);
192
193 for (kp = 0; kp < idim; kp++)
194 {
195 sbeg[kp] = spoint[kp];
196 sdir[kp] = spoint[idim + kp];
197 slutt[kp] = spoint[kk + kp];
198 }
199
200
201 /* Test if straight line is to be produced. */
202
203 if (knbpnt == 2)
204 goto straight;
205
206
207 /* The calculations on the conic are performed two-dimensionally.
208 Obtain a 2D description if the conic lies in a 3D space. */
209
210 if (idim == 3)
211 {
212
213 /* Translate the points (and not the tangents) such that
214 the first point lies in the origin. Remember translation. */
215
216 for (kp = 0; kp < idim; kp++)
217 strans[kp] = spoint[kp];
218
219 for (ki = 0; ki < knbpnt; ki++)
220 {
221 ktyp = sptyp[ki];
222 if (ktyp < 3)
223 {
224 kk = idim * ki;
225
226 for (kp = 0; kp < idim; kp++)
227 spoint[kk + kp] = spoint[kk + kp] - strans[kp];
228 }
229 }
230
231 /* Produce rotational matrix. */
232
233 kk = idim * (knbpnt - 1);
234
235 for (kp = 0; kp < idim; kp++)
236 spnt[kp] = spoint[kk + kp];
237
238 for (ki = 1; ki < knbpnt - 1; ki++)
239 {
240 kk = idim * ki;
241
242 for (kp = 0; kp < idim; kp++)
243 sdum[kp] = spoint[kk + kp];
244
245 for (kp = 0; kp < idim; kp++)
246 {
247 svect1[kp] = spnt[kp] - spoint[kp];
248 svect2[kp] = sdum[kp] - spoint[kp];
249 }
250
251 s6crss (svect1, svect2, snorm);
252
253 s6rotmat (spoint, spnt, snorm, smatrix, &kstat);
254 if (kstat == 0)
255 break;
256 }
257
258 /* If we are here with kstat< 0, we have not been able
259 to produce rotational matrix. Make straight line. */
260
261 if (kstat < 0)
262 goto straight;
263
264
265 /* Invert the matrix to prepare for rotation of points. */
266
267 s6inv4 (smatrix, sinv, &kstat);
268 if (kstat < 0)
269 goto error;
270
271
272 /* Use the inverted matrix on the points and tangents. */
273
274 for (ki = 0; ki < knbpnt; ki++)
275 {
276 kk = idim * ki;
277
278 for (kp = 0; kp < idim; kp++)
279 sdum[kp] = spoint[kk + kp];
280
281 s6mulvec (sinv, sdum, smul);
282
283 for (kp = 0; kp < idim; kp++)
284 spoint[kk + kp] = smul[kp];
285 }
286 }
287
288
289 /* Test if the points lie on the same branch if the curve
290 is a hyperbola. If kstat = 1, different branches, create straight line. */
291
292 s1615 (spoint, knbpnt, idim, sptyp, &kstat);
293 if (kstat < 0)
294 goto error;
295 if (kstat == 1)
296 goto straight;
297
298
299 /* Find the conic equation of the points. */
300
301 s1616 (spoint, knbpnt, idim, sptyp, sconic, &kstat);
302 if (kstat < 0)
303 goto error;
304 if (kstat > 0)
305 krem = kstat;
306
307 if (knbpnt == 3)
308 {
309
310 /* Circle segment */
311
312 /* Establish centerpoint. */
313
314 centerpt[0] = - sconic[3];
315 centerpt[1] = - sconic[4];
316 centerpt[2] = (double) 0.0;
317
318 /* If the dimension is 3, rotate the centerpoint
319 back into the 3D space and translate. */
320
321 if (idim == 3)
322 {
323 s6mulvec (smatrix, centerpt, smul);
324
325 for (kp = 0; kp < idim; kp++)
326 centerpt[kp] = smul[kp] + strans[kp];
327 }
328
329
330 /* Establish rotational angle and axis. */
331
332 s6diff(sbeg, centerpt, idim, svect1);
333 s6diff(slutt, centerpt, idim, svect2);
334
335 sdot = s6scpr(svect1, svect2, idim);
336
337 tlength1 = s6length(svect1, idim, &kstat1);
338 tlength2 = s6length(svect2, idim, &kstat2);
339
340 if (!kstat1 || !kstat2)
341 angle = DZERO;
342 else
343 {
344 tcos = sdot/(tlength1*tlength2);
345 tcos = MIN((double)1.0,tcos);
346 angle = acos(tcos);
347 }
348
349 if (idim == 3)
350 s6crss(svect1, sdir, axis);
351 else if ((svect1[0]*sdir[1] - svect1[1]*sdir[0]) < 0.)
352 angle = -angle;
353
354 s6diff(slutt, sbeg, idim, svect1);
355 sdot = 0.0;
356 for (kp = 0; kp < idim; kp++)
357 sdot += sdir[kp]*svect1[kp];
358 if (sdot < 0.0)
359 {
360 if (idim == 3)
361 angle = TWOPI - angle;
362 else
363 {
364 if (angle < 0)
365 angle = - (angle + TWOPI);
366 else
367 angle = TWOPI - angle;
368 }
369 }
370
371 s1303 (sbeg, aepsge, angle, centerpt, axis, idim, rc, &kstat);
372 if (kstat < 0)
373 goto error;
374
375 }
376 else
377 {
378
379 /* Produce knots for interpolation. */
380
381 s1617 (spoint, knbpnt, idim, sptyp, aepsge, sconic,
382 start, stang, stop, &tshape, &kstat);
383 if (kstat < 0)
384 goto error;
385 if (kstat == 1)
386 goto straight;
387
388
389 /* If the dimension is 3, rotate the points
390 back into the 3D space and translate. */
391
392 if (idim == 3)
393 {
394 s6mulvec (smatrix, start, smul);
395
396 for (kp = 0; kp < idim; kp++)
397 start[kp] = smul[kp] + strans[kp];
398
399 s6mulvec (smatrix, stang, smul);
400
401 for (kp = 0; kp < idim; kp++)
402 stang[kp] = smul[kp] + strans[kp];
403
404 s6mulvec (smatrix, stop, smul);
405
406 for (kp = 0; kp < idim; kp++)
407 stop[kp] = smul[kp] + strans[kp];
408 }
409
410
411 /* Make description of conic curve as
412 B-spline within user defined tolerance */
413
414 s1385 (start, stang, stop, tshape, idim, aepsge, rc, &kstat);
415 if (kstat < 0)
416 goto error;
417
418 }
419
420
421 /* Adjust knot vector to match input start parameter value */
422
423 kk = (*rc)->ik;
424 kn = (*rc)->in;
425
426 st = (*rc)->et;
427
428 tdum = astpar - st[kk - 1];
429
430 for (ki = 0; ki < kn + kk; ki++)
431 st[ki] += tdum;
432
433
434 /* Find end parameter value of curve */
435
436 *cendpar = st[kn];
437
438 goto out;
439
440 straight:
441
442 /* Create straight line, based on first and last real point. */
443
444 s1602 (sbeg, slutt, ik, idim, astpar, &tpar, rc, &kstat);
445 if (kstat < 0)
446 goto error;
447
448
449 /* Find end parameter value of curve */
450
451 *cendpar = tpar;
452
453 goto out;
454
455
456 /* Error in allocation. */
457
458 err101:
459 *jstat = -101;
460 s6err ("s1611", *jstat, kpos);
461 goto out;
462
463 /* Dimension error. */
464
465 err104:
466 *jstat = -104;
467 s6err ("s1611", *jstat, kpos);
468 goto out;
469
470 /* Error in lower level routine. */
471
472 error:
473 *jstat = kstat;
474 s6err ("s1611", *jstat, kpos);
475 goto out;
476
477 out:
478
479 /* Free space occupied by local arrays. */
480
481 if (spoint != SISL_NULL)
482 freearray (spoint);
483 if (sptyp != SISL_NULL)
484 freearray (sptyp);
485 if (ieptyp != SISL_NULL)
486 freearray (ieptyp);
487
488 if (*jstat >= 0)
489 *jstat = krem;
490 return;
491 }
492