ContactSolver.cpp

Go to the documentation of this file.

#include "ContactSolver.h"
#include "SystemSolver.h"

ContactSolver::ContactSolver(GlobalContactInfoVector* iniContacts){
   contacts=iniContacts;
   nContacts=0;
   nContactsMax=0;
   contactMatrix=NULL;
   contactMatrixTemp=NULL;
   contactVector=NULL;
   contactVectorTemp=NULL;
   contactForces=NULL;
   contactDForces=NULL;
   contactVelocities=NULL;
   contactSets=NULL;
   contactIndexC=NULL;
   contactIndexTemp=NULL;
   zeros=NULL;
   tempVector1=NULL;
   tempVector2=NULL;
   tempMatrix=NULL;
   //TODO: initialize nContactsMax and the contact matrix, vector

}

ContactSolver::~ContactSolver(){
   if(contactMatrix!=NULL){
      for (int i=0; i < nContactsMax; i++) {
         delete[] contactMatrix[i];
         delete[] contactMatrixTemp[i];
         delete[] tempMatrix[i];
      }
      delete[] contactMatrix;
      delete[] contactMatrixTemp;
      delete[] contactVector;
      delete[] contactVectorTemp;
      delete[] contactForces;
      delete[] contactDForces;
      delete[] contactVelocities;
      delete[] contactSets;
      delete[] contactIndexC;
      delete[] contactIndexTemp;
      delete[] zeros;
      delete[] tempVector1;
      delete[] tempVector2;
   }

}

void ContactSolver::init(){
   nContacts=contacts->size();

   /* Realloc memory for the contact matrix and vectors, if needed. */
   if(nContacts > nContactsMax){
      if(contactMatrix != NULL){
         {for(int i = 0; i < nContactsMax; i++){
            delete[] contactMatrix[i];
            delete[] contactMatrixTemp[i];
            delete[] tempMatrix[i];
         }}
         delete[] contactMatrix;
         delete[] contactMatrixTemp;
         delete[] contactVector;
         delete[] contactVectorTemp;
         delete[] contactForces;
         delete[] contactDForces;
         delete[] contactVelocities;
         delete[] contactSets;
         delete[] contactIndexC;
         delete[] contactIndexTemp;
         delete[] zeros;
         delete[] tempVector1;
         delete[] tempVector2;
      }
      contactMatrix=new real*[nContacts];
      contactMatrixTemp=new real*[nContacts];
      tempMatrix=new real*[nContacts];
      {for(int i = 0; i < nContacts; i++){
         contactMatrix[i]=new real[nContacts];
         contactMatrixTemp[i]=new real[nContacts];
         tempMatrix[i]=new real[nContacts];
      }}
      contactVector=new real[nContacts];
      contactVectorTemp=new real[nContacts];
      contactForces=new real[nContacts];
      contactDForces=new real[nContacts];
      contactVelocities=new real[nContacts];
      contactSets=new ContactSet[nContacts];
      contactIndexC=new int[nContacts];
      contactIndexTemp=new int[nContacts];
      zeros=new int[nContacts];
      tempVector1=new real[nContacts];
      tempVector2=new real[nContacts];
      nContactsMax=nContacts;
   }

   /* Reset to zero the contact matrix and vectors. */
   for (int i = 0; i < nContacts; i++) {
      for(int j = 0; j < nContacts; j++) {
         contactMatrix[i][j]=0.0f;
        }
      contactVector[i]=0.0f;
      contactForces[i]=0.0f;
   }
}

real ContactSolver::maxStep(int d, int *sIndex) {
   static const real zeroMinus = -1e-8;
    //the maxstep part
    //s=infinity
    real s = 1e10;
    *sIndex = -1;
    //if delta a_d>0
    if(contactVectorTemp[d] > 0) {
        *sIndex = d;
        //s=-a_d/delta a_d
        s = -contactVelocities[d]/contactVectorTemp[d];
    }
    for(int i = 0; i < d; i++){
        if(contactSets[i] == contactSetC){
            //i (- C
            //if delta f_i<0
            if(contactDForces[i]<zeroMinus){
                //assert(contactForces[i] > 0);
                //s'=-f_i/delta f_i
                real sp = -contactForces[i]/contactDForces[i];
                if (sp < s) {
                    s = sp;
                    *sIndex = i;
                }
            }
        } else {
            //i (- NC
            //if delta a_i<0
            if(contactVectorTemp[i]<zeroMinus){
                //assert(contactVelocities[i] > 0);
                //s'=-a_i/delta a_i
                real sp = -contactVelocities[i]/contactVectorTemp[i];
                if (sp < s) {
                    s = sp;
                    *sIndex = i;
                }
            }
        }
    }
    //end of maxstep part
    assert(s<1e5 && *sIndex > -1);
    return s;
}

