Can you help me implement my Clark and Wright algorithm?

Question

I am trying to implement the Clark and Wright algorithm to create an initial VRP solution. It seems to work correctly, but for some reason, the quality of the solution I get is not expected. See the description of the algorithm here.

Here is my code to calculate the savings element:

private void computeSavingsElements() {
    for(int i = 0; i<vrp.getDimension(); i++) { 
        for(int j = 0; j <  i; j++) {           
                double savingValue =  vrp.distance(i, 0) + vrp.distance(0, j) - lamda * vrp.distance(i, j);
                SavingsElement savingElement = new SavingsElement (i,j, savingValue);
                savingsElements.add(savingElement);                                                 
        }
    }
    Collections.sort(savingsElements); // sort in ascending order
    Collections.reverse(savingsElements); // but we need descending order

}

Solution building method:

private void constructSolution() {
    List<VRPNode> nodes = this.vrp.getNodesList();
    VRPNode depot = this.vrp.getDepot();
    double vehicleCapacity = this.vrp.getVehicleCapacity();


    VRPSolution solution = new VRPSolution(vehicleCapacity, depot);

    /*
     * In the initial solution, each vehicle serves exactly one customer
     */
    for (VRPNode customer:nodes) {
        if (customer.getId()!=0) { // if not depot
            VRPRoute route = new VRPRoute(vehicleCapacity, depot);
            route.addCustomer(customer);
            solution.addRoute(route);
            route = null; // eliminate obsolete reference to free resources
        }
    }   

    //System.out.println("INITIAL SOLUTION: \n"+solution.toString());

    int mergesCounter=0;
    for (SavingsElement savingElement : this.savingsElements) {
        if (savingElement.getSavingValue() > 0) { // If serving customers consecutively in a route is profitable

            VRPNode i = this.vrp.getNode(savingElement.getNodeId1());
            VRPNode j = this.vrp.getNode(savingElement.getNodeId2());

            VRPRoute route1 = solution.routeWhereTheCustomerIsTheLastOne(i);
            VRPRoute route2 = solution.routeWhereTheCustomerIsTheFirstOne(j);

            if ((route1!=null) & (route2!=null)) {
                if (route1.getDemand() + route2.getDemand() <= this.vrp.getVehicleCapacity()) { // if merge is feasible
                    /*
                     * Merge the two routes
                     */
                    solution.mergeRoutes(route1, route2);
                    mergesCounter++;
                }
            }


        }

    }   
    //System.out.println("\n\nAfter "+mergesCounter+" Merges"+"\n"+solution.toString());
    this.solutionConstructed = solution;

}

And for the route merges:

public void mergeRoutes(VRPRoute a, VRPRoute b) {
    /*
     * Provided that feasibility check has already been performed
     */
    List<VRPNode> customersFromRouteA = new LinkedList<VRPNode>(a.getCustomersInRoute());
    List<VRPNode> customersFromRouteB = new LinkedList<VRPNode>(b.getCustomersInRoute());

    /*
     * Remove the old routes
     */
    solutionRoutes.remove(a);
    solutionRoutes.remove(b);

    /*
     * Construct a new merged route
     */
    VRPRoute mergedRoute = new VRPRoute(vehicleCapacity,depot);

    /*
     * The new route has to serve all the customers 
     * both from route a and b
     */
    for (VRPNode customerFromA:  customersFromRouteA) {
        mergedRoute.addCustomer(customerFromA);
    }

    for (VRPNode customerFromB:  customersFromRouteB) {
        mergedRoute.addCustomer(customerFromB);
    }

    addRoute(mergedRoute);

    evaluateSolutionCost();
}

It seems to correctly calculate the savings and combine the route, as it should be. But the cost of the constructed solution is too high. For example, in this case, I get 1220, and it should be 820.

Answer 1

, j i, j < . - , computeSavingsElements j - (vrp.getDimension()).

, , , , . routeWhereTheCustomerIsTheLastOne?