OpenRadioss 2025.1.11
OpenRadioss project
Loading...
Searching...
No Matches
i25dst3_e2s.F File Reference
#include "implicit_f.inc"
#include "comlock.inc"
#include "assert.inc"
#include "mvsiz_p.inc"
#include "param_c.inc"
#include "sms_c.inc"
#include "vectorize.inc"

Go to the source code of this file.

Functions/Subroutines

subroutine i25dst3_e2s (jlt, cand_s, cand_m, h1s, h2s, h1m, h2m, nx, ny, nz, stif, n1, n2, m1, m2, jlt_new, xxs1, xxs2, xys1, xys2, xzs1, xzs2, xxm1, xxm2, xym1, xym2, xzm1, xzm2, vxs1, vxs2, vys1, vys2, vzs1, vzs2, vxm1, vxm2, vym1, vym2, vzm1, vzm2, ms1, ms2, mm1, mm2, iedge, nsms, index, intfric, ipartfric_es, ipartfric_em, gapve, ex, ey, ez, fx, fy, fz, ledge, irect, x, cand_p, typedgs, ias, jas, ibs, jbs, iam, itab, indx1, indx2, cs_loc4, cm_loc4, edge_id, nedge, nin, dgaploadpmax, normaln1, normaln2, normalm1, normalm2)

Function/Subroutine Documentation

◆ i25dst3_e2s()

subroutine i25dst3_e2s ( integer jlt,
integer, dimension(mvsiz) cand_s,
integer, dimension(mvsiz) cand_m,
h1s,
h2s,
h1m,
h2m,
nx,
ny,
nz,
stif,
integer, dimension(4,mvsiz) n1,
integer, dimension(4,mvsiz) n2,
integer, dimension(4,mvsiz) m1,
integer, dimension(4,mvsiz) m2,
integer jlt_new,
xxs1,
xxs2,
xys1,
xys2,
xzs1,
xzs2,
xxm1,
xxm2,
xym1,
xym2,
xzm1,
xzm2,
vxs1,
vxs2,
vys1,
vys2,
vzs1,
vzs2,
vxm1,
vxm2,
vym1,
vym2,
vzm1,
vzm2,
ms1,
ms2,
mm1,
mm2,
integer iedge,
integer, dimension(4,mvsiz) nsms,
integer, dimension(*) index,
integer intfric,
integer, dimension(4,mvsiz) ipartfric_es,
integer, dimension(4,mvsiz) ipartfric_em,
gapve,
ex,
ey,
ez,
fx,
fy,
fz,
integer, dimension(nledge,*) ledge,
integer, dimension(4,*) irect,
x,
cand_p,
integer, dimension(mvsiz) typedgs,
integer, dimension(mvsiz) ias,
integer, dimension(mvsiz) jas,
integer, dimension(mvsiz) ibs,
integer, dimension(mvsiz) jbs,
integer, dimension(mvsiz) iam,
integer, dimension(*) itab,
integer, dimension(4*mvsiz) indx1,
integer, dimension(4*mvsiz) indx2,
integer, dimension(*) cs_loc4,
integer, dimension(*) cm_loc4,
integer, dimension(2,4*mvsiz) edge_id,
integer, intent(in) nedge,
integer, intent(in) nin,
intent(in) dgaploadpmax,
dimension(3,mvsiz), intent(in) normaln1,
dimension(3,mvsiz), intent(in) normaln2,
dimension(3,4,mvsiz), intent(in) normalm1,
dimension(3,4,mvsiz), intent(in) normalm2 )

Definition at line 30 of file i25dst3_e2s.F.

