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


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

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

      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
      
      consForce = diffsq /(2 * dt * 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 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){
  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:	1255
Committed:	Wed Jun 9 16:59:03 2004 UTC (20 years, 1 month ago) by tim
File size:	9941 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			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	tim	1255	double pabDotInvMassVec;
31	tim	1254
32
33			const int conRBMaxIter = 10;
34
35			dt = info->dt;
36
37			consRB1->getOldAtomPos(oldPosA.vec);
38			consRB2->getOldAtomPos(oldPosB.vec);
39
40
41			for(int i=0 ; i < conRBMaxIter; i++){
42			consRB1->getCurAtomPos(posA.vec);
43			consRB2->getCurAtomPos(posB.vec);
44
45			pab = posA - posB;
46
47			//periodic boundary condition
48
49			info->wrapVector(pab.vec);
50
51			pabsq = dotProduct(pab, pab);
52
53			rabsq = curPair->getBondLength2();
54			diffsq = rabsq - pabsq;
55
56			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
57			rab = oldPosA - oldPosB;
58			info->wrapVector(rab.vec);
59
60			rpab = dotProduct(rab, pab);
61
62			rpabsq = rpab * rpab;
63
64
65			//if (rpabsq < (rabsq * -diffsq)){
66			// return consFail;
67			//}
68
69			bondDirUnitVec = pab;
70			bondDirUnitVec.normalize();
71
72			getEffInvMassVec(consRB1, bondDirUnitVec, rma);
73
74			getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
75
76	tim	1255	pabDotInvMassVec = dotProduct(pab, rma + rmb);
77	tim	1254
78	tim	1255	consForce = diffsq /(2 * dt * dt * pabDotInvMassVec) * bondDirUnitVec;
79	tim	1254	//integrate consRB1 using constraint force;
80			integrate(consRB1,consForce);
81
82			//integrate consRB2 using constraint force;
83			integrate(consRB2, -consForce);
84
85			}
86			else{
87			if (i ==0)
88			return consAlready;
89			else
90			return consSuccess;
91			}
92			}
93
94			return consExceedMaxIter;
95
96			}
97
98			void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
99			double invMass;
100			Vector3d tempVec1;
101			Vector3d tempVec2;
102			Vector3d refCoor;
103			Vector3d refCrossBond;
104			Mat3x3d IBody;
105			Mat3x3d IFrame;
106			Mat3x3d invIBody;
107			Mat3x3d invIFrame;
108			Mat3x3d a;
109			Mat3x3d aTrans;
110
111			invMass = 1.0 / consRB ->getMass();
112
113			invMassVec = invMass * bondDir;
114
115			consRB->getRefCoor(refCoor.vec);
116			consRB->getA(a.element);
117			consRB->getI(IBody.element);
118
119			aTrans = a.transpose();
120			invIBody = IBody.inverse();
121
122			IFrame = aTrans * invIBody * a;
123
124			refCrossBond = crossProduct(refCoor, bondDir);
125
126			tempVec1 = invIFrame * refCrossBond;
127			tempVec2 = crossProduct(tempVec1, refCoor);
128
129			invMassVec += tempVec2;
130
131			}
132
133			void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
134			StuntDouble* sd;
135			Vector3d vel;
136			Vector3d pos;
137			Vector3d Tb;
138			Vector3d ji;
139			double mass;
140			double dtOver2;
141			double dt;
142			const double eConvert = 4.184e-4;
143
144			dt = info->dt;
145			dtOver2 = dt /2;
146			sd = consRB->getStuntDouble();
147
148			sd->getVel(vel.vec);
149			sd->getPos(pos.vec);
150
151			mass = sd->getMass();
152
153			vel += eConvert * dtOver2/mass * force;
154			pos += dt * vel;
155
156			sd->setVel(vel.vec);
157			sd->setPos(pos.vec);
158
159			if (sd->isDirectional()){
160
161			// get and convert the torque to body frame
162
163			sd->getTrq(Tb.vec);
164			sd->lab2Body(Tb.vec);
165
166			// get the angular momentum, and propagate a half step
167
168			sd->getJ(ji.vec);
169
170			ji += eConvert * dtOver2 * Tb;
171
172			rotationPropagation( sd, ji.vec);
173
174			sd->setJ(ji.vec);
175			}
176
177			}
178
179			void DCRollAFunctor::rotationPropagation(StuntDouble* sd, double ji[3]){
180			double angle;
181			double A[3][3], I[3][3];
182			int i, j, k;
183			double dtOver2;
184
185			dtOver2 = info->dt /2;
186			// use the angular velocities to propagate the rotation matrix a
187			// full time step
188
189			sd->getA(A);
190			sd->getI(I);
191
192			if (sd->isLinear()) {
193			i = sd->linearAxis();
194			j = (i+1)%3;
195			k = (i+2)%3;
196
197			angle = dtOver2 * ji[j] / I[j][j];
198			this->rotate( k, i, angle, ji, A );
199
200			angle = dtOver2 * ji[k] / I[k][k];
201			this->rotate( i, j, angle, ji, A);
202
203			angle = dtOver2 * ji[j] / I[j][j];
204			this->rotate( k, i, angle, ji, A );
205
206			} else {
207			// rotate about the x-axis
208			angle = dtOver2 * ji[0] / I[0][0];
209			this->rotate( 1, 2, angle, ji, A );
210
211			// rotate about the y-axis
212			angle = dtOver2 * ji[1] / I[1][1];
213			this->rotate( 2, 0, angle, ji, A );
214
215			// rotate about the z-axis
216			angle = dtOver2 * ji[2] / I[2][2];
217			sd->addZangle(angle);
218			this->rotate( 0, 1, angle, ji, A);
219
220			// rotate about the y-axis
221			angle = dtOver2 * ji[1] / I[1][1];
222			this->rotate( 2, 0, angle, ji, A );
223
224			// rotate about the x-axis
225			angle = dtOver2 * ji[0] / I[0][0];
226			this->rotate( 1, 2, angle, ji, A );
227
228			}
229			sd->setA( A );
230			}
231
232			void DCRollAFunctor::rotate(int axes1, int axes2, double angle, double ji[3], double A[3][3]){
233			int i, j, k;
234			double sinAngle;
235			double cosAngle;
236			double angleSqr;
237			double angleSqrOver4;
238			double top, bottom;
239			double rot[3][3];
240			double tempA[3][3];
241			double tempJ[3];
242
243			// initialize the tempA
244
245			for (i = 0; i < 3; i++){
246			for (j = 0; j < 3; j++){
247			tempA[j][i] = A[i][j];
248			}
249			}
250
251			// initialize the tempJ
252
253			for (i = 0; i < 3; i++)
254			tempJ[i] = ji[i];
255
256			// initalize rot as a unit matrix
257
258			rot[0][0] = 1.0;
259			rot[0][1] = 0.0;
260			rot[0][2] = 0.0;
261
262			rot[1][0] = 0.0;
263			rot[1][1] = 1.0;
264			rot[1][2] = 0.0;
265
266			rot[2][0] = 0.0;
267			rot[2][1] = 0.0;
268			rot[2][2] = 1.0;
269
270			// use a small angle aproximation for sin and cosine
271
272			angleSqr = angle * angle;
273			angleSqrOver4 = angleSqr / 4.0;
274			top = 1.0 - angleSqrOver4;
275			bottom = 1.0 + angleSqrOver4;
276
277			cosAngle = top / bottom;
278			sinAngle = angle / bottom;
279
280			rot[axes1][axes1] = cosAngle;
281			rot[axes2][axes2] = cosAngle;
282
283			rot[axes1][axes2] = sinAngle;
284			rot[axes2][axes1] = -sinAngle;
285
286			// rotate the momentum acoording to: ji[] = rot[][] * ji[]
287
288			for (i = 0; i < 3; i++){
289			ji[i] = 0.0;
290			for (k = 0; k < 3; k++){
291			ji[i] += rot[i][k] * tempJ[k];
292			}
293			}
294
295			// rotate the Rotation matrix acording to:
296			// A[][] = A[][] * transpose(rot[][])
297
298
299			// NOte for as yet unknown reason, we are performing the
300			// calculation as:
301			// transpose(A[][]) = transpose(A[][]) * transpose(rot[][])
302
303			for (i = 0; i < 3; i++){
304			for (j = 0; j < 3; j++){
305			A[j][i] = 0.0;
306			for (k = 0; k < 3; k++){
307			A[j][i] += tempA[i][k] * rot[j][k];
308			}
309			}
310			}
311			}
312			////////////////////////////////////////////////////////////////////////////////
313			//Implementation of DCRollBFunctor
314			////////////////////////////////////////////////////////////////////////////////
315			int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
316	tim	1255	Vector3d posA;
317			Vector3d posB;
318			Vector3d velA;
319			Vector3d velB;
320			Vector3d pab;
321			Vector3d rab;
322			Vector3d vab;
323			Vector3d rma;
324			Vector3d rmb;
325			Vector3d consForce;
326			Vector3d bondDirUnitVec;
327			double dx, dy, dz;
328			double rpab;
329			double rabsq, pabsq, rpabsq;
330			double diffsq;
331			double gab;
332			double dt;
333			double pabcDotvab;
334			double pabDotInvMassVec;
335
336
337			const int conRBMaxIter = 10;
338
339			dt = info->dt;
340
341			for(int i=0 ; i < conRBMaxIter; i++){
342			consRB1->getCurAtomPos(posA.vec);
343			consRB2->getCurAtomPos(posB.vec);
344			pab = posA - posB;
345
346			consRB1->getVel(velA.vec);
347			consRB2->getVel(velB.vec);
348			vab = velA -velB;
349
350			//periodic boundary condition
351
352			info->wrapVector(pab.vec);
353
354			pabsq = pab.length2();
355
356			rabsq = curPair->getBondLength2();
357			diffsq = rabsq - pabsq;
358
359			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
360
361
362			bondDirUnitVec = pab;
363			bondDirUnitVec.normalize();
364
365			getEffInvMassVec(consRB1, bondDirUnitVec, rma);
366
367			getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
368
369			pabcDotvab = dotProduct(pab, vab);
370			pabDotInvMassVec = dotProduct(pab, rma + rmb);
371
372			consForce = pabcDotvab /(2 * dt * pabDotInvMassVec) * bondDirUnitVec;
373			//integrate consRB1 using constraint force;
374			integrate(consRB1,consForce);
375
376			//integrate consRB2 using constraint force;
377			integrate(consRB2, -consForce);
378
379			}
380			else{
381			if (i ==0)
382			return consAlready;
383			else
384			return consSuccess;
385			}
386			}
387
388			return consExceedMaxIter;
389
390	tim	1254	}
391
392			void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
393	tim	1255	double invMass;
394			Vector3d tempVec1;
395			Vector3d tempVec2;
396			Vector3d refCoor;
397			Vector3d refCrossBond;
398			Mat3x3d IBody;
399			Mat3x3d IFrame;
400			Mat3x3d invIBody;
401			Mat3x3d invIFrame;
402			Mat3x3d a;
403			Mat3x3d aTrans;
404
405			invMass = 1.0 / consRB ->getMass();
406	tim	1254
407	tim	1255	invMassVec = invMass * bondDir;
408
409			consRB->getRefCoor(refCoor.vec);
410			consRB->getA(a.element);
411			consRB->getI(IBody.element);
412
413			aTrans = a.transpose();
414			invIBody = IBody.inverse();
415
416			IFrame = aTrans * invIBody * a;
417
418			refCrossBond = crossProduct(refCoor, bondDir);
419
420			tempVec1 = invIFrame * refCrossBond;
421			tempVec2 = crossProduct(tempVec1, refCoor);
422
423			invMassVec += tempVec2;
424	tim	1254	}
425
426			void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
427	tim	1255	const double eConvert = 4.184e-4;
428			Vector3d vel;
429			Vector3d pos;
430			Vector3d Tb;
431			Vector3d ji;
432			double mass;
433			double dtOver2;
434			StuntDouble* sd;
435
436			sd = consRB->getStuntDouble();
437			dtOver2 = info->dt/2;
438	tim	1254
439	tim	1255	mass = sd->getMass();
440
441			// velocity half step
442
443			vel += eConvert * dtOver2 /mass * force;
444
445			sd->setVel(vel.vec);
446
447			if (sd->isDirectional()){
448
449			// get and convert the torque to body frame
450
451			sd->getTrq(Tb.vec);
452			sd->lab2Body(Tb.vec);
453
454			// get the angular momentum, and propagate a half step
455
456			sd->getJ(ji.vec);
457
458			ji += eConvert * dtOver2* Tb;
459
460			sd->setJ(ji.vec);
461			}
462
463			}