| 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 | 
  | 
     | 
| 70 | 
  | 
       | 
| 71 | 
  | 
      RotMat3x3d temp = A * refA_.transpose(); | 
| 72 | 
  | 
 | 
| 73 | 
+ | 
      Vector3d euler = temp.toEulerAngles(); | 
| 74 | 
+ | 
 | 
| 75 | 
  | 
      Quat4d quat = temp.toQuaternion(); | 
| 76 | 
  | 
 | 
| 77 | 
  | 
      RealType twistAngle; | 
| 80 | 
  | 
       | 
| 81 | 
  | 
      quat.toSwingTwist(swingX, swingY, twistAngle); | 
| 82 | 
  | 
 | 
| 83 | 
+ | 
 | 
| 84 | 
  | 
      RealType p; | 
| 85 | 
  | 
      Vector3d tTwist, tSwing; | 
| 86 | 
  | 
 | 
| 87 | 
  | 
      if (restType_ & rtTwist){ | 
| 88 | 
< | 
        RealType dTwist = twistAngle - twist0_; | 
| 89 | 
< | 
        RealType dVdtwist = kTwist_ * sin(dTwist); | 
| 90 | 
< | 
        p = kTwist_ * (1.0 - cos(dTwist) ); | 
| 88 | 
> | 
        RealType dTwist = twistAngle - twist0_;       | 
| 89 | 
> | 
        /// RealType dVdtwist = kTwist_ * sin(dTwist); | 
| 90 | 
> | 
        /// p = kTwist_ * (1.0 - cos(dTwist) ); | 
| 91 | 
> | 
        RealType dVdtwist = kTwist_ * dTwist; | 
| 92 | 
> | 
        p = 0.5 * kTwist_ * dTwist * dTwist; | 
| 93 | 
  | 
        pot_ += p; | 
| 94 | 
  | 
        tBody -= dVdtwist * V3Z; | 
| 95 | 
< | 
        restInfo_[rtTwist] = std::make_pair(twistAngle, p); | 
| 95 | 
> | 
        if (printRest_) restInfo_[rtTwist] = std::make_pair(twistAngle, p); | 
| 96 | 
  | 
      } | 
| 97 | 
  | 
 | 
| 98 | 
  | 
      if (restType_ & rtSwingX){ | 
| 99 | 
  | 
        RealType dSwingX = swingX - swingX0_; | 
| 100 | 
< | 
        RealType dVdswingX = kSwingX_ * 0.5 * sin(2.0 * dSwingX); | 
| 101 | 
< | 
        p = 0.25 * kSwingX_ * (1.0 - cos(2.0 * dSwingX)); | 
| 100 | 
> | 
        /// RealType dVdswingX = kSwingX_ * 0.5 * sin(2.0 * dSwingX); | 
| 101 | 
> | 
        /// p = 0.25 * kSwingX_ * (1.0 - cos(2.0 * dSwingX)); | 
| 102 | 
> | 
        RealType dVdswingX = kSwingX_ * dSwingX;         | 
| 103 | 
> | 
        p = 0.5 * kSwingX_ * dSwingX * dSwingX; | 
| 104 | 
  | 
        pot_ += p; | 
| 105 | 
  | 
        tBody -= dVdswingX * V3X; | 
| 106 | 
< | 
        restInfo_[rtSwingX] = std::make_pair(swingX, p); | 
| 106 | 
> | 
        if (printRest_) restInfo_[rtSwingX] = std::make_pair(swingX, p); | 
| 107 | 
  | 
      } | 
| 108 | 
  | 
 | 
| 109 | 
  | 
      if (restType_ & rtSwingY){ | 
| 110 | 
  | 
        RealType dSwingY = swingY - swingY0_; | 
| 111 | 
< | 
        RealType dVdswingY = kSwingY_ * 0.5 * sin(2.0 * dSwingY); | 
| 112 | 
< | 
        p = 0.25 * kSwingY_ * (1.0 - cos(2.0 * dSwingY)); | 
| 111 | 
> | 
        /// RealType dVdswingY = kSwingY_ * 0.5 * sin(2.0 * dSwingY); | 
| 112 | 
> | 
        /// p = 0.25 * kSwingY_ * (1.0 - cos(2.0 * dSwingY)); | 
| 113 | 
> | 
        RealType dVdswingY = kSwingY_ * dSwingY;         | 
| 114 | 
> | 
        p = 0.5 * kSwingX_ * dSwingY * dSwingY; | 
| 115 | 
  | 
        pot_ += p; | 
| 116 | 
  | 
        tBody -= dVdswingY * V3Y; | 
| 117 | 
< | 
        restInfo_[rtSwingY] = std::make_pair(swingY, p); | 
| 117 | 
> | 
        if (printRest_) restInfo_[rtSwingY] = std::make_pair(swingY, p); | 
| 118 | 
  | 
      } | 
| 119 | 
  | 
 | 
| 120 | 
  | 
      Vector3d tLab = A.transpose() * tBody;       |