OOPSE/libmdtools/Roll.cpp

#include <cmath>
#include "Mat3x3d.hpp"
#include "Roll.hpp"
#include "SimInfo.hpp"


////////////////////////////////////////////////////////////////////////////////
//Implementation of DCRollAFunctor
////////////////////////////////////////////////////////////////////////////////
int DCRollAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
  Vector3d posA;
  Vector3d posB;
  Vector3d oldPosA;
  Vector3d oldPosB;
  Vector3d velA;
  Vector3d velB;
  Vector3d pab;
  Vector3d tempPab;
  Vector3d rab;
  Vector3d zetaA;
  Vector3d zetaB;
  Vector3d zeta;
  Vector3d consForce;
  Vector3d bondDirUnitVec;  
  double dx, dy, dz;
  double rpab;
  double rabsq, pabsq, rpabsq;
  double diffsq;
  double gab;
  double dt;
  double pabDotZeta;
  double pabDotZeta2;
  double zeta2;
  double forceScalar;

  
  dt = info->dt;

  consAtom1->getOldPos(oldPosA.vec);
  consAtom2->getOldPos(oldPosB.vec);      


    consAtom1->getPos(posA.vec);
    consAtom2->getPos(posB.vec);

    //discard the vector convention in alan tidesley's code
    //rij =  rj - ri;
    pab = posB - posA;

    //periodic boundary condition

    info->wrapVector(pab.vec);

    pabsq = dotProduct(pab, pab);

    rabsq = curPair->getBondLength2();
    diffsq =  pabsq -rabsq;

    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
      rab = oldPosB - oldPosA;       
      info->wrapVector(rab.vec);

      //rpab = dotProduct(rab, pab);

      //rpabsq = rpab * rpab;


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

      bondDirUnitVec = pab;
      bondDirUnitVec.normalize();
          
      calcZeta(consAtom1, bondDirUnitVec, zetaA);

      calcZeta(consAtom2, bondDirUnitVec, zetaB);

      zeta = zetaA + zetaB;      
      zeta2 = dotProduct(zeta, zeta);
      
      pabDotZeta = dotProduct(pab,  zeta);
      pabDotZeta2 = pabDotZeta * pabDotZeta;

      //solve quadratic equation
      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
      //dt : time step
      // pab : 
      //G : constraint force scalar
      //d: equilibrium bond length
      
      if (pabDotZeta2 - zeta2 * diffsq < 0)   
        return consFail;
      
      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
      forceScalar = diffsq / (2 * dt * dt * pabDotZeta); 
      //forceScalar = 1 / forceScalar;
      consForce = forceScalar * bondDirUnitVec;
      //integrate consRB1 using constraint force;
      integrate(consAtom1, consForce);

      //integrate consRB2 using constraint force;
      integrate(consAtom2, -consForce);    

      return consSuccess;
    }
    else
        return consAlready;

       
}
void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
  double invMass;
  invMass = 1.0 / consAtom ->getMass();

  zeta = invMass * bondDir;
}

void DCRollAFunctor::integrate(ConstraintAtom* consAtom, const Vector3d& force){
  StuntDouble* sd;
  Vector3d vel;
  Vector3d pos;
  Vector3d tempPos;
  Vector3d tempVel;
  double mass;
  double dt;
  
  dt = info->dt;
  sd = consAtom->getStuntDouble();
  
  sd->getVel(vel.vec);
  sd->getPos(pos.vec);
  
  mass = sd->getMass();

  tempVel = dt/mass * force;
  tempPos = dt * tempVel;
        
  vel += tempVel;
  pos += tempPos;

  sd->setVel(vel.vec);
  sd->setPos(pos.vec);
}

