calculateSwapCost

/**
    Calculating the cost of swapping 2 nodes without having to recalculate the entire path.
    Saves nano seconds!
*/

public int clampIndex(int index)
{
    if(index < 0)
        return size() - 1;
    else if(index > (size() - 1))
        return 0;
    else
        return index;
}

public double getNodeToFromDistance(BlockNode node, int index)
{
    return node.distanceTo(     get(    clampIndex(index - 1))) +    // previous node
            node.distanceTo(    get(    clampIndex(index + 1)));     // next node
}

public double calculateSwapCost(int firstIndex, int secondIndex)
{
    BlockNode firstNode = group.get(firstIndex);
    BlockNode secondNode = group.get(secondIndex);

    double cost = getTotalDistance();
    // enjoy my ugly code
    if(firstIndex != (secondIndex - 1) && firstIndex != (secondIndex + 1)) {
        cost -= (getNodeToFromDistance(firstNode, firstIndex) + getNodeToFromDistance(secondNode, secondIndex));
        cost += (getNodeToFromDistance(firstNode, secondIndex) + getNodeToFromDistance(secondNode, firstIndex));
    } else {
        if(firstIndex > secondIndex) {
            cost -= (getNodeToFromDistance(secondNode, secondIndex) + firstNode.distanceTo(get(clampIndex(firstIndex + 1))));
            cost += (firstNode.distanceTo(get(clampIndex(secondIndex - 1))) + firstNode.distanceTo(get(clampIndex(secondIndex))) + secondNode.distanceTo(get(clampIndex(firstIndex + 1))));
        } else {
            cost -= (getNodeToFromDistance(firstNode, firstIndex) + secondNode.distanceTo(get(clampIndex(secondIndex + 1))));
            cost += (secondNode.distanceTo(get(clampIndex(firstIndex - 1))) + secondNode.distanceTo(get(clampIndex(firstIndex))) + firstNode.distanceTo(get(clampIndex(secondIndex + 1))));
        }
    }
    return cost;
}