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