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); 
      //
      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 dtOver2;
  double dt;
  const double eConvert = 4.184e-4;
  
  dt = info->dt;
  dtOver2 = dt /2;  
  sd = consAtom->getStuntDouble();
  
  sd->getVel(vel.vec);
  sd->getPos(pos.vec);
  
  mass = sd->getMass();

  tempVel = eConvert * dtOver2/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 = 10;
  
  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);

      //rpab = dotProduct(rab, pab);

      //rpabsq = rpab * rpab;


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

      bondDirUnitVec = pab;
      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)
        return consFail;

      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
      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;
    }
  }

  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 IFrame;
  Mat3x3d invIBody;
  Mat3x3d invIFrame;
  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();

  IFrame = aTrans * invIBody * a;

  refCrossBond = crossProduct(refCoor, bondDir);  

  tempVec1 = invIFrame * 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 tempTrq;
        Vector3d tempJi;
  double mass;
  double dtOver2;
  double dt;
  const double eConvert = 4.184e-4;
  
  dt = info->dt;
  dtOver2 = dt /2;  
  sd = consRB->getStuntDouble();
  
  sd->getVel(vel.vec);
  sd->getPos(pos.vec);
  
  mass = sd->getMass();

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

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

  if (sd->isDirectional()){

    // get and convert the torque to body frame

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

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

    sd->getJ(ji.vec);

    ji += eConvert * dtOver2 * Tb;

    rotationPropagation( sd, ji.vec);

    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 rma;
  Vector3d rmb;
  Vector3d consForce;
  Vector3d bondDirUnitVec;  
  double dx, dy, dz;
  double rpab;
  double rabsq, pabsq, rpabsq;
  double diffsq;
  double gab;
  double dt;
  double pabcDotvab;
  double pabDotInvMassVec;


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

    //periodic boundary condition

    info->wrapVector(pab.vec);

    pabsq = pab.length2();

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

    if (fabs(diffsq) > (consTolerance * rabsq * 2)){


      bondDirUnitVec = pab;
      bondDirUnitVec.normalize();
          
      getEffInvMassVec(consRB1, bondDirUnitVec, rma);

      getEffInvMassVec(consRB2, bondDirUnitVec, rmb);

      pabcDotvab = dotProduct(pab, vab);
      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
      
      consForce = pabcDotvab /(2 * dt * pabDotInvMassVec) * 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;
    }
  }

  return consExceedMaxIter;

}

void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
  double invMass;
  Vector3d tempVec1;
  Vector3d tempVec2;
  Vector3d refCoor;
  Vector3d refCrossBond;  
  Mat3x3d IBody;
  Mat3x3d IFrame;
  Mat3x3d invIBody;
  Mat3x3d invIFrame;
  Mat3x3d a;
  Mat3x3d aTrans;
  
  invMass = 1.0 / consRB ->getMass();

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

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

  IFrame = aTrans * invIBody * a;

  refCrossBond = crossProduct(refCoor, bondDir);  

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

  invMassVec += tempVec2;
}