bool ContactSolver::driveToZero(int d) {
    if (contactVelocities[d] >= 0) {
        contactSets[d] = contactSetNC;
        return true;
    }

    //contactVelocities[d] < 0)
    int cycleCount = 0;
    int sIndex;
    do {
        ++cycleCount;
        if (cycleCount > nContacts * 10) {
            return false;
        }

        //the fdirection part
        //initialize
        //delta f=0
        {for (int i = 0; i < d; i++) {
            contactDForces[i] = 0;
        }}
        //delta f_d=1
        contactDForces[d] = 1;
        
        //index the contacts in the C set
        int nC = 0;
        {for (int i=0; i < d; i++) {
            if (contactSets[i] == contactSetC) {
                //assert(contactVelocities[i] <= 0);
                //assert(contactForces[i] >= 0);
                contactIndexC[nC] = i;
                nC++;               
            } else {
                //assert(contactVelocities[i] >= 0);
                //assert(contactForces[i] == 0);
            }
        }}
    
        if(nC>0){ 
            //fill the A11 matrix, the v1 vector
            {for (int i = 0; i < nC; i++){
                for (int j = 0; j < (int)nC; j++) {
                    contactMatrixTemp[i][j] = contactMatrix[contactIndexC[i]][contactIndexC[j]];
                }
                //contactVectorTemp is now -v1
                contactVectorTemp[i] = -contactMatrix[contactIndexC[i]][d];
            }}
            //LOG(("\nsystem matrix\n"));
            //logMatrix(contactMatrixTemp, nC);
            //solve the system
            
                  //SystemSolver::luDecompose(contactMatrixTemp, nC, contactIndexTemp, tempVector1);
            //SystemSolver::luSubstitute(contactMatrixTemp, nC, contactIndexTemp, contactVectorTemp);

                  SystemSolver::svDecompose(contactMatrixTemp, nC, tempVector1, tempMatrix, tempVector2);
                  SystemSolver::svSubstitute(contactMatrixTemp, nC, tempVector1, tempMatrix, 
                     contactVectorTemp, tempVector2);


            //contactVectorTemp is now x, if using lu; tempVector1 is now x, if using sv
            //transfer x into delta f
            for (int i=0; i < nC; i++) {
                     //contactDForces[contactIndexC[i]]=contactVectorTemp[i];
                     contactDForces[contactIndexC[i]]=tempVector1[i];
            }
        }
        //end of fdirection part
        //contactVectorTemp is now delta a
        //delta a= A delta f
        {for (int i=0; i < nContacts; i++){
            contactVectorTemp[i] = 0;
            for (int j = 0; j <= d; j++) {
                contactVectorTemp[i] += contactMatrix[i][j] * contactDForces[j];
            }
        }}
        
        real s = maxStep(d, &sIndex);
        //assert(s > 0);
        
        //end of maxstep part
        assert(s < 1e5 && sIndex > -1);
        if (s == 0) {
            zeros[sIndex]++;
        } else{
            zeros[sIndex] = 0;
        }
        if (zeros[sIndex] > 1) {
            contactSets[sIndex]   = contactSetI;
            contactForces[sIndex] = 0;
        } else {
            {for (int i = 0; i <= d; i++) {
                //f=f+s delta f
                contactForces[i] += contactDForces[i] * s;
            }}
            {for(int i = 0; i < nContacts; i++) {
                //a=a+s delta a
                contactVelocities[i] += contactVectorTemp[i] * s;
            }}

            if (sIndex == d) {
                contactSets[sIndex] = contactSetC;
            } else if (contactSets[sIndex] == contactSetC) {
                contactSets[sIndex] = contactSetNC;
            } else {
                contactSets[sIndex] = contactSetC;
            }
        }
    } while (sIndex != d);
    return true;
}

void ContactSolver::initForcesVelocities() {
    //initialize
    for (int i = 0; i < nContacts; i++) {
        //f=0
        contactForces[i] = 0;
        //a=b
        contactVelocities[i] = contactVector[i];
        zeros[i] = 0;
    } 
}

void ContactSolver::computeContacts(){
   /*
   f        -> contactForces
   a        -> contactVelocities
   b        -> contactVector
   A        -> contactMatrix
   delta a  -> contactVectorTemp
   A11      -> contactMatrixTemp
   v1       -> contactVectorTemp
   delta f  -> contactDForces
   */

    initForcesVelocities();
   
    bool divergence = false;
    for (int d = 0; d < nContacts && !divergence; ++d) {
        divergence = !driveToZero(d);
    }
    
      
    if(divergence){
        computeContactsPreda();
    }
      
}

void ContactSolver::computeContactsPreda(){
   initForcesVelocities();
   for(unsigned repeat = 0; repeat < 10; repeat++){   
      for (int i = 0; i < nContacts; ++i) {
         assert(contactMatrix[i][i] > 0);
            
         real velocity = contactVector[i];
         real *line = contactMatrix[i];
         for (int j = 0; j < nContacts; ++j) {
               if (j != i) {
                     velocity += line[j] * contactForces[j];
               }
         }
         if (velocity < 0) {
               contactForces[i] = -velocity / contactMatrix[i][i];
               //now velocity becomes 0;
         } else {
               contactForces[i] = 0;
         }
      }
   }

    /* the code below is exactly equivalent with the one above,
       but the Gauss-Seidel parallel is less apparent.

    real deltaForce = - contactVelocities[i] / contactMatrix[i][i];
    if (deltaForce < -contactForces[i]) {
        deltaForce = -contactForces[i];
    }
    for (int j = 0; j < nContacts; ++j) {
        contactVelocities[j] += contactMatrix[j][i] * deltaForce;
    }
    contactForces[i] += deltaForce;
    */
}

void ContactSolver::uploadForces(){
   unsigned int k=0;
   for(GlobalContactInfoVector::iterator it = contacts->begin(), end = contacts->end(); 
         it != end; ++it){    
      //add penalty elastic force if there are penetrations, proportional to the penetration surface
      contactForces[k]+=ThyrixParameters::kContact * it->penetration;
      
      //put force in contacts
      it->force = contactForces[k];
      it->contact1->force =contactForces[k];
      if(it->contact2 != NULL) {
         it->contact2->force=contactForces[k];
        }
      k++;
   }

}

Thyrix homepage • Users' guide

(C) Arxia 2004-2005