add mass matrix computation for TET4 (Python and Fortran)

luclaurent · Mar 15, 2023 · 8dca435 · 8dca435
1 parent 3f88818
commit 8dca435
Show file tree

Hide file tree

Showing 4 changed files with 190 additions and 0 deletions.
diff --git a/SILEXlight/silex_lib_tet4_fortran.f b/SILEXlight/silex_lib_tet4_fortran.f
@@ -261,7 +261,112 @@ subroutine StiffnessMatrix(nbnodes,nodes,
       return
       end
 cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
+! CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
+
+      SUBROUTINE MassMatrix(nbnodes, 
+     &                      nodes,
+     &                      nelem,
+     &                      elements,
+     &                      rho,
+     &                      Ik,
+     &                      Jk,
+     &                      Vk)
+
+      IMPLICIT NONE
+
+      INTEGER,INTENT(IN) :: nelem,nbnodes
+      INTEGER,INTENT(IN) :: elements(nelem,4)
+      DOUBLE PRECISION,INTENT(IN) :: nodes(nbnodes,3),rho
+      !!
+      INTEGER,INTENT(OUT) :: Ik(12*12*nelem),Jk(12*12*nelem)
+      DOUBLE PRECISION,INTENT(OUT) :: Vk(12*12*nelem)
+      !!
+      INTEGER :: npgt,p,e,i,j,g
+      DOUBLE PRECISION :: det_of_sys, det44_ligne_de_un
+      INTEGER :: dofelem(12),dofx(4),dofy(4),dofz(4),idnodes(4)
+      DOUBLE PRECISION :: RG(5),SG(5),TG(5),WG(5),NN(4)
+      DOUBLE PRECISION :: A34(3,4),PHI(3,12),me(12,12)
+      DOUBLE PRECISION :: Vol
+      DOUBLE PRECISION :: X(4),Y(4),Z(4)
+
+
+    ! Define Gauss points in reference tetrahedral
+      npgt=5
+      RG=(/1.0/4.0,1.0/2.0,1.0/6.0,1.0/6.0,1.0/6.0/)
+      SG=(/1.0/4.0,1.0/6.0,1.0/2.0,1.0/6.0,1.0/6.0/)
+      TG=(/1.0/4.0,1.0/6.0,1.0/6.0,1.0/2.0,1.0/6.0/)
+      WG=(/-4.0/(5.0*6.0),9.0/(20.0*6.0),9.0/(20.0*6.0)
+     & ,9.0/(20.0*6.0),9.0/(20.0*6.0)/)
+
+      p=1
 
+      DO i = 1,3
+          DO j =1,12
+            PHI(i,j)=0.0d0
+          ENDDO
+      ENDDO
+
+      DO e =1,nelem
+          DO i = 1,4
+            idnodes(i) = elements(e,i)
+    !                          python indexing
+            dofx(i)    = (idnodes(i)-1)*3
+            dofy(i)    = (idnodes(i)-1)*3+1
+            dofz(i)    = (idnodes(i)-1)*3+2
+          ENDDO
+          DO i = 1,4
+            dofelem(1+3*(i-1)) = dofx(i)
+            dofelem(2+3*(i-1)) = dofy(i)
+            dofelem(3+3*(i-1)) = dofz(i)
+          ENDDO
+
+          DO i = 1,4
+            X(i)=nodes(idnodes(i),1)
+            Y(i)=nodes(idnodes(i),2)
+            Z(i)=nodes(idnodes(i),3)
+          ENDDO
+
+          a34(1,1) = X(1);a34(1,2) = X(2)
+          a34(1,3) = X(3);a34(1,4) = X(4)
+          a34(2,1) = Y(1);a34(2,2) = Y(2)
+          a34(2,3) = Y(3);a34(2,4) = Y(4)
+          a34(3,1) = Z(1);a34(3,2) = Z(2)
+          a34(3,3) = Z(3);a34(3,4) = Z(4)
+          det_of_sys = det44_ligne_de_un(a34)
+          Vol        = ABS(det_of_sys/6.0)
+
+          DO i = 1,12
+            DO j =1,12
+                me(i,j)=0.0d0
+            ENDDO
+          ENDDO
+
+    !       loop over Gauss Points
+          DO g=1,npgt
+
+            NN(1)=RG(g);NN(2)=SG(g);NN(3)=TG(g)
+            NN(4)=1-RG(g)-SG(g)-TG(g)
+
+            PHI(1,1)=NN(1);PHI(1,4)=NN(2);PHI(1,7)=NN(3);PHI(1,10)=NN(4)
+            PHI(2,2)=NN(1);PHI(2,5)=NN(2);PHI(2,8)=NN(3);PHI(2,11)=NN(4)
+            PHI(3,3)=NN(1);PHI(3,6)=NN(2);PHI(3,9)=NN(3);PHI(3,12)=NN(4)
+
+            me = me + MATMUL(TRANSPOSE(PHI),PHI)*rho*WG(g)*Vol*6.0
+
+          ENDDO
+
+          DO i = 1,12
+            DO j =1,12
+                Ik(p)=dofelem(i)
+                Jk(p)=dofelem(j)
+                Vk(p)=me(i,j)
+                p=p+1
+            ENDDO
+          ENDDO
+      ENDDO
+
+      RETURN
+      END SUBROUTINE MassMatrix
 cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
 C stress smoothing
 cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc

diff --git a/SILEXlight/silex_lib_tet4_python.py b/SILEXlight/silex_lib_tet4_python.py
@@ -193,6 +193,83 @@ def stiffnessmatrix(nodes, elements, material):
 
 #############################################################
 
+def massmatrix(nodes,elements,rho):
+
+    nelem   = elements.shape[0]
+    Ik      = numpy.zeros(12*12*nelem,dtype=int)
+    Jk      = numpy.zeros(12*12*nelem,dtype=int)
+    Vk      = numpy.zeros(12*12*nelem,dtype=float)
+    dofelem = numpy.zeros(12,dtype=int)
+    dofx    = numpy.zeros(4,dtype=int)
+    dofy    = numpy.zeros(4,dtype=int)
+    dofz    = numpy.zeros(4,dtype=int)
+    idnodes = numpy.zeros(4,dtype=int)
+    RG      = numpy.zeros(5,dtype=float)
+    SG      = numpy.zeros(5,dtype=float)
+    TG      = numpy.zeros(5,dtype=float)
+    WG      = numpy.zeros(5,dtype=float)
+    NN      = numpy.zeros(4,dtype=float)
+    A34     = numpy.zeros((3,4),dtype=float)
+    PHI     = numpy.zeros((3,12),dtype=float)
+
+# Define Gauss points in reference tetrahedral
+    npgt=5
+    RG[0]=1.0/4.0; SG[0]=1.0/4.0; TG[0]=1.0/4.0; WG[0]=-4.0/(5.0*6.0)
+    RG[1]=1.0/2.0; SG[1]=1.0/6.0; TG[1]=1.0/6.0; WG[1]=9.0/(20.0*6.0)
+    RG[2]=1.0/6.0; SG[2]=1.0/2.0; TG[2]=1.0/6.0; WG[2]=9.0/(20.0*6.0)
+    RG[3]=1.0/6.0; SG[3]=1.0/6.0; TG[3]=1.0/2.0; WG[3]=9.0/(20.0*6.0)
+    RG[4]=1.0/6.0; SG[4]=1.0/6.0; TG[4]=1.0/6.0; WG[4]=9.0/(20.0*6.0)
+
+    p=0
+    for e in range(nelem):
+        idnodes[:] = elements[e,:]-1
+
+        X=nodes[idnodes,0]
+        Y=nodes[idnodes,1]
+        Z=nodes[idnodes,2]
+        dofx[:]     = (idnodes)*3
+        dofy[:]     = (idnodes)*3+1
+        dofz[:]     = (idnodes)*3+2
+        dofelem[0]  = dofx[0] 
+        dofelem[1]  = dofy[0] 
+        dofelem[2]  = dofz[0] 
+        dofelem[3]  = dofx[1] 
+        dofelem[4]  = dofy[1]
+        dofelem[5]  = dofz[1] 
+        dofelem[6]  = dofx[2] 
+        dofelem[7]  = dofy[2] 
+        dofelem[8]  = dofz[2]
+        dofelem[9]  = dofx[3] 
+        dofelem[10] = dofy[3] 
+        dofelem[11] = dofz[3] 
+
+        A34[0,:]    = X
+        A34[1,:]    = Y
+        A34[2,:]    = Z
+        det_of_sys  = det44_ligne_de_un(A34)
+        Vol         = abs(det_of_sys/6)
+
+        me          = numpy.zeros((12,12),dtype=float)
+
+        # loop over Gauss Points
+        for g in range(npgt):
+            NN[0]=RG[g];NN[1]=SG[g];NN[2]=TG[g];NN[3]=1-RG[g]-SG[g]-TG[g]
+
+            PHI[0,0]=NN[0];PHI[0,3]=NN[1];PHI[0,6]=NN[2];PHI[0,9]=NN[3]
+            PHI[1,1]=NN[0];PHI[1,4]=NN[1];PHI[1,7]=NN[2];PHI[1,10]=NN[3]
+            PHI[2,2]=NN[0];PHI[2,5]=NN[1];PHI[2,8]=NN[2];PHI[2,11]=NN[3]
+
+            me = me + numpy.dot(PHI.T,PHI)*rho*WG[g]*Vol*6.0
+
+        for i in range(12):
+            Ik[p:(p+12)]=dofelem[i]
+            Jk[p:(p+12)]=dofelem[:]
+            Vk[p:(p+12)]=me[i,:]
+            p=p+12
+
+    return Ik,Jk,Vk
+
+############################################################
 
 def forceonsurface(nodes, elements, press, direction):
     nbnodes = nodes.shape[0]

diff --git a/SILEXlight/tests/test_silex_lib_tet4_fortran.py b/SILEXlight/tests/test_silex_lib_tet4_fortran.py
@@ -46,3 +46,7 @@ def test_normvector():
 def test_stiffnessmatrix():
     # sx_tet4.stiffnessmatrix()
     assert True
+
+def test_massmatrix():
+    # sx_tet4.massmatrix()
+    assert True
diff --git a/SILEXlight/tests/test_silex_lib_tet4_python.py b/SILEXlight/tests/test_silex_lib_tet4_python.py
@@ -29,3 +29,7 @@ def test_forceonsurface():
 def test_stiffnessmatrix():
     # sx_tet4.stiffnessmatrix()
     assert True
+
+def test_massmatrix():
+    # sx_tet4.massmatrix()
+    assert True