int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  Vector3d posA;
  Vector3d posB;
  Vector3d oldPosA;
  Vector3d oldPosB;
  Vector3d velA;
  Vector3d velB;
  Vector3d pab;
  Vector3d tempPab;
  Vector3d rab;
  Vector3d zetaA;
  Vector3d zetaB;
  Vector3d zeta;
  Vector3d consForce;
  Vector3d bondDirUnitVec;  
  double dx, dy, dz;
  double rpab;
  double rabsq, pabsq, rpabsq;
  double diffsq;
  double gab;
  double dt;
  double pabDotZeta;
  double pabDotZeta2;
  double zeta2;
  double forceScalar;
  
  const int conRBMaxIter = 100;
  
  dt = info->dt;

  consRB1->getOldAtomPos(oldPosA.vec);
  consRB2->getOldAtomPos(oldPosB.vec);      


  for(int i=0 ; i < conRBMaxIter; i++){   
    consRB1->getCurAtomPos(posA.vec);
    consRB2->getCurAtomPos(posB.vec);

    //discard the vector convention in alan tidesley's code
    //rij =  rj - ri;
    pab = posB - posA;

    //periodic boundary condition

    info->wrapVector(pab.vec);

    pabsq = dotProduct(pab, pab);

    rabsq = curPair->getBondLength2();
    diffsq =  pabsq -rabsq;

    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
      rab = oldPosB - oldPosA;       
      info->wrapVector(rab.vec);

      bondDirUnitVec = rab;
      bondDirUnitVec.normalize();
          
      calcZeta(consRB1, bondDirUnitVec, zetaA);

      calcZeta(consRB2, bondDirUnitVec, zetaB);

      zeta = zetaA + zetaB;      
      zeta2 = dotProduct(zeta, zeta);
      
      pabDotZeta = dotProduct(pab,  zeta);
      pabDotZeta2 = pabDotZeta * pabDotZeta;

      //solve quadratic equation
      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
      //dt : time step
      // pab : 
      //G : constraint force scalar
      //d: equilibrium bond length
      
      if (pabDotZeta2 - zeta2 * diffsq < 0){
        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;   
        return consFail;
      }
      //if pabDotZeta is close to 0, we can't neglect the square term
      if(fabs(pabDotZeta) < consTolerance)
        forceScalar = (pabDotZeta - sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
      else
        forceScalar = diffsq / (2 * dt * dt * pabDotZeta); 
        
      //
      consForce = forceScalar * bondDirUnitVec;
      //integrate consRB1 using constraint force;
      integrate(consRB1, consForce);

      //integrate consRB2 using constraint force;
      integrate(consRB2, -consForce);    
      
    }
    else{
      if (i ==0)
        return consAlready;
      else
        return consSuccess;
    }
  }

  cerr << "DCRollAFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl; 
  return consExceedMaxIter;

}

void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
  double invMass;
  Vector3d tempVec1;
  Vector3d tempVec2;
  Vector3d refCoor;
  Vector3d refCrossBond;  
  Mat3x3d IBody;
  Mat3x3d invIBody;
  Mat3x3d invILab;
  Mat3x3d a;
  Mat3x3d aTrans;
  
  invMass = 1.0 / consRB ->getMass();

  zeta = invMass * bondDir;
  
  consRB->getRefCoor(refCoor.vec);
  //consRB->getA(a.element);
  consRB->getOldA(a.element);
  consRB->getI(IBody.element);

  aTrans = a.transpose();
  invIBody = IBody.inverse();

  invILab = aTrans * invIBody * a;

  refCrossBond = crossProduct(aTrans *refCoor, bondDir);  

  tempVec1 = invILab * refCrossBond;
  tempVec2 = crossProduct(tempVec1, aTrans *refCoor);

  zeta += tempVec2;
   
}

void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& consForce){
  RigidBody* rb;
  Vector3d frc;
  Vector3d totConsForce;
  Vector3d vel;
  Vector3d pos;
  Vector3d Tb;
  Vector3d ji;
  Vector3d refCoor;
  Vector3d consTorque;
  Vector3d totConsTorque;
  Mat3x3d a;
  double mass;
  double dt;
  double dtOver2;
  const double eConvert = 4.184e-4;
  
  dt = info->dt;
  dtOver2 = dt /2;

  //restore to old status
  consRB->restoreUnconsStatus();

  //accumulate constraint force;
  consRB->addConsForce(consForce/eConvert);
  totConsForce = consRB->getConsForce();

  rb = consRB->getRigidBody();
  
  rb->addFrc(totConsForce.vec);
  
  rb->getVel(vel.vec);
  rb->getPos(pos.vec);
  rb->getFrc(frc.vec);
  mass = rb->getMass();

  // velocity half step
  vel += eConvert  * dtOver2 / mass * frc;
  // position whole step
  pos += dt * vel;

  rb->setVel(vel.vec);
  rb->setPos(pos.vec);

  //evolve orientational part
  consRB->getRefCoor(refCoor.vec);
  rb->getA(a.element);

  //calculate constraint torque in lab frame
  consTorque = crossProduct(a.transpose() * refCoor, consForce);
  consRB->addConsTorque(consTorque/eConvert);

  //add constraint torque
  totConsTorque = consRB->getConsTorque();  
  rb->addTrq(totConsTorque.vec);
  
  //get and convert the torque to body frame

  rb->getTrq(Tb.vec);
  rb->lab2Body(Tb.vec);

  //get the angular momentum, and propagate a half step

  rb->getJ(ji.vec);

  ji += eConvert * dtOver2 * Tb;

  rotationPropagation( rb, ji.vec );

  rb->setJ(ji.vec);
  
}

