fast_sweeping_method.py

import numpy as np
import pylab as plt
from sys import float_info
from math import sqrt
from skfmm import distance

N=11
phi = np.ones((N,N))
phi[5,5] = -1

working = np.ones_like(phi)*float_info.max

frozen = np.zeros_like(phi,dtype=bool)

# here we manually calculate the initial frozen set
working[5,5] = -1/2.0/np.sqrt(2.0)
working[4,5] = 0.5
working[6,5] = 0.5
working[5,4] = 0.5
working[5,6] = 0.5

frozen[5,5] = True
frozen[4,5] = True
frozen[6,5] = True
frozen[5,4] = True
frozen[5,6] = True

def bound(i,j):
    if i==-1 or j==-1 or i==N or j==N: return float_info.max
    else: return working[i,j]

def process_point(i,j):
    fijh = 1
    if frozen[i,j]: return
    uhij = working[i,j]
    a = uhxmin = min(bound(i-1,j),bound(i+1,j))
    b = uhymin = min(bound(i,j-1),bound(i,j+1))
    if abs(a-b) >= fijh:
        ubar = min(a,b) + fijh
    else:
        ubar = (a+b+sqrt(2*fijh**2-(a-b)**2))/2.0
    if ubar < uhij:
        working[i,j] = ubar

forward = range(0,N)
backward = range(N-1,-1,-1)

for i in forward:
    for j in forward:
        process_point(i,j)

for i in backward:
    for j in forward:
        process_point(i,j)

for i in backward:
    for j in backward:
        process_point(i,j)

for i in forward:
    for j in backward:
        process_point(i,j)

print working
print
print distance(phi,order=2)