Memory optimzation for constructPath
Added units for lengths