Jump to content
  • Advertisement
Sign in to follow this  
CodingDan

Problems with LCP

This topic is 1561 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

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
Advertisement
#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
Sign in to follow this  

  • Advertisement
×

Important Information

By using GameDev.net, you agree to our community Guidelines, Terms of Use, and Privacy Policy.

GameDev.net is your game development community. Create an account for your GameDev Portfolio and participate in the largest developer community in the games industry.

Sign me up!