void DCRollAFunctor::rotationPropagation(StuntDouble* sd, double ji[3]){
  double angle;
  double A[3][3], I[3][3];
  int i, j, k;
  double dtOver2;
  double dt;
  dt = info->dt;
  dtOver2 = dt /2;
  // use the angular velocities to propagate the rotation matrix a
  // full time step

  sd->getA(A);
  sd->getI(I);

  if (sd->isLinear()) {
    i = sd->linearAxis();
    j = (i+1)%3;
    k = (i+2)%3;
    
    angle = dtOver2 * ji[j] / I[j][j];
    this->rotate( k, i, angle, ji, A );

    angle = dt* ji[k] / I[k][k];
    this->rotate( i, j, angle, ji, A);

    angle = dtOver2 * ji[j] / I[j][j];
    this->rotate( k, i, angle, ji, A );

  } else {
    // rotate about the x-axis
    angle = dtOver2 * ji[0] / I[0][0];
    this->rotate( 1, 2, angle, ji, A );
    
    // rotate about the y-axis
    angle = dtOver2 * ji[1] / I[1][1];
    this->rotate( 2, 0, angle, ji, A );
    
    // rotate about the z-axis
    angle = dt * ji[2] / I[2][2];
    this->rotate( 0, 1, angle, ji, A);
    
    // rotate about the y-axis
    angle = dtOver2 * ji[1] / I[1][1];
    this->rotate( 2, 0, angle, ji, A );
    
    // rotate about the x-axis
    angle = dtOver2 * ji[0] / I[0][0];
    this->rotate( 1, 2, angle, ji, A );
    
  }
  sd->setA( A  );
}

void DCRollAFunctor::rotate(int axes1, int axes2, double angle, double ji[3], double A[3][3]){
  int i, j, k;
  double sinAngle;
  double cosAngle;
  double angleSqr;
  double angleSqrOver4;
  double top, bottom;
  double rot[3][3];
  double tempA[3][3];
  double tempJ[3];

  // initialize the tempA

  for (i = 0; i < 3; i++){
    for (j = 0; j < 3; j++){
      tempA[j][i] = A[i][j];
    }
  }

  // initialize the tempJ

  for (i = 0; i < 3; i++)
    tempJ[i] = ji[i];

  // initalize rot as a unit matrix

  rot[0][0] = 1.0;
  rot[0][1] = 0.0;
  rot[0][2] = 0.0;

  rot[1][0] = 0.0;
  rot[1][1] = 1.0;
  rot[1][2] = 0.0;

  rot[2][0] = 0.0;
  rot[2][1] = 0.0;
  rot[2][2] = 1.0;

  // use a small angle aproximation for sin and cosine

  angleSqr = angle * angle;
  angleSqrOver4 = angleSqr / 4.0;
  top = 1.0 - angleSqrOver4;
  bottom = 1.0 + angleSqrOver4;

  cosAngle = top / bottom;
  sinAngle = angle / bottom;

  rot[axes1][axes1] = cosAngle;
  rot[axes2][axes2] = cosAngle;

  rot[axes1][axes2] = sinAngle;
  rot[axes2][axes1] = -sinAngle;

  // rotate the momentum acoording to: ji[] = rot[][] * ji[]

  for (i = 0; i < 3; i++){
    ji[i] = 0.0;
    for (k = 0; k < 3; k++){
      ji[i] += rot[i][k] * tempJ[k];
    }
  }

  // rotate the Rotation matrix acording to:
  //            A[][] = A[][] * transpose(rot[][])


  // NOte for as yet unknown reason, we are performing the
  // calculation as:
  //                transpose(A[][]) = transpose(A[][]) * transpose(rot[][])

  for (i = 0; i < 3; i++){
    for (j = 0; j < 3; j++){
      A[j][i] = 0.0;
      for (k = 0; k < 3; k++){
        A[j][i] += tempA[i][k] * rot[j][k];
      }
    }
  }
}
////////////////////////////////////////////////////////////////////////////////
//Implementation of DCRollBFunctor
////////////////////////////////////////////////////////////////////////////////
int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  Vector3d posA;
  Vector3d posB;
  Vector3d velA;
  Vector3d velB;
  Vector3d pab;
  Vector3d rab;
  Vector3d vab;
  Vector3d zetaA;
  Vector3d zetaB;
  Vector3d zeta;
  Vector3d consForce;
  Vector3d bondDirUnitVec;  
  double dt;
  double pabDotvab;
  double pabDotZeta;
  double pvab;

  const int conRBMaxIter = 20;
  
  dt = info->dt;
  
  for(int i=0 ; i < conRBMaxIter; i++){   
    consRB1->getCurAtomPos(posA.vec);
    consRB2->getCurAtomPos(posB.vec);
    pab = posB - posA;

    //periodic boundary condition
    info->wrapVector(pab.vec);
    
    consRB1->getCurAtomVel(velA.vec);
    consRB2->getCurAtomVel(velB.vec);
    vab = velB -velA;

    pvab = dotProduct(pab, vab);

    if (fabs(pvab) > consTolerance ){


      bondDirUnitVec = pab;
      bondDirUnitVec.normalize();
          
      getZeta(consRB1, bondDirUnitVec, zetaA);
      getZeta(consRB2, bondDirUnitVec, zetaB);
      zeta = zetaA + zetaB;
      
      pabDotZeta = dotProduct(pab,  zeta);
      
      consForce =  2 * pvab / (dt * pabDotZeta) * bondDirUnitVec;
      //integrate consRB1 using constraint force;
      integrate(consRB1, consForce);

      //integrate consRB2 using constraint force;
      integrate(consRB2, -consForce);    
      
    }
    else{
      if (i ==0)
        return consAlready;
      else
        return consSuccess;
    }
  }

  cerr << "DCRollBFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;  
  return consExceedMaxIter;

}

