| 27 |
|
double diffsq; |
| 28 |
|
double gab; |
| 29 |
|
double dt; |
| 30 |
< |
double dt2; |
| 31 |
< |
double rijcDotInvMassVec; |
| 30 |
> |
double pabDotInvMassVec; |
| 31 |
|
|
| 32 |
|
|
| 33 |
|
const int conRBMaxIter = 10; |
| 34 |
|
|
| 35 |
|
dt = info->dt; |
| 37 |
– |
dt2 = dt * dt; |
| 36 |
|
|
| 37 |
|
consRB1->getOldAtomPos(oldPosA.vec); |
| 38 |
|
consRB2->getOldAtomPos(oldPosB.vec); |
| 73 |
|
|
| 74 |
|
getEffInvMassVec(consRB2, -bondDirUnitVec, rmb); |
| 75 |
|
|
| 76 |
< |
rijcDotInvMassVec = dotProduct(pab, rma + rmb); |
| 76 |
> |
pabDotInvMassVec = dotProduct(pab, rma + rmb); |
| 77 |
|
|
| 78 |
< |
consForce = diffsq /(2 * dt * dt * rijcDotInvMassVec) * bondDirUnitVec; |
| 78 |
> |
consForce = diffsq /(2 * dt * dt * pabDotInvMassVec) * bondDirUnitVec; |
| 79 |
|
//integrate consRB1 using constraint force; |
| 80 |
|
integrate(consRB1,consForce); |
| 81 |
|
|
| 313 |
|
//Implementation of DCRollBFunctor |
| 314 |
|
//////////////////////////////////////////////////////////////////////////////// |
| 315 |
|
int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){ |
| 316 |
< |
return consElemHandlerFail; |
| 316 |
> |
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 |
|
} |
| 391 |
|
|
| 392 |
|
void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){ |
| 393 |
+ |
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 |
|
|
| 407 |
+ |
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 |
|
} |
| 425 |
|
|
| 426 |
|
void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){ |
| 427 |
+ |
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 |
|
|
| 439 |
< |
} |
| 439 |
> |
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 |
> |
} |
