| 52 |
|
RealType r = del.length(); |
| 53 |
|
Vector3d frc = -kDisp_ * del; |
| 54 |
|
RealType p = 0.5 * kDisp_ * del.lengthSquare(); |
| 55 |
+ |
|
| 56 |
|
pot_ = p; |
| 57 |
|
force_ = frc * scaleFactor_; |
| 58 |
< |
restInfo_[rtDisplacement] = std::make_pair(r,p); |
| 58 |
> |
if (printRest_) restInfo_[rtDisplacement] = std::make_pair(r,p); |
| 59 |
|
} |
| 60 |
|
} |
| 61 |
|
|
| 87 |
|
p = kTwist_ * (1.0 - cos(dTwist) ); |
| 88 |
|
pot_ += p; |
| 89 |
|
tBody -= dVdtwist * V3Z; |
| 90 |
< |
restInfo_[rtTwist] = std::make_pair(twistAngle, p); |
| 90 |
> |
if (printRest_) restInfo_[rtTwist] = std::make_pair(twistAngle, p); |
| 91 |
|
} |
| 92 |
|
|
| 93 |
|
if (restType_ & rtSwingX){ |
| 96 |
|
p = 0.25 * kSwingX_ * (1.0 - cos(2.0 * dSwingX)); |
| 97 |
|
pot_ += p; |
| 98 |
|
tBody -= dVdswingX * V3X; |
| 99 |
< |
restInfo_[rtSwingX] = std::make_pair(swingX, p); |
| 99 |
> |
if (printRest_) restInfo_[rtSwingX] = std::make_pair(swingX, p); |
| 100 |
|
} |
| 101 |
|
|
| 102 |
|
if (restType_ & rtSwingY){ |
| 105 |
|
p = 0.25 * kSwingY_ * (1.0 - cos(2.0 * dSwingY)); |
| 106 |
|
pot_ += p; |
| 107 |
|
tBody -= dVdswingY * V3Y; |
| 108 |
< |
restInfo_[rtSwingY] = std::make_pair(swingY, p); |
| 108 |
> |
if (printRest_) restInfo_[rtSwingY] = std::make_pair(swingY, p); |
| 109 |
|
} |
| 110 |
|
|
| 111 |
|
Vector3d tLab = A.transpose() * tBody; |