Simplex.cpp

#include "Simplex.h"
 
#include <vector>
#include <cstdio>
#include <algorithm>
 
#include "Context.h"
 
namespace
{
  struct LinearProblem
  {
    int nonbasicVars;
    int basicVars;
    int stride;
    int* basicVar;
    int* nonbasicVar;
    double* dictionary;
    double* pivotColumnTmp;
    double* objectiveTmp;
    bool printSteps;
 
    void initializeDictionary(const double* objective,
                              const double* restrictions);
 
    void prettyPrint(const char* what);
 
    void pivot(int j, int i);
 
    bool isSolutionFeasible();
 
    int basicVarIndex(int variable);
 
    int nonbasicVarIndex(int variable);
 
    void emitSolution(double* solution);
 
    int findPivotColumn();
 
    int findPivotRow(int pivotColumn);
 
    int findPivotColumnBland();
 
    int findPivotRowBland(int pivotColumn);
 
    Cogs::Core::SimplexResult search(int maxIterations);
 
    Cogs::Core::SimplexResult maximize(double* solution, const int maxIterations);
  };
 
  int LinearProblem::basicVarIndex(int variable)
  {
    for (int k = 0; k < basicVars; k++) {
      if (basicVar[k] == variable) {
        return k;
      }
    }
    return -1;
  }
 
  int LinearProblem::nonbasicVarIndex(int variable)
  {
    for (int l = 0; l < nonbasicVars; l++) {
      if (nonbasicVar[l] == variable) {
        return l;
      }
    }
    return -1;
  }
 
  void LinearProblem::initializeDictionary(const double* objective,
                                           const double* restrictions)
  {
    for (int j = 0; j < basicVars; j++) {
      dictionary[j*stride + 0] = restrictions[j*(nonbasicVars + 1) + nonbasicVars];
      for (int i = 0; i < nonbasicVars; i++) {
        dictionary[j*stride + i + 1] = -restrictions[j*(nonbasicVars + 1) + i];
      }
    }
    dictionary[basicVars*stride + 0] = 0.0;
    for (int i = 0; i < nonbasicVars; i++) {
      dictionary[basicVars*stride + i + 1] = objective[i];
    }
 
    for (int i = 0; i < nonbasicVars; i++) {
      nonbasicVar[i] = i + 1;
    }
    for (int i = 0; i < basicVars; i++) {
      basicVar[i] = nonbasicVars + i + 1;
    }
  }
 
  void LinearProblem::prettyPrint(const char* what)
  {
    printf("%s:\n", what);
 
    std::vector<std::pair<int, int>> colPerm(nonbasicVars + 1);
    colPerm[0] = std::make_pair(0, 0);
    for (int i = 0; i < nonbasicVars; i++) {
      colPerm[i + 1] = std::make_pair(nonbasicVar[i], i + 1);
    }
    std::sort(colPerm.begin() + 1, colPerm.end(), [](auto & a, auto & b) -> bool {return a.first < b.first; });
 
    std::vector<std::pair<int, int>> rowPerm(basicVars);
    for (int i = 0; i < basicVars; i++) {
      rowPerm[i] = std::make_pair(basicVar[i], i);
    }
    std::sort(rowPerm.begin(), rowPerm.end(), [](auto & a, auto & b) -> bool {return a.first < b.first; });
    rowPerm.push_back(std::make_pair(0, basicVars));
 
    printf("           ");
    for (int i = 0; i < nonbasicVars; i++) {
      int l = colPerm[i + 1].second - 1;
      printf("  x%d  ", nonbasicVar[l]);
    }
    printf("\n");
    for (int j = 0; j <= basicVars; j++) {
      int k = rowPerm[j].second;
      if (j < basicVars) {
        printf("x%d = ", basicVar[k]);
      }
      else {
        printf(" z = ");
      }
      for (int i = 0; i < nonbasicVars + 1; i++) {
        int l = colPerm[i].second;
        printf("%5.2f ", dictionary[k*stride + l]);
      }
      printf("\n");
    }
  }
 
