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;
  
  const int conRBMaxIter = 10;
  
  dt = info->dt;

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


  for(int i=0 ; i < conRBMaxIter; i++){   
    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);    
      
    }
    else{
      if (i ==0)
        return consAlready;
      else
        return consSuccess;
    }
  }

  return consExceedMaxIter;

}
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->getI(IBody.element);

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

  invILab = aTrans * invIBody * a;

  refCrossBond = crossProduct(refCoor, bondDir);  

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

  zeta += tempVec2;
   
}

void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
  StuntDouble* sd;
  Vector3d vel;
  Vector3d pos;
  Vector3d Tb;
  Vector3d ji;
  Vector3d tempPos;
  Vector3d tempVel;
  Vector3d tempTrqLab;
  Vector3d tempTrqBody;  
  Vector3d tempJi;
  Vector3d refCoor;
  double mass;
  Mat3x3d oldA;
  double dt;
  double dtOver2;
  dt = info->dt;
  dtOver2 = dt /2;

  consRB->getOldA(oldA.element);
  sd = consRB->getStuntDouble();
  
  sd->getVel(vel.vec);
  sd->getPos(pos.vec);
  
  mass = sd->getMass();

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

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

  if (sd->isDirectional()){

    consRB->getRefCoor(refCoor.vec);
    tempTrqLab = crossProduct(refCoor, force);

    //convert torque in lab frame to torque in body frame using old rotation matrix
    //tempTrqBody = oldA * tempTrqLab;
    
    //tempJi = dtOver2 * tempTrqBody;
    sd->lab2Body(tempTrqLab.vec);
    tempJi = dtOver2 * tempTrqLab;
    rotationPropagation( sd, tempJi.vec);

    sd->getJ(ji.vec);

    ji += tempJi;

    sd->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;

  dtOver2 = info->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 = dtOver2 * 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 = dtOver2 * ji[2] / I[2][2];
    sd->addZangle(angle);
    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 = 100;
  
  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 = 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;
  Mat3x3d aTrans;
  
  invMass = 1.0 / consRB ->getMass();

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

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

  invILab = aTrans * invIBody * a;

  refCrossBond = crossProduct(refCoor, bondDir);  

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