void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
  const double eConvert = 4.184e-4;
  Vector3d vel;
  Vector3d pos;
  Vector3d Tb;
  Vector3d ji;
  double mass;
  double dtOver2;
  StuntDouble* sd;
  
  sd = consRB->getStuntDouble();  
  dtOver2 = info->dt/2;

  mass = sd->getMass();

  // velocity half step

  vel += eConvert * dtOver2 /mass * force;

  sd->setVel(vel.vec);

  if (sd->isDirectional()){

    // get and convert the torque to body frame

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

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

    sd->getJ(ji.vec);

    ji += eConvert * dtOver2* Tb;

    sd->setJ(ji.vec);
  }
  
}
Revision:	1268
Committed:	Fri Jun 11 17:16:21 2004 UTC (20 years, 1 month ago) by tim
File size:	13972 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			if (pabDotZeta2 - zeta2 * diffsq < 0)
95			return consFail;
96
97			//forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
98			forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
99			//
100			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
133			double mass;
134			double dtOver2;
135			double dt;
136			const double eConvert = 4.184e-4;
137
138			dt = info->dt;
139			dtOver2 = dt /2;
140			sd = consAtom->getStuntDouble();
141
142			sd->getVel(vel.vec);
143			sd->getPos(pos.vec);
144
145			mass = sd->getMass();
146
147			tempVel = eConvert * dtOver2/mass * force;
148			tempPos = dt * tempVel;
149
150			vel += tempVel;
151			pos += tempPos;
152
153			sd->setVel(vel.vec);
154			sd->setPos(pos.vec);
155			}
156
157			int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
158			Vector3d posA;
159			Vector3d posB;
160			Vector3d oldPosA;
161			Vector3d oldPosB;
162			Vector3d velA;
163			Vector3d velB;
164			Vector3d pab;
165			Vector3d tempPab;
166			Vector3d rab;
167			Vector3d zetaA;
168			Vector3d zetaB;
169			Vector3d zeta;
170			Vector3d consForce;
171			Vector3d bondDirUnitVec;
172			double dx, dy, dz;
173			double rpab;
174			double rabsq, pabsq, rpabsq;
175			double diffsq;
176			double gab;
177			double dt;
178			double pabDotZeta;
179			double pabDotZeta2;
180			double zeta2;
181			double forceScalar;
182
183	tim	1254	const int conRBMaxIter = 10;
184
185			dt = info->dt;
186
187			consRB1->getOldAtomPos(oldPosA.vec);
188			consRB2->getOldAtomPos(oldPosB.vec);
189
190
191			for(int i=0 ; i < conRBMaxIter; i++){
192			consRB1->getCurAtomPos(posA.vec);
193			consRB2->getCurAtomPos(posB.vec);
194
195	tim	1268	//discard the vector convention in alan tidesley's code
196			//rij = rj - ri;
197			pab = posB - posA;
198	tim	1254
199			//periodic boundary condition
200
201			info->wrapVector(pab.vec);
202
203			pabsq = dotProduct(pab, pab);
204
205			rabsq = curPair->getBondLength2();
206	tim	1268	diffsq = pabsq -rabsq;
207	tim	1254
208			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
209	tim	1268	rab = oldPosB - oldPosA;
210	tim	1254	info->wrapVector(rab.vec);
211
212	tim	1268	//rpab = dotProduct(rab, pab);
213	tim	1254
214	tim	1268	//rpabsq = rpab * rpab;
215	tim	1254
216
217			//if (rpabsq < (rabsq * -diffsq)){
218			// return consFail;
219			//}
220
221			bondDirUnitVec = pab;
222			bondDirUnitVec.normalize();
223
224	tim	1268	calcZeta(consRB1, bondDirUnitVec, zetaA);
225	tim	1254
226	tim	1268	calcZeta(consRB2, bondDirUnitVec, zetaB);
227	tim	1254
228	tim	1268	zeta = zetaA + zetaB;
229			zeta2 = dotProduct(zeta, zeta);
230	tim	1254
231	tim	1268	pabDotZeta = dotProduct(pab, zeta);
232			pabDotZeta2 = pabDotZeta * pabDotZeta;
233
234			//solve quadratic equation
235			//dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
236			//dt : time step
237			// pab :
238			//G : constraint force scalar
239			//d: equilibrium bond length
240
241			if (pabDotZeta2 - zeta2 * diffsq < 0)
242			return consFail;
243
244			//forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
245			forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
246			//
247			consForce = forceScalar * bondDirUnitVec;
248	tim	1254	//integrate consRB1 using constraint force;
249	tim	1268	integrate(consRB1, consForce);
250	tim	1254
251			//integrate consRB2 using constraint force;
252			integrate(consRB2, -consForce);
253
254			}
255			else{
256			if (i ==0)
257			return consAlready;
258			else
259			return consSuccess;
260			}
261			}
262
263			return consExceedMaxIter;
264
265			}
266
267	tim	1268	void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
268	tim	1254	double invMass;
269			Vector3d tempVec1;
270			Vector3d tempVec2;
271			Vector3d refCoor;
272			Vector3d refCrossBond;
273			Mat3x3d IBody;
274			Mat3x3d IFrame;
275			Mat3x3d invIBody;
276			Mat3x3d invIFrame;
277			Mat3x3d a;
278			Mat3x3d aTrans;
279
280			invMass = 1.0 / consRB ->getMass();
281
282	tim	1268	zeta = invMass * bondDir;
283	tim	1254
284			consRB->getRefCoor(refCoor.vec);
285			consRB->getA(a.element);
286			consRB->getI(IBody.element);
287
288			aTrans = a.transpose();
289			invIBody = IBody.inverse();
290
291			IFrame = aTrans * invIBody * a;
292
293			refCrossBond = crossProduct(refCoor, bondDir);
294
295			tempVec1 = invIFrame * refCrossBond;
296			tempVec2 = crossProduct(tempVec1, refCoor);
297
298	tim	1268	zeta += tempVec2;
299	tim	1254
300			}
301
302			void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
303			StuntDouble* sd;
304			Vector3d vel;
305			Vector3d pos;
306			Vector3d Tb;
307			Vector3d ji;
308	tim	1268	Vector3d tempPos;
309			Vector3d tempVel;
310			Vector3d tempTrq;
311			Vector3d tempJi;
312	tim	1254	double mass;
313			double dtOver2;
314			double dt;
315			const double eConvert = 4.184e-4;
316
317			dt = info->dt;
318			dtOver2 = dt /2;
319			sd = consRB->getStuntDouble();
320
321			sd->getVel(vel.vec);
322			sd->getPos(pos.vec);
323
324			mass = sd->getMass();
325
326	tim	1268	tempVel = eConvert * dtOver2/mass * force;
327			tempPos = dt * tempVel;
328
329			vel += tempVel;
330			pos += tempPos;
331	tim	1254
332			sd->setVel(vel.vec);
333			sd->setPos(pos.vec);
334
335			if (sd->isDirectional()){
336
337			// get and convert the torque to body frame
338
339			sd->getTrq(Tb.vec);
340			sd->lab2Body(Tb.vec);
341
342			// get the angular momentum, and propagate a half step
343
344			sd->getJ(ji.vec);
345
346			ji += eConvert * dtOver2 * Tb;
347
348			rotationPropagation( sd, ji.vec);
349
350			sd->setJ(ji.vec);
351			}
352
353			}
354
355			void DCRollAFunctor::rotationPropagation(StuntDouble* sd, double ji[3]){
356			double angle;
357			double A[3][3], I[3][3];
358			int i, j, k;
359			double dtOver2;
360
361			dtOver2 = info->dt /2;
362			// use the angular velocities to propagate the rotation matrix a
363			// full time step
364
365			sd->getA(A);
366			sd->getI(I);
367
368			if (sd->isLinear()) {
369			i = sd->linearAxis();
370			j = (i+1)%3;
371			k = (i+2)%3;
372
373			angle = dtOver2 * ji[j] / I[j][j];
374			this->rotate( k, i, angle, ji, A );
375
376			angle = dtOver2 * ji[k] / I[k][k];
377			this->rotate( i, j, angle, ji, A);
378
379			angle = dtOver2 * ji[j] / I[j][j];
380			this->rotate( k, i, angle, ji, A );
381
382			} else {
383			// rotate about the x-axis
384			angle = dtOver2 * ji[0] / I[0][0];
385			this->rotate( 1, 2, angle, ji, A );
386
387			// rotate about the y-axis
388			angle = dtOver2 * ji[1] / I[1][1];
389			this->rotate( 2, 0, angle, ji, A );
390
391			// rotate about the z-axis
392			angle = dtOver2 * ji[2] / I[2][2];
393			sd->addZangle(angle);
394			this->rotate( 0, 1, 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 x-axis
401			angle = dtOver2 * ji[0] / I[0][0];
402			this->rotate( 1, 2, angle, ji, A );
403
404			}
405			sd->setA( A );
406			}
407
408			void DCRollAFunctor::rotate(int axes1, int axes2, double angle, double ji[3], double A[3][3]){
409			int i, j, k;
410			double sinAngle;
411			double cosAngle;
412			double angleSqr;
413			double angleSqrOver4;
414			double top, bottom;
415			double rot[3][3];
416			double tempA[3][3];
417			double tempJ[3];
418
419			// initialize the tempA
420
421			for (i = 0; i < 3; i++){
422			for (j = 0; j < 3; j++){
423			tempA[j][i] = A[i][j];
424			}
425			}
426
427			// initialize the tempJ
428
429			for (i = 0; i < 3; i++)
430			tempJ[i] = ji[i];
431
432			// initalize rot as a unit matrix
433
434			rot[0][0] = 1.0;
435			rot[0][1] = 0.0;
436			rot[0][2] = 0.0;
437
438			rot[1][0] = 0.0;
439			rot[1][1] = 1.0;
440			rot[1][2] = 0.0;
441
442			rot[2][0] = 0.0;
443			rot[2][1] = 0.0;
444			rot[2][2] = 1.0;
445
446			// use a small angle aproximation for sin and cosine
447
448			angleSqr = angle * angle;
449			angleSqrOver4 = angleSqr / 4.0;
450			top = 1.0 - angleSqrOver4;
451			bottom = 1.0 + angleSqrOver4;
452
453			cosAngle = top / bottom;
454			sinAngle = angle / bottom;
455
456			rot[axes1][axes1] = cosAngle;
457			rot[axes2][axes2] = cosAngle;
458
459			rot[axes1][axes2] = sinAngle;
460			rot[axes2][axes1] = -sinAngle;
461
462			// rotate the momentum acoording to: ji[] = rot[][] * ji[]
463
464			for (i = 0; i < 3; i++){
465			ji[i] = 0.0;
466			for (k = 0; k < 3; k++){
467			ji[i] += rot[i][k] * tempJ[k];
468			}
469			}
470
471			// rotate the Rotation matrix acording to:
472			// A[][] = A[][] * transpose(rot[][])
473
474
475			// NOte for as yet unknown reason, we are performing the
476			// calculation as:
477			// transpose(A[][]) = transpose(A[][]) * transpose(rot[][])
478
479			for (i = 0; i < 3; i++){
480			for (j = 0; j < 3; j++){
481			A[j][i] = 0.0;
482			for (k = 0; k < 3; k++){
483			A[j][i] += tempA[i][k] * rot[j][k];
484			}
485			}
486			}
487			}
488			////////////////////////////////////////////////////////////////////////////////
489			//Implementation of DCRollBFunctor
490			////////////////////////////////////////////////////////////////////////////////
491			int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
492	tim	1255	Vector3d posA;
493			Vector3d posB;
494			Vector3d velA;
495			Vector3d velB;
496			Vector3d pab;
497			Vector3d rab;
498			Vector3d vab;
499			Vector3d rma;
500			Vector3d rmb;
501			Vector3d consForce;
502			Vector3d bondDirUnitVec;
503			double dx, dy, dz;
504			double rpab;
505			double rabsq, pabsq, rpabsq;
506			double diffsq;
507			double gab;
508			double dt;
509			double pabcDotvab;
510			double pabDotInvMassVec;
511
512
513			const int conRBMaxIter = 10;
514
515			dt = info->dt;
516
517			for(int i=0 ; i < conRBMaxIter; i++){
518			consRB1->getCurAtomPos(posA.vec);
519			consRB2->getCurAtomPos(posB.vec);
520			pab = posA - posB;
521
522			consRB1->getVel(velA.vec);
523			consRB2->getVel(velB.vec);
524			vab = velA -velB;
525
526			//periodic boundary condition
527
528			info->wrapVector(pab.vec);
529
530			pabsq = pab.length2();
531
532			rabsq = curPair->getBondLength2();
533			diffsq = rabsq - pabsq;
534
535			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
536
537
538			bondDirUnitVec = pab;
539			bondDirUnitVec.normalize();
540
541			getEffInvMassVec(consRB1, bondDirUnitVec, rma);
542
543	tim	1268	getEffInvMassVec(consRB2, bondDirUnitVec, rmb);
544	tim	1255
545			pabcDotvab = dotProduct(pab, vab);
546			pabDotInvMassVec = dotProduct(pab, rma + rmb);
547
548			consForce = pabcDotvab /(2 * dt * pabDotInvMassVec) * bondDirUnitVec;
549			//integrate consRB1 using constraint force;
550			integrate(consRB1,consForce);
551
552			//integrate consRB2 using constraint force;
553			integrate(consRB2, -consForce);
554
555			}
556			else{
557			if (i ==0)
558			return consAlready;
559			else
560			return consSuccess;
561			}
562			}
563
564			return consExceedMaxIter;
565
566	tim	1254	}
567
568			void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
569	tim	1255	double invMass;
570			Vector3d tempVec1;
571			Vector3d tempVec2;
572			Vector3d refCoor;
573			Vector3d refCrossBond;
574			Mat3x3d IBody;
575			Mat3x3d IFrame;
576			Mat3x3d invIBody;
577			Mat3x3d invIFrame;
578			Mat3x3d a;
579			Mat3x3d aTrans;
580
581			invMass = 1.0 / consRB ->getMass();
582	tim	1254
583	tim	1255	invMassVec = invMass * bondDir;
584
585			consRB->getRefCoor(refCoor.vec);
586			consRB->getA(a.element);
587			consRB->getI(IBody.element);
588
589			aTrans = a.transpose();
590			invIBody = IBody.inverse();
591
592			IFrame = aTrans * invIBody * a;
593
594			refCrossBond = crossProduct(refCoor, bondDir);
595
596			tempVec1 = invIFrame * refCrossBond;
597			tempVec2 = crossProduct(tempVec1, refCoor);
598
599			invMassVec += tempVec2;
600	tim	1254	}
601
602			void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
603	tim	1255	const double eConvert = 4.184e-4;
604			Vector3d vel;
605			Vector3d pos;
606			Vector3d Tb;
607			Vector3d ji;
608			double mass;
609			double dtOver2;
610			StuntDouble* sd;
611
612			sd = consRB->getStuntDouble();
613			dtOver2 = info->dt/2;
614	tim	1254
615	tim	1255	mass = sd->getMass();
616
617			// velocity half step
618
619			vel += eConvert * dtOver2 /mass * force;
620
621			sd->setVel(vel.vec);
622
623			if (sd->isDirectional()){
624
625			// get and convert the torque to body frame
626
627			sd->getTrq(Tb.vec);
628			sd->lab2Body(Tb.vec);
629
630			// get the angular momentum, and propagate a half step
631
632			sd->getJ(ji.vec);
633
634			ji += eConvert * dtOver2* Tb;
635
636			sd->setJ(ji.vec);
637			}
638
639			}