  void LinearProblem::pivot(int j, int i)
  {
    // The pivot column will be a mix between the column
    // of the entering and leaving variable.
 
    // Store pivot column, set all entries except the one on pivot row to zero
    // such that these entries will accumulate the contents of the leaving
    // variable column. The pivot row entry will remain that of the entering
    // column such that its contents will be spread out into the leaving column's
    // entries
    for (int k = 0; k <= basicVars; k++) {
      pivotColumnTmp[k] = dictionary[k*stride + i];
      if (k != j) {
        dictionary[k*stride + i] = 0.0;
      }
    }
 
    // Conceptually make entries on the pivot column to zero by subtracting an
    // multiple of the pivot row. 
    for (int k = 0; k <= basicVars; k++) {
      if (k == j) continue;
      for (int l = 0; l < nonbasicVars + 1; l++) {
        if (l == i) continue;
        dictionary[k*stride + l] -= (pivotColumnTmp[k] * dictionary[j*stride + l]) / pivotColumnTmp[j];
      }
      // Existing value on the pivot column is in principle minus one (which it
      // isn't since we retain that value of the entering variable in the pivot column.
      dictionary[k*stride + i] += pivotColumnTmp[k] / pivotColumnTmp[j];
    }
    for (int l = 0; l < nonbasicVars + 1; l++) {
      if (l == i) continue;
      // Update pivot row. This is just scaled such that the pivot element becomes -1.
      dictionary[j*stride + l] = -dictionary[j*stride + l] / pivotColumnTmp[j];
    }
    // And finally, set the pivot element to the value of the leaving variable.
    dictionary[j*stride + i] = 1.0 / pivotColumnTmp[j];
 
    // And update book-keeping arrays.
    std::swap(basicVar[j], nonbasicVar[i - 1]);
  }
 
  bool LinearProblem::isSolutionFeasible()
  {
    for (int k = 0; k < basicVars; k++) {
      if (dictionary[k*stride] < 0.0) {
        if (printSteps) prettyPrint("Solution is not feasible");
        return false;
      }
    }
    return true;
  }
 
  int LinearProblem::findPivotColumn()
  {
    // Currently greedy. We might have change to Bland's rule.
 
    int column = -1;
    double best_val = std::numeric_limits<double>::epsilon();
    for (int k = 0; k < nonbasicVars; k++) {
 
      // Weight for steepest edge column rule
      auto weightDenominator = 1.0;
 
      bool hasNegativeCoefficient = false;
      for (int j = 0; j < basicVars; j++) {
        const auto nonBasicCoefficent = dictionary[j*stride + k + 1];
        weightDenominator += nonBasicCoefficent*nonBasicCoefficent;
        hasNegativeCoefficient = hasNegativeCoefficient || (nonBasicCoefficent < -std::numeric_limits<float>::epsilon());
      }
 
      if (hasNegativeCoefficient) {
        auto val = dictionary[basicVars*stride + k + 1] / weightDenominator;
        if (best_val < val) {
          column = k + 1;
          best_val = val;
        }
      }
    }
 
    // Returns -1 if solution is optimal.
    return column;
  }
 
 
  int LinearProblem::findPivotRow(int pivotColumn)
  {
    int pivotRow = -1;
    double worstRatio = std::numeric_limits<double>::max();
 
    for (int k = 0; k < basicVars; k++) {
 
      const auto basicVal = dictionary[k*stride + 0];
      if (basicVal < -std::numeric_limits<double>::epsilon()) {
        // We consider this a degeneracy.
        return -1;
      }
 
      const auto nonBasicVal = dictionary[k*stride + pivotColumn];
      if (nonBasicVal < -std::numeric_limits<double>::epsilon()) {
        const auto ratio = -basicVal / nonBasicVal;
        if (ratio < worstRatio) {
          pivotRow = k;
          worstRatio = ratio;
        }
      }
    }
 
    return pivotRow;
  }
 
  int LinearProblem::findPivotColumnBland()
  {
    int column = -1;
    int lowest = std::numeric_limits<int>::max();
    for (int l = 0; l < nonbasicVars; l++) {
 
      bool hasNegativeCoefficient = false;
      for (int j = 0; (j < basicVars) && !hasNegativeCoefficient; j++) {
        hasNegativeCoefficient = dictionary[j*stride + l + 1] < -std::numeric_limits<float>::epsilon();
      }
 
      if (hasNegativeCoefficient) {
        auto i = nonbasicVar[l];
        auto v = dictionary[basicVars*stride + l + 1];
        if ((0.0 < v) && (i < lowest)) {
          lowest = i;
          column = l + 1;
        }
      }
    }
    return column;
  }
 
