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