| 48 |
|
pot_ = 0.0; |
| 49 |
|
|
| 50 |
|
if (restType_ & rtDisplacement) { |
| 51 |
< |
Vector3d del = struc - refPos_; |
| 51 |
> |
Vector3d del = struc - refPos_; |
| 52 |
> |
RealType r = del.length(); |
| 53 |
|
Vector3d frc = -kDisp_ * del; |
| 54 |
< |
pot_ += 0.5 * kDisp_ * del.lengthSquare(); |
| 54 |
> |
RealType p = 0.5 * kDisp_ * del.lengthSquare(); |
| 55 |
> |
pot_ += p; |
| 56 |
|
force_ = frc * scaleFactor_; |
| 57 |
+ |
restInfo_[rtDisplacement] = std::make_pair(r,p); |
| 58 |
|
} |
| 59 |
|
} |
| 60 |
|
|
| 110 |
|
restInfo_[rtSwingY] = std::make_pair(swingY, p); |
| 111 |
|
} |
| 112 |
|
|
| 110 |
– |
std::cerr << "sw = " << swingAngle << " tw = " << twistAngle << "\n"; |
| 111 |
– |
std::cerr << "tbod = " << tBody << "\n"; |
| 113 |
|
Vector3d tLab = A.transpose() * tBody; |
| 114 |
|
torque_ = tLab * scaleFactor_; |
| 115 |
|
} |