| 8 |
|
objType = OT_RIGIDBODY; |
| 9 |
|
is_linear = false; |
| 10 |
|
linear_axis = -1; |
| 11 |
+ |
momIntTol = 1e-6; |
| 12 |
|
} |
| 13 |
|
|
| 14 |
|
RigidBody::~RigidBody() { |
| 86 |
|
theF[i] = frc[i]; |
| 87 |
|
} |
| 88 |
|
|
| 89 |
+ |
void RigidBody::setFrc(double theF[3]){ |
| 90 |
+ |
for (int i = 0; i < 3 ; i++) |
| 91 |
+ |
frc[i] = theF[i]; |
| 92 |
+ |
} |
| 93 |
+ |
|
| 94 |
|
void RigidBody::addFrc(double theF[3]){ |
| 95 |
|
for (int i = 0; i < 3 ; i++) |
| 96 |
|
frc[i] += theF[i]; |
| 188 |
|
A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] ); |
| 189 |
|
A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] ); |
| 190 |
|
A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr; |
| 185 |
– |
|
| 191 |
|
} |
| 192 |
|
|
| 193 |
|
void RigidBody::getA( double the_A[3][3] ){ |
| 223 |
|
void RigidBody::getTrq(double theT[3]){ |
| 224 |
|
for (int i = 0; i < 3 ; i++) |
| 225 |
|
theT[i] = trq[i]; |
| 226 |
+ |
} |
| 227 |
+ |
|
| 228 |
+ |
void RigidBody::setTrq(double theT[3]){ |
| 229 |
+ |
for (int i = 0; i < 3 ; i++) |
| 230 |
+ |
trq[i] = theT[i]; |
| 231 |
|
} |
| 232 |
|
|
| 233 |
|
void RigidBody::addTrq(double theT[3]){ |
| 268 |
|
r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]); |
| 269 |
|
r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]); |
| 270 |
|
r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]); |
| 271 |
+ |
|
| 272 |
+ |
} |
| 273 |
+ |
|
| 274 |
+ |
double RigidBody::getZangle( ){ |
| 275 |
+ |
return zAngle; |
| 276 |
+ |
} |
| 277 |
+ |
|
| 278 |
+ |
void RigidBody::setZangle( double zAng ){ |
| 279 |
+ |
zAngle = zAng; |
| 280 |
+ |
} |
| 281 |
|
|
| 282 |
+ |
void RigidBody::addZangle( double zAng ){ |
| 283 |
+ |
zAngle += zAng; |
| 284 |
|
} |
| 285 |
|
|
| 286 |
|
void RigidBody::calcRefCoords( ) { |
| 654 |
|
|
| 655 |
|
//for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter) |
| 656 |
|
// (*atomIter)->accept(v); |
| 657 |
< |
} |
| 657 |
> |
} |
| 658 |
> |
void RigidBody::getAtomRefCoor(double pos[3], int index){ |
| 659 |
> |
vec3 ref; |
| 660 |
> |
|
| 661 |
> |
ref = refCoords[index]; |
| 662 |
> |
pos[0] = ref[0]; |
| 663 |
> |
pos[1] = ref[1]; |
| 664 |
> |
pos[2] = ref[2]; |
| 665 |
> |
|
| 666 |
> |
} |
| 667 |
> |
|
| 668 |
> |
|
| 669 |
> |
void RigidBody::getAtomPos(double theP[3], int index){ |
| 670 |
> |
vec3 ref; |
| 671 |
> |
|
| 672 |
> |
if (index >= myAtoms.size()) |
| 673 |
> |
cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl; |
| 674 |
> |
|
| 675 |
> |
ref = refCoords[index]; |
| 676 |
> |
body2Lab(ref.vec); |
| 677 |
> |
|
| 678 |
> |
theP[0] = pos[0] + ref[0]; |
| 679 |
> |
theP[1] = pos[1] + ref[1]; |
| 680 |
> |
theP[2] = pos[2] + ref[2]; |
| 681 |
> |
} |
| 682 |
> |
|
| 683 |
> |
|
| 684 |
> |
void RigidBody::getAtomVel(double theV[3], int index){ |
| 685 |
> |
vec3 ref; |
| 686 |
> |
double velRot[3]; |
| 687 |
> |
double skewMat[3][3]; |
| 688 |
> |
double aSkewMat[3][3]; |
| 689 |
> |
double aSkewTransMat[3][3]; |
| 690 |
> |
|
| 691 |
> |
//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ |
| 692 |
> |
|
| 693 |
> |
if (index >= myAtoms.size()) |
| 694 |
> |
cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl; |
| 695 |
> |
|
| 696 |
> |
ref = refCoords[index]; |
| 697 |
> |
|
| 698 |
> |
skewMat[0][0] =0; |
| 699 |
> |
skewMat[0][1] = ji[2] /I[2][2]; |
| 700 |
> |
skewMat[0][2] = -ji[1] /I[1][1]; |
| 701 |
> |
|
| 702 |
> |
skewMat[1][0] = -ji[2] /I[2][2]; |
| 703 |
> |
skewMat[1][1] = 0; |
| 704 |
> |
skewMat[1][2] = ji[0]/I[0][0]; |
| 705 |
> |
|
| 706 |
> |
skewMat[2][0] =ji[1] /I[1][1]; |
| 707 |
> |
skewMat[2][1] = -ji[0]/I[0][0]; |
| 708 |
> |
skewMat[2][2] = 0; |
| 709 |
> |
|
| 710 |
> |
matMul3(A, skewMat, aSkewMat); |
| 711 |
> |
|
| 712 |
> |
transposeMat3(aSkewMat, aSkewTransMat); |
| 713 |
> |
|
| 714 |
> |
matVecMul3(aSkewTransMat, ref.vec, velRot); |
| 715 |
> |
theV[0] = vel[0] + velRot[0]; |
| 716 |
> |
theV[1] = vel[1] + velRot[1]; |
| 717 |
> |
theV[2] = vel[2] + velRot[2]; |
| 718 |
> |
} |
| 719 |
> |
|
| 720 |
> |
|
