| 645 |
|
|
| 646 |
|
//for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter) |
| 647 |
|
// (*atomIter)->accept(v); |
| 648 |
+ |
} |
| 649 |
+ |
void RigidBody::getAtomRefCoor(double pos[3], int index){ |
| 650 |
+ |
vec3 ref; |
| 651 |
+ |
|
| 652 |
+ |
ref = refCoords[index]; |
| 653 |
+ |
pos[0] = ref[0]; |
| 654 |
+ |
pos[1] = ref[1]; |
| 655 |
+ |
pos[2] = ref[2]; |
| 656 |
+ |
|
| 657 |
|
} |
| 658 |
|
|
| 659 |
+ |
|
| 660 |
|
void RigidBody::getAtomPos(double theP[3], int index){ |
| 661 |
|
vec3 ref; |
| 662 |
|
|
| 672 |
|
} |
| 673 |
|
|
| 674 |
|
|
| 675 |
< |
void RigidBody::getAtomRefCoor(double pos[3], int index){ |
| 675 |
> |
void RigidBody::getAtomVel(double theV[3], int index){ |
| 676 |
|
vec3 ref; |
| 677 |
+ |
double velRot[3]; |
| 678 |
+ |
double skewMat[3][3]; |
| 679 |
+ |
double aSkewMat[3][3]; |
| 680 |
+ |
double aSkewTransMat[3][3]; |
| 681 |
+ |
|
| 682 |
+ |
//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ |
| 683 |
|
|
| 684 |
+ |
if (index >= myAtoms.size()) |
| 685 |
+ |
cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl; |
| 686 |
+ |
|
| 687 |
|
ref = refCoords[index]; |
| 688 |
< |
pos[0] = ref[0]; |
| 689 |
< |
pos[1] = ref[1]; |
| 690 |
< |
pos[2] = ref[2]; |
| 688 |
> |
|
| 689 |
> |
skewMat[0][0] =0; |
| 690 |
> |
skewMat[0][1] = ji[2] /I[2][2]; |
| 691 |
> |
skewMat[0][2] = -ji[1] /I[1][1]; |
| 692 |
> |
|
| 693 |
> |
skewMat[1][0] = -ji[2] /I[2][2]; |
| 694 |
> |
skewMat[1][1] = 0; |
| 695 |
> |
skewMat[1][2] = ji[0]/I[0][0]; |
| 696 |
> |
|
| 697 |
> |
skewMat[2][0] =ji[1] /I[1][1]; |
| 698 |
> |
skewMat[2][1] = -ji[0]/I[0][0]; |
| 699 |
> |
skewMat[2][2] = 0; |
| 700 |
|
|
| 701 |
+ |
matMul3(A, skewMat, aSkewMat); |
| 702 |
+ |
|
| 703 |
+ |
transposeMat3(aSkewMat, aSkewTransMat); |
| 704 |
+ |
|
| 705 |
+ |
matVecMul3(aSkewTransMat, ref.vec, velRot); |
| 706 |
+ |
theV[0] = vel[0] + velRot[0]; |
| 707 |
+ |
theV[1] = vel[1] + velRot[1]; |
| 708 |
+ |
theV[2] = vel[2] + velRot[2]; |
| 709 |
|
} |
| 710 |
+ |
|
| 711 |
+ |
|
