Thanks for sharing code. It is faster one than what I used for my research.
One problem I found: when I used constant speed, shortestpath code will crash. Also, shortestpath for 3D case (just like 2D since constant value in y direction) is not consistent with 2D case. I can send your my results if you want to take a look at them.