  int LinearProblem::findPivotRowBland(int pivotColumn)
  {
    int row = -1;
    int lowest = std::numeric_limits<int>::max();
    double worstRatio = std::numeric_limits<double>::max();
 
    for (int k = 0; k < basicVars; k++) {
      auto j = basicVar[k];
 
      auto r = dictionary[k*stride + pivotColumn];
      if (r < -std::numeric_limits<double>::epsilon()) {
        auto v = dictionary[k*stride + 0];
        auto ratio = -v / r;
 
        if ((ratio < worstRatio) || ((ratio == worstRatio) && (j < lowest))) {
          worstRatio = ratio;
          lowest = j;
          row = k;
        }
      }
    }
    return row;
  }
 
 
  void LinearProblem::emitSolution(double* solution)
  {
    for (int i = 0; i < basicVars; i++) {
      auto k = basicVar[i] - 1;
      if (k < nonbasicVars) {
        solution[k] = dictionary[i*stride + 0];
      }
    }
    for (int i = 0; i < nonbasicVars; i++) {
      auto k = nonbasicVar[i] - 1;
      if (k < nonbasicVars) {
        solution[k] = 0.0;
      }
    }
  }
 
  Cogs::Core::SimplexResult LinearProblem::search(int maxIterations)
  {
    if(printSteps) prettyPrint("Started search");
    for (int it = 0; it < maxIterations; it++) {
      int pivotColumn = findPivotColumn();
      if (pivotColumn < 0) {
        return Cogs::Core::SimplexResult::OptimalSolutionFound;
      }
 
      int pivotRow = findPivotRow(pivotColumn);
      if (pivotRow < 0) {
        if (printSteps) printf("We're stalling, trying Bland's rule.\n");
 
        // We're stalling, use Bland's rule instead
        pivotColumn = findPivotColumnBland();
        if (pivotColumn < 0) {
          if (printSteps) printf("Failed to find column.\n");
          return Cogs::Core::SimplexResult::BlandsFailed;
        }
        pivotRow = findPivotRowBland(pivotColumn);
        if (pivotRow < 0) {
          if (printSteps) printf("Failed to find row.\n");
          return Cogs::Core::SimplexResult::BlandsFailed;
        }
      }
 
      pivot(pivotRow, pivotColumn);
      if (printSteps) prettyPrint("Applied pivot");
    }
    return Cogs::Core::SimplexResult::TooManyIterations;
  }
 
  Cogs::Core::SimplexResult LinearProblem::maximize(double* solution,
                                                    const int maxIterations)
  {
 
    if (isSolutionFeasible()) {
      if (printSteps) prettyPrint("Initial solution is feasible");
    }
    else {
      if (printSteps) prettyPrint("Initial solution is not feasible");
 
      // Store objective function
      for (int l = 0; l < nonbasicVars; l++) {
        objectiveTmp[l] = dictionary[basicVars*stride + l + 1];
      }
 
      // Extend dictionary with nonbasic variable x0 and add new objective function
      nonbasicVar[nonbasicVars] = 0;
      nonbasicVars++;
      for (int k = 0; k < basicVars; k++) {
        dictionary[k*stride + nonbasicVars] = 1.0;
      }
      dictionary[basicVars*stride + nonbasicVars] = -1.0;
      for (int l = 0; l < nonbasicVars; l++) {
        dictionary[basicVars*stride + l] = 0.0;
      }
 
      if(printSteps) prettyPrint("Extended problem to find feasible solution");
 
      // Find the 'most infeasible' variable that should leave the basis for x0
      int leaving = -1;
      double mostInfeasibleValue = std::numeric_limits<double>::max();
      for (int k = 0; k < basicVars; k++) {
        auto value = dictionary[k*stride + 0];
        if (value < mostInfeasibleValue) {
          leaving = k;
          mostInfeasibleValue = value;
        }
      }
      // And swap x0 into the basis
      pivot(leaving, nonbasicVars);
      if(printSteps) prettyPrint("Applied pivot");
 
      auto termination = search(maxIterations);
      if (termination != Cogs::Core::SimplexResult::OptimalSolutionFound) {
        return termination;
      }
 
      if (!isSolutionFeasible()) {
        return Cogs::Core::SimplexResult::NoFeasibleSolutions;
      }
 
      int x0_index = nonbasicVarIndex(0);
      if (x0_index < 0) {
        // x0 is in basis while we reached an optimum,
        // we conclude that problem is infeasible.
        if (printSteps) prettyPrint("x0=%d is in basis while we reached an optimum, infeasible solution?");
        return Cogs::Core::SimplexResult::NoFeasibleSolutions;
      }
 
      // Remove x0 from dictionary
      nonbasicVars--;
      for (int l = x0_index; l < nonbasicVars; l++) {
        nonbasicVar[l] = nonbasicVar[l + 1];
      }
      for (int k = 0; k < basicVars; k++) {
        for (int l = x0_index; l < nonbasicVars; l++) {
          dictionary[k*stride + l + 1] = dictionary[k*stride + l + 2];
        }
      }
      assert(nonbasicVarIndex(0) == -1);
      assert(basicVarIndex(0) == -1);
 
      // form original objective function in new dictionary
      dictionary[basicVars*stride + 0] = 0.0;
      for (int l = 0; l < nonbasicVars; l++) {
        auto i = nonbasicVar[l] - 1;
        dictionary[basicVars*stride + l + 1] = i < nonbasicVars ? objectiveTmp[i] : 0.0;
      }
 
      for (int k = 0; k < basicVars; k++) {
        auto j = basicVar[k] - 1;
        if (j < nonbasicVars) {
          auto s = objectiveTmp[j];
          for (int l = 0; l <= nonbasicVars; l++) {
            dictionary[basicVars*stride + l] += s*dictionary[k*stride + l];
          }
        }
      }
    }
 
 
    auto termination = search(maxIterations);
    if (termination == Cogs::Core::SimplexResult::OptimalSolutionFound) {
      emitSolution(solution);
    }
    return termination;
  }
 
}
 
 
size_t Cogs::Core::simplexScratchSize(const size_t variableCount,
                                      const size_t /*objectiveCount*/,
                                      const size_t restrictionCount)
{
  return
    sizeof(int)*(restrictionCount) +                            // basicVar
    sizeof(int)*(variableCount + 1) +                           // nonbasicVar
    sizeof(double)*(variableCount + 2)*(restrictionCount + 1) + // dictionary
    sizeof(double)*(restrictionCount + 1) +                     // pivotColumnTmp
    sizeof(double)*(variableCount) + 8;                             // objectiveTmp
}
 
