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