OOPSE/libmdtools/Roll.cpp

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


////////////////////////////////////////////////////////////////////////////////
//Implementation of DCRollAFunctor
////////////////////////////////////////////////////////////////////////////////
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 rma;
  Vector3d rmb;
  Vector3d consForce;
  Vector3d bondDirUnitVec;  
  double dx, dy, dz;
  double rpab;
  double rabsq, pabsq, rpabsq;
  double diffsq;
  double gab;
  double dt;
  double dt2;
  double rijcDotInvMassVec;


  const int conRBMaxIter = 10;
  
  dt = info->dt;
  dt2 = dt * dt;

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


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

    pab = posA - posB;

    //periodic boundary condition

    info->wrapVector(pab.vec);

    pabsq = dotProduct(pab, pab);

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

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

      rpab = dotProduct(rab, pab);

      rpabsq = rpab * rpab;


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

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

      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);

      rijcDotInvMassVec = dotProduct(pab,  rma + rmb);
      
      consForce = diffsq /(2 * dt * dt * rijcDotInvMassVec) * 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::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 DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
  StuntDouble* sd;
  Vector3d vel;
  Vector3d pos;
  Vector3d Tb;
  Vector3d ji;
  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();

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

  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){
  return consElemHandlerFail;
}

void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){

}