void DCRollBFunctor::getZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
  double invMass;
  Vector3d tempVec1;
  Vector3d tempVec2;
  Vector3d refCoor;
  Vector3d refCrossBond;  
  Mat3x3d IBody;
  Mat3x3d ILab;
  Mat3x3d invIBody;
  Mat3x3d invILab;
  Mat3x3d a;
  
  invMass = 1.0 / consRB ->getMass();

  zeta = invMass * bondDir;
  
  consRB->getRefCoor(refCoor.vec);
  consRB->getA(a.element);
  consRB->getI(IBody.element);

  invIBody = IBody.inverse();

  
  refCrossBond = crossProduct(refCoor, a * bondDir);  

  tempVec1 = invIBody * refCrossBond;

  tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor;
  
  zeta += tempVec2;

}

void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
  Vector3d vel;
  Vector3d ji;
  Vector3d tempJi;
  Vector3d tempTrq;
  Vector3d refCoor;
  double mass;
  double dtOver2;
  Mat3x3d a;
  StuntDouble* sd;
  
  sd = consRB->getStuntDouble();  
  dtOver2 = info->dt/2;

  sd->getVel(vel.vec);
  mass = sd->getMass();
  vel +=dtOver2 /mass * force;
  sd->setVel(vel.vec);

  if (sd->isDirectional()){
    sd->getA(a.element);
    consRB->getRefCoor(refCoor.vec);
    tempTrq = crossProduct(refCoor, a *force);

    tempJi = dtOver2* tempTrq;
    sd->getJ(ji.vec);
    ji += tempJi;
    sd->setJ(ji.vec);
  }
  
}
Revision:	1452
Committed:	Mon Aug 23 15:11:36 2004 UTC (21 years, 2 months ago) by tim
File size:	14433 byte(s)
Log Message:	* empty log message *
#	User	Rev	Content
1	tim	1254	#include <cmath>
2			#include "Mat3x3d.hpp"
3			#include "Roll.hpp"
4			#include "SimInfo.hpp"
5
6
7			////////////////////////////////////////////////////////////////////////////////
8			//Implementation of DCRollAFunctor
9			////////////////////////////////////////////////////////////////////////////////
10	tim	1268	int DCRollAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
11	tim	1254	Vector3d posA;
12			Vector3d posB;
13			Vector3d oldPosA;
14			Vector3d oldPosB;
15			Vector3d velA;
16			Vector3d velB;
17			Vector3d pab;
18			Vector3d tempPab;
19			Vector3d rab;
20	tim	1268	Vector3d zetaA;
21			Vector3d zetaB;
22			Vector3d zeta;
23	tim	1254	Vector3d consForce;
24			Vector3d bondDirUnitVec;
25			double dx, dy, dz;
26			double rpab;
27			double rabsq, pabsq, rpabsq;
28			double diffsq;
29			double gab;
30			double dt;
31	tim	1268	double pabDotZeta;
32			double pabDotZeta2;
33			double zeta2;
34			double forceScalar;
35	tim	1452
36	tim	1268
37			dt = info->dt;
38	tim	1254
39	tim	1268	consAtom1->getOldPos(oldPosA.vec);
40			consAtom2->getOldPos(oldPosB.vec);
41	tim	1254
42	tim	1268
43			consAtom1->getPos(posA.vec);
44			consAtom2->getPos(posB.vec);
45
46			//discard the vector convention in alan tidesley's code
47			//rij = rj - ri;
48			pab = posB - posA;
49
50			//periodic boundary condition
51
52			info->wrapVector(pab.vec);
53
54			pabsq = dotProduct(pab, pab);
55
56			rabsq = curPair->getBondLength2();
57			diffsq = pabsq -rabsq;
58
59			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
60			rab = oldPosB - oldPosA;
61			info->wrapVector(rab.vec);
62
63			//rpab = dotProduct(rab, pab);
64
65			//rpabsq = rpab * rpab;
66
67
68			//if (rpabsq < (rabsq * -diffsq)){
69			// return consFail;
70			//}
71
72			bondDirUnitVec = pab;
73			bondDirUnitVec.normalize();
74
75			calcZeta(consAtom1, bondDirUnitVec, zetaA);
76
77			calcZeta(consAtom2, bondDirUnitVec, zetaB);
78
79			zeta = zetaA + zetaB;
80			zeta2 = dotProduct(zeta, zeta);
81
82			pabDotZeta = dotProduct(pab, zeta);
83			pabDotZeta2 = pabDotZeta * pabDotZeta;
84
85			//solve quadratic equation
86			//dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
87			//dt : time step
88			// pab :
89			//G : constraint force scalar
90			//d: equilibrium bond length
91
92	tim	1284	if (pabDotZeta2 - zeta2 * diffsq < 0)
93	tim	1268	return consFail;
94	tim	1284
95	tim	1268	//forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
96			forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
97	tim	1284	//forceScalar = 1 / forceScalar;
98	tim	1268	consForce = forceScalar * bondDirUnitVec;
99			//integrate consRB1 using constraint force;
100			integrate(consAtom1, consForce);
101
102			//integrate consRB2 using constraint force;
103			integrate(consAtom2, -consForce);
104	tim	1452
105			return consSuccess;
106	tim	1268	}
107	tim	1452	else
108	tim	1268	return consAlready;
109
110	tim	1452
111	tim	1268
112	tim	1452
113	tim	1268	}
114			void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
115			double invMass;
116			invMass = 1.0 / consAtom ->getMass();
117
118			zeta = invMass * bondDir;
119			}
120
121			void DCRollAFunctor::integrate(ConstraintAtom* consAtom, const Vector3d& force){
122			StuntDouble* sd;
123			Vector3d vel;
124			Vector3d pos;
125			Vector3d tempPos;
126			Vector3d tempVel;
127			double mass;
128			double dt;
129
130			dt = info->dt;
131			sd = consAtom->getStuntDouble();
132
133			sd->getVel(vel.vec);
134			sd->getPos(pos.vec);
135
136			mass = sd->getMass();
137
138	tim	1284	tempVel = dt/mass * force;
139	tim	1268	tempPos = dt * tempVel;
140
141			vel += tempVel;
142			pos += tempPos;
143
144			sd->setVel(vel.vec);
145			sd->setPos(pos.vec);
146			}
147
148			int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
149			Vector3d posA;
150			Vector3d posB;
151			Vector3d oldPosA;
152			Vector3d oldPosB;
153			Vector3d velA;
154			Vector3d velB;
155			Vector3d pab;
156			Vector3d tempPab;
157			Vector3d rab;
158			Vector3d zetaA;
159			Vector3d zetaB;
160			Vector3d zeta;
161			Vector3d consForce;
162			Vector3d bondDirUnitVec;
163			double dx, dy, dz;
164			double rpab;
165			double rabsq, pabsq, rpabsq;
166			double diffsq;
167			double gab;
168			double dt;
169			double pabDotZeta;
170			double pabDotZeta2;
171			double zeta2;
172			double forceScalar;
173
174	tim	1284	const int conRBMaxIter = 100;
175	tim	1254
176			dt = info->dt;
177
178			consRB1->getOldAtomPos(oldPosA.vec);
179			consRB2->getOldAtomPos(oldPosB.vec);
180
181
182			for(int i=0 ; i < conRBMaxIter; i++){
183			consRB1->getCurAtomPos(posA.vec);
184			consRB2->getCurAtomPos(posB.vec);
185
186	tim	1268	//discard the vector convention in alan tidesley's code
187			//rij = rj - ri;
188			pab = posB - posA;
189	tim	1254
190			//periodic boundary condition
191
192			info->wrapVector(pab.vec);
193
194			pabsq = dotProduct(pab, pab);
195
196			rabsq = curPair->getBondLength2();
197	tim	1268	diffsq = pabsq -rabsq;
198	tim	1254
199			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
200	tim	1268	rab = oldPosB - oldPosA;
201	tim	1254	info->wrapVector(rab.vec);
202
203	tim	1284	bondDirUnitVec = rab;
204	tim	1254	bondDirUnitVec.normalize();
205
206	tim	1268	calcZeta(consRB1, bondDirUnitVec, zetaA);
207	tim	1254
208	tim	1268	calcZeta(consRB2, bondDirUnitVec, zetaB);
209	tim	1254
210	tim	1268	zeta = zetaA + zetaB;
211			zeta2 = dotProduct(zeta, zeta);
212	tim	1254
213	tim	1268	pabDotZeta = dotProduct(pab, zeta);
214			pabDotZeta2 = pabDotZeta * pabDotZeta;
215
216			//solve quadratic equation
217			//dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
218			//dt : time step
219			// pab :
220			//G : constraint force scalar
221			//d: equilibrium bond length
222
223	tim	1284	if (pabDotZeta2 - zeta2 * diffsq < 0){
224			cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;
225	tim	1268	return consFail;
226	tim	1284	}
227			//if pabDotZeta is close to 0, we can't neglect the square term
228			if(fabs(pabDotZeta) < consTolerance)
229			forceScalar = (pabDotZeta - sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
230			else
231			forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
232
233	tim	1268	//
234			consForce = forceScalar * bondDirUnitVec;
235	tim	1254	//integrate consRB1 using constraint force;
236	tim	1268	integrate(consRB1, consForce);
237	tim	1254
238			//integrate consRB2 using constraint force;
239			integrate(consRB2, -consForce);
240
241			}
242			else{
243			if (i ==0)
244			return consAlready;
245			else
246			return consSuccess;
247			}
248			}
249
250	tim	1284	cerr << "DCRollAFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;
251	tim	1254	return consExceedMaxIter;
252
253			}
254
255	tim	1268	void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
256	tim	1254	double invMass;
257			Vector3d tempVec1;
258			Vector3d tempVec2;
259			Vector3d refCoor;
260			Vector3d refCrossBond;
261			Mat3x3d IBody;
262			Mat3x3d invIBody;
263	tim	1284	Mat3x3d invILab;
264	tim	1254	Mat3x3d a;
265			Mat3x3d aTrans;
266
267			invMass = 1.0 / consRB ->getMass();
268
269	tim	1268	zeta = invMass * bondDir;
270	tim	1254
271			consRB->getRefCoor(refCoor.vec);
272	tim	1452	//consRB->getA(a.element);
273			consRB->getOldA(a.element);
274	tim	1254	consRB->getI(IBody.element);
275
276			aTrans = a.transpose();
277			invIBody = IBody.inverse();
278
279	tim	1284	invILab = aTrans * invIBody * a;
280	tim	1254
281	tim	1452	refCrossBond = crossProduct(aTrans *refCoor, bondDir);
282	tim	1254
283	tim	1284	tempVec1 = invILab * refCrossBond;
284	tim	1452	tempVec2 = crossProduct(tempVec1, aTrans *refCoor);
285	tim	1254
286	tim	1268	zeta += tempVec2;
287	tim	1254
288			}
289
290	tim	1452	void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& consForce){
291			RigidBody* rb;
292			Vector3d frc;
293			Vector3d totConsForce;
294	tim	1254	Vector3d vel;
295			Vector3d pos;
296			Vector3d Tb;
297			Vector3d ji;
298	tim	1284	Vector3d refCoor;
299	tim	1452	Vector3d consTorque;
300			Vector3d totConsTorque;
301			Mat3x3d a;
302	tim	1254	double mass;
303	tim	1284	double dt;
304	tim	1254	double dtOver2;
305	tim	1452	const double eConvert = 4.184e-4;
306
307	tim	1254	dt = info->dt;
308	tim	1284	dtOver2 = dt /2;
309
310	tim	1452	//restore to old status
311			consRB->restoreUnconsStatus();
312
313			//accumulate constraint force;
314			consRB->addConsForce(consForce/eConvert);
315			totConsForce = consRB->getConsForce();
316
317			rb = consRB->getRigidBody();
318	tim	1254
319	tim	1452	rb->addFrc(totConsForce.vec);
320	tim	1254
321	tim	1452	rb->getVel(vel.vec);
322			rb->getPos(pos.vec);
323			rb->getFrc(frc.vec);
324			mass = rb->getMass();
325	tim	1254
326	tim	1452	// velocity half step
327			vel += eConvert * dtOver2 / mass * frc;
328			// position whole step
329			pos += dt * vel;
330	tim	1254
331	tim	1452	rb->setVel(vel.vec);
332			rb->setPos(pos.vec);
333	tim	1254
334	tim	1452	//evolve orientational part
335			consRB->getRefCoor(refCoor.vec);
336			rb->getA(a.element);
337	tim	1254
338	tim	1452	//calculate constraint torque in lab frame
339			consTorque = crossProduct(a.transpose() * refCoor, consForce);
340			consRB->addConsTorque(consTorque/eConvert);
341	tim	1254
342	tim	1452	//add constraint torque
343			totConsTorque = consRB->getConsTorque();
344			rb->addTrq(totConsTorque.vec);
345
346			//get and convert the torque to body frame
347	tim	1254
348	tim	1452	rb->getTrq(Tb.vec);
349			rb->lab2Body(Tb.vec);
350	tim	1254
351	tim	1452	//get the angular momentum, and propagate a half step
352	tim	1254
353	tim	1452	rb->getJ(ji.vec);
354	tim	1284
355	tim	1452	ji += eConvert * dtOver2 * Tb;
356
357			rotationPropagation( rb, ji.vec );
358
359			rb->setJ(ji.vec);
360	tim	1254
361			}
362
363			void DCRollAFunctor::rotationPropagation(StuntDouble* sd, double ji[3]){
364			double angle;
365			double A[3][3], I[3][3];
366			int i, j, k;
367			double dtOver2;
368	tim	1452	double dt;
369			dt = info->dt;
370			dtOver2 = dt /2;
371	tim	1254	// use the angular velocities to propagate the rotation matrix a
372			// full time step
373
374			sd->getA(A);
375			sd->getI(I);
376
377			if (sd->isLinear()) {
378			i = sd->linearAxis();
379			j = (i+1)%3;
380			k = (i+2)%3;
381
382			angle = dtOver2 * ji[j] / I[j][j];
383			this->rotate( k, i, angle, ji, A );
384
385	tim	1452	angle = dt* ji[k] / I[k][k];
386	tim	1254	this->rotate( i, j, angle, ji, A);
387
388			angle = dtOver2 * ji[j] / I[j][j];
389			this->rotate( k, i, angle, ji, A );
390
391			} else {
392			// rotate about the x-axis
393			angle = dtOver2 * ji[0] / I[0][0];
394			this->rotate( 1, 2, angle, ji, A );
395
396			// rotate about the y-axis
397			angle = dtOver2 * ji[1] / I[1][1];
398			this->rotate( 2, 0, angle, ji, A );
399
400			// rotate about the z-axis
401	tim	1452	angle = dt * ji[2] / I[2][2];
402	tim	1254	this->rotate( 0, 1, angle, ji, A);
403
404			// rotate about the y-axis
405			angle = dtOver2 * ji[1] / I[1][1];
406			this->rotate( 2, 0, angle, ji, A );
407
408			// rotate about the x-axis
409			angle = dtOver2 * ji[0] / I[0][0];
410			this->rotate( 1, 2, angle, ji, A );
411
412			}
413			sd->setA( A );
414			}
415
416			void DCRollAFunctor::rotate(int axes1, int axes2, double angle, double ji[3], double A[3][3]){
417			int i, j, k;
418			double sinAngle;
419			double cosAngle;
420			double angleSqr;
421			double angleSqrOver4;
422			double top, bottom;
423			double rot[3][3];
424			double tempA[3][3];
425			double tempJ[3];
426
427			// initialize the tempA
428
429			for (i = 0; i < 3; i++){
430			for (j = 0; j < 3; j++){
431			tempA[j][i] = A[i][j];
432			}
433			}
434
435			// initialize the tempJ
436
437			for (i = 0; i < 3; i++)
438			tempJ[i] = ji[i];
439
440			// initalize rot as a unit matrix
441
442			rot[0][0] = 1.0;
443			rot[0][1] = 0.0;
444			rot[0][2] = 0.0;
445
446			rot[1][0] = 0.0;
447			rot[1][1] = 1.0;
448			rot[1][2] = 0.0;
449
450			rot[2][0] = 0.0;
451			rot[2][1] = 0.0;
452			rot[2][2] = 1.0;
453
454			// use a small angle aproximation for sin and cosine
455
456			angleSqr = angle * angle;
457			angleSqrOver4 = angleSqr / 4.0;
458			top = 1.0 - angleSqrOver4;
459			bottom = 1.0 + angleSqrOver4;
460
461			cosAngle = top / bottom;
462			sinAngle = angle / bottom;
463
464			rot[axes1][axes1] = cosAngle;
465			rot[axes2][axes2] = cosAngle;
466
467			rot[axes1][axes2] = sinAngle;
468			rot[axes2][axes1] = -sinAngle;
469
470			// rotate the momentum acoording to: ji[] = rot[][] * ji[]
471
472			for (i = 0; i < 3; i++){
473			ji[i] = 0.0;
474			for (k = 0; k < 3; k++){
475			ji[i] += rot[i][k] * tempJ[k];
476			}
477			}
478
479			// rotate the Rotation matrix acording to:
480			// A[][] = A[][] * transpose(rot[][])
481
482
483			// NOte for as yet unknown reason, we are performing the
484			// calculation as:
485			// transpose(A[][]) = transpose(A[][]) * transpose(rot[][])
486
487			for (i = 0; i < 3; i++){
488			for (j = 0; j < 3; j++){
489			A[j][i] = 0.0;
490			for (k = 0; k < 3; k++){
491			A[j][i] += tempA[i][k] * rot[j][k];
492			}
493			}
494			}
495			}
496			////////////////////////////////////////////////////////////////////////////////
497			//Implementation of DCRollBFunctor
498			////////////////////////////////////////////////////////////////////////////////
499			int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
500	tim	1255	Vector3d posA;
501			Vector3d posB;
502			Vector3d velA;
503			Vector3d velB;
504			Vector3d pab;
505			Vector3d rab;
506			Vector3d vab;
507	tim	1284	Vector3d zetaA;
508			Vector3d zetaB;
509			Vector3d zeta;
510	tim	1255	Vector3d consForce;
511			Vector3d bondDirUnitVec;
512			double dt;
513	tim	1284	double pabDotvab;
514			double pabDotZeta;
515			double pvab;
516	tim	1255
517	tim	1452	const int conRBMaxIter = 20;
518	tim	1255
519			dt = info->dt;
520
521			for(int i=0 ; i < conRBMaxIter; i++){
522			consRB1->getCurAtomPos(posA.vec);
523			consRB2->getCurAtomPos(posB.vec);
524	tim	1284	pab = posB - posA;
525	tim	1255
526			//periodic boundary condition
527			info->wrapVector(pab.vec);
528	tim	1284
529			consRB1->getCurAtomVel(velA.vec);
530			consRB2->getCurAtomVel(velB.vec);
531			vab = velB -velA;
532	tim	1255
533	tim	1284	pvab = dotProduct(pab, vab);
534	tim	1255
535	tim	1284	if (fabs(pvab) > consTolerance ){
536	tim	1255
537
538			bondDirUnitVec = pab;
539			bondDirUnitVec.normalize();
540
541	tim	1284	getZeta(consRB1, bondDirUnitVec, zetaA);
542			getZeta(consRB2, bondDirUnitVec, zetaB);
543			zeta = zetaA + zetaB;
544	tim	1255
545	tim	1284	pabDotZeta = dotProduct(pab, zeta);
546
547	tim	1452	consForce = 2 * pvab / (dt * pabDotZeta) * bondDirUnitVec;
548	tim	1255	//integrate consRB1 using constraint force;
549	tim	1284	integrate(consRB1, consForce);
550	tim	1255
551			//integrate consRB2 using constraint force;
552			integrate(consRB2, -consForce);
553
554			}
555			else{
556			if (i ==0)
557			return consAlready;
558			else
559			return consSuccess;
560			}
561			}
562
563	tim	1284	cerr << "DCRollBFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;
564	tim	1255	return consExceedMaxIter;
565
566	tim	1254	}
567
568	tim	1284	void DCRollBFunctor::getZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
569	tim	1255	double invMass;
570			Vector3d tempVec1;
571			Vector3d tempVec2;
572			Vector3d refCoor;
573			Vector3d refCrossBond;
574			Mat3x3d IBody;
575	tim	1284	Mat3x3d ILab;
576	tim	1255	Mat3x3d invIBody;
577	tim	1284	Mat3x3d invILab;
578	tim	1255	Mat3x3d a;
579
580			invMass = 1.0 / consRB ->getMass();
581	tim	1254
582	tim	1284	zeta = invMass * bondDir;
583	tim	1255
584			consRB->getRefCoor(refCoor.vec);
585			consRB->getA(a.element);
586			consRB->getI(IBody.element);
587
588			invIBody = IBody.inverse();
589
590	tim	1452
591			refCrossBond = crossProduct(refCoor, a * bondDir);
592	tim	1255
593	tim	1452	tempVec1 = invIBody * refCrossBond;
594	tim	1255
595	tim	1452	tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor;
596
597			zeta += tempVec2;
598	tim	1255
599	tim	1254	}
600
601			void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
602	tim	1255	Vector3d vel;
603			Vector3d ji;
604	tim	1284	Vector3d tempJi;
605			Vector3d tempTrq;
606			Vector3d refCoor;
607	tim	1255	double mass;
608			double dtOver2;
609	tim	1452	Mat3x3d a;
610	tim	1255	StuntDouble* sd;
611
612			sd = consRB->getStuntDouble();
613			dtOver2 = info->dt/2;
614	tim	1254
615	tim	1284	sd->getVel(vel.vec);
616	tim	1255	mass = sd->getMass();
617	tim	1284	vel +=dtOver2 /mass * force;
618	tim	1255	sd->setVel(vel.vec);
619
620			if (sd->isDirectional()){
621	tim	1452	sd->getA(a.element);
622			consRB->getRefCoor(refCoor.vec);
623			tempTrq = crossProduct(refCoor, a *force);
624
625	tim	1284	tempJi = dtOver2* tempTrq;
626	tim	1255	sd->getJ(ji.vec);
627	tim	1284	ji += tempJi;
628	tim	1255	sd->setJ(ji.vec);
629			}
630
631			}