1 /* Calculates the minimum distance between two filaments */
2 /* Among other things, like Gaussian Quadrature weights */
3 #include "induct.h"
4
5 /* this is where the Gaussian Quadrature are defined */
6 double **Gweight, **Gpoint;
7
8 double dist_between();
9 double min_endpt_sep();
10 double dist_betw_pt_and_fil();
11
12 /* The line defined by fil1 is r = s1 + t1*D1 where s1 is the vector
13 (fil1->x[0], y[0], z[0]) and D1 is (x[1] - x[0], y[1] - y[0], z[1] - z[0]).
14 t1 is a scalar from 0 to 1. Similarly for fil2 */
15
dist_betw_fils(fil1,fil2,parallel)16 double dist_betw_fils(fil1, fil2, parallel)
17 FILAMENT *fil1, *fil2;
18 int *parallel;
19 {
20 double k1,k2,k3,k4,c1,c2,c3,c4,a1,a2,a3,b1,b2,b3;
21 double x1,y1,z1,x2,y2,z2;
22 double D1[3], D2[3], s1[3], s2[3], e[3], *D, t1, t2, s1ms2[3], s1me[3];
23 double D1D1, D1D2, D2D2, D1s1s2, D2s1s2;
24 double tmp1, tmp2;
25
26 s1[XX] = fil1->x[0];
27 s1[YY] = fil1->y[0];
28 s1[ZZ] = fil1->z[0];
29 s2[XX] = fil2->x[0];
30 s2[YY] = fil2->y[0];
31 s2[ZZ] = fil2->z[0];
32
33 s1ms2[XX] = s1[XX] - s2[XX];
34 s1ms2[YY] = s1[YY] - s2[YY];
35 s1ms2[ZZ] = s1[ZZ] - s2[ZZ];
36
37 getD(fil1, D1);
38 getD(fil2, D2);
39
40 D1D1 = vdotp(D1,D1);
41 D1D2 = vdotp(D1,D2);
42 D2D2 = vdotp(D2,D2);
43 D1s1s2 = vdotp(D1,s1ms2);
44 D2s1s2 = vdotp(D2,s1ms2);
45
46 tmp1 = D1D2*D1D2/D1D1 - D2D2;
47
48 if (fabs(tmp1/D2D2) < EPS) {
49 /* fils are parallel */
50 *parallel = 1;
51 return min_endpt_sep(fil1,fil2);
52 }
53 else
54 *parallel = 0;
55
56 t2 = (D1D2*D1s1s2/D1D1 - D2s1s2)/tmp1;
57 t1 = (t2*D1D2 - D1s1s2)/D1D1;
58
59 if (t1 <= 1 && t1 >= 0) {
60 if (t2 <= 1 && t2 >= 0) {
61 getr(&x1,&y1,&z1,s1,t1,D1);
62 getr(&x2,&y2,&z2,s2,t2,D2);
63 return dist_between(x1,y1,z1,x2,y2,z2);
64 }
65 else
66 /* nearest point along fil2 is outside line segment defining filament */
67 return dist_betw_pt_and_fil(fil1, D1, s1, D1D1, fil2,t2);
68 }
69 else {
70 if (t2 <= 1 && t2 >= 0) {
71 /* nearest point along fil1 is outside line segment defining filament */
72 return dist_betw_pt_and_fil(fil2, D2, s2, D2D2, fil1,t1);
73 }
74 else
75 /* both point are out of range, just compare endpoints */
76 return min_endpt_sep(fil1,fil2);
77 }
78
79 }
80
getD(fil,D)81 getD(fil, D)
82 FILAMENT *fil;
83 double *D;
84 {
85 D[XX] = fil->x[1] - fil->x[0];
86 D[YY] = fil->y[1] - fil->y[0];
87 D[ZZ] = fil->z[1] - fil->z[0];
88 }
89
getr(x,y,z,s,t,D)90 getr(x,y,z,s,t,D)
91 double *x,*y,*z;
92 double *s,t,*D;
93 {
94 *x = s[XX] + t*D[XX];
95 *y = s[YY] + t*D[YY];
96 *z = s[ZZ] + t*D[ZZ];
97 }
98
vdotp(v1,v2)99 double vdotp(v1, v2)
100 double *v1,*v2;
101 {
102 return v1[XX]*v2[XX] + v1[YY]*v2[YY] + v1[ZZ]*v2[ZZ];
103 }
104
dist_between(x1,y1,z1,x2,y2,z2)105 double dist_between(x1,y1,z1,x2,y2,z2)
106 double x1,y1,z1,x2,y2,z2;
107 {
108 return sqrt(SQUARE(x1 - x2) + SQUARE(y1 - y2) + SQUARE(z1 - z2));
109 }
110
111 /* returns the minimum distance between an endpt of fil1 and one of fil2 */
min_endpt_sep(fil1,fil2)112 double min_endpt_sep(fil1,fil2)
113 FILAMENT *fil1, *fil2;
114 {
115 double min, tmp;
116 int idx1, idx2;
117
118 idx1 = 0;
119 idx2 = 0;
120 min = dist_between(fil1->x[idx1],fil1->y[idx1],fil1->z[idx1],
121 fil2->x[idx2],fil2->y[idx2],fil2->z[idx2]);
122
123 idx1 = 1;
124 idx2 = 0;
125 tmp = dist_between(fil1->x[idx1],fil1->y[idx1],fil1->z[idx1],
126 fil2->x[idx2],fil2->y[idx2],fil2->z[idx2]);
127 if (tmp < min) min = tmp;
128
129 idx1 = 0;
130 idx2 = 1;
131 tmp = dist_between(fil1->x[idx1],fil1->y[idx1],fil1->z[idx1],
132 fil2->x[idx2],fil2->y[idx2],fil2->z[idx2]);
133 if (tmp < min) min = tmp;
134
135 idx1 = 1;
136 idx2 = 1;
137 tmp = dist_between(fil1->x[idx1],fil1->y[idx1],fil1->z[idx1],
138 fil2->x[idx2],fil2->y[idx2],fil2->z[idx2]);
139 if (tmp < min) min = tmp;
140
141 return min;
142 }
143
144 /* this finds the shortest distance between the line defined by
145 r = s + tnew*D, t in [0,1] (which is along fil_line) and the endpoint
146 on fil which is closer to fil_line (determined by the value of t) */
dist_betw_pt_and_fil(fil_line,D,s,DD,fil,t)147 double dist_betw_pt_and_fil(fil_line, D, s, DD, fil,t)
148 FILAMENT *fil_line, *fil;
149 double *D, *s, t, DD;
150 {
151 double e[3], sme[3], x,y,z;
152 double tnew, Dsme;
153 int idx;
154
155 if (t < 0) {
156 e[XX] = fil->x[0];
157 e[YY] = fil->y[0];
158 e[ZZ] = fil->z[0];
159 }
160 else if (t > 1) {
161 e[XX] = fil->x[1];
162 e[YY] = fil->y[1];
163 e[ZZ] = fil->z[1];
164 }
165 else {
166 fprintf(stderr, "Internal err: dist_bet_pt_and_fil: why is t = %lg?\n", t);
167 exit(1);
168 }
169
170 sme[XX] = s[XX] - e[XX];
171 sme[YY] = s[YY] - e[YY];
172 sme[ZZ] = s[ZZ] - e[ZZ];
173
174 Dsme = vdotp(D,sme);
175
176 tnew = -Dsme/DD;
177
178 if (tnew <= 1 && tnew >= 0) {
179 /* This will be the case when a small fil is near a big one (fil_line).*/
180 /* Calculate r = (s - e) + tnew*D */
181 getr(&x,&y,&z,sme,tnew,D);
182 return sqrt(x*x + y*y + z*z);
183 }
184 else {
185 /* just find the distance between the nearest endpt and e[] */
186 if (tnew < 0)
187 idx = 0;
188 else
189 idx = 1;
190
191 return dist_between(e[XX], e[YY], e[ZZ],
192 fil_line->x[idx], fil_line->y[idx], fil_line->z[idx]);
193 }
194 }
195
aspectratio(fil)196 double aspectratio(fil)
197 FILAMENT *fil;
198 {
199 double rat;
200
201 rat = fil->height/fil->width;
202 if (rat >= 1.0)
203 return rat;
204 else
205 return 1.0/rat;
206 }
207
fill_Gquad()208 fill_Gquad()
209 {
210 int i,j;
211
212 Gweight = (double **)MattAlloc(MAXsubfils+1, sizeof(double *));
213 Gpoint = (double **)MattAlloc(MAXsubfils+1, sizeof(double *));
214
215 for(i = 1; i <= MAXsubfils; i++) {
216 Gweight[i] = (double *)MattAlloc(i, sizeof(double));
217 Gpoint[i] = (double *)MattAlloc(i, sizeof(double));
218 }
219
220 Gweight[1][0] = 2.0;
221 Gpoint[1][0] = 0.0;
222
223 for(i = 2; i <= MAXsubfils; i++)
224 /* subtract 1 from pointers since this function starts with p[1] */
225 gquad_weights(i,Gpoint[i] - 1, Gweight[i] - 1);
226 }
227
findnfils(fil,subfils,nfils)228 findnfils(fil, subfils, nfils)
229 FILAMENT *fil, *subfils;
230 int nfils;
231 {
232 double hx,hy,hz,mag,wx,wy,wz,dx,dy,dz;
233 int i,j;
234
235 if (fil->segm->widthdir != NULL) {
236 wx = fil->segm->widthdir[XX];
237 wy = fil->segm->widthdir[YY];
238 wz = fil->segm->widthdir[ZZ];
239 }
240 else {
241 /* default for width direction is in x-y plane perpendic to length*/
242 /* so do cross product with unit z*/
243 wx = -(fil->y[1] - fil->y[0])*1.0;
244 wy = (fil->x[1] - fil->x[0])*1.0;
245 wz = 0;
246 if ( fabs(wx/fil->segm->length) < EPS && fabs(wy/fil->segm->length) < EPS) {
247 /* if all of x-y is perpendic to length, then choose x direction */
248 wx = 1.0;
249 wy = 0;
250 }
251 mag = sqrt(wx*wx + wy*wy + wz*wz);
252 wx = wx/mag;
253 wy = wy/mag;
254 wz = wz/mag;
255 }
256
257 hx = -wy*(fil->z[1] - fil->z[0]) + (fil->y[1] - fil->y[0])*wz;
258 hy = -wz*(fil->x[1] - fil->x[0]) + (fil->z[1] - fil->z[0])*wx;
259 hz = -wx*(fil->y[1] - fil->y[0]) + (fil->x[1] - fil->x[0])*wy;
260 mag = sqrt(hx*hx + hy*hy + hz*hz);
261 hx = hx/mag;
262 hy = hy/mag;
263 hz = hz/mag;
264
265 if (fil->width > fil->height) {
266 dx = fil->width*wx/2;
267 dy = fil->width*wy/2;
268 dz = fil->width*wz/2;
269 }
270 else {
271 dx = fil->height*hx/2;
272 dy = fil->height*hy/2;
273 dz = fil->height*hz/2;
274 }
275
276 /* all mutualfil needs are the filament coordinates and length */
277 for (j = 0; j < nfils; j++) {
278 for(i = 0; i < 2; i++) {
279 subfils[j].x[i] = fil->x[i] + dx*Gpoint[nfils][j];
280 subfils[j].y[i] = fil->y[i] + dy*Gpoint[nfils][j];
281 subfils[j].z[i] = fil->z[i] + dz*Gpoint[nfils][j];
282 }
283 subfils[j].length = fil->length;
284 }
285 }
286
287 #if 1==0
288 /* main for testing if these work */
main()289 main()
290 {
291 FILAMENT fil1, fil2;
292
293 while(1) {
294 printf("fil1 1: ");
295 scanf("%lf %lf %lf", &(fil1.x[0]), &(fil1.y[0]), &(fil1.z[0]));
296 printf("fil1 2: ");
297 scanf("%lf %lf %lf", &(fil1.x[1]), &(fil1.y[1]), &(fil1.z[1]));
298
299 printf("fil2 1: ");
300 scanf("%lf %lf %lf", &(fil2.x[0]), &(fil2.y[0]), &(fil2.z[0]));
301 printf("fil2 2: ");
302 scanf("%lf %lf %lf", &(fil2.x[1]), &(fil2.y[1]), &(fil2.z[1]));
303
304 printf("dist = %lg\n",dist_betw_fils(&fil1, &fil2));
305 }
306 }
307 #endif
308
gquad_weights(N,p,w)309 gquad_weights(N,p,w)
310 int N;
311 double *p,*w;
312 {
313 int i;
314
315 if ( !((N>=2)&&(N<=6)) &&(N!=19)&&(N!=30)&&(N!=25)&&(N!=15)&&(N!=10)) {
316 fprintf(stderr,"Hey, bad number of Guassian Quad points\n");
317 exit(1);
318 }
319 if (N==10) {
320 p[1]= -.9739065285; p[10]= -p[1];
321 p[2]= -.8650633666; p[9] = -p[2];
322 p[3]= -.6794095682; p[8] = -p[3];
323 p[4]= -.4333953941; p[7] = -p[4];
324 p[5]= -.1488743389; p[6] = -p[5];
325 w[1]=w[10]=.0666713443;
326 w[2]=w[9]= .1494513491;
327 w[3]=w[8]= .2190863625;
328 w[4]=w[7]= .2692667193;
329 w[5]=w[6]= .2955242247;
330 }
331 if (N==15) {
332 p[1]= -.9879925180;
333 p[2]= -.9372733924;
334 p[3]= -.8482065834;
335 p[4]= -.7244177313;
336 p[5]= -.5709721726;
337 p[6]= -.3941513470;
338 p[7]= -.2011940939;
339 p[8]= 0.0;
340 p[9]= -p[7];
341 p[10]= -p[6];
342 p[11]= -p[5];
343 p[12]= -p[4];
344 p[13]= -p[3];
345 p[14]= -p[2];
346 p[15]= -p[1];
347 w[1]=w[15]= .0307532419;
348 w[2]=w[14]= .0703660474;
349 w[3]=w[13]= .1071592204;
350 w[4]=w[12]= .1395706779;
351 w[5]=w[11]= .1662692058;
352 w[6]=w[10]= .1861610000;
353 w[7]=w[9]= .1984314853;
354 w[8]= .2025782419;
355 }
356 if (N==25){
357 p[1]= -.9955568687; p[25]= -p[1];
358 p[2]= -.9766639214; p[24]= -p[2];
359 p[3]= -.9429745712; p[23]= -p[3];
360 p[4]= -.8949919978; p[22]= -p[4];
361 p[5]= -.8334426287; p[21]= -p[5];
362 p[6]= -.7592592630; p[20]= -p[6];
363 p[7]= -.6735663684; p[19]= -p[7];
364 p[8]= -.5776629302; p[18]= -p[8];
365 p[9]= -.4730027314; p[17]= -p[9];
366 p[10]= -.3611723058; p[16]= -p[10];
367 p[11]= -.2438668837; p[15]= -p[11];
368 p[12]= -.1228646926; p[14]= -p[12];
369 p[13]= 0.0;
370 w[1]=w[25]= .0113937985;
371 w[2]=w[24]= .0263549866;
372 w[3]=w[23]= .0409391567;
373 w[4]=w[22]= .0549046959;
374 w[5]=w[21]= .0680383338;
375 w[6]=w[20]= .0801407003;
376 w[7]=w[19]= .0910282619;
377 w[8]=w[18]= .1005359490;
378 w[9]=w[17]= .1085196244;
379 w[10]=w[16]= .1148582591;
380 w[11]=w[15]= .1194557635;
381 w[12]=w[14]= .1222424429;
382 w[13]= .1231760537;
383 }
384 if (N==2) {
385 p[1]= -.57735027;
386 p[2]= .57735027;
387 w[1]=w[2]=1.0;
388 }
389 if (N==3) {
390 p[1]= -.77459667;
391 p[2]= 0.;
392 p[3]= .77459667;
393 w[1]= .55555555;
394 w[2]= .88888888;
395 w[3]= .55555555;
396 }
397 if (N==4) {
398 p[1]= -.86113631;
399 p[2]= -.33998104;
400 p[3]= .33998104;
401 p[4]= .86113631;
402 w[1]=w[4]=.34785485;
403 w[2]=w[3]=.65214515;
404 }
405 if (N==5){
406 p[1] = -.90617984;
407 p[2] = -.53846931;
408 p[3] = 0.;
409 p[4] = .53846931;
410 p[5] = .90617984;
411 w[1]=w[5]=.23692688;
412 w[2]=w[4]=.47862867;
413 w[3]=.56888888;
414 }
415 if (N==6){
416 p[1]= -.93246951;
417 p[2]= -.66120938;
418 p[3]= -.23861918;
419 p[4]= -p[3];
420 p[5]= -p[2];
421 p[6]= -p[1];
422 w[1]=w[6]= .17132449;
423 w[2]=w[5]= .36076157;
424 w[3]=w[4]= .46791393;
425 }
426 if (N==19) {
427 p[1]= -.9931285991;
428 p[2]= -.9602081521;
429 p[3]= -.9031559036;
430 p[4]= -.8227146565;
431 p[5]= -.7209661773;
432 p[6]= -.6005453046;
433 p[7]= -.4645707413;
434 p[8]= -.3165640999;
435 p[9]= -.1603586456;
436 p[10] = 0.;
437 p[11] = -p[9];
438 p[12] = -p[8];
439 p[13] = -p[7];
440 p[14] = -p[6];
441 p[15] = -p[5];
442 p[16] = -p[4];
443 p[17] = -p[3];
444 p[18] = -p[2];
445 p[19] = -p[1];
446 w[1]=w[19]= .0194617882;
447 w[2]=w[18]= .0448142267;
448 w[3]=w[17]= .0690445427;
449 w[4]=w[16]=.0914900216;
450 w[5]=w[15]=.1115666455;
451 w[6]=w[14]=.1287539625;
452 w[7]=w[13]=.1426067021;
453 w[8]=w[12]=.1527660420;
454 w[9]=w[11]=.1589688433;
455 w[10]=.1610544498;
456 }
457 if (N==30) {
458 p[1]= -.9968934840;
459 p[2]= -.9836681232;
460 p[3]= -.9600218649;
461 p[4]= -.9262000474;
462 p[5]= -.8825605357;
463 p[6]= -.8295657623;
464 p[7]= -.7677774321;
465 p[8]= -.6978504947;
466 p[9]= -.6206261829;
467 p[10]= -.5366241481;
468 p[11]= -.4470337695;
469 p[12]= -.3527047255;
470 p[13]= -.2546369261;
471 p[14]= -.1538699136;
472 p[15]= -.0514718425;
473 p[16]= -p[15];
474 p[17]= -p[14];
475 p[18]= -p[13];
476 p[19]= -p[12];
477 p[20]= -p[11];
478 p[21]= -p[10];
479 p[22]= -p[9];
480 p[23]= -p[8];
481 p[24]= -p[7];
482 p[25]= -p[6];
483 p[26]= -p[5];
484 p[27]= -p[4];
485 p[28]= -p[3];
486 p[29]= -p[2];
487 p[30]= -p[1];
488 w[1]=w[30]= .0079681924;
489 w[2]=w[29]= .0184664683;
490 w[3]=w[28]= .0287847078;
491 w[4]=w[27]= .0387991925;
492 w[5]=w[26]= .0484026728;
493 w[6]=w[25]= .0574931562;
494 w[7]=w[24]= .0659742298;
495 w[8]=w[23]= .0737559747;
496 w[9]=w[22]= .0807558952;
497 w[10]=w[21]= .0868997872;
498 w[11]=w[20]= .0921225222;
499 w[12]=w[19]= .0963687371;
500 w[13]=w[18]= .0995934205;
501 w[14]=w[17]= .1017623897;
502 w[15]=w[16]= .1028526528;
503 }
504 }
505
506
507
508
509
510