1291
1292
1293
1295
1296
1297
1298#include "implicit_f.inc"
1299
1300
1301
1302 INTEGER NSN, NMN,ILEV,
1303 . IRECT(4,*), MSR(*), NSV(*), IRTL(*), WEIGHT(*), IDEL2
1304
1306 . x(3,*),v(3,*),a(3,*),ar(3,*), mmass(*), crst(2,*),
1307 . dpara(7,*), ms(*), in(*),stifn(*),stifr(*),dmast,adm(*),
1308 . smass(*), siner(*),fsav(*), fncont(3,*), fncontp(3,*),
1309 . ftcontp(3,*)
1310 TYPE (H3D_DATABASE) :: H3D_DATA
1311
1312
1313
1314#include "scr14_c.inc"
1315#include "scr16_c.inc"
1316#include "impl1_c.inc"
1317
1318
1319
1320 INTEGER NIR, I, J, J1,J2,J3,J4, , L, JJ
1321
1323 . h(4),xm(4),ym(4),zm(4),
1324 . xmsj, ss, st, xmsi, fs(3),sp,sm,tp,tm,facm,
1325 . moms(3),det,fx0,fy0,fz0,ins,
1326 . x0,x1,x2,x3,x4,xs,y0,y1,y2,y3,y4,ys,z0,z1,z2,z3,z4,zs,
1327 . x12,x22,x32,x42,y12,y22,y32,y42,z12,z22,z32,z42,
1328 . xx,yy,zz,xxx,yyy,zzz,xy,yz,zx,xy2,yz2,zx2,s,t,
1329 . a1,a2,a3,b1,b2,b3,c1,c2,c3,mr,mrx,mry,mrz,inx,iny,inz,stf,
1330 . fx(4),fy(4),fz(4)
1331
1332 nir=4
1333
1334
1335
1336 IF(anim_n(2)+outp_n(2)+h3d_data%N_SCAL_DMAS >0.AND.ilev==1) THEN
1337 DO ii=1,nmn
1338 j=msr(ii)
1339 adm(j) = adm(j)*mmass(ii)
1340 ENDDO
1341 ENDIF
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352 IF(impl_s>0) THEN
1353 DO ii=1,nsn
1354 i=nsv(ii)
1355 IF(i>0)THEN
1356 l=irtl(ii)
1357
1358 s = crst(1,ii)
1359 t = crst(2,ii)
1360 sp=one + s
1361 sm=one - s
1362 tp=fourth*(one + t)
1363 tm=fourth*(one - t)
1364
1365 h(1)=one/nir
1366 h(2)=one/nir
1367 h(3)=one/nir
1368 h(4)=one/nir
1369
1370 nir=4
1371 DO jj=1,nir
1372 j=irect(jj,l)
1373 xm(jj)=x(1,j)
1374 ym(jj)=x(2,j)
1375 zm(jj)=x(3,j)
1376 ENDDO
1377 IF(irect(3,l)==irect(4,l)) THEN
1378 nir=3
1379 xm(4)=zero
1380 ym(4)=zero
1381 zm(4)=zero
1382 ENDIF
1383 facm = one / nir
1384
1385
1386
1387 x0=facm*(xm(1)+xm(2)+xm(3)+xm(4))
1388 y0=facm*(ym(1)+ym(2)+ym(3)+ym(4))
1389 z0=facm*(zm(1)+zm(2)+zm(3)+zm(4))
1390 DO j=1,nir
1391 xm(j)=xm(j)-x0
1392 ym(j)=ym(j)-y0
1393 zm(j)=zm(j)-z0
1394 ENDDO
1395 xs=x(1,i)-x0
1396 ys=x(2,i)-y0
1397 zs=x(3,i)-z0
1398 xx=0
1399 yy=0
1400 zz=0
1401 xy=0
1402 yz=0
1403 zx=0
1404 DO j=1,nir
1405 xx=xx+ xm(j)*xm(j)
1406 yy=yy+ ym(j)*ym(j)
1407 zz=zz+ zm(j)*zm(j)
1408 xy=xy+ xm(j)*ym(j)
1409 yz=yz+ ym(j)*zm(j)
1410 zx=zx+ zm(j)*xm(j)
1411 ENDDO
1412 zzz=xx+yy
1413 xxx=yy+zz
1414 yyy=zz+xx
1415 xy2=xy*xy
1416 yz2=yz*yz
1417 zx2=zx*zx
1418 det= xxx*yyy*zzz - xxx*yz2 - yyy*zx2 - zzz*xy2 - two*xy*yz*zx
1419 det=one/det
1420 b1=zzz*yyy-yz2
1421 b2=xxx*zzz-zx2
1422 b3=yyy*xxx-xy2
1423 c3=zzz*xy+yz*zx
1424 c1=xxx*yz+zx*xy
1425 c2=yyy*zx+xy*yz
1426
1427 dpara(1,ii)=det
1428 dpara(2,ii)=b1
1429 dpara(3,ii)=b2
1430 dpara(4,ii)=b3
1431 dpara(5,ii)=c1
1432 dpara(6,ii)=c2
1433 dpara(7,ii)=c3
1434
1435 xmsi=ms(i)*weight(i)
1436 fs(1)=a(1,i)*weight(i)
1437 fs(2)=a(2,i)*weight(i)
1438 fs(3)=a(3,i)*weight(i)
1439 ins=in(i)*weight(i)
1440 moms(1)=ar(1,i)*weight(i) + ys*fs(3) - zs*fs(2)
1441 moms(2)=ar(2,i)*weight(i) + zs*fs(1) - xs*fs(3)
1442 moms(3)=ar(3,i)*weight(i) + xs*fs(2) - ys*fs(1)
1443
1444 a1=det*(moms(1)*b1+moms(2)*c3+moms(3)*c2)
1445 a2=det*(moms(2)*b2+moms(3)*c1+moms(1)*c3)
1446 a3=det*(moms(3)*b3+moms(1)*c2+moms(2)*c1)
1447
1448 fx0=fs(1)*facm
1449 fy0=fs(2)*facm
1450 fz0=fs(3)*facm
1451
1452
1453
1454 DO jj=1,nir
1455 j=irect(jj,l)
1456 fx(jj) = fx0 + a2*zm(jj) - a3*ym(jj)
1457 fy(jj) = fy0 + a3*xm(jj) - a1*zm(jj)
1458 fz(jj) = fz0 + a1*ym(jj) - a2*xm(jj)
1459 a(1,j)=a(1,j) + fx(jj)
1460 a(2,j)=a(2,j) + fy(jj)
1461 a(3,j)=a(3,j) + fz(jj)
1462 ENDDO
1463
1464
1465
1466
1467 inx=ins + xmsi*(xs*xs+ys*ys+zs*zs)
1468 mrx = (b1+c3+c2)
1469 mry = (b2+c1+c3)
1470 mrz = (b3+c2+c1)
1471 mr=det*inx*
max(mrx,mry,mrz)
1472
1473
1474
1475
1476 IF(ilev==1)THEN
1477 xmsi=
max(facm*xmsi,mr)
1478 ELSEIF(ilev==3)THEN
1479 xmsi=
max(facm*xmsi,mr)
1480 ENDIF
1481 dmast = dmast + nir*xmsi - ms(i)
1482 IF (anim_n(2)+outp_n(2)+h3d_data%N_SCAL_DMAS >0) THEN
1483 DO jj=1,nir
1484 j=irect(jj,l)
1485 adm(j) = adm(j) + xmsi - facm*ms(i)
1486 ENDDO
1487 ENDIF
1488 stf = ( facm*stifn(i)
1490 . )*weight(i)
1491 DO jj=1,nir
1492 j=irect(jj,l)
1493 ms(j)=ms(j)+xmsi
1494 stifn(j)=stifn(j) + stf
1495 ENDDO
1496
1497 stifn(i)=em20
1498 IF(idel2/=0.AND.ms(i)/=0.)smass(ii)=ms(i)
1499 ms(i)=zero
1500 stifr(i)=em20
1501 IF(idel2/=0.AND.in(i)/=0.)siner(ii)=in(i)
1502 in(i)=zero
1503 ENDIF
1504
1505 IF(i>0)THEN
1506
1508 . irect(1,l),nir ,fsav ,fncont ,fncontp,
1509 . ftcontp ,weight ,h3d_data,i ,h)
1510 ENDIF
1511
1512 ENDDO
1513
1514 ELSE
1515 DO ii=1,nsn
1516 i=nsv(ii)
1517 IF(i>0)THEN
1518 l=irtl(ii)
1519
1520 s = crst(1,ii)
1521 t = crst(2,ii)
1522 sp=one + s
1523 sm=one - s
1524 tp=fourth*(one + t)
1525 tm=fourth*(one - t)
1526
1527 h(1)=one/nir
1528 h(2)=one/nir
1529 h(3)=one/nir
1530 h(4)=one/nir
1531
1532 j1=irect(1,l)
1533 j2=irect(2,l)
1534 j3=irect(3,l)
1535 j4=irect(4,l)
1536 x1=x(1,j1)
1537 y1=x(2,j1)
1538 z1=x(3,j1)
1539 x2=x(1,j2)
1540 y2=x(2,j2)
1541 z2=x(3,j2)
1542 x3=x(1,j3)
1543 y3=x(2,j3)
1544 z3=x(3,j3)
1545 x4=x(1,j4)
1546 y4=x(2,j4)
1547 z4=x(3,j4)
1548 x0=fourth*(x1+x2+x3+x4)
1549 y0=fourth*(y1+y2+y3+y4)
1550 z0=fourth*(z1+z2+z3+z4)
1551 x1=x1-x0
1552 y1=y1-y0
1553 z1=z1-z0
1554 x2=x2-x0
1555 y2=y2-y0
1556 z2=z2-z0
1557 x3=x3-x0
1558 y3=y3-y0
1559 z3=z3-z0
1560 x4=x4-x0
1561 y4=y4-y0
1562 z4=z4-z0
1563 xs=x(1,i)-x0
1564 ys=x(2,i)-y0
1565 zs=x(3,i)-z0
1566
1567 x12=x1*x1
1568 x22=x2*x2
1569 x32=x3*x3
1570 x42=x4*x4
1571 y12=y1*y1
1572 y22=y2*y2
1573 y32=y3*y3
1574 y42=y4*y4
1575 z12=z1*z1
1576 z22=z2*z2
1577 z32=z3*z3
1578 z42=z4*z4
1579 xx=x12 + x22 + x32 + x42
1580 yy=y12 + y22 + y32 + y42
1581 zz=z12 + z22 + z32 + z42
1582 xy=x1*y1 + x2*y2 + x3*y3 + x4*y4
1583 yz=y1*z1 + y2*z2 + y3*z3 + y4*z4
1584 zx=z1*x1 + z2*x2 + z3*x3 + z4*x4
1585 zzz=xx+yy
1586 xxx=yy+zz
1587 yyy=zz+xx
1588 xy2=xy*xy
1589 yz2=yz*yz
1590 zx2=zx*zx
1591 det= xxx*yyy*zzz - xxx*yz2 - yyy*zx2 - zzz*xy2 - two*xy*yz*zx
1592 det=one/det
1593 b1=zzz*yyy-yz2
1594 b2=xxx*zzz-zx2
1595 b3=yyy*xxx-xy2
1596 c3=zzz*xy+yz*zx
1597 c1=xxx*yz+zx*xy
1598 c2=yyy*zx+xy*yz
1599
1600 dpara(1,ii)=det
1601 dpara(2,ii)=b1
1602 dpara(3,ii)=b2
1603 dpara(4,ii)=b3
1604 dpara(5,ii)=c1
1605 dpara(6,ii)=c2
1606 dpara(7,ii)=c3
1607
1608 xmsi=ms(i)*weight(i)
1609 fs(1)=a(1,i)*weight(i)
1610 fs(2)=a(2,i)*weight(i)
1611 fs(3)=a(3,i)*weight(i)
1612 ins=in(i)*weight(i)
1613 moms(1)=ar(1,i)*weight(i) + ys*fs(3) - zs*fs(2)
1614 moms(2)=ar(2,i)*weight(i) + zs*fs(1) - xs*fs(3)
1615 moms(3)=ar(3,i)*weight(i) + xs*fs(2) - ys*fs(1)
1616
1617 a1=det*(moms(1)*b1+moms(2)*c3+moms(3)*c2)
1618 a2=det*(moms(2)*b2+moms(3)*c1+moms(1)*c3)
1619 a3=det*(moms(3)*b3+moms(1)*c2+moms(2)*c1)
1620
1621 fx0=fs(1)*fourth
1622 fy0=fs(2)*fourth
1623 fz0=fs(3)*fourth
1624
1625
1626
1627 fx(1) = fx0 + a2*z1 - a3*y1
1628 fy(1) = fy0 + a3*x1 - a1*z1
1629 fz(1) = fz0 + a1*y1 - a2*x1
1630 a(1,j1)=a(1,j1) + fx(1)
1631 a(2,j1)=a(2,j1) + fy(1)
1632 a(3,j1)=a(3,j1) + fz(1)
1633 fx(2) = fx0 + a2*z2 - a3*y2
1634 fy(2) = fy0 + a3*x2 - a1*z2
1635 fz(2) = fz0 + a1*y2 - a2*x2
1636 a(1,j2)=a(1,j2) + fx(2)
1637 a(2,j2)=a(2,j2) + fy(2)
1638 a(3,j2)=a(3,j2) + fz(2)
1639 fx(3) = fx0 + a2*z3 - a3*y3
1640 fy(3) = fy0 + a3*x3 - a1*z3
1641 fz(3) = fz0 + a1*y3 - a2*x3
1642 a(1,j3)=a(1,j3) + fx(3)
1643 a(2,j3)=a(2,j3) + fy(3)
1644 a(3,j3)=a(3,j3) + fz(3)
1645 fx(4) = fx0 + a2*z4 - a3*y4
1646 fy(4) = fy0 + a3*x4 - a1*z4
1647 fz(4) = fz0 + a1*y4 - a2*x4
1648 a(1,j4)=a(1,j4) + fx(4)
1649 a(2,j4)=a(2,j4) + fy(4)
1650 a(3,j4)=a(3,j4) + fz(4)
1651
1652
1653
1654
1655 inx=ins + xmsi*(xs*xs+ys*ys+zs*zs)
1656 mrx = (b1+c3+c2
1657 mry = (b2+c1+c3)
1658 mrz = (b3+c2+c1)
1659 mr=det*inx*
max(mrx,mry,mrz)
1660
1661
1662
1663
1664 IF(ilev==1)THEN
1665 xmsi=fourth*xmsi+mr
1666 ELSEIF(ilev==3)THEN
1667 xmsi=
max(fourth*xmsi,mr)
1668 ENDIF
1669 dmast = dmast + four*xmsi - ms(i)
1670 IF (anim_n(2)+outp_n(2THEN
1671 adm(j1) = adm(j1) + xmsi - fourth*ms(i)
1672 adm(j2) = adm(j2) + xmsi - fourth*ms(i)
1673 adm(j3) = adm(j3) + xmsi - fourth*ms(i)
1674 adm(j4) = adm(j4) + xmsi - fourth*ms(i)
1675 ENDIF
1676 ms(j1)=ms(j1)+xmsi
1677 ms(j2)=ms(j2)+xmsi
1678 ms(j3)=ms(j3)+xmsi
1679 ms(j4)=ms(j4)+xmsi
1680 stf = ( fourth*stifn(i)
1681 . + det*
max(mrx,mry,mrz)*(stifr(i)+stifn(i)*(xs*xs+ys*ys+zs*zs))
1682 . )*weight(i)
1683 stifn(j1)=stifn(j1) + stf
1684 stifn(j2)=stifn(j2) + stf
1685 stifn(j3)=stifn(j3) + stf
1686 stifn(j4)=stifn(j4) + stf
1687
1688 stifn(i)=em20
1689 IF(idel2/=0.AND.ms(i)/=0.)smass(ii)=ms(i)
1690 ms(i)=zero
1691 stifr(i)=em20
1692 IF(idel2/=0.AND.in(i)/=0.)siner(ii)=in(i)
1693 in(i)=zero
1694 ENDIF
1695
1696 IF(i>0)THEN
1697
1699 . irect(1,l),nir ,fsav ,fncont ,fncontp,
1700 . ftcontp ,weight ,h3d_data,i ,h)
1701 ENDIF
1702
1703 ENDDO
1704 ENDIF
1705
1706
1707
1708
1709 IF(anim_n(2)+outp_n(2)+h3d_data%N_SCAL_DMAS >0.AND.ilev==1) THEN
1710#include "vectorize.inc"
1711 DO ii=1,nmn
1712 j=msr(ii)
1713 adm(j) = adm(j)/
max(mmass(ii),em20)
1714 ENDDO
1715 ENDIF
1716
1717 RETURN
subroutine i2forces(x, fs, fx, fy, fz, irect, nir, fsav, fncont, fncontp, ftcontp, weight, h3d_data, nsl, h)