************************************************************************
*   
*   Penetration Judging Program for Swumsuit
*                                                   since 2005.10.2
*                      (c) Motomu Nakashima, 2005
*
************************************************************************
      subroutine judgepen(numb,div,divt,xbm,ybm,zbm,pene)
      implicit double precision(a-z)
      integer numb,div,divt,divtm1,i,i2,j,j2,k,k2
      double precision 
     &   xbm(numb,div,divt),ybm(numb,div,divt),zbm(numb,div,divt)
     &  ,dismax(numb)

      pi=3.14159265358979d0

      write(*,*) 'Judging penetration start...'
      write(*,*) numb,div,divt
**************************************************
* compute maximum distance in each body segment
**************************************************
      do j=1,numb
          dismax(j) = 0.0d0
          i=1
          do k=1,divt-1
              write(*,*) j,i,k
              write(*,*) xbm(j,i,k),ybm(j,i,k),zbm(j,i,k)
              write(*,*) xbm(j,i,k+1),ybm(j,i,k+1),zbm(j,i,k+1)
              dis = (xbm(j,i,k)-xbm(j,i,k+1))**2
     &             +(ybm(j,i,k)-ybm(j,i,k+1))**2
     &             +(zbm(j,i,k)-zbm(j,i,k+1))**2
              write(*,*) dis
c             if(dis.gt.dismax(j))then
c                 dismax(j) = dis
c             endif
          enddo

      stop

          i=div
          do k=1,divt-1
              dis = (xbm(j,i,k)-xbm(j,i,k+1))**2
     &             +(ybm(j,i,k)-ybm(j,i,k+1))**2
     &             +(zbm(j,i,k)-zbm(j,i,k+1))**2
              if(dis.gt.dismax(j))then
                  dismax(j) = dis
              endif
          enddo

          dis = (xbm(j,1,1)-xbm(j,2,1))**2
     &         +(ybm(j,1,1)-ybm(j,2,1))**2
     &         +(zbm(j,1,1)-zbm(j,2,1))**2
          if(dis.gt.dismax(j))then
              dismax(j) = dis
          endif

      enddo


*********************************
* (1) right upper limb - trunk
*********************************
      pene=0.0d0
      do j=1,9
      do j2=18,20,2
      if(dismax(j).gt.dismax(j2))then
          dismax0 = dismax(j)
      else
          dismax0 = dismax(j2)
      endif
      do i=1,div
      do k=1,divt
      do i2=1,div
      do k2=1,divt
          dis = (xbm(j,i,k)-xbm(j2,i2,k2))**2
     &         +(ybm(j,i,k)-ybm(j2,i2,k2))**2
     &         +(zbm(j,i,k)-zbm(j2,i2,k2))**2
          if(dis.lt.dismax0)then
              pene = pene + dismax0
          endif
      enddo
      enddo
      enddo
      enddo

      enddo
      enddo

      write(*,*) 'Judging penetration done!'
      return
      end
