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

Go to the source code of this file.

Functions/Subroutines

subroutine i25dst3_e2s (jlt, iedge, cand_s, cand_m, n1, n2, m1, m2, xxs1, xxs2, xys1, xys2, xzs1, xzs2, xxm1, xxm2, xym1, xym2, xzm1, xzm2, gapve, pene, ex, ey, ez, fx, fy, fz, ledge, irect, x, itab, e2s_nod_normal, admsr)

Function/Subroutine Documentation

◆ i25dst3_e2s()

subroutine i25dst3_e2s ( integer jlt,
integer iedge,
integer, dimension(*) cand_s,
integer, dimension(*) cand_m,
integer, dimension(mvsiz) n1,
integer, dimension(mvsiz) n2,
integer, dimension(4,mvsiz) m1,
integer, dimension(4,mvsiz) m2,
xxs1,
xxs2,
xys1,
xys2,
xzs1,
xzs2,
xxm1,
xxm2,
xym1,
xym2,
xzm1,
xzm2,
gapve,
pene,
ex,
ey,
ez,
fx,
fy,
fz,
integer, dimension(nledge,*) ledge,
integer, dimension(4,*) irect,
x,
integer, dimension(*) itab,
real*4, dimension(3,*) e2s_nod_normal,
integer, dimension(4,*) admsr )

Definition at line 28 of file i25dst3_e2s.F.

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