void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){

}
Revision:	1254
Committed:	Wed Jun 9 16:16:33 2004 UTC (20 years, 3 months ago) by tim
File size:	7204 byte(s)
Log Message:	1. adding some useful math classes(Mat3x3d, Vector3d, Quaternion, Euler3) these classes use anonymous union and struct to support double[3], double[3][3] and double[4] 2. adding roll constraint algorithm
#	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			int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
11			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			Vector3d rma;
21			Vector3d rmb;
22			Vector3d consForce;
23			Vector3d bondDirUnitVec;
24			double dx, dy, dz;
25			double rpab;
26			double rabsq, pabsq, rpabsq;
27			double diffsq;
28			double gab;
29			double dt;
30			double dt2;
31			double rijcDotInvMassVec;
32
33
34			const int conRBMaxIter = 10;
35
36			dt = info->dt;
37			dt2 = dt * dt;
38
39			consRB1->getOldAtomPos(oldPosA.vec);
40			consRB2->getOldAtomPos(oldPosB.vec);
41
42
43			for(int i=0 ; i < conRBMaxIter; i++){
44			consRB1->getCurAtomPos(posA.vec);
45			consRB2->getCurAtomPos(posB.vec);
46
47			pab = posA - posB;
48
49			//periodic boundary condition
50
51			info->wrapVector(pab.vec);
52
53			pabsq = dotProduct(pab, pab);
54
55			rabsq = curPair->getBondLength2();
56			diffsq = rabsq - pabsq;
57
58			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
59			rab = oldPosA - oldPosB;
60			info->wrapVector(rab.vec);
61
62			rpab = dotProduct(rab, pab);
63
64			rpabsq = rpab * rpab;
65
66
67			//if (rpabsq < (rabsq * -diffsq)){
68			// return consFail;
69			//}
70
71			bondDirUnitVec = pab;
72			bondDirUnitVec.normalize();
73
74			getEffInvMassVec(consRB1, bondDirUnitVec, rma);
75
76			getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
77
78			rijcDotInvMassVec = dotProduct(pab, rma + rmb);
79
80			consForce = diffsq /(2 * dt * dt * rijcDotInvMassVec) * bondDirUnitVec;
81			//integrate consRB1 using constraint force;
82			integrate(consRB1,consForce);
83
84			//integrate consRB2 using constraint force;
85			integrate(consRB2, -consForce);
86
87			}
88			else{
89			if (i ==0)
90			return consAlready;
91			else
92			return consSuccess;
93			}
94			}
95
96			return consExceedMaxIter;
97
98			}
99
100			void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
101			double invMass;
102			Vector3d tempVec1;
103			Vector3d tempVec2;
104			Vector3d refCoor;
105			Vector3d refCrossBond;
106			Mat3x3d IBody;
107			Mat3x3d IFrame;
108			Mat3x3d invIBody;
109			Mat3x3d invIFrame;
110			Mat3x3d a;
111			Mat3x3d aTrans;
112
113			invMass = 1.0 / consRB ->getMass();
114
115			invMassVec = invMass * bondDir;
116
117			consRB->getRefCoor(refCoor.vec);
118			consRB->getA(a.element);
119			consRB->getI(IBody.element);
120
121			aTrans = a.transpose();
122			invIBody = IBody.inverse();
123
124			IFrame = aTrans * invIBody * a;
125
126			refCrossBond = crossProduct(refCoor, bondDir);
127
128			tempVec1 = invIFrame * refCrossBond;
129			tempVec2 = crossProduct(tempVec1, refCoor);
130
131			invMassVec += tempVec2;
132
133			}
134
135			void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
136			StuntDouble* sd;
137			Vector3d vel;
138			Vector3d pos;
139			Vector3d Tb;
140			Vector3d ji;
141			double mass;
142			double dtOver2;
143			double dt;
144			const double eConvert = 4.184e-4;
145
146			dt = info->dt;
147			dtOver2 = dt /2;
148			sd = consRB->getStuntDouble();
149
150			sd->getVel(vel.vec);
151			sd->getPos(pos.vec);
152
153			mass = sd->getMass();
154
155			vel += eConvert * dtOver2/mass * force;
156			pos += dt * vel;
157
158			sd->setVel(vel.vec);
159			sd->setPos(pos.vec);
160
161			if (sd->isDirectional()){
162
163			// get and convert the torque to body frame
164
165			sd->getTrq(Tb.vec);
166			sd->lab2Body(Tb.vec);
167
168			// get the angular momentum, and propagate a half step
169
170			sd->getJ(ji.vec);
171
172			ji += eConvert * dtOver2 * Tb;
173
174			rotationPropagation( sd, ji.vec);
175
176			sd->setJ(ji.vec);
177			}
178
179			}
180
181			void DCRollAFunctor::rotationPropagation(StuntDouble* sd, double ji[3]){
182			double angle;
183			double A[3][3], I[3][3];
184			int i, j, k;
185			double dtOver2;
186
187			dtOver2 = info->dt /2;
188			// use the angular velocities to propagate the rotation matrix a
189			// full time step
190
191			sd->getA(A);
192			sd->getI(I);
193
194			if (sd->isLinear()) {
195			i = sd->linearAxis();
196			j = (i+1)%3;
197			k = (i+2)%3;
198
199			angle = dtOver2 * ji[j] / I[j][j];
200			this->rotate( k, i, angle, ji, A );
201
202			angle = dtOver2 * ji[k] / I[k][k];
203			this->rotate( i, j, angle, ji, A);
204
205			angle = dtOver2 * ji[j] / I[j][j];
206			this->rotate( k, i, angle, ji, A );
207
208			} else {
209			// rotate about the x-axis
210			angle = dtOver2 * ji[0] / I[0][0];
211			this->rotate( 1, 2, angle, ji, A );
212
213			// rotate about the y-axis
214			angle = dtOver2 * ji[1] / I[1][1];
215			this->rotate( 2, 0, angle, ji, A );
216
217			// rotate about the z-axis
218			angle = dtOver2 * ji[2] / I[2][2];
219			sd->addZangle(angle);
220			this->rotate( 0, 1, angle, ji, A);
221
222			// rotate about the y-axis
223			angle = dtOver2 * ji[1] / I[1][1];
224			this->rotate( 2, 0, angle, ji, A );
225
226			// rotate about the x-axis
227			angle = dtOver2 * ji[0] / I[0][0];
228			this->rotate( 1, 2, angle, ji, A );
229
230			}
231			sd->setA( A );
232			}
233
234			void DCRollAFunctor::rotate(int axes1, int axes2, double angle, double ji[3], double A[3][3]){
235			int i, j, k;
236			double sinAngle;
237			double cosAngle;
238			double angleSqr;
239			double angleSqrOver4;
240			double top, bottom;
241			double rot[3][3];
242			double tempA[3][3];
243			double tempJ[3];
244
245			// initialize the tempA
246
247			for (i = 0; i < 3; i++){
248			for (j = 0; j < 3; j++){
249			tempA[j][i] = A[i][j];
250			}
251			}
252
253			// initialize the tempJ
254
255			for (i = 0; i < 3; i++)
256			tempJ[i] = ji[i];
257
258			// initalize rot as a unit matrix
259
260			rot[0][0] = 1.0;
261			rot[0][1] = 0.0;
262			rot[0][2] = 0.0;
263
264			rot[1][0] = 0.0;
265			rot[1][1] = 1.0;
266			rot[1][2] = 0.0;
267
268			rot[2][0] = 0.0;
269			rot[2][1] = 0.0;
270			rot[2][2] = 1.0;
271
272			// use a small angle aproximation for sin and cosine
273
274			angleSqr = angle * angle;
275			angleSqrOver4 = angleSqr / 4.0;
276			top = 1.0 - angleSqrOver4;
277			bottom = 1.0 + angleSqrOver4;
278
279			cosAngle = top / bottom;
280			sinAngle = angle / bottom;
281
282			rot[axes1][axes1] = cosAngle;
283			rot[axes2][axes2] = cosAngle;
284
285			rot[axes1][axes2] = sinAngle;
286			rot[axes2][axes1] = -sinAngle;
287
288			// rotate the momentum acoording to: ji[] = rot[][] * ji[]
289
290			for (i = 0; i < 3; i++){
291			ji[i] = 0.0;
292			for (k = 0; k < 3; k++){
293			ji[i] += rot[i][k] * tempJ[k];
294			}
295			}
296
297			// rotate the Rotation matrix acording to:
298			// A[][] = A[][] * transpose(rot[][])
299
300
301			// NOte for as yet unknown reason, we are performing the
302			// calculation as:
303			// transpose(A[][]) = transpose(A[][]) * transpose(rot[][])
304
305			for (i = 0; i < 3; i++){
306			for (j = 0; j < 3; j++){
307			A[j][i] = 0.0;
308			for (k = 0; k < 3; k++){
309			A[j][i] += tempA[i][k] * rot[j][k];
310			}
311			}
312			}
313			}
314			////////////////////////////////////////////////////////////////////////////////
315			//Implementation of DCRollBFunctor
316			////////////////////////////////////////////////////////////////////////////////
317			int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
318			return consElemHandlerFail;
319			}
320
321			void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
322
323			}
324
325			void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
326
327			}