program lsf_reg_test

implicit none

double precision xp(8),yp(8),zp(8),lsfp(8)
double precision xp0(8),yp0(8),zp0(8)
double precision x,y,z
double precision xn,yn,zn
double precision r(7)
integer i

xp0=(/0.d0,1.d0,0.d0,1.d0,0.d0,1.d0,0.d0,1.d0/)
yp0=(/0.d0,0.d0,1.d0,1.d0,0.d0,0.d0,1.d0,1.d0/)
zp0=(/0.d0,0.d0,0.d0,0.d0,1.d0,1.d0,1.d0,1.d0/)

! first try with a linerar lsf function
! in this test, the unit cube defined by xp0,yp0,zp0 is stretched
! by a factor r(7) and translated by an offset (r(4),r(5),r(6))
! a random point is generated xp, yp ,zp which is inside the cube
! the values of the level set function at the corners of the cube
! are  obtained as the distance to a plane

do i=1,100
print*,i
call random_number (r)
r(4:6)=0.d0
r(7)=1.d0
x=r(4)+r(1)*r(7)
y=r(5)+r(2)*r(7)
z=r(6)+r(3)*r(7)
xp=r(4)+xp0*r(7)
yp=r(5)+yp0*r(7)
zp=r(6)+zp0*r(7)
lsfp=0.3-xp/2.d0-yp/4.d0-zp/4.d0
call lsf_reg (xp,yp,zp,lsfp,x,y,z,xn,yn,zn)
write (*,'("from",f,f,f)') x,y,z
write (*,'("  to",f,f,f)') xn,yn,zn
write (*,'("norm",f,f,f)') x-xn,y-yn,z-zn
enddo

end program lsf_reg_test

!-------------------------

subroutine lsf_reg (xp,yp,zp,lsfp,x,y,z,xn,yn,zn)

! this routine takes the values of the level set function lsfp(8) known at the 8 nodes of a
! cube (or element) of coordinates xp(8),yp(8),zp(8) and relocates a point of coordinates
! x,y,z that is located in the vicinity of the surface defined by the zero value of the level
! set function by projecting it onto the surface (lsfp=0)
! the new location of the point is stored in xn,yn,zn

! this is accomplished by assuming that the normal to the surface (lsfp=0) is correctly
! approximated by the vector gradient of lsfp calculated at the original point x,y,z
! the algorithm is applied repetitively until the distance to the surface is within a set
! tolerance (here one millionth of the size of the cube)

implicit none

double precision xp(8),yp(8),zp(8),lsfp(8)
double precision x,y,z
double precision xn,yn,zn
double precision r,s,t
double precision h(8),dhdr(8),dhds(8),dhdt(8)
double precision nr,ns,nt
double precision phi

r=(x-xp(1))/(xp(8)-xp(1))*2.d0-1.d0
s=(y-yp(1))/(yp(8)-yp(1))*2.d0-1.d0
t=(z-zp(1))/(zp(8)-zp(1))*2.d0-1.d0

1 continue

h(1)=(1.d0-r)*(1.d0-s)*(1.d0-t)/8.d0
h(2)=(1.d0+r)*(1.d0-s)*(1.d0-t)/8.d0
h(3)=(1.d0-r)*(1.d0+s)*(1.d0-t)/8.d0
h(4)=(1.d0+r)*(1.d0+s)*(1.d0-t)/8.d0
h(5)=(1.d0-r)*(1.d0-s)*(1.d0+t)/8.d0
h(6)=(1.d0+r)*(1.d0-s)*(1.d0+t)/8.d0
h(7)=(1.d0-r)*(1.d0+s)*(1.d0+t)/8.d0
h(8)=(1.d0+r)*(1.d0+s)*(1.d0+t)/8.d0

dhdr(1)=-(1.d0-s)*(1.d0-t)/8.d0
dhdr(2)=(1.d0-s)*(1.d0-t)/8.d0
dhdr(3)=-(1.d0+s)*(1.d0-t)/8.d0
dhdr(4)=(1.d0+s)*(1.d0-t)/8.d0
dhdr(5)=-(1.d0-s)*(1.d0+t)/8.d0
dhdr(6)=(1.d0-s)*(1.d0+t)/8.d0
dhdr(7)=-(1.d0+s)*(1.d0+t)/8.d0
dhdr(8)=(1.d0+s)*(1.d0+t)/8.d0

dhds(1)=-(1.d0-r)*(1.d0-t)/8.d0
dhds(2)=-(1.d0+r)*(1.d0-t)/8.d0
dhds(3)=(1.d0-r)*(1.d0-t)/8.d0
dhds(4)=(1.d0+r)*(1.d0-t)/8.d0
dhds(5)=-(1.d0-r)*(1.d0+t)/8.d0
dhds(6)=-(1.d0+r)*(1.d0+t)/8.d0
dhds(7)=(1.d0-r)*(1.d0+t)/8.d0
dhds(8)=(1.d0+r)*(1.d0+t)/8.d0

dhdt(1)=-(1.d0-r)*(1.d0-s)/8.d0
dhdt(2)=-(1.d0+r)*(1.d0-s)/8.d0
dhdt(3)=-(1.d0-r)*(1.d0+s)/8.d0
dhdt(4)=-(1.d0+r)*(1.d0+s)/8.d0
dhdt(5)=(1.d0-r)*(1.d0-s)/8.d0
dhdt(6)=(1.d0+r)*(1.d0-s)/8.d0
dhdt(7)=(1.d0-r)*(1.d0+s)/8.d0
dhdt(8)=(1.d0+r)*(1.d0+s)/8.d0

nr=sum(dhdr*lsfp)
ns=sum(dhds*lsfp)
nt=sum(dhdt*lsfp)

phi=sum(h*lsfp)

r=r-nr*phi
s=s-ns*phi
t=t-nt*phi

if (abs(phi).gt.1.d-6) goto 1

xn=xp(1)+(r+1.d0)/2.d0*(xp(8)-xp(1))
yn=yp(1)+(s+1.d0)/2.d0*(yp(8)-yp(1))
zn=zp(1)+(t+1.d0)/2.d0*(zp(8)-zp(1))

return
end
