Fix most of path, try to avoid rounding error, need to add rotation later