Fixed bugs in path calculations

Fixed bugs in path calculations

@mcm69 or @shamansir please review and test whether it works.