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: s1312.c,v 1.2 2001-03-19 15:58:44 afr Exp $
45 *
46 */
47
48
49 #define S1312
50
51 #include "sislP.h"
52
53 #if defined(SISLNEEDPROTOTYPES)
54 void
s1312(double egeo[],int idim,int inbinf,int ipar,double epar[],SISLCurve ** rcurve,int * jstat)55 s1312(double egeo[],int idim,int inbinf,int ipar,double epar[],
56 SISLCurve **rcurve,int *jstat)
57 #else
58 void s1312(egeo,idim,inbinf,ipar,epar,rcurve,jstat)
59 double egeo[];
60 int idim;
61 int inbinf;
62 int ipar;
63 double epar[];
64 SISLCurve **rcurve;
65 int *jstat;
66 #endif
67 /*
68 *********************************************************************
69 *
70 * PURPOSE : To represent the curve described in egeo as
71 * an Hermit curve on a B-spline format.
72 *
73 *
74 * INPUT : egeo - The geometry of the point to be interpolated
75 * The sequence of the information for each point
76 * is: position, unit tangent, curvature vector
77 * and radius of curvature.
78 * When the dimension is 2 this is 7 doubles
79 * When the dimension is 3 this is 10 doubles
80 * Total size of egeo is thus:
81 * idim=2 : 7*inbinf doubles
82 * idim=3 : 10*inbinf doubles
83 * idim - Dimension of the spcae the points lie in
84 * only 2 and 3 is legal
85 * inbinf - Number of points
86 * ipar - Array telling if input parametrization (in epar)
87 * is to be used:
88 * ipar = 0 : Don't use input parametrization
89 * ipar = 1 : Use input parametrization
90 *
91 * INPUT/OUTPUT:
92 * epar - Parametrization of the points. ipar determines
93 * if this is input or output
94 *
95 * OUTPUT : rcurve - The curve produced
96 * jstat - status messages
97 * > 0 : warning
98 * = 0 : ok
99 * < 0 : error
100 *
101 *
102 * METHOD :
103 *
104 * REFERENCES :
105 *
106 *-
107 * CALLS :
108 *
109 * WRITTEN BY : Tor Dokken, SI, OSLO, Norway, 30. June 1988
110 *
111 *********************************************************************
112 */
113 {
114 int kn; /* Number of vertices */
115 int kk = 4; /* Order of b-spline basis */
116 int knt; /* Number of knots produced so far */
117 int kvert; /* Pointer to first free variable in vertex array */
118 int kpos =1; /* Position of error */
119 int kstat; /* Local status variable */
120 int ki,kj; /* Running variables in loop */
121 int kv1,kv2,kv3; /* Running variables in loop */
122 int kincre; /* Number of doubles for each point in egeo */
123 double *sprevp; /* Pointer to position at start of current segment */
124 double *sprevt; /* Pointer to tangent at start of current segment */
125 double *sprevc; /* Pointer to curvature at start of current segment */
126 double *sprevr; /* Pointer to radius of curvature start current segment*/
127 double snprevt[3]; /* Nomralized version of sprevc */
128 double *scurp; /* Pointer to position at end of current segment */
129 double *scurt; /* Pointer to tangent at end of current segment */
130 double *scurc; /* Pointer to curvature at end of current segment */
131 double *scurr; /* Pointer to radius of curvature end current segment*/
132 double sncurt[3]; /* Normalized version of scurc */
133 double tcos; /* Description of angle */
134 double tl1,tl2; /* Tangent lengths */
135 double tangle; /* Arcus cosinus if tcos */
136 double tdist; /* Distance between start and end of current segment */
137 double tpar; /* Parameter value at end of segment */
138 double *st = SISL_NULL; /* Pointer to knot vector */
139 double *scoef=SISL_NULL; /* Pointer to vertices */
140
141 /* Allocate space for knots and vertices */
142
143 if (idim != 2 && idim != 3) goto err105;
144
145 if (idim==2)
146 kincre = 7;
147 else
148 kincre = 10;
149
150 kn = 3*(inbinf-1) + 1;
151 scoef = newarray(idim*kn,DOUBLE);
152 if (scoef == SISL_NULL) goto err101;
153
154 st = newarray(kk+kn,DOUBLE);
155 if (st == SISL_NULL) goto err101;
156
157 /* Make four first knots */
158 if (ipar==0)
159 epar[0] = DZERO;
160
161 st[0] = epar[0];
162 st[1] = epar[0];
163 st[2] = epar[0];
164 st[3] = epar[0];
165
166 /* Make first vertex */
167 memcopy(scoef,egeo,idim,DOUBLE);
168
169
170 /* Set pointers to start point, tangent, curvature and radius of curvature */
171
172 sprevp = egeo;
173 sprevt = sprevp + idim;
174 sprevc = sprevt + idim;
175 sprevr = sprevc + idim;
176
177 /* Normalize curvature vector at start */
178
179 (void)s6norm(sprevt,idim,snprevt,&kstat);
180
181 for (ki=1,knt=4,kvert=idim;ki<inbinf;ki++)
182 {
183
184 /* For each pair of adjacent points in egeo make an Hermit segment */
185
186 /* Set pointers position, tangent, curvature and radius of end of
187 current segment segment */
188
189 scurp = sprevp + kincre;
190 scurt = sprevt + kincre;
191 scurc = sprevc + kincre;
192 scurr = sprevr + kincre;
193
194 /* Normalize curvature vector at end of segment */
195
196 (void)s6norm(scurt,idim,sncurt,&kstat);
197
198 /* Make cosine of angle between tangent vectors by making the scalar
199 product of the normalized versions of the two vectors */
200
201 tcos = s6scpr(snprevt,sncurt,idim);
202
203 /* Find the actual angle by making the arcus tangens of this value */
204
205 if (tcos >= DZERO)
206 tcos = MIN((double)1.0,tcos);
207 else
208 tcos = MAX((double)-1.0,tcos);
209
210 tangle = fabs(acos(tcos));
211
212 if (tangle < ANGULAR_TOLERANCE) tangle = DZERO;
213
214 tdist = s6dist(sprevp,scurp,idim);
215
216 /* Make tangent length of start of segment */
217
218 if (tangle == DZERO || *sprevr <= DZERO)
219 {
220 /* Parallel tangents or infinit radius of curvature use 1/3 of
221 the distance between the points as tangent length */
222 tl1 = tdist/(double)3.0;
223 }
224 else
225 {
226 /* Base tangent length on radius of curvature and opening angle */
227 tl1 = s1325(*sprevr,tangle);
228 }
229
230 /* Make tangent length of end of segment */
231
232 if (DEQUAL(tangle,DZERO) || *scurr < DZERO)
233 {
234 /* Parallel tangents or infinit radius of curvature use 1/3 of
235 the distance between the points as tangent length */
236 tl2 = tdist/(double)3.0;
237 }
238 else
239 {
240 /* Base tangent length on radius of curvature and opening angle */
241 tl2 = s1325(*scurr,tangle);
242 }
243
244 /* Make sure that the tangent does not explode due to numeric errors,
245 and make a controlled tangent when the radius is zero or almost zero*/
246
247 if ( tl1 > tdist) tl1 = tdist/(double)3.0;
248 if ( tl2 > tdist) tl2 = tdist/(double)3.0;
249
250 /* We want to have a parametrization that is as close as possible to an
251 arc length parametrization */
252
253
254 if (ipar==0)
255 {
256 /* Make parametrization of segment by making an average of arc of a
257 circle with radius sprevr and scurr spanning an angle tangle.
258 If one or both radius infinit use the distance between the
259 points */
260
261 if (DNEQUAL(*sprevr,(double)-1.0) &&
262 DNEQUAL(*scurr,(double)-1.0))
263 {
264 tpar = (double)0.5*tangle*(*sprevr+*scurr);
265 }
266 else if (DNEQUAL(*sprevr,(double)-1.0) &&
267 DEQUAL(*scurr,(double)-1.0))
268 {
269 tpar = (double)0.5*(*sprevr*tangle + tdist);
270 }
271 else if (DEQUAL(*sprevr,(double)-1.0) &&
272 DNEQUAL(*scurr,(double)-1.0))
273 {
274 tpar = (double)0.5*(tdist + tangle*(*scurr));
275 }
276 else
277 {
278 tpar = tdist;
279 }
280
281 tpar = MAX(tpar,tdist);
282
283 if (DEQUAL((epar[ki-1]+tpar),epar[ki-1]))
284 {
285 tpar = fabs(epar[ki-1])*(double)0.1;
286 }
287
288 if (DEQUAL(tpar,DZERO))
289 {
290 tpar = (double)1.0;
291 }
292
293 epar[ki] = epar[ki-1] + tpar;
294
295 }
296
297 /* Make 3 new knots */
298 st[knt] = epar[ki];
299 st[knt+1] = epar[ki];
300 st[knt+2] = epar[ki];
301
302 /* Make 3 new vertices of segment */
303
304 for (kj=0,kv1=kvert,kv2=kv1+idim,kv3=kv2+idim ; kj<idim ;
305 kj++,kv1++,kv2++,kv3++)
306 {
307 scoef[kv1] = sprevp[kj] + tl1*sprevt[kj];
308 scoef[kv2] = scurp[kj] - tl2*scurt[kj];
309 scoef[kv3] = scurp[kj];
310 }
311
312 /* Update pointers */
313 sprevp = scurp;
314 sprevt = scurt;
315 sprevc = scurc;
316 sprevr = scurr;
317 for (kj=0;kj<idim;kj++) snprevt[kj] = sncurt[kj];
318
319 /* Only update number of vertices if epar[ki-1] != epar[ki] */
320
321 if (DNEQUAL(epar[ki-1],epar[ki]))
322 {
323 kvert+=(3*idim);
324 knt+=3;
325 }
326 }
327
328 /* Insert last knot */
329
330 st[kn+kk-1] = st[kn+kk-2];
331
332 /* Update number of vertices */
333
334 kn = kvert/idim;
335
336
337 /* Make the curve */
338
339 kpos = 1;
340 *rcurve = SISL_NULL;
341 *rcurve = newCurve(kn,kk,st,scoef,1,idim,1);
342 if (*rcurve == SISL_NULL) goto err101;
343
344 *jstat = 0;
345 goto out;
346
347 /* Error in space allocation. */
348
349 err101: *jstat = -101;
350 s6err("s1312",*jstat,kpos);
351 goto out;
352
353 /* Error in input, negative relative tolerance given */
354
355 err105: *jstat = -105;
356 s6err("s1312",*jstat,kpos);
357 goto out;
358
359 /* Free allocated arrays */
360 out:
361
362
363 if (st != SISL_NULL) freearray(st);
364 if (scoef != SISL_NULL) freearray(scoef);
365
366
367 return;
368 }
369