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