Distance between two points (x0,y0,z0) and (x1,y1,z1):
dist = math.sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0) + (z1-z0)*(z1-z0))