47C-----------------------------------------------
48C M o d u l e s
49C-----------------------------------------------
50 USE tri25ebox
51C-----------------------------------------------
52C I m p l i c i t T y p e s
53C-----------------------------------------------
54#include "implicit_f.inc"
55#include "comlock.inc"
56C-----------------------------------------------
57C G l o b a l P a r a m e t e r s
58C-----------------------------------------------
59#include "assert.inc"
60#include "mvsiz_p.inc"
61#include "param_c.inc"
62#include "sms_c.inc"
63C-----------------------------------------------
64C D u m m y A r g u m e n t s
65C-----------------------------------------------
66 INTEGER, INTENT(IN) :: NIN
67 INTEGER, INTENT(IN) :: NEDGE
68 INTEGER :: EDGE_ID(2,4*MVSIZ)
69 INTEGER JLT,JLT_NEW, IEDGE,INTFRIC
70 INTEGER CAND_S(MVSIZ),CAND_M(MVSIZ),INDEX(*),CS_LOC4(*),CM_LOC4(*),
71 . N1(4,MVSIZ),N2(4,MVSIZ),M1(4,MVSIZ),M2(4,MVSIZ),
72 . TYPEDGS(MVSIZ),IAS(MVSIZ),JAS(MVSIZ),IBS(MVSIZ),JBS(MVSIZ),IAM(MVSIZ),
73 . NSMS(4,MVSIZ),IPARTFRIC_ES(4,MVSIZ),IPARTFRIC_EM(4,MVSIZ),
74 . IRECT(4,*),LEDGE(NLEDGE,*),ITAB(*), INDX1(4*MVSIZ), INDX2(4*MVSIZ)
75 my_real
76 . h1s(4,*),h2s(4,*),h1m(4,*),h2m(4,*),nx(4,*),ny(4,*),nz(4,*),stif(4,*),
77 . xxs1(4,*), xxs2(4,*), xys1(4,*), xys2(4,*), xzs1(4,*) , xzs2(4,*),
78 . xxm1(4,*), xxm2(4,*) , xym1(4,*), xym2(4,*), xzm1(4,*), xzm2(4,*),
79 . vxs1(4,*), vxs2(4,*), vys1(4,*), vys2(4,*), vzs1(4,*), vzs2(4,*) ,
80 . vxm1(4,*), vxm2(4,*), vym1(4,*), vym2(4,*), vzm1(4,*), vzm2(4,*),
81 . ms1(4,*) ,ms2(4,*) ,mm1(4,*) ,mm2(4,*),
82 . gapve(4,*), x(3,*), cand_p(4,*),
83 . ex(4,mvsiz), ey(4,mvsiz), ez(4,mvsiz),
84 . fx(mvsiz), fy(mvsiz) , fz(mvsiz)
85 my_real , INTENT(IN) :: dgaploadpmax
86 my_real , INTENT(IN) :: normaln1(3,mvsiz),normaln2(3,mvsiz),
87 . normalm1(3,4,mvsiz),normalm2(3,4,mvsiz)
88C-----------------------------------------------
89C L o c a l V a r i a b l e s
90C-----------------------------------------------
91 INTEGER I, IA, JA, IB, JB, SOL_EDGE, SH_EDGE, K, NJNDX, N4A,
92 . EJ, I1, I2, I3, I4, I0, IT, EJ_NEW, I_NEW,
93 . IX1(MVSIZ), IX2(MVSIZ), IX3(MVSIZ), IX4(MVSIZ),
94 . JX1(MVSIZ), JX2(MVSIZ), JX3(MVSIZ), JX4(MVSIZ),
95 . JNDX(MVSIZ), I4A(MVSIZ)
96 my_real
97 . xa0(mvsiz),xa1(mvsiz),xa2(mvsiz),xa3(mvsiz),xa4(mvsiz),
98 . ya0(mvsiz),ya1(mvsiz),ya2(mvsiz),ya3(mvsiz),ya4(mvsiz),
99 . za0(mvsiz),za1(mvsiz),za2(mvsiz),za3(mvsiz),za4(mvsiz),
100 . xa(5,mvsiz),ya(5,mvsiz),za(5,mvsiz)
101 my_real
102 . pene2(4,mvsiz),
103 . xs12(4,mvsiz),ys12(4,mvsiz),zs12(4,mvsiz),xm12(4,mvsiz),
104 . ym12(4,mvsiz),zm12(4,mvsiz),xaa,xbb,xs2(4,mvsiz),xm2(4,mvsiz),
105 . xsm,xs2m2,ys2,ym2,ysm,ys2m2,zs2,zm2,zsm,zs2m2,
106 . xx,yy,zz,als,alm,det,aaa,bbb,gap2,p1,p2,nn(4,mvsiz),
107 . x0a(mvsiz,4),y0a(mvsiz,4),z0a(mvsiz,4),
108 . x0b(mvsiz,4),y0b(mvsiz,4),z0b(mvsiz,4),
109 . xna(mvsiz,4), yna(mvsiz,4), zna(mvsiz,4), xnb(mvsiz,4), ynb(mvsiz,4), znb(mvsiz,4), ps,
110 . xs, ys, zs, xm, ym, zm, da, db, cnvx, da1, db1, da2, db2,
111 . rzero, run, rdix, rem30, rep30,
112 . alp,xxs,xys,xzs,xxm,xym,xzm,
113 . xi0,yi0,zi0,xi1,yi1,zi1,xi2,yi2,zi2,
114 . sx1,sy1,sz1,sx2,sy2,sz2,
115 . lba(mvsiz,4),lca(mvsiz,4),
116 . nnnn,nm(3),ns(3),
117 . nncx,nncy,nncz,nncp,dist,pedg,nedg
118 INTEGER :: EDGE_ID_CP(2,MVSIZ)
119 INTEGER NTRIA(3,4)
120 INTEGER :: INTFRIC_LOC,IDTMINS_LOC
121 DATA ntria/1,2,4,2,4,1,0,0,0,4,1,2/
122C-----------------------------------------------
123C RZERO = 0.
124C RUN = 1.
125C RDIX = 10.
126C REP30 = RDIX**30
127C REM30 = RUN/REP30
128 ! Local version for better compiler optimization
129 intfric_loc = intfric
130 idtmins_loc = idtmins
131C
132 jlt_new = 0
133C--------------------------------------------------------
134C
135C--------------------------------------------------------
136C F = [A*X1+(1-A)*X2-B*X3-(1-B)*X4]^2 + [..Y..]^2 + [..Z..]^2
137C DF/DA = 0 = (X1-X2)(A(X1-X2)+X2-X4 +B(X4-X3))+...
138C DF/DA = 0 = A(X1-X2)^2 +X2-X4 + B(X1-X2)(X4-X3))+...
139C DF/DA = 0 = A[(X1-X2)^2 + (Y1-Y2)^2 + (Z1-Z2)^2]
140C + B[(X1-X2)(X4-X3) + (Y1-Y2)(Y4-Y3) + (Z1-Z2)(Z4-Z3)]
141C + (X1-X2)(X2-X4) + (Y1-Y2)(Y2-Y4) + (Z1-Z2)(Z2-Z4)
142C DF/DB = 0 = (X4-X3)(A(X1-X2)+X2-X4 +B(X4-X3))+...
143C DF/DB = 0 = B[(X4-X3)^2 + (Y4-Y3)^2 + (Z4-Z3)^2]
144C + A[(X1-X2)(X4-X3) + (Y1-Y2)(Y4-Y3) + (Z1-Z2)(Z4-Z3)]
145C + (X4-X3)(X2-X4) + (Y4-Y3)(Y2-Y4) + (Z4-Z3)(Z2-Z4)
146C XS2 = [(X1-X2)^2 + (Y1-Y2)^2 + (Z1-Z2)^2]
147C XM2 = [(X4-X3)^2 + (Y4-Y3)^2 + (Z4-Z3)^2]
148C XSM = [(X1-X2)(X4-X3) + (Y1-Y2)(Y4-Y3) + (Z1-Z2)(Z4-Z3)]
149C XA = (X1-X2)(X2-X4) + (Y1-Y2)(Y2-Y4) + (Z1-Z2)(Z2-Z4)
150C XB = (X4-X3)(X2-X4) + (Y4-Y3)(Y2-Y4) + (Z4-Z3)(Z2-Z4)
151C A XS2 + B XSM + XA = 0
152C A XSM + B XM2 + XB = 0
153C
154C A = -(XA + B XSM)/XS2
155C -(XA + B XSM)*XSM + B XM2*XS2 + XB*XS2 = 0
156C -B XSM*XSM + B XM2*XS2 + XB*XS2-XA*XSM = 0
157C B*(XM2*XS2 - XSM*XSM) = -XB*XS2+XA*XSM
158C B = (XA*XSM-XB*XS2) / (XM2*XS2 - XSM*XSM)
159C A = (XB*XSM-XA*XM2) / (XM2*XS2 - XSM*XSM)
160C
161C IF B<0 => B=0
162C
163C XS2 = [(X1-X2)^2 + (Y1-Y2)^2 + (Z1-Z2)^2]
164C XA = (X1-X2)(X2-X4) + (Y1-Y2)(Y2-Y4) + (Z1-Z2)(Z2-Z4)
165C A = - XA /XS2
166C B = 0
167C
168C ELSEIF B>1 => B=1
169C
170C B = 1
171C XS2 = [(X1-X2)^2 + (Y1-Y2)^2 + (Z1-Z2)^2]
172C XSM = [(X1-X2)(X4-X3) + (Y1-Y2)(Y4-Y3) + (Z1-Z2)(Z4-Z3)]
173C XA = (X1-X2)(X2-X4) + (Y1-Y2)(Y2-Y4) + (Z1-Z2)(Z2-Z4)
174C A = -(XA + XSM)/XS2
175C
176C IF A<0 => A=0
177C
178C
179C ELSEIF A>1 => A=1
180C
181C
182 pene2(1:4,1:jlt)=ep20
183C
184 DO i=1,jlt
185 DO ej=1,4
186
187 IF(ej==3.AND.m1(ej,i)==m2(ej,i)) THEN
188 pene2(ej,i)=zero
189 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(ej,i))
190 cycle
191 END IF
192
193 xm12(ej,i) = xxm2(ej,i)-xxm1(ej,i)
194 ym12(ej,i) = xym2(ej,i)-xym1(ej,i)
195 zm12(ej,i) = xzm2(ej,i)-xzm1(ej,i)
196 xm2(ej,i) = xm12(ej,i)*xm12(ej,i) + ym12(ej,i)*ym12(ej,i) + zm12(ej,i)*zm12(ej,i)
197
198 xs12(ej,i) = xxs2(ej,i)-xxs1(ej,i)
199 ys12(ej,i) = xys2(ej,i)-xys1(ej,i)
200 zs12(ej,i) = xzs2(ej,i)-xzs1(ej,i)
201 xs2(ej,i) = xs12(ej,i)*xs12(ej,i) + ys12(ej,i)*ys12(ej,i) + zs12(ej,i)*zs12(ej,i)
202 xsm = - (xs12(ej,i)*xm12(ej,i) + ys12(ej,i)*ym12(ej,i) + zs12(ej,i)*zm12(ej,i))
203 xs2m2 = xxm2(ej,i)-xxs2(ej,i)
204 ys2m2 = xym2(ej,i)-xys2(ej,i)
205 zs2m2 = xzm2(ej,i)-xzs2(ej,i)
206
207 xaa = xs12(ej,i)*xs2m2 + ys12(ej,i)*ys2m2 + zs12(ej,i)*zs2m2
208 xbb = -xm12(ej,i)*xs2m2 - ym12(ej,i)*ys2m2 - zm12(ej,i)*zs2m2
209 det = xm2(ej,i)*xs2(ej,i) - xsm*xsm
210 det = max(em20,det)
211C
212 h1m(ej,i) = (xaa*xsm-xbb*xs2(ej,i)) / det
213 IF(h1m(ej,i) < -em03 .OR. h1m(ej,i) > onep03) THEN
214 pene2(ej,i)=zero
215 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(ej,i))
216 cycle ! no contact
217 END IF
218C
219 xs2(ej,i) = max(xs2(ej,i),em20)
220 xm2(ej,i) = max(xm2(ej,i),em20)
221 h1m(ej,i)=min(one,max(zero,h1m(ej,i)))
222 h1s(ej,i) = -(xaa + h1m(ej,i)*xsm) / xs2(ej,i)
223 IF(h1s(ej,i) < -em03 .OR. h1s(ej,i) > onep03) THEN
224 pene2(ej,i)=zero
225 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(ej,i))
226
227 cycle ! no contact
228 END IF
229
230 h1s(ej,i)=min(one,max(zero,h1s(ej,i)))
231 h1m(ej,i) = -(xbb + h1s(ej,i)*xsm) / xm2(ej,i)
232 h1m(ej,i)=min(one,max(zero,h1m(ej,i)))
233
234 h2s(ej,i) = one -h1s(ej,i)
235 h2m(ej,i) = one -h1m(ej,i)
236C !!!!!!!!!!!!!!!!!!!!!!!!!!
237C PENE = GAP^2 - DIST^2 Used for testing non zero penetration
238C!!!!!!!!!!!!!!!!!!!!!!!!!!!
239 nx(ej,i) = h1s(ej,i)*xxs1(ej,i) + h2s(ej,i)*xxs2(ej,i)
240 . - h1m(ej,i)*xxm1(ej,i) - h2m(ej,i)*xxm2(ej,i)
241 ny(ej,i) = h1s(ej,i)*xys1(ej,i) + h2s(ej,i)*xys2(ej,i)
242 . - h1m(ej,i)*xym1(ej,i) - h2m(ej,i)*xym2(ej,i)
243 nz(ej,i) = h1s(ej,i)*xzs1(ej,i) + h2s(ej,i)*xzs2(ej,i)
244 . - h1m(ej,i)*xzm1(ej,i) - h2m(ej,i)*xzm2(ej,i)
245
246 nn(ej,i) = nx(ej,i)**2 + ny(ej,i)**2 + nz(ej,i)**2
247
248 nx(ej,i) = -nx(ej,i)
249 ny(ej,i) = -ny(ej,i)
250 nz(ej,i) = -nz(ej,i)
251
252 pene2(ej,i) = dgaploadpmax*dgaploadpmax + nn(ej,i)
253 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(ej,i))
254
255 ENDDO
256 ENDDO
257C
258 sol_edge =iedge/10 ! solids
259 sh_edge =iedge-10*sol_edge ! shells
260C
261C IF(SH_EDGE/=0)THEN
262C DO I=1,JLT
263C
264C
265C Free edges, looking vs positive normal only
266C
267C / S
268C / x
269C M /
270C <------x Sector with Zero force
271C n(M) \
272C \
273C \
274C P2=ZERO
275C IF(IBS(I)==0)
276C . P2= NX(I)*FX(I)+NY(I)*FY(I)+NZ(I)*FZ(I) ! (n(S),MS) > 45 degrees
277C
278C IF(P2 > ZERO)PENE2(I)=ZERO
279C ENDDO
280C END IF
281C
282 IF(sol_edge/=0)THEN
283 DO i=1,jlt
284
285 IF(pene2(1,i)+pene2(2,i)+pene2(3,i)+pene2(4,i)==zero) cycle
286
287 IF(typedgs(i)/=1)cycle
288C---------------------------------------
289C Solid on secnd side !
290C---------------------------------------
291
292c IF(IAS(I)==0 .OR. IBS(I)==0) THEN
293c print *,' internal error - i25dst3_e2s' ! This error message is desactivated : E2E solid erosion will be taken into account
294c END IF
295
296
297 DO ej=1,4
298
299
300 IF(pene2(ej,i)==zero) cycle
301
302C
303C Only contact solid/solid
304C Common normal : contact normal computing using cross product
305C contact normal can't be normal to secondary and main surfaces :
306C using node normal
307C +- 90 with tolerance ( 10deg)
308
309
310C Computing cross product :
311
312 pedg = xm12(ej,i) *xs12(ej,i) + ym12(ej,i) *ys12(ej,i) + zm12(ej,i) *zs12(ej,i)
313 nedg = sqrt(xm2(ej,i)) * sqrt(xs2(ej,i))
314 IF(abs(pedg)>zep999*nedg) THEN
315 pene2(ej,i)=zero
316 cycle ! no contact
317 END IF
318
319 nncx = ys12(ej,i)*zm12(ej,i)- zs12(ej,i)*ym12(ej,i)
320 nncy = zs12(ej,i)*xm12(ej,i)- zm12(ej,i)*xs12(ej,i)
321 nncz = xs12(ej,i)*ym12(ej,i)- ys12(ej,i)*xm12(ej,i)
322
323 nncp = sqrt(nncx*nncx+nncy*nncy+nncz*nncz)
324
325 nncx = nncx / max(em30,nncp)
326 nncy = nncy / max(em30,nncp)
327 nncz = nncz / max(em30,nncp)
328
329 dist = nncx*nx(ej,i)+nncy*ny(ej,i)+nncz*nz(ej,i)
330
331 nx(ej,i) = nncx * dist
332 ny(ej,i) = nncy * dist
333 nz(ej,i) = nncz * dist
334
335 nn(ej,i)=sqrt(nx(ej,i)**2 + ny(ej,i)**2 + nz(ej,i)**2)
336
337 pene2(ej,i) = nn(ej,i)
338
339C Exclusions :
340
341 nm(1:3)=h1m(ej,i)*normalm1(1:3,ej,i)+h2m(ej,i)*normalm2(1:3,ej,i)
342 ns(1:3)=h1s(ej,i)*normaln1(1:3,i)+h2s(ej,i)*normaln2(1:3,i)
343
344 p1=-(nx(ej,i)*nm(1)+ny(ej,i)*nm(2)+nz(ej,i)*nm(3))
345 p2= nx(ej,i)*ns(1)+ny(ej,i)*ns(2)+nz(ej,i)*ns(3)
346
347 nnnn=nn(ej,i)
348
349
350 IF(p2 > em04*nnnn.OR.p1 > em04*nnnn)THEN ! Tolerance EM04
351 pene2(ej,i)=zero
352 ENDIF
353
354 IF(abs(p1) <= zep2*nnnn .OR. abs(p2) <= zep2*nnnn) THEN ! Tolerance :+-80deg
355 pene2(ej,i)=zero
356 cycle ! no contact
357 ENDIF
358
359 ENDDO
360
361 ENDDO
362 END IF
363
364
365 IF(sol_edge/=0)THEN ! provisoire pour solides
366C-----------Keep this condition for the moment (need?) : increase tolerance ( missing contacts )
367
368 lba(1:jlt,1:4) = -one ! cf test LBA, LCA
369 lca(1:jlt,1:4) = zero
370C---------------------------------------
371C Extraction des coordonnees et pre-filtrage des candidats vs diedre main
372C---------------------------------------
373 DO i=1,jlt
374
375 IF(pene2(1,i)+pene2(2,i)+pene2(3,i)+pene2(4,i)==zero) cycle
376
377C---------------------------------------
378C Solid on main side !
379C---------------------------------------
380
381 ix1(i)=irect(1,iam(i))
382 ix2(i)=irect(2,iam(i))
383 ix3(i)=irect(3,iam(i))
384 ix4(i)=irect(4,iam(i))
385
386 xa(1,i)=x(1,ix1(i))
387 ya(1,i)=x(2,ix1(i))
388 za(1,i)=x(3,ix1(i))
389 xa(2,i)=x(1,ix2(i))
390 ya(2,i)=x(2,ix2(i))
391 za(2,i)=x(3,ix2(i))
392 xa(3,i)=x(1,ix3(i))
393 ya(3,i)=x(2,ix3(i))
394 za(3,i)=x(3,ix3(i))
395 xa(4,i)=x(1,ix4(i))
396 ya(4,i)=x(2,ix4(i))
397 za(4,i)=x(3,ix4(i))
398
399 IF(ix3(i)/=ix4(i))THEN
400 xa(5,i) = fourth*(xa(1,i)+xa(2,i)+xa(3,i)+xa(4,i))
401 ya(5,i) = fourth*(ya(1,i)+ya(2,i)+ya(3,i)+ya(4,i))
402 za(5,i) = fourth*(za(1,i)+za(2,i)+za(3,i)+za(4,i))
403 ELSE
404 xa(5,i) = xa(3,i)
405 ya(5,i) = ya(3,i)
406 za(5,i) = za(3,i)
407 ENDIF
408C
409 END DO
410
411 DO i=1,jlt
412
413 IF(pene2(1,i)+pene2(2,i)+pene2(3,i)+pene2(4,i)==zero) cycle
414C
415 x0a(i,1) = xa(1,i) - xa(5,i)
416 y0a(i,1) = ya(1,i) - ya(5,i)
417 z0a(i,1) = za(1,i) - za(5,i)
418C
419 x0a(i,2) = xa(2,i) - xa(5,i)
420 y0a(i,2) = ya(2,i) - ya(5,i)
421 z0a(i,2) = za(2,i) - za(5,i)
422C
423 x0a(i,3) = xa(3,i) - xa(5,i)
424 y0a(i,3) = ya(3,i) - ya(5,i)
425 z0a(i,3) = za(3,i) - za(5,i)
426C
427 x0a(i,4) = xa(4,i) - xa(5,i)
428 y0a(i,4) = ya(4,i) - ya(5,i)
429 z0a(i,4) = za(4,i) - za(5,i)
430C
431 xna(i,1) = -(y0a(i,1)*z0a(i,2) - z0a(i,1)*y0a(i,2))
432 yna(i,1) = -(z0a(i,1)*x0a(i,2) - x0a(i,1)*z0a(i,2))
433 zna(i,1) = -(x0a(i,1)*y0a(i,2) - y0a(i,1)*x0a(i,2))
434C
435 IF(ix3(i)/=ix4(i))THEN
436C
437 xna(i,2) = -(y0a(i,2)*z0a(i,3) - z0a(i,2)*y0a(i,3))
438 yna(i,2) = -(z0a(i,2)*x0a(i,3) - x0a(i,2)*z0a(i,3))
439 zna(i,2) = -(x0a(i,2)*y0a(i,3) - y0a(i,2)*x0a(i,3))
440C
441 xna(i,3) = -(y0a(i,3)*z0a(i,4) - z0a(i,3)*y0a(i,4))
442 yna(i,3) = -(z0a(i,3)*x0a(i,4) - x0a(i,3)*z0a(i,4))
443 zna(i,3) = -(x0a(i,3)*y0a(i,4) - y0a(i,3)*x0a(i,4))
444C
445 xna(i,4) = -(y0a(i,4)*z0a(i,1) - z0a(i,4)*y0a(i,1))
446 yna(i,4) = -(z0a(i,4)*x0a(i,1) - x0a(i,4)*z0a(i,1))
447 zna(i,4) = -(x0a(i,4)*y0a(i,1) - y0a(i,4)*x0a(i,1))
448C
449 ELSE
450C
451 xna(i,2) = xna(i,1)
452 yna(i,2) = yna(i,1)
453 zna(i,2) = zna(i,1)
454C
455C XNA(I,3) = XNA(I,1)
456C YNA(I,3) = YNA(I,1)
457C ZNA(I,3) = ZNA(I,1)
458C
459 xna(i,4) = xna(i,1)
460 yna(i,4) = yna(i,1)
461 zna(i,4) = zna(i,1)
462C
463 END IF
464C
465 END DO
466
467C---------------------------------------
468C Solid on main side !
469C---------------------------------------
470 DO i=1,jlt
471
472 IF(typedgs(i)==1)cycle ! keep condition only Shell/solid
473
474 DO ej=1,4
475
476 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(ej,i))
477
478 IF(pene2(ej,i)==zero) cycle
479
480 IF(ix3(i)==ix4(i))THEN
481 IF(ej==3) cycle
482 i1=ntria(1,ej)
483 i2=ntria(2,ej)
484 i3=ntria(3,ej)
485 i4=i3
486 i0=i3
487 ELSE
488 i1=ej
489 i2=mod(ej ,4)+1
490 i3=mod(ej+1,4)+1
491 i4=mod(ej+2,4)+1
492 i0=5
493 END IF
494C-----------------------------------------
495C Solid on main side !
496C-----------------------------------------
497C
498C
499C Normal to neighboring segment (cf E == Bisector)
500 ps=xna(i,ej)*ex(ej,i)+yna(i,ej)*ey(ej,i)+zna(i,ej)*ez(ej,i)
501 xnb(i,ej) = -xna(i,ej)+two*ps*ex(ej,i)
502 ynb(i,ej) = -yna(i,ej)+two*ps*ey(ej,i)
503 znb(i,ej) = -zna(i,ej)+two*ps*ez(ej,i)
504C
505 xs = h1s(ej,i)*xxs1(ej,i) + h2s(ej,i)*xxs2(ej,i)
506 ys = h1s(ej,i)*xys1(ej,i) + h2s(ej,i)*xys2(ej,i)
507 zs = h1s(ej,i)*xzs1(ej,i) + h2s(ej,i)*xzs2(ej,i)
508 xm = h1m(ej,i)*xxm1(ej,i) + h2m(ej,i)*xxm2(ej,i)
509 ym = h1m(ej,i)*xym1(ej,i) + h2m(ej,i)*xym2(ej,i)
510 zm = h1m(ej,i)*xzm1(ej,i) + h2m(ej,i)*xzm2(ej,i)
511 da = (xs-xm)*xna(i,ej)+(ys-ym)*yna(i,ej)+(zs-zm)*zna(i,ej)
512 db = (xs-xm)*xnb(i,ej)+(ys-ym)*ynb(i,ej)+(zs-zm)*znb(i,ej)
513C
514 cnvx= (xa(i0,i)-xa(i1,i))*xnb(i,ej)
515 . +(ya(i0,i)-ya(i1,i))*ynb(i,ej)
516 . +(za(i0,i)-za(i1,i))*znb(i,ej)
517
518 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,cnvx)
519
520 IF(cnvx >= zero)THEN
521 IF(da <= zero .OR. db <= zero) THEN ! Don't compute a force in the wrong direction.
522 pene2(ej,i)=zero
523 ENDIF
524 ELSE IF(da <= zero .AND. db <= zero) THEN
525 pene2(ej,i)=zero
526 END IF
527
528 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(ej,i))
529
530 ENDDO
531 ENDDO
532C---------------------------------------
533C Calculer uniquement candidats filtres par la suite...
534C---------------------------------------
535 njndx = 0
536 n4a = 0
537 DO i=1,jlt
538
539 IF(pene2(1,i)+pene2(2,i)+pene2(3,i)+pene2(4,i)==zero) cycle
540
541C---------------------------------------
542C Solid on main side !
543C---------------------------------------
544 njndx=njndx+1
545 jndx(njndx)=i
546
547 IF(ix3(i)/=ix4(i))THEN
548 n4a=n4a+1
549 i4a(n4a)=i
550 END IF
551 END DO
552C---------------------------------------
553#include "vectorize.inc"
554 DO k=1,njndx
555C
556C
557C---------------------------------------
558C Solid on main side !
559C---------------------------------------
560 i=jndx(k)
561C
562 da1 = (xxs1(1,i)-xa(5,i))*xna(i,1)+(xys1(1,i)-ya(5,i))*yna(i,1)+(xzs1(1,i)-za(5,i))*zna(i,1)
563 da2 = (xxs2(1,i)-xa(5,i))*xna(i,1)+(xys2(1,i)-ya(5,i))*yna(i,1)+(xzs2(1,i)-za(5,i))*zna(i,1)
564C
565C On traite y-compris le cas ou S1 ou S2 sont exactement sur la surface
566C (en dehors du 3eme point du triangle, que l'on peut rater)
567 IF(da1*da2 <= zero)THEN
568 alp=-da2/sign(max(em20,abs(da1-da2)),da1-da2)
569 xxs=alp*xxs1(1,i)+(one-alp)*xxs2(1,i)
570 xys=alp*xys1(1,i)+(one-alp)*xys2(1,i)
571 xzs=alp*xzs1(1,i)+(one-alp)*xzs2(1,i)
572
573 xi0 = xa(5,i) - xxs
574 yi0 = ya(5,i) - xys
575 zi0 = za(5,i) - xzs
576C
577 xi1 = xa(1,i) - xxs
578 yi1 = ya(1,i) - xys
579 zi1 = za(1,i) - xzs
580C
581 xi2 = xa(2,i) - xxs
582 yi2 = ya(2,i) - xys
583 zi2 = za(2,i) - xzs
584C
585 sx1 = yi0*zi1 - zi0*yi1
586 sy1 = zi0*xi1 - xi0*zi1
587 sz1 = xi0*yi1 - yi0*xi1
588C
589 sx2 = yi0*zi2 - zi0*yi2
590 sy2 = zi0*xi2 - xi0*zi2
591 sz2 = xi0*yi2 - yi0*xi2
592C
593 aaa=one/max(em30,xna(i,1)*xna(i,1)+yna(i,1)*yna(i,1)+zna(i,1)*zna(i,1))
594 lba(i,1) = -(-(xna(i,1)*sx2 + yna(i,1)*sy2 + zna(i,1)*sz2))*aaa
595 lca(i,1) = -( (xna(i,1)*sx1 + yna(i,1)*sy1 + zna(i,1)*sz1))*aaa
596C
597C ELSE
598C LBA(I,1) = -ONE
599C LCA(I,1) = ZERO
600 END IF
601C
602 ENDDO
603
604#include "vectorize.inc"
605 DO k=1,n4a
606 i=i4a(k)
607C
608 da1 = (xxs1(1,i)-xa(5,i))*xna(i,2)+(xys1(1,i)-ya(5,i))*yna(i,2)+(xzs1(1,i)-za(5,i))*zna(i,2)
609 da2 = (xxs2(1,i)-xa(5,i))*xna(i,2)+(xys2(1,i)-ya(5,i))*yna(i,2)+(xzs2(1,i)-za(5,i))*zna(i,2)
610C
611C On traite y-compris le cas ou S1 ou S2 sont exactement sur la surface
612C (en dehors du 3eme point du triangle, que l'on peut rater)
613 IF(da1*da2 <= zero)THEN
614 alp=-da2/sign(max(em20,abs(da1-da2)),da1-da2)
615 xxs=alp*xxs1(1,i)+(one-alp)*xxs2(1,i)
616 xys=alp*xys1(1,i)+(one-alp)*xys2(1,i)
617 xzs=alp*xzs1(1,i)+(one-alp)*xzs2(1,i)
618
619 xi0 = xa(5,i) - xxs
620 yi0 = ya(5,i) - xys
621 zi0 = za(5,i) - xzs
622C
623 xi1 = xa(2,i) - xxs
624 yi1 = ya(2,i) - xys
625 zi1 = za(2,i) - xzs
626C
627 xi2 = xa(3,i) - xxs
628 yi2 = ya(3,i) - xys
629 zi2 = za(3,i) - xzs
630C
631 sx1 = yi0*zi1 - zi0*yi1
632 sy1 = zi0*xi1 - xi0*zi1
633 sz1 = xi0*yi1 - yi0*xi1
634C
635 sx2 = yi0*zi2 - zi0*yi2
636 sy2 = zi0*xi2 - xi0*zi2
637 sz2 = xi0*yi2 - yi0*xi2
638C
639 aaa=one/max(em30,xna(i,2)*xna(i,2)+yna(i,2)*yna(i,2)+zna(i,2)*zna(i,2))
640 lba(i,2) = -(-(xna(i,2)*sx2 + yna(i,2)*sy2 + zna(i,2)*sz2))*aaa
641 lca(i,2) = -( (xna(i,2)*sx1 + yna(i,2)*sy1 + zna(i,2)*sz1))*aaa
642C
643C ELSE
644C LBA(I,2) = -ONE
645C LCA(I,2) = ZERO
646 END IF
647C
648 ENDDO
649
650#include "vectorize.inc"
651 DO k=1,n4a
652 i=i4a(k)
653C
654 da1 = (xxs1(1,i)-xa(5,i))*xna(i,3)+(xys1(1,i)-ya(5,i))*yna(i,3)+(xzs1(1,i)-za(5,i))*zna(i,3)
655 da2 = (xxs2(1,i)-xa(5,i))*xna(i,3)+(xys2(1,i)-ya(5,i))*yna(i,3)+(xzs2(1,i)-za(5,i))*zna(i,3)
656C
657C On traite y-compris le cas ou S1 ou S2 sont exactement sur la surface
658C (en dehors du 3eme point du triangle, que l'on peut rater)
659 IF(da1*da2 <= zero)THEN
660 alp=-da2/sign(max(em20,abs(da1-da2)),da1-da2)
661 xxs=alp*xxs1(1,i)+(one-alp)*xxs2(1,i)
662 xys=alp*xys1(1,i)+(one-alp)*xys2(1,i)
663 xzs=alp*xzs1(1,i)+(one-alp)*xzs2(1,i)
664
665 xi0 = xa(5,i) - xxs
666 yi0 = ya(5,i) - xys
667 zi0 = za(5,i) - xzs
668C
669 xi1 = xa(3,i) - xxs
670 yi1 = ya(3,i) - xys
671 zi1 = za(3,i) - xzs
672C
673 xi2 = xa(4,i) - xxs
674 yi2 = ya(4,i) - xys
675 zi2 = za(4,i) - xzs
676C
677 sx1 = yi0*zi1 - zi0*yi1
678 sy1 = zi0*xi1 - xi0*zi1
679 sz1 = xi0*yi1 - yi0*xi1
680C
681 sx2 = yi0*zi2 - zi0*yi2
682 sy2 = zi0*xi2 - xi0*zi2
683 sz2 = xi0*yi2 - yi0*xi2
684C
685 aaa=one/max(em30,xna(i,3)*xna(i,3)+yna(i,3)*yna(i,3)+zna(i,3)*zna(i,3))
686 lba(i,3) = -(-(xna(i,3)*sx2 + yna(i,3)*sy2 + zna(i,3)*sz2))*aaa
687 lca(i,3) = -( (xna(i,3)*sx1 + yna(i,3)*sy1 + zna(i,3)*sz1))*aaa
688C
689C ELSE
690C LBA(I,3) = -ONE
691C LCA(I,3) = ZERO
692 END IF
693C
694 ENDDO
695
696#include "vectorize.inc"
697 DO k=1,n4a
698 i=i4a(k)
699C
700 da1 = (xxs1(1,i)-xa(5,i))*xna(i,4)+(xys1(1,i)-ya(5,i))*yna(i,4)+(xzs1(1,i)-za(5,i))*zna(i,4)
701 da2 = (xxs2(1,i)-xa(5,i))*xna(i,4)+(xys2(1,i)-ya(5,i))*yna(i,4)+(xzs2(1,i)-za(5,i))*zna(i,4)
702C
703C On traite y-compris le cas ou S1 ou S2 sont exactement sur la surface
704C (en dehors du 3eme point du triangle, que l'on peut rater)
705 IF(da1*da2 <= zero)THEN
706 alp=-da2/sign(max(em20,abs(da1-da2)),da1-da2)
707 xxs=alp*xxs1(1,i)+(one-alp)*xxs2(1,i)
708 xys=alp*xys1(1,i)+(one-alp)*xys2(1,i)
709 xzs=alp*xzs1(1,i)+(one-alp)*xzs2(1,i)
710
711 xi0 = xa(5,i) - xxs
712 yi0 = ya(5,i) - xys
713 zi0 = za(5,i) - xzs
714C
715 xi1 = xa(4,i) - xxs
716 yi1 = ya(4,i) - xys
717 zi1 = za(4,i) - xzs
718C
719 xi2 = xa(1,i) - xxs
720 yi2 = ya(1,i) - xys
721 zi2 = za(1,i) - xzs
722C
723 sx1 = yi0*zi1 - zi0*yi1
724 sy1 = zi0*xi1 - xi0*zi1
725 sz1 = xi0*yi1 - yi0*xi1
726C
727 sx2 = yi0*zi2 - zi0*yi2
728 sy2 = zi0*xi2 - xi0*zi2
729 sz2 = xi0*yi2 - yi0*xi2
730C
731 aaa=one/max(em30,xna(i,4)*xna(i,4)+yna(i,4)*yna(i,4)+zna(i,4)*zna(i,4))
732 lba(i,4) = -(-(xna(i,4)*sx2 + yna(i,4)*sy2 + zna(i,4)*sz2))*aaa
733 lca(i,4) = -( (xna(i,4)*sx1 + yna(i,4)*sy1 + zna(i,4)*sz1))*aaa
734C
735C ELSE
736C LBA(I,4) = -ONE
737C LCA(I,4) = ZERO
738 END IF
739C
740 ENDDO
741
742C
743#include "vectorize.inc"
744 DO k=1,njndx
745 i=jndx(k)
746C
747C
748C---------------------------------------
749C Solid on main side !
750C---------------------------------------
751
752 IF(lba(i,1) < -em01 .OR. lca(i,1) < -em01 .OR. lba(i,1)+lca(i,1) > onep01)
753 . pene2(1,i)=zero
754 IF(lba(i,2) < -em01 .OR. lca(i,2) < -em01 .OR. lba(i,2)+lca(i,2) > onep01)
755 . pene2(2,i)=zero
756 IF(lba(i,3) < -em01 .OR. lca(i,3) < -em01 .OR. lba(i,3)+lca(i,3) > onep01)
757 . pene2(3,i)=zero
758 IF(lba(i,4) < -em01 .OR. lca(i,4) < -em01 .OR. lba(i,4)+lca(i,4) > onep01)
759 . pene2(4,i)=zero
760
761 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(1,i))
762
763
764 ENDDO
765
766 END IF
767C
768 DO i=1,jlt
769 DO ej=1,4
770 IF(pene2(ej,i)==zero) cand_p(ej,index(i))=zero ! Reset CAND_P
771 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,cand_p(ej,index(i)))
772 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,ej)
773 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,i)
774 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,index(i))
775 ENDDO
776 ENDDO
777C
778 i_new =1
779 ej_new=0
780 indx1(1:4*mvsiz) = -666
781 edge_id_cp(1:2,1:mvsiz) = edge_id(1:2,1:mvsiz)
782 DO i=1,jlt
783 DO ej=1,4
784
785 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,stif(ej,i))
786 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,pene2(ej,i))
787 debug_e2e(edge_id(1,i)==d_em .AND. edge_id(2,i) == d_es,nsms(ej,i))
788
789
790 IF( stif(ej,i)/=zero.AND.pene2(ej,i)/=zero)THEN
791 jlt_new = jlt_new + 1
792 ej_new=ej_new+1
793 IF(ej_new > 4)THEN
794 ej_new=1
795 i_new=i_new+1
796 END IF
797 edge_id(1,jlt_new) = edge_id_cp(1,i)
798 edge_id(2,jlt_new) = edge_id_cp(2,i)
799 assert(cand_s(i) > 0)
800 assert(cand_m(i) > 0)
801 cs_loc4(jlt_new)=cand_s(i)
802 cm_loc4(jlt_new)=cand_m(i)
803 n1(ej_new,i_new) = n1(ej,i)
804 n2(ej_new,i_new) = n2(ej,i)
805 m1(ej_new,i_new) = m1(ej,i)
806 m2(ej_new,i_new) = m2(ej,i)
807 h1s(ej_new,i_new) = h1s(ej,i)
808 h2s(ej_new,i_new) = h2s(ej,i)
809 h1m(ej_new,i_new) = h1m(ej,i)
810 h2m(ej_new,i_new) = h2m(ej,i)
811 nx(ej_new,i_new) = nx(ej,i)
812 ny(ej_new,i_new) = ny(ej,i)
813 nz(ej_new,i_new) = nz(ej,i)
814 stif(ej_new,i_new) = stif(ej,i)
815 gapve(ej_new,i_new)= gapve(ej,i)
816 vxs1(ej_new,i_new) = vxs1(ej,i)
817 vys1(ej_new,i_new) = vys1(ej,i)
818 vzs1(ej_new,i_new) = vzs1(ej,i)
819 vxs2(ej_new,i_new) = vxs2(ej,i)
820 vys2(ej_new,i_new) = vys2(ej,i)
821 vzs2(ej_new,i_new) = vzs2(ej,i)
822 vxm1(ej_new,i_new) = vxm1(ej,i)
823 vym1(ej_new,i_new) = vym1(ej,i)
824 vzm1(ej_new,i_new) = vzm1(ej,i)
825 vxm2(ej_new,i_new) = vxm2(ej,i)
826 vym2(ej_new,i_new) = vym2(ej,i)
827 vzm2(ej_new,i_new) = vzm2(ej,i)
828 ms1(ej_new,i_new) = ms1(ej,i)
829 ms2(ej_new,i_new) = ms2(ej,i)
830 mm1(ej_new,i_new) = mm1(ej,i)
831 mm2(ej_new,i_new) = mm2(ej,i)
832 indx1(jlt_new) =index(i)
833 indx2(jlt_new) =ej
834 IF(intfric_loc /= 0) ipartfric_es(ej_new,i_new)=ipartfric_es(ej,i)
835 IF(intfric_loc /= 0) ipartfric_em(ej_new,i_new)=ipartfric_em(ej,i)
836 IF(idtmins_loc == 2) nsms(ej_new,i_new) = nsms(ej,i)
837C#ifdef WITH_ASSERT
838C WRITE(6,*) EJ_NEW,I_NEW,NSMS(EJ_NEW,I_NEW)
839C#endif
840C PRINTIF(.TRUE.,INDX1(JLT_NEW))
841C PRINTIF(.TRUE.,INDX2(JLT_NEW))
842 ENDIF
843 ENDDO
844 ENDDO
845C WRITE(6,*) "JLT_NEW=",JLT_NEW
846 RETURN
my_real
#define my_real
Definition cppsort.cpp:32
min
#define min(a, b)
Definition macros.h:20
max
#define max(a, b)
Definition macros.h:21