OpenRadioss 2025.1.11
OpenRadioss project
Loading...
Searching...
No Matches
i21dst3.F
Go to the documentation of this file.
1Copyright> OpenRadioss
2Copyright> Copyright (C) 1986-2025 Altair Engineering Inc.
3Copyright>
4Copyright> This program is free software: you can redistribute it and/or modify
5Copyright> it under the terms of the GNU Affero General Public License as published by
6Copyright> the Free Software Foundation, either version 3 of the License, or
7Copyright> (at your option) any later version.
8Copyright>
9Copyright> This program is distributed in the hope that it will be useful,
10Copyright> but WITHOUT ANY WARRANTY; without even the implied warranty of
11Copyright> MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12Copyright> GNU Affero General Public License for more details.
13Copyright>
14Copyright> You should have received a copy of the GNU Affero General Public License
15Copyright> along with this program. If not, see <https://www.gnu.org/licenses/>.
16Copyright>
17Copyright>
18Copyright> Commercial Alternative: Altair Radioss Software
19Copyright>
20Copyright> As an alternative to this open-source version, Altair also offers Altair Radioss
21Copyright> software under a commercial license. Contact Altair to discuss further if the
22Copyright> commercial version may interest you: https://www.altair.com/radioss/.
23!||====================================================================
24!|| i21dst3 ../starter/source/interfaces/inter3d1/i21dst3.F
25!||--- called by ------------------------------------------------------
26!|| inint3_thkvar ../starter/source/interfaces/inter3d1/inint3_thkvar.F
27!||====================================================================
28 SUBROUTINE i21dst3(
29 1 JLT ,CAND_N ,CAND_E ,IRECT ,NSV ,
30 2 GAP_S ,X ,IRTLM ,CSTS ,DEPTH ,
31 3 NOD_NORMAL,XM0 ,PENE ,PENI ,IFPEN ,
32 4 IGAP ,GAP ,GAPMAX ,GAPMIN ,DRAD ,
33 5 DGAPLOAD)
34C-----------------------------------------------
35C I m p l i c i t T y p e s
36C-----------------------------------------------
37#include "implicit_f.inc"
38C-----------------------------------------------
39C G l o b a l P a r a m e t e r s
40C-----------------------------------------------
41#include "mvsiz_p.inc"
42C-----------------------------------------------
43C D u m m y A r g u m e n t s
44C-----------------------------------------------
45 INTEGER JLT, CAND_N(*), CAND_E(*), IRECT(4,*),
46 . IRTLM(2,*), NSV(*), IFPEN(*)
47 INTEGER IGAP
48 my_real
49 . X(3,*), CSTS(2,*), DEPTH, NOD_NORMAL(3,*),
50 . xm0(3,*), pene(*), peni(*), gap_s(*), gap, gapmin, gapmax,
51 . drad
52 my_real , INTENT(IN) :: dgapload
53C-----------------------------------------------
54C L o c a l V a r i a b l e s
55C-----------------------------------------------
56 INTEGER I, IG, J, IS, L, N, N4N
57 INTEGER IX1(MVSIZ), IX2(MVSIZ), IX3(MVSIZ), IX4(MVSIZ),
58 . NSVG(MVSIZ), FAR(MVSIZ), I4N(MVSIZ),
59 . IRTLM_L(2,MVSIZ)
60 my_real
61 . gapv(mvsiz),
62 . nx1(mvsiz), nx2(mvsiz), nx3(mvsiz), nx4(mvsiz),
63 . ny1(mvsiz), ny2(mvsiz), ny3(mvsiz), ny4(mvsiz),
64 . nz1(mvsiz), nz2(mvsiz), nz3(mvsiz), nz4(mvsiz),
65 . lb1(mvsiz), lb2(mvsiz), lb3(mvsiz), lb4(mvsiz),
66 . lc1(mvsiz), lc2(mvsiz), lc3(mvsiz), lc4(mvsiz),
67 . x1(mvsiz), x2(mvsiz), x3(mvsiz), x4(mvsiz),
68 . y1(mvsiz), y2(mvsiz), y3(mvsiz), y4(mvsiz),
69 . z1(mvsiz), z2(mvsiz), z3(mvsiz), z4(mvsiz),
70 . x0(mvsiz), y0(mvsiz), z0(mvsiz),
71 . xi(mvsiz), yi(mvsiz), zi(mvsiz),
72 . p1(mvsiz), p2(mvsiz), p3(mvsiz), p4(mvsiz)
73 my_real
74 . nnx1(mvsiz), nnx2(mvsiz), nnx3(mvsiz), nnx4(mvsiz),
75 . nny1(mvsiz), nny2(mvsiz), nny3(mvsiz), nny4(mvsiz),
76 . nnz1(mvsiz), nnz2(mvsiz), nnz3(mvsiz), nnz4(mvsiz),
77 . nnx0(mvsiz), nny0(mvsiz), nnz0(mvsiz)
78 my_real
79 . x01(mvsiz), x02(mvsiz), x03(mvsiz), x04(mvsiz),
80 . y01(mvsiz), y02(mvsiz), y03(mvsiz), y04(mvsiz),
81 . z01(mvsiz), z02(mvsiz), z03(mvsiz), z04(mvsiz),
82 . xi1(mvsiz), xi2(mvsiz),
83 . yi1(mvsiz), yi2(mvsiz),
84 . zi1(mvsiz), zi2(mvsiz)
85 my_real
86 . s2,d1,d2,d3,d4,
87 . x12,x23,x34,x41,xi0,
88 . y12,y23,y34,y41,yi0,
89 . z12,z23,z34,z41,zi0,
90 . xn1(mvsiz),yn1(mvsiz),zn1(mvsiz),
91 . xn2(mvsiz),yn2(mvsiz),zn2(mvsiz),
92 . xn3(mvsiz),yn3(mvsiz),zn3(mvsiz),
93 . xn4(mvsiz),yn4(mvsiz),zn4(mvsiz),
94 . sx1(mvsiz),sx2(mvsiz),
95 . sy1(mvsiz),sy2(mvsiz),
96 . sz1(mvsiz),sz2(mvsiz),
97 . xi0v(mvsiz), yi0v(mvsiz), zi0v(mvsiz),
98 . xh(mvsiz), yh(mvsiz), zh(mvsiz),
99 . nx(mvsiz), ny(mvsiz), nz(mvsiz),
100 . gap2(mvsiz), nn,
101 . x0h(mvsiz), y0h(mvsiz), z0h(mvsiz),
102 . x1h(mvsiz), y1h(mvsiz), z1h(mvsiz),
103 . x2h(mvsiz), y2h(mvsiz), z2h(mvsiz), hh(mvsiz), ll,
104 . hxi0, hyi0, hzi0, hxi1, hyi1, hzi1, hxi2, hyi2, hzi2,
105 . hx01, hy01, hz01, hx02, hy02, hz02,
106 . hxn1, hyn1, hzn1, hsx1, hsy1, hsz1, hsx2, hsy2, hsz2,
107 . side, crit, crito, lbo, lco, lb, lc, la, sl,
108 . lambda, xp, yp, zp, dist, depth2, drad2,gapp
109 my_real
110 . cmaj(mvsiz), csts_l(2,mvsiz)
111C--------------------------------------------------------
112 IF(igap==0)THEN
113 DO i=1,jlt
114 gapv(i)=gap
115 END DO
116 ELSE
117C ... on pourrait utiliser le fait Depth >= max(gapv) dans le starter ...
118 DO i=1,jlt
119 gapv(i)=gap_s(cand_n(i))
120 gapv(i)=min(gapv(i),gapmax)
121 gapv(i)=max(gapmin,gapv(i))
122 END DO
123 END IF
124 depth2=depth*depth
125 drad2 =drad*drad
126C--------------------------------------------------------
127C
128 DO i=1,jlt
129 nsvg(i)= nsv(cand_n(i))
130 l = cand_e(i)
131C
132 ix1(i)=irect(1,l)
133 ix2(i)=irect(2,l)
134 ix3(i)=irect(3,l)
135 ix4(i)=irect(4,l)
136 END DO
137C
138 DO i=1,jlt
139 xi(i)=x(1,nsvg(i))
140 yi(i)=x(2,nsvg(i))
141 zi(i)=x(3,nsvg(i))
142 x1(i)=xm0(1,ix1(i))
143 y1(i)=xm0(2,ix1(i))
144 z1(i)=xm0(3,ix1(i))
145 x2(i)=xm0(1,ix2(i))
146 y2(i)=xm0(2,ix2(i))
147 z2(i)=xm0(3,ix2(i))
148 x3(i)=xm0(1,ix3(i))
149 y3(i)=xm0(2,ix3(i))
150 z3(i)=xm0(3,ix3(i))
151 x4(i)=xm0(1,ix4(i))
152 y4(i)=xm0(2,ix4(i))
153 z4(i)=xm0(3,ix4(i))
154C
155 pene(i)=zero
156 END DO
157C
158 DO i=1,jlt
159 irtlm_l(1,i)=0
160 irtlm_l(2,i)=0
161 csts_l(1,i) =-one
162 csts_l(2,i) =-one
163 END DO
164C--------------------------------------------------------
165C CAS DES PAQUETS MIXTES
166C--------------------------------------------------------
167 DO i=1,jlt
168 IF(ix3(i)/=ix4(i))THEN
169 x0(i) = fourth*(x1(i)+x2(i)+x3(i)+x4(i))
170 y0(i) = fourth*(y1(i)+y2(i)+y3(i)+y4(i))
171 z0(i) = fourth*(z1(i)+z2(i)+z3(i)+z4(i))
172 ELSE
173 x0(i) = x3(i)
174 y0(i) = y3(i)
175 z0(i) = z3(i)
176 ENDIF
177 ENDDO
178
179 DO i=1,jlt
180
181 nnx1(i)=nod_normal(1,ix1(i))
182 nny1(i)=nod_normal(2,ix1(i))
183 nnz1(i)=nod_normal(3,ix1(i))
184
185 nnx2(i)=nod_normal(1,ix2(i))
186 nny2(i)=nod_normal(2,ix2(i))
187 nnz2(i)=nod_normal(3,ix2(i))
188
189 nnx3(i)=nod_normal(1,ix3(i))
190 nny3(i)=nod_normal(2,ix3(i))
191 nnz3(i)=nod_normal(3,ix3(i))
192
193 nnx4(i)=nod_normal(1,ix4(i))
194 nny4(i)=nod_normal(2,ix4(i))
195 nnz4(i)=nod_normal(3,ix4(i))
196
197 ENDDO
198
199 DO i=1,jlt
200
201 IF(ix3(i)/=ix4(i))THEN
202 nnx0(i)=fourth*(nnx1(i)+nnx2(i)+nnx3(i)+nnx4(i))
203 nny0(i)=fourth*(nny1(i)+nny2(i)+nny3(i)+nny4(i))
204 nnz0(i)=fourth*(nnz1(i)+nnz2(i)+nnz3(i)+nnz4(i))
205 ELSE
206 nnx0(i)=nnx3(i)
207 nny0(i)=nny3(i)
208 nnz0(i)=nnz3(i)
209 ENDIF
210
211 ENDDO
212C
213c---------------------------------------------------------
214c courbure cubique
215c---------------------------------------------------------
216c IF(ICURV == 3)THEN
217c CALL I7CMAJ(JLT ,CMAJ ,IRECT ,NOD_NORMAL,CAND_E,
218c 2 X1 ,X2 ,X3 ,X4 ,
219c 3 Y1 ,Y2 ,Y3 ,Y4 ,
220c 4 Z1 ,Z2 ,Z3 ,Z4 ,
221c 5 NNX1 ,NNX2 ,NNX3 ,NNX4 ,
222c 6 NNY1 ,NNY2 ,NNY3 ,NNY4 ,
223c 7 NNZ1 ,NNZ2 ,NNZ3 ,NNZ4 )
224c ENDIF
225c---------------------------------------------------------
226 DO i=1,jlt
227c CMAJ(I) = ZERO
228C GAP2=(GAPV(I)+CMAJ(I))*(GAPV(I)+CMAJ(I))
229 gapp = gapv(i)+dgapload
230 gap2(i)=gapp*gapp
231C
232 x01(i) = x1(i) - x0(i)
233 y01(i) = y1(i) - y0(i)
234 z01(i) = z1(i) - z0(i)
235C
236 x02(i) = x2(i) - x0(i)
237 y02(i) = y2(i) - y0(i)
238 z02(i) = z2(i) - z0(i)
239C
240 x03(i) = x3(i) - x0(i)
241 y03(i) = y3(i) - y0(i)
242 z03(i) = z3(i) - z0(i)
243C
244 x04(i) = x4(i) - x0(i)
245 y04(i) = y4(i) - y0(i)
246 z04(i) = z4(i) - z0(i)
247C
248 ENDDO
249C
250C normales aux triangles (recalculees ici pour pas les stocker).
251 DO i=1,jlt
252
253 xn1(i) = y01(i)*z02(i) - z01(i)*y02(i)
254 yn1(i) = z01(i)*x02(i) - x01(i)*z02(i)
255 zn1(i) = x01(i)*y02(i) - y01(i)*x02(i)
256
257 xn2(i) = y02(i)*z03(i) - z02(i)*y03(i)
258 yn2(i) = z02(i)*x03(i) - x02(i)*z03(i)
259 zn2(i) = x02(i)*y03(i) - y02(i)*x03(i)
260
261 xn3(i) = y03(i)*z04(i) - z03(i)*y04(i)
262 yn3(i) = z03(i)*x04(i) - x03(i)*z04(i)
263 zn3(i) = x03(i)*y04(i) - y03(i)*x04(i)
264
265 xn4(i) = y04(i)*z01(i) - z04(i)*y01(i)
266 yn4(i) = z04(i)*x01(i) - x04(i)*z01(i)
267 zn4(i) = x04(i)*y01(i) - y04(i)*x01(i)
268
269 ENDDO
270C
271C------
272C 1er triangle ...
273 DO i=1,jlt
274C
275 xi0v(i) = x0(i) - xi(i)
276 yi0v(i) = y0(i) - yi(i)
277 zi0v(i) = z0(i) - zi(i)
278C
279 xi1(i) = x1(i) - xi(i)
280 yi1(i) = y1(i) - yi(i)
281 zi1(i) = z1(i) - zi(i)
282C
283 xi2(i) = x2(i) - xi(i)
284 yi2(i) = y2(i) - yi(i)
285 zi2(i) = z2(i) - zi(i)
286C
287 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
288 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
289 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
290C
291 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
292 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
293 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
294C
295 s2 = one/
296 . max(em30,(xn1(i)*xn1(i)+ yn1(i)*yn1(i)+ zn1(i)*zn1(i)))
297 lb1(i) = -(xn1(i)*sx2(i) + yn1(i)*sy2(i) + zn1(i)*sz2(i))*s2
298 lc1(i) = (xn1(i)*sx1(i) + yn1(i)*sy1(i) + zn1(i)*sz1(i))*s2
299C
300 ENDDO
301C
302 DO i=1,jlt
303C
304 nx(i) = xi(i)-(x0(i) + lb1(i)*x01(i) + lc1(i)*x02(i))
305 ny(i) = yi(i)-(y0(i) + lb1(i)*y01(i) + lc1(i)*y02(i))
306 nz(i) = zi(i)-(z0(i) + lb1(i)*z01(i) + lc1(i)*z02(i))
307 hh(i) = nx(i)*xn1(i) + ny(i)*yn1(i) +nz(i)*zn1(i)
308C
309 ENDDO
310C
311C elevation : triangle //
312 DO i=1,jlt
313C
314 ll = hh(i)/
315 . max(em30,nnx1(i)*xn1(i)+nny1(i)*yn1(i)+nnz1(i)*zn1(i))
316 x1h(i)=x1(i)+ll*nnx1(i)
317 y1h(i)=y1(i)+ll*nny1(i)
318 z1h(i)=z1(i)+ll*nnz1(i)
319C
320 ll = hh(i)/
321 . max(em30,nnx2(i)*xn1(i)+nny2(i)*yn1(i)+nnz2(i)*zn1(i))
322 x2h(i)=x2(i)+ll*nnx2(i)
323 y2h(i)=y2(i)+ll*nny2(i)
324 z2h(i)=z2(i)+ll*nnz2(i)
325C
326 ll = hh(i)/
327 . max(em30,nnx0(i)*xn1(i)+nny0(i)*yn1(i)+nnz0(i)*zn1(i))
328 x0h(i)=x0(i)+ll*nnx0(i)
329 y0h(i)=y0(i)+ll*nny0(i)
330 z0h(i)=z0(i)+ll*nnz0(i)
331C
332 ENDDO
333C
334C coordonnees locale dans le triangle //
335 DO i=1,jlt
336C
337 far(i)=0
338C
339 hxi0 = x0h(i) - xi(i)
340 hyi0 = y0h(i) - yi(i)
341 hzi0 = z0h(i) - zi(i)
342C
343 hxi1 = x1h(i) - xi(i)
344 hyi1 = y1h(i) - yi(i)
345 hzi1 = z1h(i) - zi(i)
346C
347 hxi2 = x2h(i) - xi(i)
348 hyi2 = y2h(i) - yi(i)
349 hzi2 = z2h(i) - zi(i)
350C
351 hx01 = x1h(i) - x0h(i)
352 hy01 = y1h(i) - y0h(i)
353 hz01 = z1h(i) - z0h(i)
354C
355 hx02 = x2h(i) - x0h(i)
356 hy02 = y2h(i) - y0h(i)
357 hz02 = z2h(i) - z0h(i)
358C
359 hxn1 = hy01*hz02 - hz01*hy02
360 hyn1 = hz01*hx02 - hx01*hz02
361 hzn1 = hx01*hy02 - hy01*hx02
362C
363 IF(hxn1*xn1(i)+hyn1*yn1(i)+hzn1*zn1(i) <= zero) THEN
364C a optimiser
365 far(i)=1
366 cycle
367 END IF
368C
369 hsx1 = hyi0*hzi1 - hzi0*hyi1
370 hsy1 = hzi0*hxi1 - hxi0*hzi1
371 hsz1 = hxi0*hyi1 - hyi0*hxi1
372C
373 hsx2 = hyi0*hzi2 - hzi0*hyi2
374 hsy2 = hzi0*hxi2 - hxi0*hzi2
375 hsz2 = hxi0*hyi2 - hyi0*hxi2
376C
377 s2 = one/
378 . max(em30,(hxn1*hxn1+ hyn1*hyn1+ hzn1*hzn1))
379 lb1(i) = -(hxn1*hsx2 + hyn1*hsy2 + hzn1*hsz2)*s2
380 lc1(i) = (hxn1*hsx1 + hyn1*hsy1 + hzn1*hsz1)*s2
381C
382 IF(lb1(i) < -zep05 .OR. lc1(i) < -zep05 .OR.
383 . lb1(i)+lc1(i) > onep05) far(i)=1
384C
385 ENDDO
386C
387C necessaire au calcul de distance // normale
388 DO i=1,jlt
389C
390 IF(far(i)==1)cycle
391C
392C normale interpolee dans le triangle //
393 nx(i)=(one-lb1(i)-lc1(i))*nnx0(i)+lb1(i)*nnx1(i)+lc1(i)*nnx2(i)
394 ny(i)=(one-lb1(i)-lc1(i))*nny0(i)+lb1(i)*nny1(i)+lc1(i)*nny2(i)
395 nz(i)=(one-lb1(i)-lc1(i))*nnz0(i)+lb1(i)*nnz1(i)+lc1(i)*nnz2(i)
396C
397C projection sur triangle 1
398 nn = nx(i)*xn1(i)+ ny(i)*yn1(i)+ nz(i)*zn1(i)
399 IF(nn <= zero) THEN
400C cas limite
401 far(i)=1
402 cycle
403 END IF
404 nn = one/max(em30,nn)
405C
406 lambda=(xn1(i)*xi0v(i)+yn1(i)*yi0v(i)+zn1(i)*zi0v(i))*nn
407 xp=xi(i)+lambda*nx(i)
408 yp=yi(i)+lambda*ny(i)
409 zp=zi(i)+lambda*nz(i)
410C
411 xi0v(i) = x0(i) - xp
412 yi0v(i) = y0(i) - yp
413 zi0v(i) = z0(i) - zp
414C
415 xi1(i) = x1(i) - xp
416 yi1(i) = y1(i) - yp
417 zi1(i) = z1(i) - zp
418C
419 xi2(i) = x2(i) - xp
420 yi2(i) = y2(i) - yp
421 zi2(i) = z2(i) - zp
422C
423 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
424 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
425 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
426C
427 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
428 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
429 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
430C
431 nn = one/
432 . max(em30,(xn1(i)*xn1(i)+ yn1(i)*yn1(i)+ zn1(i)*zn1(i)))
433 lb1(i) = -(xn1(i)*sx2(i) + yn1(i)*sy2(i) + zn1(i)*sz2(i))*nn
434 lc1(i) = (xn1(i)*sx1(i) + yn1(i)*sy1(i) + zn1(i)*sz1(i))*nn
435C
436 ENDDO
437C
438 DO i=1,jlt
439C
440 IF(far(i)==1)cycle
441C
442 lb = min(one,max(lb1(i),zero))
443 lc = min(one,max(lc1(i),zero))
444 la = min(one,max(one-lb1(i)-lc1(i),zero))
445
446 sl=one/max(em20,la+lb+lc)
447 lb = lb*sl
448 lc = lc*sl
449 la = la*sl
450
451 nx1(i) = xi(i)-(la*x0(i) + lb*x1(i) + lc*x2(i))
452 ny1(i) = yi(i)-(la*y0(i) + lb*y1(i) + lc*y2(i))
453 nz1(i) = zi(i)-(la*z0(i) + lb*z1(i) + lc*z2(i))
454 p1(i) = nx1(i)*nx1(i) + ny1(i)*ny1(i) +nz1(i)*nz1(i)
455C
456C NX(I)=(1-LB1(I)-LC1(I))*NNX0(I)+LB1(I)*NNX1(I)+LC1(I)*NNX2(I)
457C NY(I)=(1-LB1(I)-LC1(I))*NNY0(I)+LB1(I)*NNY1(I)+LC1(I)*NNY2(I)
458C NZ(I)=(1-LB1(I)-LC1(I))*NNZ0(I)+LB1(I)*NNZ1(I)+LC1(I)*NNZ2(I)
459C SIDE=NX1(I)*NX(I)+NY1(I)*NY(I)+NZ1(I)*NZ(I)
460
461 side=sign(one,nx1(i)*xn1(i)+ny1(i)*yn1(i)+nz1(i)*zn1(i))
462 IF((side >= zero .AND. p1(i) < max(gap2(i),drad2)) .OR.
463 . (side < zero .AND. p1(i) < depth2))THEN
464 IF(lb1(i) < -zep05 .OR. lc1(i) < -zep05 .OR.
465 . lb1(i)+lc1(i) > onep05) cycle
466 crit = abs(third-lb1(i))
467 . + abs(third-lc1(i))
468 . + abs(two_third-lb1(i)-lc1(i))
469 lbo = csts_l(1,i)
470 lco = csts_l(2,i)
471 crito = abs(third-lbo)
472 . + abs(third-lco)
473 . + abs(two_third-lbo-lco)
474 IF(crit < crito)THEN
475
476 la =one-lb1(i)-lc1(i)
477 nx(i)=lb1(i)*nnx1(i)+lc1(i)*nnx2(i)+la*nnx0(i)
478 ny(i)=lb1(i)*nny1(i)+lc1(i)*nny2(i)+la*nny0(i)
479 nz(i)=lb1(i)*nnz1(i)+lc1(i)*nnz2(i)+la*nnz0(i)
480 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
481 nx(i)=nx(i)*nn
482 ny(i)=ny(i)*nn
483 nz(i)=nz(i)*nn
484
485 nx1(i) = xi(i)-(x0(i) + lb1(i)*x01(i) + lc1(i)*x02(i))
486 ny1(i) = yi(i)-(y0(i) + lb1(i)*y01(i) + lc1(i)*y02(i))
487 nz1(i) = zi(i)-(z0(i) + lb1(i)*z01(i) + lc1(i)*z02(i))
488
489 dist =nx1(i)*nx(i)+ny1(i)*ny(i)+nz1(i)*nz(i)
490 pene(i)=gapv(i)-dist
491
492 irtlm_l(1,i) = cand_e(i)
493 irtlm_l(2,i) = nint(side)
494 csts_l(1,i) = lb1(i)
495 csts_l(2,i) = lc1(i)
496 END IF
497 ELSEIF(side >= zero .AND. p1(i) < drad2) THEN
498 IF(lb1(i) < -zep05 .OR. lc1(i) < -zep05 .OR.
499 . lb1(i)+lc1(i) > onep05) cycle
500 crit = abs(third-lb1(i))
501 . + abs(third-lc1(i))
502 . + abs(two_third-lb1(i)-lc1(i))
503 lbo = csts_l(1,i)
504 lco = csts_l(2,i)
505 crito = abs(third-lbo)
506 . + abs(third-lco)
507 . + abs(two_third-lbo-lco)
508 IF(crit < crito)THEN
509
510 la =one-lb1(i)-lc1(i)
511 nx(i)=lb1(i)*nnx1(i)+lc1(i)*nnx2(i)+la*nnx0(i)
512 ny(i)=lb1(i)*nny1(i)+lc1(i)*nny2(i)+la*nny0(i)
513 nz(i)=lb1(i)*nnz1(i)+lc1(i)*nnz2(i)+la*nnz0(i)
514 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
515 nx(i)=nx(i)*nn
516 ny(i)=ny(i)*nn
517 nz(i)=nz(i)*nn
518
519 nx1(i) = xi(i)-(x0(i) + lb1(i)*x01(i) + lc1(i)*x02(i))
520 ny1(i) = yi(i)-(y0(i) + lb1(i)*y01(i) + lc1(i)*y02(i))
521 nz1(i) = zi(i)-(z0(i) + lb1(i)*z01(i) + lc1(i)*z02(i))
522
523 dist =nx1(i)*nx(i)+ny1(i)*ny(i)+nz1(i)*nz(i)
524 pene(i)=gapv(i)-dist
525
526 irtlm_l(1,i) = -cand_e(i)
527 irtlm_l(2,i) = nint(side)
528 csts_l(1,i) = lb1(i)
529 csts_l(2,i) = lc1(i)
530 END IF
531 END IF
532C
533 ENDDO
534C------
535 n4n=0
536 DO i=1,jlt
537 IF(ix3(i)/=ix4(i))THEN
538 n4n = n4n+1
539 i4n(n4n)=i
540 ELSE
541 ENDIF
542 ENDDO
543C------
544C 2eme triangle ...
545 DO n=1,n4n
546C
547 i=i4n(n)
548C
549 xi0v(i) = x0(i) - xi(i)
550 yi0v(i) = y0(i) - yi(i)
551 zi0v(i) = z0(i) - zi(i)
552C
553 xi1(i) = x2(i) - xi(i)
554 yi1(i) = y2(i) - yi(i)
555 zi1(i) = z2(i) - zi(i)
556C
557 xi2(i) = x3(i) - xi(i)
558 yi2(i) = y3(i) - yi(i)
559 zi2(i) = z3(i) - zi(i)
560C
561 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
562 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
563 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
564C
565 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
566 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
567 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
568C
569 s2 = one/
570 . max(em30,(xn2(i)*xn2(i)+ yn2(i)*yn2(i)+ zn2(i)*zn2(i)))
571 lb2(i) = -(xn2(i)*sx2(i) + yn2(i)*sy2(i) + zn2(i)*sz2(i))*s2
572 lc2(i) = (xn2(i)*sx1(i) + yn2(i)*sy1(i) + zn2(i)*sz1(i))*s2
573C
574 ENDDO
575C
576 DO n=1,n4n
577C
578 i=i4n(n)
579C
580 nx(i) = xi(i)-(x0(i) + lb2(i)*x02(i) + lc2(i)*x03(i))
581 ny(i) = yi(i)-(y0(i) + lb2(i)*y02(i) + lc2(i)*y03(i))
582 nz(i) = zi(i)-(z0(i) + lb2(i)*z02(i) + lc2(i)*z03(i))
583 hh(i) = nx(i)*xn2(i) + ny(i)*yn2(i) +nz(i)*zn2(i)
584C
585 ENDDO
586C
587C elevation : triangle //
588 DO n=1,n4n
589C
590 i=i4n(n)
591C
592 ll = hh(i)/
593 . max(em30,nnx2(i)*xn2(i)+nny2(i)*yn2(i)+nnz2(i)*zn2(i))
594 x1h(i)=x2(i)+ll*nnx2(i)
595 y1h(i)=y2(i)+ll*nny2(i)
596 z1h(i)=z2(i)+ll*nnz2(i)
597C
598 ll = hh(i)/
599 . max(em30,nnx3(i)*xn2(i)+nny3(i)*yn2(i)+nnz3(i)*zn2(i))
600 x2h(i)=x3(i)+ll*nnx3(i)
601 y2h(i)=y3(i)+ll*nny3(i)
602 z2h(i)=z3(i)+ll*nnz3(i)
603C
604 ll = hh(i)/
605 . max(em30,nnx0(i)*xn2(i)+nny0(i)*yn2(i)+nnz0(i)*zn2(i))
606 x0h(i)=x0(i)+ll*nnx0(i)
607 y0h(i)=y0(i)+ll*nny0(i)
608 z0h(i)=z0(i)+ll*nnz0(i)
609C
610 ENDDO
611C
612C coordonnees locale dans le triangle //
613 DO n=1,n4n
614C
615 i=i4n(n)
616C
617 far(i)=0
618C
619 hxi0 = x0h(i) - xi(i)
620 hyi0 = y0h(i) - yi(i)
621 hzi0 = z0h(i) - zi(i)
622C
623 hxi1 = x1h(i) - xi(i)
624 hyi1 = y1h(i) - yi(i)
625 hzi1 = z1h(i) - zi(i)
626C
627 hxi2 = x2h(i) - xi(i)
628 hyi2 = y2h(i) - yi(i)
629 hzi2 = z2h(i) - zi(i)
630C
631 hx01 = x1h(i) - x0h(i)
632 hy01 = y1h(i) - y0h(i)
633 hz01 = z1h(i) - z0h(i)
634C
635 hx02 = x2h(i) - x0h(i)
636 hy02 = y2h(i) - y0h(i)
637 hz02 = z2h(i) - z0h(i)
638C
639 hxn1 = hy01*hz02 - hz01*hy02
640 hyn1 = hz01*hx02 - hx01*hz02
641 hzn1 = hx01*hy02 - hy01*hx02
642C
643 IF(hxn1*xn2(i)+hyn1*yn2(i)+hzn1*zn2(i) <= zero) THEN
644C a optimiser
645 far(i)=1
646 cycle
647 END IF
648C
649 hsx1 = hyi0*hzi1 - hzi0*hyi1
650 hsy1 = hzi0*hxi1 - hxi0*hzi1
651 hsz1 = hxi0*hyi1 - hyi0*hxi1
652C
653 hsx2 = hyi0*hzi2 - hzi0*hyi2
654 hsy2 = hzi0*hxi2 - hxi0*hzi2
655 hsz2 = hxi0*hyi2 - hyi0*hxi2
656C
657 s2 = one/
658 . max(em30,(hxn1*hxn1+ hyn1*hyn1+ hzn1*hzn1))
659 lb2(i) = -(hxn1*hsx2 + hyn1*hsy2 + hzn1*hsz2)*s2
660 lc2(i) = (hxn1*hsx1 + hyn1*hsy1 + hzn1*hsz1)*s2
661C
662 IF(lb2(i) < -zep05 .OR. lc2(i) < -zep05 .OR.
663 . lb2(i)+lc2(i) > onep05) far(i)=1
664 ENDDO
665C
666C necessaire au calcul de distance // normale
667 DO n=1,n4n
668C
669 i=i4n(n)
670C
671 IF(far(i)==1)cycle
672C
673C normale interpolee dans le triangle //
674 nx(i)=(1-lb2(i)-lc2(i))*nnx0(i)+lb2(i)*nnx2(i)+lc2(i)*nnx3(i)
675 ny(i)=(1-lb2(i)-lc2(i))*nny0(i)+lb2(i)*nny2(i)+lc2(i)*nny3(i)
676 nz(i)=(1-lb2(i)-lc2(i))*nnz0(i)+lb2(i)*nnz2(i)+lc2(i)*nnz3(i)
677C
678C projection sur triangle 2
679 nn = nx(i)*xn2(i)+ ny(i)*yn2(i)+ nz(i)*zn2(i)
680 IF(nn <= zero) THEN
681C cas limite
682 far(i)=1
683 cycle
684 END IF
685 nn = one/max(em30,nn)
686C
687 lambda=(xn2(i)*xi0v(i)+yn2(i)*yi0v(i)+zn2(i)*zi0v(i))*nn
688 xp=xi(i)+lambda*nx(i)
689 yp=yi(i)+lambda*ny(i)
690 zp=zi(i)+lambda*nz(i)
691C
692 xi0v(i) = x0(i) - xp
693 yi0v(i) = y0(i) - yp
694 zi0v(i) = z0(i) - zp
695C
696 xi1(i) = x2(i) - xp
697 yi1(i) = y2(i) - yp
698 zi1(i) = z2(i) - zp
699C
700 xi2(i) = x3(i) - xp
701 yi2(i) = y3(i) - yp
702 zi2(i) = z3(i) - zp
703C
704 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
705 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
706 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
707C
708 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
709 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
710 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
711C
712 nn = one/
713 . max(em30,(xn2(i)*xn2(i)+ yn2(i)*yn2(i)+ zn2(i)*zn2(i)))
714 lb2(i) = -(xn2(i)*sx2(i) + yn2(i)*sy2(i) + zn2(i)*sz2(i))*nn
715 lc2(i) = (xn2(i)*sx1(i) + yn2(i)*sy1(i) + zn2(i)*sz1(i))*nn
716C
717 ENDDO
718C
719 DO n=1,n4n
720C
721 i=i4n(n)
722C
723 IF(far(i)==1)cycle
724C
725 lb = min(one,max(lb2(i),zero))
726 lc = min(one,max(lc2(i),zero))
727 la = min(one,max(one-lb2(i)-lc2(i),zero))
728
729 sl=one/max(em20,la+lb+lc)
730 lb = lb*sl
731 lc = lc*sl
732 la = la*sl
733
734 nx2(i) = xi(i)-(la*x0(i) + lb*x2(i) + lc*x3(i))
735 ny2(i) = yi(i)-(la*y0(i) + lb*y2(i) + lc*y3(i))
736 nz2(i) = zi(i)-(la*z0(i) + lb*z2(i) + lc*z3(i))
737 p2(i) = nx2(i)*nx2(i) + ny2(i)*ny2(i) +nz2(i)*nz2(i)
738C
739C NX(I)=(1-LB2(I)-LC2(I))*NNX0(I)+LB2(I)*NNX2(I)+LC2(I)*NNX3(I)
740C NY(I)=(1-LB2(I)-LC2(I))*NNY0(I)+LB2(I)*NNY2(I)+LC2(I)*NNY3(I)
741C NZ(I)=(1-LB2(I)-LC2(I))*NNZ0(I)+LB2(I)*NNZ2(I)+LC2(I)*NNZ3(I)
742C SIDE=NX2(I)*NX(I)+NY2(I)*NY(I)+NZ2(I)*NZ(I)
743
744 side=sign(one,nx2(i)*xn2(i)+ny2(i)*yn2(i)+nz2(i)*zn2(i))
745 IF((side >= zero .AND. p2(i) < max(gap2(i),drad2)) .OR.
746 . (side < zero .AND. p2(i) < depth2))THEN
747 IF(lb2(i) < -zep05 .OR. lc2(i) < -zep05 .OR.
748 . lb2(i)+lc2(i) > onep05) cycle
749 crit = abs(third-lb2(i))
750 . + abs(third-lc2(i))
751 . + abs(two_third-lb2(i)-lc2(i))
752 lbo = csts_l(1,i)
753 lco = csts_l(2,i)
754 crito = abs(third-lbo)
755 . + abs(third-lco)
756 . + abs(two_third-lbo-lco)
757 IF(crit < crito)THEN
758
759 la =one-lb2(i)-lc2(i)
760 nx(i)=lb2(i)*nnx2(i)+lc2(i)*nnx3(i)+la*nnx0(i)
761 ny(i)=lb2(i)*nny2(i)+lc2(i)*nny3(i)+la*nny0(i)
762 nz(i)=lb2(i)*nnz2(i)+lc2(i)*nnz3(i)+la*nnz0(i)
763 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
764 nx(i)=nx(i)*nn
765 ny(i)=ny(i)*nn
766 nz(i)=nz(i)*nn
767
768 nx2(i) = xi(i)-(x0(i) + lb2(i)*x02(i) + lc2(i)*x03(i))
769 ny2(i) = yi(i)-(y0(i) + lb2(i)*y02(i) + lc2(i)*y03(i))
770 nz2(i) = zi(i)-(z0(i) + lb2(i)*z02(i) + lc2(i)*z03(i))
771
772 dist =nx2(i)*nx(i)+ny2(i)*ny(i)+nz2(i)*nz(i)
773 pene(i)=gapv(i)-dist
774
775 irtlm_l(1,i) = cand_e(i)
776 irtlm_l(2,i) = nint(two*side)
777 csts_l(1,i) = lb2(i)
778 csts_l(2,i) = lc2(i)
779 END IF
780 ELSEIF(side >= zero .AND. p2(i) < drad2) THEN
781 IF(lb2(i) < -zep05 .OR. lc2(i) < -zep05 .OR.
782 . lb2(i)+lc2(i) > onep05) cycle
783 crit = abs(third-lb2(i))
784 . + abs(third-lc2(i))
785 . + abs(two_third-lb2(i)-lc2(i))
786 lbo = csts_l(1,i)
787 lco = csts_l(2,i)
788 crito = abs(third-lbo)
789 . + abs(third-lco)
790 . + abs(two_third-lbo-lco)
791 IF(crit < crito)THEN
792
793 la =one-lb2(i)-lc2(i)
794 nx(i)=lb2(i)*nnx2(i)+lc2(i)*nnx3(i)+la*nnx0(i)
795 ny(i)=lb2(i)*nny2(i)+lc2(i)*nny3(i)+la*nny0(i)
796 nz(i)=lb2(i)*nnz2(i)+lc2(i)*nnz3(i)+la*nnz0(i)
797 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
798 nx(i)=nx(i)*nn
799 ny(i)=ny(i)*nn
800 nz(i)=nz(i)*nn
801
802 nx2(i) = xi(i)-(x0(i) + lb2(i)*x02(i) + lc2(i)*x03(i))
803 ny2(i) = yi(i)-(y0(i) + lb2(i)*y02(i) + lc2(i)*y03(i))
804 nz2(i) = zi(i)-(z0(i) + lb2(i)*z02(i) + lc2(i)*z03(i))
805
806 dist =nx2(i)*nx(i)+ny2(i)*ny(i)+nz2(i)*nz(i)
807 pene(i)=gapv(i)-dist
808
809 irtlm_l(1,i) = -cand_e(i)
810 irtlm_l(2,i) = nint(two*side)
811 csts_l(1,i) = lb2(i)
812 csts_l(2,i) = lc2(i)
813 END IF
814 END IF
815C
816 ENDDO
817C------
818C 3eme triangle ...
819 DO n=1,n4n
820C
821 i=i4n(n)
822C
823 xi0v(i) = x0(i) - xi(i)
824 yi0v(i) = y0(i) - yi(i)
825 zi0v(i) = z0(i) - zi(i)
826C
827 xi1(i) = x3(i) - xi(i)
828 yi1(i) = y3(i) - yi(i)
829 zi1(i) = z3(i) - zi(i)
830C
831 xi2(i) = x4(i) - xi(i)
832 yi2(i) = y4(i) - yi(i)
833 zi2(i) = z4(i) - zi(i)
834C
835 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
836 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
837 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
838C
839 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
840 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
841 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
842C
843 s2 = one/
844 . max(em30,(xn3(i)*xn3(i)+ yn3(i)*yn3(i)+ zn3(i)*zn3(i)))
845 lb3(i) = -(xn3(i)*sx2(i) + yn3(i)*sy2(i) + zn3(i)*sz2(i))*s2
846 lc3(i) = (xn3(i)*sx1(i) + yn3(i)*sy1(i) + zn3(i)*sz1(i))*s2
847C
848 ENDDO
849C
850 DO n=1,n4n
851C
852 i=i4n(n)
853C
854 nx(i) = xi(i)-(x0(i) + lb3(i)*x03(i) + lc3(i)*x04(i))
855 ny(i) = yi(i)-(y0(i) + lb3(i)*y03(i) + lc3(i)*y04(i))
856 nz(i) = zi(i)-(z0(i) + lb3(i)*z03(i) + lc3(i)*z04(i))
857 hh(i) = nx(i)*xn3(i) + ny(i)*yn3(i) +nz(i)*zn3(i)
858C
859 ENDDO
860C
861C elevation : triangle //
862 DO n=1,n4n
863C
864 i=i4n(n)
865C
866 ll = hh(i)/
867 . max(em30,nnx3(i)*xn3(i)+nny3(i)*yn3(i)+nnz3(i)*zn3(i))
868 x1h(i)=x3(i)+ll*nnx3(i)
869 y1h(i)=y3(i)+ll*nny3(i)
870 z1h(i)=z3(i)+ll*nnz3(i)
871C
872 ll = hh(i)/
873 . max(em30,nnx4(i)*xn3(i)+nny4(i)*yn3(i)+nnz4(i)*zn3(i))
874 x2h(i)=x4(i)+ll*nnx4(i)
875 y2h(i)=y4(i)+ll*nny4(i)
876 z2h(i)=z4(i)+ll*nnz4(i)
877C
878 ll = hh(i)/
879 . max(em30,nnx0(i)*xn3(i)+nny0(i)*yn3(i)+nnz0(i)*zn3(i))
880 x0h(i)=x0(i)+ll*nnx0(i)
881 y0h(i)=y0(i)+ll*nny0(i)
882 z0h(i)=z0(i)+ll*nnz0(i)
883C
884 ENDDO
885C
886C coordonnees locale dans le triangle //
887 DO n=1,n4n
888C
889 i=i4n(n)
890C
891 far(i)=0
892C
893 hxi0 = x0h(i) - xi(i)
894 hyi0 = y0h(i) - yi(i)
895 hzi0 = z0h(i) - zi(i)
896C
897 hxi1 = x1h(i) - xi(i)
898 hyi1 = y1h(i) - yi(i)
899 hzi1 = z1h(i) - zi(i)
900C
901 hxi2 = x2h(i) - xi(i)
902 hyi2 = y2h(i) - yi(i)
903 hzi2 = z2h(i) - zi(i)
904C
905 hx01 = x1h(i) - x0h(i)
906 hy01 = y1h(i) - y0h(i)
907 hz01 = z1h(i) - z0h(i)
908C
909 hx02 = x2h(i) - x0h(i)
910 hy02 = y2h(i) - y0h(i)
911 hz02 = z2h(i) - z0h(i)
912C
913 hxn1 = hy01*hz02 - hz01*hy02
914 hyn1 = hz01*hx02 - hx01*hz02
915 hzn1 = hx01*hy02 - hy01*hx02
916C
917 IF(hxn1*xn3(i)+hyn1*yn3(i)+hzn1*zn3(i) <= zero) THEN
918C a optimiser
919 far(i)=1
920 cycle
921 END IF
922C
923 hsx1 = hyi0*hzi1 - hzi0*hyi1
924 hsy1 = hzi0*hxi1 - hxi0*hzi1
925 hsz1 = hxi0*hyi1 - hyi0*hxi1
926C
927 hsx2 = hyi0*hzi2 - hzi0*hyi2
928 hsy2 = hzi0*hxi2 - hxi0*hzi2
929 hsz2 = hxi0*hyi2 - hyi0*hxi2
930C
931 s2 = one/
932 . max(em30,(hxn1*hxn1+ hyn1*hyn1+ hzn1*hzn1))
933 lb3(i) = -(hxn1*hsx2 + hyn1*hsy2 + hzn1*hsz2)*s2
934 lc3(i) = (hxn1*hsx1 + hyn1*hsy1 + hzn1*hsz1)*s2
935C
936 IF(lb3(i) < -zep05 .OR. lc3(i) < -zep05 .OR.
937 . lb3(i)+lc3(i) > onep05) far(i)=1
938 ENDDO
939C
940C necessaire au calcul de distance // normale
941 DO n=1,n4n
942C
943 i=i4n(n)
944C
945 IF(far(i)==1)cycle
946C
947C normale interpolee dans le triangle //
948 nx(i)=(1-lb3(i)-lc3(i))*nnx0(i)+lb3(i)*nnx3(i)+lc3(i)*nnx4(i)
949 ny(i)=(1-lb3(i)-lc3(i))*nny0(i)+lb3(i)*nny3(i)+lc3(i)*nny4(i)
950 nz(i)=(1-lb3(i)-lc3(i))*nnz0(i)+lb3(i)*nnz3(i)+lc3(i)*nnz4(i)
951C
952C projection sur triangle 3
953 nn = nx(i)*xn3(i)+ ny(i)*yn3(i)+ nz(i)*zn3(i)
954 IF(nn <= zero) THEN
955C cas limite
956 far(i)=1
957 cycle
958 END IF
959 nn = one/max(em30,nn)
960C
961 lambda=(xn3(i)*xi0v(i)+yn3(i)*yi0v(i)+zn3(i)*zi0v(i))*nn
962 xp=xi(i)+lambda*nx(i)
963 yp=yi(i)+lambda*ny(i)
964 zp=zi(i)+lambda*nz(i)
965C
966 xi0v(i) = x0(i) - xp
967 yi0v(i) = y0(i) - yp
968 zi0v(i) = z0(i) - zp
969C
970 xi1(i) = x3(i) - xp
971 yi1(i) = y3(i) - yp
972 zi1(i) = z3(i) - zp
973C
974 xi2(i) = x4(i) - xp
975 yi2(i) = y4(i) - yp
976 zi2(i) = z4(i) - zp
977C
978 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
979 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
980 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
981C
982 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
983 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
984 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
985C
986 nn = one/
987 . max(em30,(xn3(i)*xn3(i)+ yn3(i)*yn3(i)+ zn3(i)*zn3(i)))
988 lb3(i) = -(xn3(i)*sx2(i) + yn3(i)*sy2(i) + zn3(i)*sz2(i))*nn
989 lc3(i) = (xn3(i)*sx1(i) + yn3(i)*sy1(i) + zn3(i)*sz1(i))*nn
990C
991 ENDDO
992C
993 DO n=1,n4n
994C
995 i=i4n(n)
996C
997 IF(far(i)==1)cycle
998C
999 lb = min(one,max(lb3(i),zero))
1000 lc = min(one,max(lc3(i),zero))
1001 la = min(one,max(one-lb3(i)-lc3(i),zero))
1002
1003 sl=one/max(em20,la+lb+lc)
1004 lb = lb*sl
1005 lc = lc*sl
1006 la = la*sl
1007
1008 nx3(i) = xi(i)-(la*x0(i) + lb*x3(i) + lc*x4(i))
1009 ny3(i) = yi(i)-(la*y0(i) + lb*y3(i) + lc*y4(i))
1010 nz3(i) = zi(i)-(la*z0(i) + lb*z3(i) + lc*z4(i))
1011 p3(i) = nx3(i)*nx3(i) + ny3(i)*ny3(i) +nz3(i)*nz3(i)
1012C
1013C NX(I)=(1-LB3(I)-LC3(I))*NNX0(I)+LB3(I)*NNX3(I)+LC3(I)*NNX4(I)
1014C NY(I)=(1-LB3(I)-LC3(I))*NNY0(I)+LB3(I)*NNY3(I)+LC3(I)*NNY4(I)
1015C NZ(I)=(1-LB3(I)-LC3(I))*NNZ0(I)+LB3(I)*NNZ3(I)+LC3(I)*NNZ4(I)
1016C SIDE=NX3(I)*NX(I)+NY3(I)*NY(I)+NZ3(I)*NZ(I)
1017
1018 side=sign(one,nx3(i)*xn3(i)+ny3(i)*yn3(i)+nz3(i)*zn3(i))
1019 IF((side >= zero .AND. p3(i) < max(gap2(i),drad2)) .OR.
1020 . (side < zero .AND. p3(i) < depth2))THEN
1021 IF(lb3(i) < -zep05 .OR. lc3(i) < -zep05 .OR.
1022 . lb3(i)+lc3(i) > onep05) cycle
1023 crit = abs(third-lb3(i))
1024 . + abs(third-lc3(i))
1025 . + abs(two_third-lb3(i)-lc3(i))
1026 lbo = csts_l(1,i)
1027 lco = csts_l(2,i)
1028 crito = abs(third-lbo)
1029 . + abs(third-lco)
1030 . + abs(two_third-lbo-lco)
1031 IF(crit < crito)THEN
1032
1033 la =one-lb3(i)-lc3(i)
1034 nx(i)=lb3(i)*nnx3(i)+lc3(i)*nnx4(i)+la*nnx0(i)
1035 ny(i)=lb3(i)*nny3(i)+lc3(i)*nny4(i)+la*nny0(i)
1036 nz(i)=lb3(i)*nnz3(i)+lc3(i)*nnz4(i)+la*nnz0(i)
1037 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
1038 nx(i)=nx(i)*nn
1039 ny(i)=ny(i)*nn
1040 nz(i)=nz(i)*nn
1041
1042 nx3(i) = xi(i)-(x0(i) + lb3(i)*x03(i) + lc3(i)*x04(i))
1043 ny3(i) = yi(i)-(y0(i) + lb3(i)*y03(i) + lc3(i)*y04(i))
1044 nz3(i) = zi(i)-(z0(i) + lb3(i)*z03(i) + lc3(i)*z04(i))
1045
1046 dist =nx3(i)*nx(i)+ny3(i)*ny(i)+nz3(i)*nz(i)
1047 pene(i)=gapv(i)-dist
1048
1049 irtlm_l(1,i) = cand_e(i)
1050 irtlm_l(2,i) = nint(three*side)
1051 csts_l(1,i) = lb3(i)
1052 csts_l(2,i) = lc3(i)
1053 END IF
1054 ELSEIF(side >= zero .AND. p3(i) < drad2) THEN
1055 IF(lb3(i) < -zep05 .OR. lc3(i) < -zep05 .OR.
1056 . lb3(i)+lc3(i) > onep05) cycle
1057 crit = abs(third-lb3(i))
1058 . + abs(third-lc3(i))
1059 . + abs(two_third-lb3(i)-lc3(i))
1060 lbo = csts_l(1,i)
1061 lco = csts_l(2,i)
1062 crito = abs(third-lbo)
1063 . + abs(third-lco)
1064 . + abs(two_third-lbo-lco)
1065 IF(crit < crito)THEN
1066
1067 la =one-lb3(i)-lc3(i)
1068 nx(i)=lb3(i)*nnx3(i)+lc3(i)*nnx4(i)+la*nnx0(i)
1069 ny(i)=lb3(i)*nny3(i)+lc3(i)*nny4(i)+la*nny0(i)
1070 nz(i)=lb3(i)*nnz3(i)+lc3(i)*nnz4(i)+la*nnz0(i)
1071 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
1072 nx(i)=nx(i)*nn
1073 ny(i)=ny(i)*nn
1074 nz(i)=nz(i)*nn
1075
1076 nx3(i) = xi(i)-(x0(i) + lb3(i)*x03(i) + lc3(i)*x04(i))
1077 ny3(i) = yi(i)-(y0(i) + lb3(i)*y03(i) + lc3(i)*y04(i))
1078 nz3(i) = zi(i)-(z0(i) + lb3(i)*z03(i) + lc3(i)*z04(i))
1079
1080 dist =nx3(i)*nx(i)+ny3(i)*ny(i)+nz3(i)*nz(i)
1081 pene(i)=gapv(i)-dist
1082
1083 irtlm_l(1,i) = -cand_e(i)
1084 irtlm_l(2,i) = nint(three*side)
1085 csts_l(1,i) = lb3(i)
1086 csts_l(2,i) = lc3(i)
1087 END IF
1088 END IF
1089C
1090 END DO
1091C------
1092C 4eme triangle ...
1093 DO n=1,n4n
1094C
1095 i=i4n(n)
1096C
1097 xi0v(i) = x0(i) - xi(i)
1098 yi0v(i) = y0(i) - yi(i)
1099 zi0v(i) = z0(i) - zi(i)
1100C
1101 xi1(i) = x4(i) - xi(i)
1102 yi1(i) = y4(i) - yi(i)
1103 zi1(i) = z4(i) - zi(i)
1104C
1105 xi2(i) = x1(i) - xi(i)
1106 yi2(i) = y1(i) - yi(i)
1107 zi2(i) = z1(i) - zi(i)
1108C
1109 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
1110 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
1111 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
1112C
1113 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
1114 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
1115 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
1116C
1117 s2 = one/
1118 . max(em30,(xn4(i)*xn4(i)+ yn4(i)*yn4(i)+ zn4(i)*zn4(i)))
1119 lb4(i) = -(xn4(i)*sx2(i) + yn4(i)*sy2(i) + zn4(i)*sz2(i))*s2
1120 lc4(i) = (xn4(i)*sx1(i) + yn4(i)*sy1(i) + zn4(i)*sz1(i))*s2
1121C
1122 ENDDO
1123C
1124 DO n=1,n4n
1125C
1126 i=i4n(n)
1127C
1128 nx(i) = xi(i)-(x0(i) + lb4(i)*x04(i) + lc4(i)*x01(i))
1129 ny(i) = yi(i)-(y0(i) + lb4(i)*y04(i) + lc4(i)*y01(i))
1130 nz(i) = zi(i)-(z0(i) + lb4(i)*z04(i) + lc4(i)*z01(i))
1131 hh(i) = nx(i)*xn4(i) + ny(i)*yn4(i) +nz(i)*zn4(i)
1132C
1133 ENDDO
1134C
1135C elevation : triangle //
1136 DO n=1,n4n
1137C
1138 i=i4n(n)
1139C
1140 ll = hh(i)/
1141 . max(em30,nnx4(i)*xn4(i)+nny4(i)*yn4(i)+nnz4(i)*zn4(i))
1142 x1h(i)=x4(i)+ll*nnx4(i)
1143 y1h(i)=y4(i)+ll*nny4(i)
1144 z1h(i)=z4(i)+ll*nnz4(i)
1145C
1146 ll = hh(i)/
1147 . max(em30,nnx1(i)*xn4(i)+nny1(i)*yn4(i)+nnz1(i)*zn4(i))
1148 x2h(i)=x1(i)+ll*nnx1(i)
1149 y2h(i)=y1(i)+ll*nny1(i)
1150 z2h(i)=z1(i)+ll*nnz1(i)
1151C
1152 ll = hh(i)/
1153 . max(em30,nnx0(i)*xn4(i)+nny0(i)*yn4(i)+nnz0(i)*zn4(i))
1154 x0h(i)=x0(i)+ll*nnx0(i)
1155 y0h(i)=y0(i)+ll*nny0(i)
1156 z0h(i)=z0(i)+ll*nnz0(i)
1157C
1158 ENDDO
1159C
1160C coordonnees locale dans le triangle //
1161 DO n=1,n4n
1162C
1163 i=i4n(n)
1164C
1165 far(i)=0
1166C
1167 hxi0 = x0h(i) - xi(i)
1168 hyi0 = y0h(i) - yi(i)
1169 hzi0 = z0h(i) - zi(i)
1170C
1171 hxi1 = x1h(i) - xi(i)
1172 hyi1 = y1h(i) - yi(i)
1173 hzi1 = z1h(i) - zi(i)
1174C
1175 hxi2 = x2h(i) - xi(i)
1176 hyi2 = y2h(i) - yi(i)
1177 hzi2 = z2h(i) - zi(i)
1178C
1179 hx01 = x1h(i) - x0h(i)
1180 hy01 = y1h(i) - y0h(i)
1181 hz01 = z1h(i) - z0h(i)
1182C
1183 hx02 = x2h(i) - x0h(i)
1184 hy02 = y2h(i) - y0h(i)
1185 hz02 = z2h(i) - z0h(i)
1186C
1187 hxn1 = hy01*hz02 - hz01*hy02
1188 hyn1 = hz01*hx02 - hx01*hz02
1189 hzn1 = hx01*hy02 - hy01*hx02
1190C
1191 IF(hxn1*xn4(i)+hyn1*yn4(i)+hzn1*zn4(i) <= zero) THEN
1192C a optimiser
1193 far(i)=1
1194 cycle
1195 END IF
1196C
1197 hsx1 = hyi0*hzi1 - hzi0*hyi1
1198 hsy1 = hzi0*hxi1 - hxi0*hzi1
1199 hsz1 = hxi0*hyi1 - hyi0*hxi1
1200C
1201 hsx2 = hyi0*hzi2 - hzi0*hyi2
1202 hsy2 = hzi0*hxi2 - hxi0*hzi2
1203 hsz2 = hxi0*hyi2 - hyi0*hxi2
1204C
1205 s2 = one/
1206 . max(em30,(hxn1*hxn1+ hyn1*hyn1+ hzn1*hzn1))
1207 lb4(i) = -(hxn1*hsx2 + hyn1*hsy2 + hzn1*hsz2)*s2
1208 lc4(i) = (hxn1*hsx1 + hyn1*hsy1 + hzn1*hsz1)*s2
1209C
1210 IF(lb4(i) < -zep05 .OR. lc4(i) < -zep05 .OR.
1211 . lb4(i)+lc4(i) > onep05) far(i)=1
1212 ENDDO
1213C
1214C necessaire au calcul de distance // normale
1215 DO n=1,n4n
1216C
1217 i=i4n(n)
1218C
1219 IF(far(i)==1)cycle
1220C
1221C normale interpolee dans le triangle //
1222 nx(i)=(1-lb4(i)-lc4(i))*nnx0(i)+lb4(i)*nnx4(i)+lc4(i)*nnx1(i)
1223 ny(i)=(1-lb4(i)-lc4(i))*nny0(i)+lb4(i)*nny4(i)+lc4(i)*nny1(i)
1224 nz(i)=(1-lb4(i)-lc4(i))*nnz0(i)+lb4(i)*nnz4(i)+lc4(i)*nnz1(i)
1225C
1226C projection sur triangle 4
1227 nn = nx(i)*xn4(i)+ ny(i)*yn4(i)+ nz(i)*zn4(i)
1228 IF(nn <= zero) THEN
1229C cas limite
1230 far(i)=1
1231 cycle
1232 END IF
1233 nn = one/max(em30,nn)
1234C
1235 lambda=(xn4(i)*xi0v(i)+yn4(i)*yi0v(i)+zn4(i)*zi0v(i))*nn
1236 xp=xi(i)+lambda*nx(i)
1237 yp=yi(i)+lambda*ny(i)
1238 zp=zi(i)+lambda*nz(i)
1239C
1240 xi0v(i) = x0(i) - xp
1241 yi0v(i) = y0(i) - yp
1242 zi0v(i) = z0(i) - zp
1243C
1244 xi1(i) = x4(i) - xp
1245 yi1(i) = y4(i) - yp
1246 zi1(i) = z4(i) - zp
1247C
1248 xi2(i) = x1(i) - xp
1249 yi2(i) = y1(i) - yp
1250 zi2(i) = z1(i) - zp
1251C
1252 sx1(i) = yi0v(i)*zi1(i) - zi0v(i)*yi1(i)
1253 sy1(i) = zi0v(i)*xi1(i) - xi0v(i)*zi1(i)
1254 sz1(i) = xi0v(i)*yi1(i) - yi0v(i)*xi1(i)
1255C
1256 sx2(i) = yi0v(i)*zi2(i) - zi0v(i)*yi2(i)
1257 sy2(i) = zi0v(i)*xi2(i) - xi0v(i)*zi2(i)
1258 sz2(i) = xi0v(i)*yi2(i) - yi0v(i)*xi2(i)
1259C
1260 nn = one/
1261 . max(em30,(xn4(i)*xn4(i)+ yn4(i)*yn4(i)+ zn4(i)*zn4(i)))
1262 lb4(i) = -(xn4(i)*sx2(i) + yn4(i)*sy2(i) + zn4(i)*sz2(i))*nn
1263 lc4(i) = (xn4(i)*sx1(i) + yn4(i)*sy1(i) + zn4(i)*sz1(i))*nn
1264C
1265 ENDDO
1266
1267
1268 DO n=1,n4n
1269C
1270 i=i4n(n)
1271C
1272 IF(far(i)==1)cycle
1273C
1274 lb = min(one,max(lb4(i),zero))
1275 lc = min(one,max(lc4(i),zero))
1276 la = min(one,max(one-lb4(i)-lc4(i),zero))
1277
1278 sl=one/max(em20,la+lb+lc)
1279 lb = lb*sl
1280 lc = lc*sl
1281 la = la*sl
1282
1283 nx4(i) = xi(i)-(la*x0(i) + lb*x4(i) + lc*x1(i))
1284 ny4(i) = yi(i)-(la*y0(i) + lb*y4(i) + lc*y1(i))
1285 nz4(i) = zi(i)-(la*z0(i) + lb*z4(i) + lc*z1(i))
1286 p4(i) = nx4(i)*nx4(i) + ny4(i)*ny4(i) +nz4(i)*nz4(i)
1287
1288 side=sign(one,nx4(i)*xn4(i)+ny4(i)*yn4(i)+nz4(i)*zn4(i))
1289 IF((side >= zero .AND. p4(i) < gap2(i)) .OR.
1290 . (side < zero .AND. p4(i) < depth2))THEN
1291 IF(lb4(i) < -zep05 .OR. lc4(i) < -zep05 .OR.
1292 . lb4(i)+lc4(i) > onep05) cycle
1293 crit = abs(third-lb4(i))
1294 . + abs(third-lc4(i))
1295 . + abs(two_third-lb4(i)-lc4(i))
1296
1297 lbo = csts_l(1,i)
1298 lco = csts_l(2,i)
1299 crito = abs(third-lbo)
1300 . + abs(third-lco)
1301 . + abs(two_third-lbo-lco)
1302 IF(crit < crito)THEN
1303
1304 la =one-lb4(i)-lc4(i)
1305 nx(i)=lb4(i)*nnx4(i)+lc4(i)*nnx1(i)+la*nnx0(i)
1306 ny(i)=lb4(i)*nny4(i)+lc4(i)*nny1(i)+la*nny0(i)
1307 nz(i)=lb4(i)*nnz4(i)+lc4(i)*nnz1(i)+la*nnz0(i)
1308 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
1309 nx(i)=nx(i)*nn
1310 ny(i)=ny(i)*nn
1311 nz(i)=nz(i)*nn
1312
1313 nx4(i) = xi(i)-(x0(i) + lb4(i)*x04(i) + lc4(i)*x01(i))
1314 ny4(i) = yi(i)-(y0(i) + lb4(i)*y04(i) + lc4(i)*y01(i))
1315 nz4(i) = zi(i)-(z0(i) + lb4(i)*z04(i) + lc4(i)*z01(i))
1316
1317 dist =nx4(i)*nx(i)+ny4(i)*ny(i)+nz4(i)*nz(i)
1318 pene(i)=gapv(i)-dist
1319
1320 irtlm_l(1,i) = cand_e(i)
1321 irtlm_l(2,i) = nint(four*side)
1322 csts_l(1,i) = lb4(i)
1323 csts_l(2,i) = lc4(i)
1324 END IF
1325 ELSEIF(side >= zero .AND. p4(i) < drad2) THEN
1326 IF(lb4(i) < -zep05 .OR. lc4(i) < -zep05 .OR.
1327 . lb4(i)+lc4(i) > onep05) cycle
1328 crit = abs(third-lb4(i))
1329 . + abs(third-lc4(i))
1330 . + abs(two_third-lb4(i)-lc4(i))
1331
1332 lbo = csts_l(1,i)
1333 lco = csts_l(2,i)
1334 crito = abs(third-lbo)
1335 . + abs(third-lco)
1336 . + abs(two_third-lbo-lco)
1337 IF(crit < crito)THEN
1338
1339 la =one-lb4(i)-lc4(i)
1340 nx(i)=lb4(i)*nnx4(i)+lc4(i)*nnx1(i)+la*nnx0(i)
1341 ny(i)=lb4(i)*nny4(i)+lc4(i)*nny1(i)+la*nny0(i)
1342 nz(i)=lb4(i)*nnz4(i)+lc4(i)*nnz1(i)+la*nnz0(i)
1343 nn=one/max(em20,sqrt(nx(i)*nx(i)+ny(i)*ny(i)+nz(i)*nz(i)))
1344 nx(i)=nx(i)*nn
1345 ny(i)=ny(i)*nn
1346 nz(i)=nz(i)*nn
1347
1348 nx4(i) = xi(i)-(x0(i) + lb4(i)*x04(i) + lc4(i)*x01(i))
1349 ny4(i) = yi(i)-(y0(i) + lb4(i)*y04(i) + lc4(i)*y01(i))
1350 nz4(i) = zi(i)-(z0(i) + lb4(i)*z04(i) + lc4(i)*z01(i))
1351
1352 dist =nx4(i)*nx(i)+ny4(i)*ny(i)+nz4(i)*nz(i)
1353 pene(i)=gapv(i)-dist
1354
1355 irtlm_l(1,i) = -cand_e(i)
1356 irtlm_l(2,i) = nint(four*side)
1357 csts_l(1,i) = lb4(i)
1358 csts_l(2,i) = lc4(i)
1359 END IF
1360 END IF
1361C
1362 ENDDO
1363C---------------------
1364C
1365C---------------------
1366 DO i=1,jlt
1367 IF(irtlm_l(1,i)/=0)THEN
1368 lb=csts_l(1,i)
1369 lc=csts_l(2,i)
1370 crit = abs(third-lb)
1371 . + abs(third-lc)
1372 . + abs(two_third-lb-lc)
1373 lbo = csts(1,cand_n(i))
1374 lco = csts(2,cand_n(i))
1375 crito = abs(third-lbo)
1376 . + abs(third-lco)
1377 . + abs(two_third-lbo-lco)
1378
1379 IF(crit < crito)THEN
1380
1381 peni(cand_n(i)) = max(pene(i),zero)
1382 IF(irtlm(1,cand_n(i)) > 0) ifpen(cand_n(i)) = 1
1383
1384 irtlm(1,cand_n(i)) = irtlm_l(1,i)
1385
1386 irtlm(2,cand_n(i)) = irtlm_l(2,i)
1387 csts(1,cand_n(i)) = lb
1388 csts(2,cand_n(i)) = lc
1389 END IF
1390 END IF
1391C
1392 ENDDO
1393
1394C---------------------
1395 RETURN
1396 END
min
#define min(a, b)
Definition macros.h:20
max
#define max(a, b)
Definition macros.h:21
i21dst3
subroutine i21dst3(jlt, cand_n, cand_e, irect, nsv, gap_s, x, irtlm, csts, depth, nod_normal, xm0, pene, peni, ifpen, igap, gap, gapmax, gapmin, drad, dgapload)
Definition i21dst3.F:34