Sign in to follow this  
CodingDan

Problems with LCP

Recommended Posts

Hi Guys! 
I have a problem. I am taking part in developing a small game. My job is to program an LCP Solver for Collision response, which I did.
But I don't know if it works like it should. With some examples, it does work, but when I set up an example by myself, which is extremly much work, I don't get the results I desire. Because my program worked before, I think the failing is due to the data. Therefore I was hoping if you could give me realistic data as to verify that my program works. I'll also send my code to you because I don't know how to add it. I'd be extremly thankful for advise.

Share this post


Link to post
Share on other sites
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <vector>
#include <string>
#include <sstream>
#include <cstring>
#include <cmath>
#include <algorithm>

using namespace std;

//Variablen
vector<double> q;
vector<double> epsilon;
vector< vector<double> > M;
vector< vector<double> > M2;
int n;

//Methoden
string ReadFromCin();
void MakeM2();
int GetNegativeQ();
void NormRow(int,int);
void UpdateRow(int,int,double,double);
void ShowM2();
int ChangeLine(int);
void UpdateMatrix(int,int);
void NormPivots(double*);
int ChangeVariable(int);
void Evaluate(double*);
int CheckQ();

int main()
{
    cout << "Geben sie q ein:"<<endl;   //Read Vektor
    stringstream ss;
    string s=ReadFromCin();

    ss <<s;

    n=0;
    while(ss.good())
    {
        double d1;
        ss >> d1;
        q.push_back(d1);
        n++;
    }

    for(int i=0;i<n;i++)
    {
        epsilon.push_back(pow(0.5,i+1));  //Perturbation-Method
    }

    cout << "Geben sie die Matrix ein"<<endl;  //Read Matrix
    for(int i=0;i<n;i++)
    {
        ss.str("");
        int m=0;
        s.clear();
        s=ReadFromCin();
        ss.str(s);
        vector<double> T;
        ss.clear();
        while(ss.good())
        {
            if(m==n) {cout<<"Dimensionsfehler"<<endl;exit(EXIT_FAILURE);}
            double d;
            ss>>d;
            T.push_back(d);
            m++;
        }
        M.push_back(T);
        T.clear();
    }

    MakeM2();                          //Build Matrix (like with Simplex-Method)
    ShowM2();
    if(CheckQ()==0) {cout<<"Triviale Loesung"<<endl;exit(1);}
    double Pivots[n];                  //Saves dictionary: One Basic variable in every line, Pivots saves Row-Number of Basic Variable
    int NotTaken[2*n];

    for(int i=0;i<n;i++) Pivots[i]=i;
    for(int i=0;i<2*n;i++) NotTaken[i]=1;

    //Lemke Phase 1
    int i=GetNegativeQ();
    int k=Pivots[i];
    NotTaken[k]=0;
    k=ChangeVariable(k);
    Pivots[i]=2*n;
    UpdateMatrix(i,2*n);
    NormPivots(Pivots);

    //Lemke Phase 2
    for(;;)
    {
        if(NotTaken[k]==0) {cout<<"LCP nicht lösbar"<<endl;exit(1);}
        else NotTaken[k]=0;
        int j=ChangeLine(k);
        if(j==-1) {cout << "LCP nicht lösbar"<<endl;exit(1);}
        int m=Pivots[j];
        Pivots[j]=k;
        UpdateMatrix(j,k);
        NormPivots(Pivots);
        if(m==2*n) break;
        else k=ChangeVariable(m);
    }
    Evaluate(Pivots);
    system("PAUSE");

}

