34
35
36
37#include "implicit_f.inc"
38
39
40
41#include "mvsiz_p.inc"
42
43
44
45 INTEGER JLT, NRTM
46 INTEGER IX1(MVSIZ), IX2(MVSIZ), IX3(MVSIZ), IX4(MVSIZ), ETYP(MVSIZ), IBC(MVSIZ)
48 . gapv(mvsiz), marge, gap
50 . x1(mvsiz), x2(mvsiz), x3(mvsiz), x4(mvsiz),
51 . y1(mvsiz), y2(mvsiz), y3(mvsiz), y4(mvsiz),
52 . z1(mvsiz), z2(mvsiz), z3(mvsiz), z4(mvsiz),
53 . xi(mvsiz), yi(mvsiz), zi(mvsiz), pene(mvsiz)
54
55
56
57 INTEGER I, IG,I3N
59 . x0(mvsiz), y0(mvsiz), z0(mvsiz), gap2(mvsiz),
60 . nx1(mvsiz), nx2(mvsiz), nx3(mvsiz), nx4(mvsiz),
61 . ny1(mvsiz), ny2(mvsiz), ny3(mvsiz), ny4(mvsiz),
62 . nz1(mvsiz), nz2(mvsiz), nz3(mvsiz), nz4(mvsiz),
63 . lb1(mvsiz), lb2(mvsiz), lb3(mvsiz), lb4(mvsiz),
64 . lc1(mvsiz), lc2(mvsiz), lc3(mvsiz), lc4(mvsiz),
65 . al1(mvsiz), al2(mvsiz), al3(mvsiz), al4(mvsiz),
66 . p1(mvsiz), p2(mvsiz), p3(mvsiz), p4(mvsiz),
67 . x01(mvsiz), x02(mvsiz), x03(mvsiz), x04(mvsiz),
68 . y01(mvsiz), y02(mvsiz), y03(mvsiz), y04(mvsiz),
69 . z01(mvsiz), z02(mvsiz), z03(mvsiz), z04(mvsiz),
70 . xi1(mvsiz), xi2(mvsiz), xi3(mvsiz), xi4(mvsiz),
71 . yi1(mvsiz), yi2(mvsiz), yi3(mvsiz), yi4(mvsiz),
72 . zi1(mvsiz), zi2(mvsiz), zi3(mvsiz), zi4(mvsiz),
73 . hlb1(mvsiz), hlc1(mvsiz), hlb2(mvsiz),hlc2(mvsiz),
74 . hlb3(mvsiz), hlc3(mvsiz), hlb4(mvsiz),hlc4(mvsiz)
76 . dti, s2,a1,a2,a3,a4,d1,d2,d3,d4,
77 . x12,x23,x34,x41,xi0,sx1,sx2,sx3,sx4,sx0,
78 . y12,y23,y34,y41,yi0,sy1,sy2,sy3,sy4,sy0,
79 . z12,z23,z34,z41,zi0,sz1,sz2,sz3,sz4,sz0,
80 . la, hla, aaa, zoneinf
82 . dx, dy, dz, dd,
83 . xim, yim, zim
84
85
86 DO i = 1, jlt
87 zoneinf = gapv(i)+marge
88 gap2(i)= zoneinf*zoneinf
89 END DO
90
91
92 i3n=0
93 DO i=1,jlt
94 IF(ix3(i)==ix4(i))i3n=i3n+1
95 ENDDO
96 IF(i3n==jlt)THEN
97 i3n=1
98 ELSEIF(i3n/=0)THEN
99 i3n=2
100 ENDIF
101
102
103
104 IF(i3n==0) THEN
105 DO i=1,jlt
106 x0(i) = fourth*(x1(i)+x2(i)+x3(i)+x4(i))
107 y0(i) = fourth*(y1(i)+y2(i)+y3(i)+y4(i))
108 z0(i) = fourth*(z1(i)+z2(i)+z3(i)+z4(i))
109 ENDDO
110
111
112
113 ELSEIF(i3n==2) THEN
114 DO i=1,jlt
115 IF(ix3(i)/=ix4(i))THEN
116 x0(i) = fourth*(x1(i)+x2(i)+x3(i)+x4(i))
117 y0(i) = fourth*(y1(i)+y2(i)+y3(i)+y4(i))
118 z0(i) = fourth*(z1(i)+z2(i)+z3(i)+z4(i))
119 ELSE
120 x0(i) = x3(i)
121 y0(i) = y3(i)
122 z0(i) = z3(i)
123 ENDIF
124 ENDDO
125 ENDIF
126
127
128
129 IF(i3n==1) THEN
130 DO i=1,jlt
131
132 x01(i) = x1(i) - x3(i)
133 y01(i) = y1(i) - y3(i)
134 z01(i) = z1(i) - z3(i)
135
136 x02(i) = x2(i) - x3(i)
137 y02(i) = y2(i) - y3(i)
138 z02(i) = z2(i) - z3(i)
139
140 xi0 = x3(i) - xi(i)
141 yi0 = y3(i) - yi(i)
142 zi0 = z3(i) - zi(i)
143
144 xi1(i) = x1(i) - xi(i)
145 yi1(i) = y1(i) - yi(i)
146 zi1(i) = z1(i) - zi(i)
147
148 xi2(i) = x2(i) - xi(i)
149 yi2(i) = y2(i) - yi(i)
150 zi2(i) = z2(i) - zi(i)
151
152 sx1 = yi0*zi1(i) - zi0*yi1(i)
153 sy1 = zi0*xi1(i) - xi0*zi1(i)
154 sz1 = xi0*yi1(i) - yi0*xi1(i)
155
156 sx2 = yi0*zi2(i) - zi0*yi2(i)
157 sy2 = zi0*xi2(i) - xi0*zi2(i)
158 sz2 = xi0*yi2(i) - yi0*xi2(i)
159
160 sx0 = y01(i)*z02(i) - z01(i)*y02(i)
161 sy0 = z01(i)*x02(i) - x01(i)*z02(i)
162 sz0 = x01(i)*y02(i) - y01(i)*x02(i)
163 s2 = 1./
max(em30,sx0*sx0 + sy0*sy0 + sz0*sz0)
164
165 lb1(i) = -(sx0*sx2 + sy0*sy2 + sz0*sz2) * s2
166 lc1(i) = (sx0*sx1 + sy0*sy1 + sz0*sz1) * s2
167
168 aaa = one/
max(em30,x01(i)*x01(i)+y01(i)*y01(i)+z01(i)*z01(i))
169 hlc1(i)= lc1(i)*abs(lc1(i))*aaa
170 al1(i) = -(xi0*x01(i)+yi0*y01(i)+zi0*z01(i))*aaa
171 al1(i) =
max(zero,
min(one,al1(i)))
172 aaa = one/
max(em30,x02(i)*x02(i)+y02(i)*y02(i)+z02(i)*z02(i))
173 hlb1(i)= lb1(i)*abs(lb1(i))*aaa
174 al2(i) = -(xi0*x02(i)+yi0*y02(i)+zi0*z02(i))*aaa
175 al2(i) =
max(zero,
min(one,al2(i)))
176
177 ENDDO
178
179 DO i=1,jlt
180 x12 = x2(i) - x1(i)
181 y12 = y2(i) - y1(i)
182 z12 = z2(i) - z1(i)
183 la = one - lb1(i) - lc1(i)
184 aaa = one /
max(em20,x12*x12+y12*y12+z12*z12)
185 hla= la*abs(la) * aaa
186 IF(la<zero.AND.
187 + hla<=hlb1(i).AND.hla<=hlc1(i))THEN
188 lb1(i) = (xi2(i)*x12+yi2(i)*y12+zi2(i)*z12)*aaa
189 lb1(i) =
max(zero,
min(one,lb1(i)))
190 lc1(i) = one - lb1(i)
191 ELSEIF(lb1(i)<zero.AND.
192 + hlb1(i)<=hlc1(i).AND.hlb1(i)<=hla)THEN
193 lb1(i) = zero
194 lc1(i) = al2(i)
195 ELSEIF(lc1(i)<zero.AND.
196 + hlc1(i)<=hla.AND.hlc1(i)<=hlb1(i))THEN
197 lc1(i) = zero
198 lb1(i) = al1(i)
199 ENDIF
200 ENDDO
201
202 DO i=1,jlt
203
204 nx1(i) = xi(i)-(x3(i) + lb1(i)*x01(i) + lc1(i)*x02(i))
205 ny1(i) = yi(i)-(y3(i) + lb1(i)*y01(i) + lc1(i)*y02(i))
206 nz1(i) = zi(i)-(z3(i) + lb1(i)*z01(i) + lc1(i)*z02(i
207 p1(i) = nx1(i)*nx1(i) + ny1(i)*ny1(i) +nz1(i)
208
209
210
211 pene(i) =
max(zero, gap2(i) - p1(i))
212
213 ENDDO
214
215
216
217 ELSE
218
219 DO i=1,jlt
220
221 x01(i) = x1(i) - x0(i)
222 y01(i) = y1(i) - y0(i)
223 z01(i) = z1(i) - z0(i)
224
225 x02(i) = x2(i) - x0(i)
226 y02(i) = y2(i) - y0(i)
227 z02(i) = z2(i) - z0(i)
228
229 x03(i) = x3(i) - x0(i)
230 y03(i) = y3(i) - y0(i)
231 z03(i) = z3(i) - z0(i)
232
233 x04(i) = x4(i) - x0(i)
234 y04(i) = y4(i) - y0(i)
235 z04(i) = z4(i) - z0(i)
236
237 xi0 = x0(i) - xi(i)
238 yi0 = y0(i) - yi(i)
239 zi0 = z0(i) - zi(i)
240
241 xi1(i) = x1(i) - xi(i)
242 yi1(i) = y1(i) - yi(i)
243 zi1(i) = z1(i) - zi(i)
244
245 xi2(i) = x2(i) - xi(i)
246 yi2(i) = y2(i) - yi(i)
247 zi2(i) = z2(i) - zi(i)
248
249 xi3(i) = x3(i) - xi(i)
250 yi3(i) = y3(i) - yi(i)
251 zi3(i) = z3(i) - zi(i)
252
253 xi4(i) = x4(i) - xi(i)
254 yi4(i) = y4(i) - yi(i)
255 zi4(i) = z4(i) - zi(i)
256
257 sx1 = yi0*zi1(i) - zi0*yi1(i)
258 sy1 = zi0*xi1(i) - xi0*zi1(i)
259 sz1 = xi0*yi1(i) - yi0*xi1(i)
260
261 sx2 = yi0*zi2(i) - zi0*yi2(i)
262 sy2 = zi0*xi2(i) - xi0*zi2(i)
263 sz2 = xi0*yi2(i) - yi0*xi2(i)
264
265 sx0 = y01(i)*z02(i) - z01(i)*y02(i)
266 sy0 = z01(i)*x02(i) - x01(i)*z02(i)
267 sz0 = x01(i)*y02(i) - y01(i)*x02(i)
268 s2 = one/
max(em30,sx0*sx0 + sy0*sy0 + sz0*sz0)
269
270 lb1(i) = -(sx0*sx2 + sy0*sy2 + sz0*sz2) * s2
271 lc1(i) = (sx0*sx1 + sy0*sy1 + sz0*sz1) * s2
272
273 sx3 = yi0*zi3(i) - zi0*yi3(i)
274 sy3 = zi0*xi3(i) - xi0*zi3(i)
275 sz3 = xi0*yi3(i) - yi0*xi3(i)
276
277 sx0 = y02(i)*z03(i) - z02(i)*y03(i)
278 sy0 = z02(i)*x03(i) - x02(i)*z03(i)
279 sz0 = x02(i)*y03(i) - y02(i)*x03(i)
280 s2 = one/
max(em30,sx0*sx0 + sy0*sy0 + sz0*sz0)
281
282 lb2(i) = -(sx0*sx3 + sy0*sy3 + sz0*sz3) * s2
283 lc2(i) = (sx0*sx2 + sy0*sy2 + sz0*sz2) * s2
284
285 sx4 = yi0*zi4(i) - zi0*yi4(i)
286 sy4 = zi0*xi4(i) - xi0*zi4(i)
287 sz4 = xi0*yi4(i) - yi0*xi4(i)
288
289 sx0 = y03(i)*z04(i) - z03(i)*y04(i)
290 sy0 = z03(i)*x04(i) - x03(i)*z04(i)
291 sz0 = x03(i)*y04(i) - y03(i)*x04(i)
292 s2 = one/
max(em30,sx0*sx0 + sy0*sy0 + sz0*sz0)
293
294 lb3(i) = -(sx0*sx4 + sy0*sy4 + sz0*sz4) * s2
295 lc3(i) = (sx0*sx3 + sy0*sy3 + sz0*sz3) * s2
296
297 sx0 = y04(i)*z01(i) - z04(i)*y01(i)
298 sy0 = z04(i)*x01(i) - x04(i)*z01(i)
299 sz0 = x04(i)*y01(i) - y04(i)*x01(i)
300 s2 = one/
max(em30,sx0*sx0 + sy0*sy0 + sz0*sz0)
301
302 lb4(i) = -(sx0*sx1 + sy0*sy1 + sz0*sz1) * s2
303 lc4(i) = (sx0*sx4 + sy0*sy4 + sz0*sz4) * s2
304
305 aaa = one/
max(em30,x01(i)*x01(i)+y01(i)*y01(i)+z01(i)*z01(i))
306 hlc1(i)= lc1(i)*abs(lc1(i))*aaa
307 hlb4(i)= lb4(i)*abs(lb4(i))*aaa
308 al1(i) = -(xi0*x01(i)+yi0*y01(i)+zi0*z01(i))*aaa
309 al1(i) =
max(zero,
min(one,al1(i)))
310 aaa = one/
max(em30,x02(i)*x02(i)+y02(i)*y02(i)+z02(i)*z02(i))
311 hlc2(i)= lc2(i)*abs(lc2(i))*aaa
312 hlb1(i)= lb1(i)*abs(lb1(i))*aaa
313 al2(i) = -(xi0*x02(i)+yi0*y02(i)+zi0*z02(i))*aaa
314 al2(i) =
max(zero,
min(one,al2(i)))
315 aaa = one/
max(em30,x03(i)*x03(i)+y03(i)*y03(i)+z03(i)*z03(i))
316 hlc3(i)= lc3(i)*abs(lc3(i))*aaa
317 hlb2(i)= lb2(i)*abs(lb2(i))*aaa
318 al3(i) = -(xi0*x03(i)+yi0*y03(i)+zi0*z03(i))*aaa
319 al3(i) =
max(zero,
min(one,al3(i)))
320 aaa = one/
max(em30,x04(i)*x04(i)+y04(i)*y04(i)+z04(i)*z04(i))
321 hlc4(i)= lc4(i)*abs(lc4(i))*aaa
322 hlb3(i)= lb3(i)*abs(lb3(i))*aaa
323 al4(i) = -(xi0*x04(i)+yi0*y04(i)+zi0*z04(i))*aaa
324 al4(i) =
max(zero,
min(one,al4(i)))
325
326 ENDDO
327
328 DO i=1,jlt
329 x12 = x2(i) - x1(i)
330 y12 = y2(i) - y1(i)
331 z12 = z2(i) - z1(i)
332 la = one - lb1(i) - lc1(i)
333 aaa = one /
max(em20,x12*x12+y12*y12+z12*z12)
334 hla= la*abs(la) * aaa
335 IF(la<zero.AND.
336 + hla<=hlb1(i).AND.hla<=hlc1(i))THEN
337 lb1(i) = (xi2(i)*x12+yi2(i)*y12+zi2(i)*z12) * aaa
338 lb1(i) =
max(zero,
min(one,lb1(i)))
339 lc1(i) = one - lb1(i)
340 ELSEIF(lb1(i)<zero.AND.
341 + hlb1(i)<=hlc1(i).AND.hlb1(i)<=hla)THEN
342 lb1(i) = zero
343 lc1(i) = al2(i)
344 ELSEIF(lc1(i)<zero.AND.
345 + hlc1(i)<=hla.AND.hlc1(i)<=hlb1(i))THEN
346 lc1(i) = zero
347 lb1(i) = al1(i)
348 ENDIF
349 ENDDO
350
351 DO i=1,jlt
352 x23 = x3(i) - x2(i)
353 y23 = y3(i) - y2(i)
354 z23 = z3(i) - z2(i)
355 la = one - lb2(i) - lc2(i)
356 aaa = one /
max(em20,x23*x23+y23*y23+z23*z23)
357 hla= la*abs(la) * aaa
358 IF(la<zero.AND.
359 + hla<=hlb2(i).AND.hla<=hlc2(i))THEN
360 lb2(i) = (xi3(i)*x23+yi3(i)*y23+zi3(i)*z23)*aaa
361 lb2(i) =
max(zero,
min(one,lb2(i)))
362 lc2(i) = one - lb2(i)
363 ELSEIF(lb2(i)<zero.AND.
364 + hlb2(i)<=hlc2(i).AND.hlb2(i)<=hla)THEN
365 lb2(i) = zero
366 lc2(i) = al3(i)
367 ELSEIF(lc2(i)<zero.AND.
368 + hlc2(i)<=hla.AND.hlc2(i)<=hlb2(i))THEN
369 lc2(i) = zero
370 lb2(i) = al2(i)
371 ENDIF
372 ENDDO
373
374 DO i=1,jlt
375 x34 = x4(i) - x3(i)
376 y34 = y4(i) - y3(i)
377 z34 = z4(i) - z3(i)
378 la = one - lb3(i) - lc3(i)
379 aaa = one /
max(em20,x34*x34+y34*y34+z34*z34)
380 hla= la*abs(la) * aaa
381 IF(la<zero.AND.
382 + hla<=hlb3(i).AND.hla<=hlc3(i))THEN
383 lb3(i) = (xi4(i)*x34+yi4(i)*y34+zi4(i)*z34)*aaa
384 lb3(i) =
max(zero,
min(one,lb3(i)))
385 lc3(i) = one - lb3(i)
386 ELSEIF(lb3(i)<zero.AND.
387 + hlb3(i)<=hlc3(i).AND.hlb3(i)<=hla)THEN
388 lb3(i) = zero
389 lc3(i) = al4(i)
390 ELSEIF(lc3(i)<zero.AND.
391 + hlc3(i)<=hla.AND.hlc3(i)<=hlb3(i))THEN
392 lc3(i) = zero
393 lb3(i) = al3(i)
394 ENDIF
395 ENDDO
396
397 DO i=1,jlt
398 x41 = x1(i) - x4(i)
399 y41 = y1(i) - y4(i)
400 z41 = z1(i) - z4(i)
401 la = one - lb4(i) - lc4(i)
402 aaa = one /
max(em20,x41*x41+y41*y41+z41*z41)
403 hla= la*abs(la) * aaa
404 IF(la<zero.AND.
405 + hla<=hlb4(i).AND.hla<=hlc4(i))THEN
406 lb4(i) = (xi1(i)*x41+yi1(i)*y41+zi1(i)*z41)*aaa
407 lb4(i) =
max(zero,
min(one,lb4(i)))
408 lc4(i) = one - lb4(i)
409 ELSEIF(lb4(i)<zero.AND.
410 + hlb4(i)<=hlc4(i).AND.hlb4(i)<=hla)THEN
411 lb4(i) = zero
412 lc4(i) = al1(i)
413 ELSEIF(lc4(i)<zero.AND.
414 + hlc4(i)<=hla.AND.hlc4(i)<=hlb4(i))THEN
415 lc4(i) = zero
416 lb4(i) = al4(i)
417 ENDIF
418 ENDDO
419
420
421 DO i=1,jlt
422
423 nx1(i) = xi(i)-(x0(i) + lb1(i)*x01(i) + lc1(i)*x02(i))
424 ny1(i) = yi(i)-(y0(i) + lb1(i)*y01(i) + lc1(i)*y02(i))
425 nz1(i) = zi(i)-(z0(i) + lb1(i)*z01(i) + lc1(i)*z02(i))
426 p1(i) = nx1(i)*nx1(i) + ny1(i)*ny1(i) +nz1(i)*nz1(i)
427 d1 =
max(zero, gap2(i) - p1(i))
428
429 nx2(i) = xi(i)-(x0(i) + lb2(i)*x02(i) + lc2(i)*x03(i))
430 ny2(i) = yi(i)-(y0(i) + lb2(i)*y02(i) + lc2(i)*y03(i))
431 nz2(i) = zi(i)-(z0(i) + lb2(i)*z02(i) + lc2(i)*z03(i))
432 p2(i) = nx2(i)*nx2(i) + ny2(i)*ny2(i) +nz2(i)*nz2(i)
433 d2 =
max(zero, gap2(i) - p2(i))
434
435 nx3(i) = xi(i)-(x0(i) + lb3(i)*x03(i) + lc3(i)*x04(i))
436 ny3(i) = yi(i)-(y0(i) + lb3(i)*y03(i) + lc3(i)*y04(i))
437 nz3(i) = zi(i)-(z0(i) + lb3(i)*z03(i) + lc3(i)*z04(i))
438 p3(i) = nx3(i)*nx3(i) + ny3(i)*ny3(i) +nz3(i)*nz3(i)
439 d3 =
max(zero, gap2(i) - p3(i))
440
441 nx4(i) = xi(i)-(x0(i) + lb4(i)*x04(i) + lc4(i)*x01(i))
442 ny4(i) = yi(i)-(y0(i) + lb4(i)*y04(i) + lc4(i)*y01(i))
443 nz4(i) = zi(i)-(z0(i) + lb4(i)*z04(i) + lc4(i)*z01(i))
444 p4(i) = nx4(i)*nx4(i) + ny4(i)*ny4(i) +nz4(i)*nz4(i)
445 d4 =
max(zero, gap2(i) - p4(i))
446
447
448
449 pene(i) =
max(d1,d2,d3,d4)
450
451 ENDDO
452 ENDIF
453
454
455
456 DO i=1,jlt
457
458 IF(.NOT.(etyp(i)==0 .OR. etyp(i) > nrtm))cycle
459
460 dx =
max(x1(i),x2(i),x3(i),x4(i))-
min(x1(i),x2(i),x3(i),x4(i))
461 dy =
max(y1(i),y2(i),y3(i),y4(i))-
min(y1(i),y2(i),y3(i),y4(i))
462 dz =
max(z1(i),z2(i),z3(i),z4(i))-
min(z1(i),z2(i),z3(i),z4(i))
464 IF(ibc(i)==1.OR.ibc(i)==3.OR.ibc(i)==5.OR.ibc(i)==7)THEN
465 zi1(i)=zi(i)-z1(i)
466 zi2(i)=zi(i)-z2(i)
467 zi3(i)=zi(i)-z3(i)
468 zi4(i)=zi(i)-z4(i)
469 zim=
max(abs(zi1(i)),abs(zi2(i)),abs(zi3(i)),abs(zi4(i)))
470 IF(zim < em03*dd)THEN
471 pene(i)=zero
472 END IF
473 END IF
474 IF(ibc(i)==2.OR.ibc(i)==3.OR.ibc(i)==6.OR.ibc(i)==7)THEN
475 yi1(i)=yi(i)-y1(i)
476 yi2(i)=yi(i)-y2(i)
477 yi3(i)=yi(i)-y3(i)
478 yi4(i)=yi(i)-y4(i)
479 yim=
max(abs(yi1(i)),abs(yi2(i)),abs(yi3(i)),abs(yi4(i)))
480 IF(yim < em03*dd)THEN
481 pene(i)=zero
482 END IF
483 END IF
484 IF(ibc(i)==4.OR.ibc(i)==5.OR.ibc(i)==6.OR.ibc(i)==7)THEN
485 xi1(i)=xi(i)-x1(i)
486 xi2(i)=xi(i)-x2(i)
487 xi3(i)=xi(i)-x3(i)
488 xi4(i)=xi(i)-x4(i)
489 xim=
max(abs(xi1(i)),abs(xi2(i)),abs(xi3(i)),abs(xi4(i)))
490 IF(xim < em03*dd)THEN
491 pene(i)=zero
492 END IF
493 END IF
494
495 END DO
496
497 RETURN