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