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 |
|
} |