void ShowM2()
{
    for(vector< vector<double> >::iterator i=M2.begin();i<M2.end();i++)
    {
        vector<double> A =*i;
        for(vector<double>::iterator j=A.begin();j<A.end();j++)
        {
            cout << *j << " \t";
        }
        cout << endl;
    }
}
void MakeM2()
{
    for(int i=0;i<n;i++)
    {
        vector<double> T;
        for(int j=0;j<(2*n+3);j++)
        {
            double d;
            if(j<n)
            {
                if(j==i) d=1;
                else d=0;
            }

            else if(j<2*n)
            {
                d=-M.at(i).at(j-n);
            }
            else if(j==2*n) d=-1;
            else if(j==2*n+1) d=q.at(i);
            else d=epsilon.at(i);

            T.push_back(d);
        }
        M2.push_back(T);
        T.clear();
    }

}

string ReadFromCin()
{
    string s;
    cin.clear();
    fflush(stdin);
    char c;
    while(cin.good() && (c=cin.get()) != '\n')
    {
        s.push_back(c);
    }
    while(*(s.begin())==' ') s.erase(s.begin());
    while(*(s.end()-1)==' ') s.erase(s.end()-1);
    return s;
}

int GetNegativeQ()
{
    int k=-1;
    double value=0;
    for(int i=0;i<M2.size();i++)
    {
        if(M2.at(i).at(2*n+1)==value && value != 0)
        {
            if(M2.at(i).at(2*n+2)<M2.at(k).at(2*n+2) && k != -1)
            {
                value=M2.at(i).at(2*n+1);
                k=i;
            }
        }
        else if(M2.at(i).at(2*n+1)<value)
        {
            value=M2.at(i).at(2*n+1);
            k=i;
        }
    }
    return k;
}


void NormRow(int i, int j)
{
    double d=M2.at(i).at(j);

    for(int k=0;k<M2.at(i).size();k++)
    {
        M2.at(i).at(k)=(M2.at(i).at(k))/d;
    }
}

void UpdateRow(int k,int i,double d1,double d2)
{
    for(int j=0;j<(2*n+3);j++)
    {
        M2.at(i).at(j)=M2.at(i).at(j)-(d2/d1)*M2.at(k).at(j);
    }
}
void UpdateMatrix(int i,int j)
{
    double d1=M2.at(i).at(j);
    for(int m=0;m<n;m++)
    {
        if(m==i) continue;
        double d2=M2.at(m).at(j);
        UpdateRow(i,m,d1,d2);

    }
}

void NormPivots(double* p)
{
    for(int i=0;i<n;i++,p++)
    {
        int j=(*p);
        NormRow(i,j);
    }
}


int ChangeVariable(int m)
{
    if(m<n) return (m+n);
    else return (m-n);
}

int ChangeLine(int k)
{
    double value=INFINITY;
    int j=-1;
    for(int i=0;i<n;i++)
    {
        if(M2.at(i).at(k)==0) continue;
        double temp=M2.at(i).at(2*n+1)/M2.at(i).at(k);
        if(temp<0) continue;
        if(temp<value)
        {
            j=i;
            value=temp;
        }
        else if (temp==value && j!=-1)
        {
            if(M2.at(i).at(2*n+2)<M2.at(j).at(2*n+2))
            {
                j=i;
                value=temp;
            }
        }
    }
    return j;
}
void Evaluate(double* P)
{
    cout << "Vektor w: "<<endl;
    cout << "(";
    for (int i=0;i<n;i++)
    {
        double* pointer=find(P,P+n,i);
        if(pointer != P+n) cout << M2.at(i).at(2*n+1)<<" ";
        else cout << "0 ";
        if(i!=n-1) cout << ", ";
    }
    cout << ")"<<endl;
    cout << "Vektor z: "<<endl;
    cout << "(";
    for(int i=n;i<2*n;i++)
    {
        double* pointer=find(P,P+n,i);
        if(pointer != P+n) cout << M2.at(pointer-P).at(2*n+1)<<" ";
        else cout << "0 ";
        if(i!= 2*n-1) cout << ", ";
    }
    cout << ")"<<endl;
}

int CheckQ()
{
    for(vector<double>::iterator i=q.begin();i<q.end();i++)
    {
        if(*i<0) return 1;
    }
    return 0;
}



Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this