线性规划(UOJ179）

        昨天按照算法导论里写了一个单纯形的线性规划，自动驾驶motion planning里面很多问题是喂给求解器的，所以想写一个最优化算法加深一下感受。在uoj179里面测试第39组extra的数据难以通过，感觉是由于精度问题。

​
#include <cstdio>
#include <algorithm>
#include <cassert>
using namespace std;
typedef long long ll;

const double inf = 1e15;
int n, m, t;
const int N = 25;
const double eps = 1e-8;
double c[N], b[N], a[N][N], initc[N];int idx[N<<1];

double f,initf;
double ans[N];
//l [0-m], e[0-n]
bool initSimplex();
void initpivot();
inline int sgn(double  x)
{
    return (-eps<=x&&x<=eps)?0:(x>0?1:-1);
}

void pivot(int l, int e)
{
    swap(idx[l], idx[m + e]);
    b[l] /= a[l][e];
    double tmp = a[l][e];
    a[l][e] = 1;
    for(int j = 0;  j < n; j++)
    {
        a[l][j] /= tmp;
    }
    for(int i = 0; i < m; i++)
    {
        if(abs(a[i][e]) < eps)
            continue;
        if(i != l)
        {
            b[i] -= a[i][e] * b[l];
        
            for(int j = 0;  j < n; j++)
            {
                if(j != e)
                    a[i][j] -= a[i][e] * a[l][j];
            }
            a[i][e] *= -a[l][e];
        }
    }
// target = f - sigma (c[i] * x[i])
    f -= c[e] * b[l];
    //initf -= c[e] * b[l];
    for(int j = 0; j < n; j++)
    {
        if(j != e)
            c[j] -= c[e] * a[l][j];
    }
    c[e] *= -a[l][e];
}


int Simplex()
{
    if(!initSimplex())
    {
        printf("Infeasible\n");
        return 1;
    }
    //f = 0;
    int cnt = 0;
    while (true)
    {
        /* code */
        int e = -1;
        double tmp = -100 * eps;
        for(int j = 0; j < n; j++)
        {
            if(c[j] <  tmp)
            {
                e = j;
                tmp = c[j];
                if(rand() % 3 == 2)
                    break;
            }
        }
        if(e == -1)
            break;
        double upb = inf;
        int l = -1;
        for(int i = 0; i < m; i++)
        {
            if(a[i][e] < eps)
                continue;
            double tmp = b[i] / a[i][e];
            if(tmp < upb)
            {
                l = i;
                upb = tmp;
            }
        }
        if(-1 == l)
        {
            cnt++;
            if(cnt == 10)
            {
                printf("Unbounded\n");
                return 2;     
            }
            continue;
        }
        pivot(l, e);
    }
    return 0;
    
}

void initpivot(int l,int e)
{
    swap(idx[l], idx[m + e]);
    
    b[l] /= a[l][e];
    double tmp = a[l][e];
    a[l][e] = 1;
    for(int j = 0;  j <= n; j++)
    {
        a[l][j] /= tmp;
    }
    for(int i = 0; i < m; i++)
    {
        if(abs(a[i][e]) < eps)
            continue;
        if(i != l)
        {
            b[i] -= a[i][e] * b[l];
        
            for(int j = 0;  j <= n; j++)
            {
                if(j != e)
                    a[i][j] -= a[i][e] * a[l][j];
            }
            a[i][e] *= -a[l][e];
        }
    }
// target = f - sigma (c[i] * x[i])
    f -= c[e] * b[l];
    initf -= initc[e] * b[l];
    for(int j = 0; j <= n; j++)
    {
        if(j != e)
        {
            c[j] -= c[e] * a[l][j];
            initc[j] -= initc[e] * a[l][j];
        }
    }
    initc[e] *= -a[l][e];
    c[e] *= -a[l][e];
}

bool initSimplex()
{
    int l = -1;
    double bmin = -eps;
    f = 0;
    for (int i = 0; i < m; i++)
    {
        if (b[i] < bmin)
        {
            l = i;
            bmin = b[i];
        }
    }
    if (l == -1)
        return true;
    for (int i = 0; i < n; i++)
        initc[i] = 0;
    
    for(int i = 0; i < m; i++)
        a[i][n] = -1;

    initc[n] = 1;
    c[n] = 0;
    initf = 0;
    idx[n + m] = n + m;
    initpivot(l, n);
    while (true)
    {
        int e = -1;
        double tmp = -100 * eps;
        for (int j = 0; j <= n; j++)
        {
            if (initc[j] < tmp)
            {
                e = j;
                tmp = initc[j]; 
                //break;
            }
        }
        if (e == -1)
            break;
        double upb = inf;
        int l = -1;
        for (int i = 0; i < m; i++)
        {
            if (a[i][e] < eps)
                continue;
            double tmp = b[i] / a[i][e];
            if (tmp < upb)
            {
                l = i;
                upb = tmp;
            }
        }
        if(l == -1)
            return false;
        initpivot(l, e);
    }
    if(initf > -eps)
    {
        for(int i = 0; i < m + n; i++)
        {
            if(idx[i] == n + m)
            {
                int ti = i;
                if(i < m)
                {
                    for(int j = 0; j <= n; j++)
                        if(abs(a[i][j]) > eps)
                        {   
                            initpivot(i, j);
                            ti = j + m;
                            break;
                        }
                }
                if(ti >= m)
                {
                    swap(c[ti - m], c[n]);
                    swap(idx[ti], idx[m + n]);
                    for(int j = 0; j < m; j++)
                    {
                        swap(a[j][ti - m], a[j][n]);  
                    }
                }
                break;
            }
            
        }
        return true;
    }
    return false;
}
int main()
{
    //printf("program start\n");
    srand(317);
    scanf("%d%d%d", &n, &m, &t);
    for(int i = 0; i < n; i++)
    {
        f = 0;
        scanf("%lf", c + i);
        c[i] = -c[i];
    }
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < n; j++)
            scanf("%lf", &a[i][j]);
        scanf("%lf", &b[i]);
    }
    for(int i = 0; i < n; i++)
    {
        idx[i + m] = i;
    }

    for(int i = 0; i < m; i++)
    {
        idx[i] = i + n;
    }

    if(0 == Simplex())
    {
        printf("%.8f\n", f);
        if(1 == t)
        {
            for(int i = 0; i < n; i++)
                ans[i] = 0;
            for(int i = 0; i < m; i++)
            {
                if(idx[i] < n)
                    ans[idx[i]] = b[i]; 
            }
            for(int i = 0; i < n; i++)
            {
                printf("%.8f%c", ans[i], (i == n-1)?'\n':' ');
            }
        }
    }
}

​

其实也想从中看看能否发现某些高维空间中的奥秘，但是感觉没有得到这方面的收获。UOJ179如果有数据的大佬可以私信我。