import sympy as sp
from sympy.interactive import printing
printing.init_printing(use_latex=True)
P, C, t0, t1, t2 = sp.symbols('P C t0 t1 t2')
X = sp.Matrix([[C, 0], [C, P]])
n = X.inv() * sp.Matrix([[t1-t0], [t2-t0]])
roots = sp.solve( n.norm()-1, t0 )
roots[1]
C = 1.0
P = 2.0
def calc_t0(t1, t2):
D = 1.0 - ((t1-t2) / P)**2
if D < 0.0 or t1 < t2:
return min(t1 + C, t2 + sqrt(C*C + P*P))
return t1 + C*sqrt(D)
h, w = 50, 100
a = np.zeros((h, w), np.float64)
a[:] = 1000
a[25,0] = 0
for j in xrange(1,w):
for i in xrange(1,h-1):
a[i, j] = min(calc_t0(a[i, j-1], a[i-1, j-1]), a[i, j])
a[i, j] = min(calc_t0(a[i, j-1], a[i+1, j-1]), a[i, j])
imshow(a, vmax=80)
AxesImage(60,40;372x248)
v0, v1, v2 = sp.symbols('v0, v1, v2')