src/constraints/Rattle.cpp


#include "constraints/Rattle.hpp"
#include "primitives/Molecule.hpp"
#include "utils/simError.h"
namespace oopse {

Rattle::Rattle(SimInfo* info) : info_(info), maxConsIteration_(10), consTolerance_(1.0e-6) {
    
    if (info_->getSimParams()->haveDt()) {
        dt_ = info_->getSimParams()->getDt();
    } else {
            sprintf(painCave.errMsg,
                    "Integrator Error: dt is not set\n");
            painCave.isFatal = 1;
            simError();
    }    
    
    currentSnapshot_ = info_->getSnapshotManager()->getCurrentSnapshot();
}

void Rattle::constraintA() {
    if (info_->getNConstraints() > 0) {
        doConstraint(&Rattle::constraintPairA);
    }
}
void Rattle::constraintB() {
    if (info_->getNConstraints() > 0) {
        doConstraint(&Rattle::constraintPairB);
    }
}

void Rattle::doConstraint(ConstraintPairFuncPtr func) {
    Molecule* mol;
    SimInfo::MoleculeIterator mi;
    ConstraintElem* consElem;
    Molecule::ConstraintElemIterator cei;
    ConstraintPair* consPair;
    Molecule::ConstraintPairIterator cpi;
    
    for (mol = info_->beginMolecule(mi); mol != NULL; mol = info_->nextMolecule(mi)) {
        for (consElem = mol->beginConstraintElem(cei); consElem != NULL; consElem = mol->nextConstraintElem(cei)) {
            consElem->setMoved(true);
            consElem->setMoving(false);
        }
    }
    
    //main loop of constraint algorithm
    bool done = false;
    int iteration = 0;
    while(!done && iteration < maxConsIteration_){
        done = true;

        //loop over every constraint pair

        for (mol = info_->beginMolecule(mi); mol != NULL; mol = info_->nextMolecule(mi)) {
            for (consPair = mol->beginConstraintPair(cpi); consPair != NULL; consPair = mol->nextConstraintPair(cpi)) {


                //dispatch constraint algorithm
                if(consPair->isMoved()) {
                    int exeStatus = (this->*func)(consPair);

                    switch(exeStatus){
                        case consFail:
                            sprintf(painCave.errMsg,
                            "Constraint failure in Rattle::constrainA, Constraint Fail\n");
                            painCave.isFatal = 1;
                            simError();                             
                            
                            break;
                        case consSuccess:
                            //constrain the pair by moving two elements
                            done = false;
                            consPair->getConsElem1()->setMoving(true);
                            consPair->getConsElem2()->setMoving(true);
                            break;
                        case consAlready:
                            //current pair is already constrained, do not need to move the elements
                            break;
                        default:          
                            sprintf(painCave.errMsg, "ConstraintAlgorithm::doConstrain() Error: unrecognized status");
                            painCave.isFatal = 1;
                            simError();                           
                            break;
                    }      
                }
            }
        }//end for(iter->first())


        for (mol = info_->beginMolecule(mi); mol != NULL; mol = info_->nextMolecule(mi)) {
            for (consElem = mol->beginConstraintElem(cei); consElem != NULL; consElem = mol->nextConstraintElem(cei)) {
                consElem->setMoved(consElem->getMoving());
                consElem->setMoving(false);
            }
        }

        iteration++;
    }//end while

    if (!done){
      sprintf(painCave.errMsg,
              "Constraint failure in Rattle::constrainA, too many iterations: %d\n",
              iteration);
      painCave.isFatal = 1;
      simError();    
    }
}

int Rattle::constraintPairA(ConstraintPair* consPair){
  ConstraintElem* consElem1 = consPair->getConsElem1();
  ConstraintElem* consElem2 = consPair->getConsElem2();

  Vector3d posA = consElem1->getPos();
  Vector3d posB = consElem2->getPos();

  Vector3d pab = posA -posB;   

  //periodic boundary condition

  currentSnapshot_->wrapVector(pab);

  double pabsq = pab.lengthSquare();

  double rabsq = consPair->getConsDistSquare();
  double diffsq = rabsq - pabsq;

  // the original rattle code from alan tidesley
  if (fabs(diffsq) > (consTolerance_ * rabsq * 2)){
    
    Vector3d oldPosA = consElem1->getPrevPos();
    Vector3d oldPosB = consElem2->getPrevPos();      

    Vector3d rab = oldPosA - oldPosB;    

    currentSnapshot_->wrapVector(rab);

    double rpab = dot(rab, pab);
    double rpabsq = rpab * rpab;

    if (rpabsq < (rabsq * -diffsq)){
      return consFail;
    }

    double rma = 1.0 / consElem1->getMass();
    double rmb = 1.0 / consElem2->getMass();

    double gab = diffsq / (2.0 * (rma + rmb) * rpab);

    Vector3d delta = rab * gab;

    //set atom1's position
    posA += rma * delta;    
    consElem1->setPos(posA);

    //set atom2's position
    posB -= rmb * delta;
    consElem2->setPos(posB);

    delta /= dt_;
    
    //set atom1's velocity
    Vector3d velA = consElem1->getVel();
    velA += rma * delta;
    consElem1->setVel(velA);

    //set atom2's velocity
    Vector3d velB = consElem2->getVel();
    velB -= rmb * delta;
    consElem2->setVel(velB);

    return consSuccess;
  }
  else
    return consAlready;
  
}


int Rattle::constraintPairB(ConstraintPair* consPair){
    ConstraintElem* consElem1 = consPair->getConsElem1();
    ConstraintElem* consElem2 = consPair->getConsElem2();

  
    Vector3d velA = consElem1->getVel();
    Vector3d velB = consElem2->getVel();

    Vector3d dv = velA - velB;

    Vector3d posA = consElem1->getPos();
    Vector3d posB = consElem2->getPos();

    Vector3d rab = posA - posB;

    currentSnapshot_->wrapVector(rab);

    double rma = 1.0 / consElem1->getMass();
    double rmb = 1.0 / consElem2->getMass();

    double rvab = dot(rab, dv);

    double gab = -rvab / ((rma + rmb) * consPair->getConsDistSquare());

    if (fabs(gab) > consTolerance_){
      Vector3d delta = rab * gab;
      
      velA += rma * delta;
      consElem1->setVel(velA);
      
      velB -= rmb * delta;
      consElem2->setVel(velB);

      return consSuccess;
    }
    else
      return consAlready;

}

}
Revision:	1916
Committed:	Tue Jan 11 15:52:45 2005 UTC (20 years, 4 months ago) by tim
File size:	6358 byte(s)
Log Message:	adding Rattle Algorithm
#	User	Rev	Content
1	tim	1916
2			#include "constraints/Rattle.hpp"
3			#include "primitives/Molecule.hpp"
4			#include "utils/simError.h"
5			namespace oopse {
6
7			Rattle::Rattle(SimInfo* info) : info_(info), maxConsIteration_(10), consTolerance_(1.0e-6) {
8
9			if (info_->getSimParams()->haveDt()) {
10			dt_ = info_->getSimParams()->getDt();
11			} else {
12			sprintf(painCave.errMsg,
13			"Integrator Error: dt is not set\n");
14			painCave.isFatal = 1;
15			simError();
16			}
17
18			currentSnapshot_ = info_->getSnapshotManager()->getCurrentSnapshot();
19			}
20
21			void Rattle::constraintA() {
22			if (info_->getNConstraints() > 0) {
23			doConstraint(&Rattle::constraintPairA);
24			}
25			}
26			void Rattle::constraintB() {
27			if (info_->getNConstraints() > 0) {
28			doConstraint(&Rattle::constraintPairB);
29			}
30			}
31
32			void Rattle::doConstraint(ConstraintPairFuncPtr func) {
33			Molecule* mol;
34			SimInfo::MoleculeIterator mi;
35			ConstraintElem* consElem;
36			Molecule::ConstraintElemIterator cei;
37			ConstraintPair* consPair;
38			Molecule::ConstraintPairIterator cpi;
39
40			for (mol = info_->beginMolecule(mi); mol != NULL; mol = info_->nextMolecule(mi)) {
41			for (consElem = mol->beginConstraintElem(cei); consElem != NULL; consElem = mol->nextConstraintElem(cei)) {
42			consElem->setMoved(true);
43			consElem->setMoving(false);
44			}
45			}
46
47			//main loop of constraint algorithm
48			bool done = false;
49			int iteration = 0;
50			while(!done && iteration < maxConsIteration_){
51			done = true;
52
53			//loop over every constraint pair
54
55			for (mol = info_->beginMolecule(mi); mol != NULL; mol = info_->nextMolecule(mi)) {
56			for (consPair = mol->beginConstraintPair(cpi); consPair != NULL; consPair = mol->nextConstraintPair(cpi)) {
57
58
59			//dispatch constraint algorithm
60			if(consPair->isMoved()) {
61			int exeStatus = (this->*func)(consPair);
62
63			switch(exeStatus){
64			case consFail:
65			sprintf(painCave.errMsg,
66			"Constraint failure in Rattle::constrainA, Constraint Fail\n");
67			painCave.isFatal = 1;
68			simError();
69
70			break;
71			case consSuccess:
72			//constrain the pair by moving two elements
73			done = false;
74			consPair->getConsElem1()->setMoving(true);
75			consPair->getConsElem2()->setMoving(true);
76			break;
77			case consAlready:
78			//current pair is already constrained, do not need to move the elements
79			break;
80			default:
81			sprintf(painCave.errMsg, "ConstraintAlgorithm::doConstrain() Error: unrecognized status");
82			painCave.isFatal = 1;
83			simError();
84			break;
85			}
86			}
87			}
88			}//end for(iter->first())
89
90
91			for (mol = info_->beginMolecule(mi); mol != NULL; mol = info_->nextMolecule(mi)) {
92			for (consElem = mol->beginConstraintElem(cei); consElem != NULL; consElem = mol->nextConstraintElem(cei)) {
93			consElem->setMoved(consElem->getMoving());
94			consElem->setMoving(false);
95			}
96			}
97
98			iteration++;
99			}//end while
100
101			if (!done){
102			sprintf(painCave.errMsg,
103			"Constraint failure in Rattle::constrainA, too many iterations: %d\n",
104			iteration);
105			painCave.isFatal = 1;
106			simError();
107			}
108			}
109
110			int Rattle::constraintPairA(ConstraintPair* consPair){
111			ConstraintElem* consElem1 = consPair->getConsElem1();
112			ConstraintElem* consElem2 = consPair->getConsElem2();
113
114			Vector3d posA = consElem1->getPos();
115			Vector3d posB = consElem2->getPos();
116
117			Vector3d pab = posA -posB;
118
119			//periodic boundary condition
120
121			currentSnapshot_->wrapVector(pab);
122
123			double pabsq = pab.lengthSquare();
124
125			double rabsq = consPair->getConsDistSquare();
126			double diffsq = rabsq - pabsq;
127
128			// the original rattle code from alan tidesley
129			if (fabs(diffsq) > (consTolerance_ * rabsq * 2)){
130
131			Vector3d oldPosA = consElem1->getPrevPos();
132			Vector3d oldPosB = consElem2->getPrevPos();
133
134			Vector3d rab = oldPosA - oldPosB;
135
136			currentSnapshot_->wrapVector(rab);
137
138			double rpab = dot(rab, pab);
139			double rpabsq = rpab * rpab;
140
141			if (rpabsq < (rabsq * -diffsq)){
142			return consFail;
143			}
144
145			double rma = 1.0 / consElem1->getMass();
146			double rmb = 1.0 / consElem2->getMass();
147
148			double gab = diffsq / (2.0 * (rma + rmb) * rpab);
149
150			Vector3d delta = rab * gab;
151
152			//set atom1's position
153			posA += rma * delta;
154			consElem1->setPos(posA);
155
156			//set atom2's position
157			posB -= rmb * delta;
158			consElem2->setPos(posB);
159
160			delta /= dt_;
161
162			//set atom1's velocity
163			Vector3d velA = consElem1->getVel();
164			velA += rma * delta;
165			consElem1->setVel(velA);
166
167			//set atom2's velocity
168			Vector3d velB = consElem2->getVel();
169			velB -= rmb * delta;
170			consElem2->setVel(velB);
171
172			return consSuccess;
173			}
174			else
175			return consAlready;
176
177			}
178
179
180			int Rattle::constraintPairB(ConstraintPair* consPair){
181			ConstraintElem* consElem1 = consPair->getConsElem1();
182			ConstraintElem* consElem2 = consPair->getConsElem2();
183
184
185			Vector3d velA = consElem1->getVel();
186			Vector3d velB = consElem2->getVel();
187
188			Vector3d dv = velA - velB;
189
190			Vector3d posA = consElem1->getPos();
191			Vector3d posB = consElem2->getPos();
192
193			Vector3d rab = posA - posB;
194
195			currentSnapshot_->wrapVector(rab);
196
197			double rma = 1.0 / consElem1->getMass();
198			double rmb = 1.0 / consElem2->getMass();
199
200			double rvab = dot(rab, dv);
201
202			double gab = -rvab / ((rma + rmb) * consPair->getConsDistSquare());
203
204			if (fabs(gab) > consTolerance_){
205			Vector3d delta = rab * gab;
206
207			velA += rma * delta;
208			consElem1->setVel(velA);
209
210			velB -= rmb * delta;
211			consElem2->setVel(velB);
212
213			return consSuccess;
214			}
215			else
216			return consAlready;
217
218			}
219
220			}