Cogs::Core::SimplexResult Cogs::Core::simplexMaximize(Context* /*context*/,
                                                      uintptr_t scratch,
                                                      double* solutions,
                                                      const int variableCount,
                                                      const double* objectives,
                                                      const int objectiveCount,
                                                      const double* restrictions,
                                                      const int restrictionCount,
                                                      const int maxIterationCount,
                                                      const bool printSteps)
{
  //std::vector<int> basicVar(restrictionCount);
  //std::vector<int> nonbasicVar(variableCount+1);
  //std::vector<double> dictionary((variableCount + 2)*(restrictionCount + 1));
  //std::vector<double> pivotColumnTmp(restrictionCount+1);
  //std::vector<double> objectiveTmp(variableCount);
 
  LinearProblem problem;
  problem.nonbasicVars = variableCount;
  problem.basicVars = restrictionCount;
  problem.stride = variableCount + 2; // one for constant, and one if we must expand problem for initial solution.
 
        // align scratch to 8 byte boundary (double) by finding the first pointer larger or equal to scratch which is a multiple of 8 :
  scratch = (scratch + 7u) & ~uintptr_t(7); //add 7 and then set the last 3-bits to 0
  problem.dictionary = reinterpret_cast<double*>(scratch); scratch += sizeof(double)*(variableCount + 2)*(restrictionCount + 1); //dictionary.data();
  problem.pivotColumnTmp = reinterpret_cast<double*>(scratch); scratch += sizeof(double)*(restrictionCount + 1);  //pivotColumnTmp.data();
  problem.objectiveTmp = reinterpret_cast<double*>(scratch); scratch += sizeof(double)*(variableCount); // objectiveTmp.data();
  problem.basicVar = reinterpret_cast<int*>(scratch); scratch += sizeof(int)*(restrictionCount); //basicVar.data();
  problem.nonbasicVar = reinterpret_cast<int*>(scratch); scratch += sizeof(int)*(variableCount + 1); //nonbasicVar.data();
  problem.printSteps = printSteps;
 
 
  // Do it brute force for now, not recycling initial feasible solution.
  SimplexResult rv = SimplexResult::OptimalSolutionFound;
  for (int i = 0; i < objectiveCount && (rv == SimplexResult::OptimalSolutionFound); i++) {
    problem.initializeDictionary(objectives + i*variableCount,
                                 restrictions);
    rv = problem.maximize(solutions + i*variableCount,
                          maxIterationCount);
  }
  return rv;
}