7 |
|
//////////////////////////////////////////////////////////////////////////////// |
8 |
|
//Implementation of DCRollAFunctor |
9 |
|
//////////////////////////////////////////////////////////////////////////////// |
10 |
< |
int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){ |
10 |
> |
int DCRollAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){ |
11 |
|
Vector3d posA; |
12 |
|
Vector3d posB; |
13 |
|
Vector3d oldPosA; |
17 |
|
Vector3d pab; |
18 |
|
Vector3d tempPab; |
19 |
|
Vector3d rab; |
20 |
< |
Vector3d rma; |
21 |
< |
Vector3d rmb; |
20 |
> |
Vector3d zetaA; |
21 |
> |
Vector3d zetaB; |
22 |
> |
Vector3d zeta; |
23 |
|
Vector3d consForce; |
24 |
|
Vector3d bondDirUnitVec; |
25 |
|
double dx, dy, dz; |
28 |
|
double diffsq; |
29 |
|
double gab; |
30 |
|
double dt; |
31 |
< |
double pabDotInvMassVec; |
31 |
> |
double pabDotZeta; |
32 |
> |
double pabDotZeta2; |
33 |
> |
double zeta2; |
34 |
> |
double forceScalar; |
35 |
> |
|
36 |
> |
const int conRBMaxIter = 10; |
37 |
> |
|
38 |
> |
dt = info->dt; |
39 |
> |
|
40 |
> |
consAtom1->getOldPos(oldPosA.vec); |
41 |
> |
consAtom2->getOldPos(oldPosB.vec); |
42 |
> |
|
43 |
> |
|
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 |
|
const int conRBMaxIter = 10; |
184 |
|
|
185 |
|
dt = info->dt; |
192 |
|
consRB1->getCurAtomPos(posA.vec); |
193 |
|
consRB2->getCurAtomPos(posB.vec); |
194 |
|
|
195 |
< |
pab = posA - posB; |
195 |
> |
//discard the vector convention in alan tidesley's code |
196 |
> |
//rij = rj - ri; |
197 |
> |
pab = posB - posA; |
198 |
|
|
199 |
|
//periodic boundary condition |
200 |
|
|
203 |
|
pabsq = dotProduct(pab, pab); |
204 |
|
|
205 |
|
rabsq = curPair->getBondLength2(); |
206 |
< |
diffsq = rabsq - pabsq; |
206 |
> |
diffsq = pabsq -rabsq; |
207 |
|
|
208 |
|
if (fabs(diffsq) > (consTolerance * rabsq * 2)){ |
209 |
< |
rab = oldPosA - oldPosB; |
209 |
> |
rab = oldPosB - oldPosA; |
210 |
|
info->wrapVector(rab.vec); |
211 |
|
|
212 |
< |
rpab = dotProduct(rab, pab); |
212 |
> |
//rpab = dotProduct(rab, pab); |
213 |
|
|
214 |
< |
rpabsq = rpab * rpab; |
214 |
> |
//rpabsq = rpab * rpab; |
215 |
|
|
216 |
|
|
217 |
|
//if (rpabsq < (rabsq * -diffsq)){ |
221 |
|
bondDirUnitVec = pab; |
222 |
|
bondDirUnitVec.normalize(); |
223 |
|
|
224 |
< |
getEffInvMassVec(consRB1, bondDirUnitVec, rma); |
224 |
> |
calcZeta(consRB1, bondDirUnitVec, zetaA); |
225 |
|
|
226 |
< |
getEffInvMassVec(consRB2, -bondDirUnitVec, rmb); |
226 |
> |
calcZeta(consRB2, bondDirUnitVec, zetaB); |
227 |
|
|
228 |
< |
pabDotInvMassVec = dotProduct(pab, rma + rmb); |
228 |
> |
zeta = zetaA + zetaB; |
229 |
> |
zeta2 = dotProduct(zeta, zeta); |
230 |
|
|
231 |
< |
consForce = diffsq /(2 * dt * dt * pabDotInvMassVec) * bondDirUnitVec; |
231 |
> |
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 |
|
//integrate consRB1 using constraint force; |
249 |
< |
integrate(consRB1,consForce); |
249 |
> |
integrate(consRB1, consForce); |
250 |
|
|
251 |
|
//integrate consRB2 using constraint force; |
252 |
|
integrate(consRB2, -consForce); |
264 |
|
|
265 |
|
} |
266 |
|
|
267 |
< |
void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){ |
267 |
> |
void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){ |
268 |
|
double invMass; |
269 |
|
Vector3d tempVec1; |
270 |
|
Vector3d tempVec2; |
279 |
|
|
280 |
|
invMass = 1.0 / consRB ->getMass(); |
281 |
|
|
282 |
< |
invMassVec = invMass * bondDir; |
282 |
> |
zeta = invMass * bondDir; |
283 |
|
|
284 |
|
consRB->getRefCoor(refCoor.vec); |
285 |
|
consRB->getA(a.element); |
295 |
|
tempVec1 = invIFrame * refCrossBond; |
296 |
|
tempVec2 = crossProduct(tempVec1, refCoor); |
297 |
|
|
298 |
< |
invMassVec += tempVec2; |
298 |
> |
zeta += tempVec2; |
299 |
|
|
300 |
|
} |
301 |
|
|
305 |
|
Vector3d pos; |
306 |
|
Vector3d Tb; |
307 |
|
Vector3d ji; |
308 |
+ |
Vector3d tempPos; |
309 |
+ |
Vector3d tempVel; |
310 |
+ |
Vector3d tempTrq; |
311 |
+ |
Vector3d tempJi; |
312 |
|
double mass; |
313 |
|
double dtOver2; |
314 |
|
double dt; |
323 |
|
|
324 |
|
mass = sd->getMass(); |
325 |
|
|
326 |
< |
vel += eConvert * dtOver2/mass * force; |
327 |
< |
pos += dt * vel; |
326 |
> |
tempVel = eConvert * dtOver2/mass * force; |
327 |
> |
tempPos = dt * tempVel; |
328 |
> |
|
329 |
> |
vel += tempVel; |
330 |
> |
pos += tempPos; |
331 |
|
|
332 |
|
sd->setVel(vel.vec); |
333 |
|
sd->setPos(pos.vec); |
540 |
|
|
541 |
|
getEffInvMassVec(consRB1, bondDirUnitVec, rma); |
542 |
|
|
543 |
< |
getEffInvMassVec(consRB2, -bondDirUnitVec, rmb); |
543 |
> |
getEffInvMassVec(consRB2, bondDirUnitVec, rmb); |
544 |
|
|
545 |
|
pabcDotvab = dotProduct(pab, vab); |
546 |
|
pabDotInvMassVec = dotProduct(pab, rma + rmb); |