| 77 | 
  | 
       | 
| 78 | 
  | 
      quat.toSwingTwist(swingX, swingY, twistAngle); | 
| 79 | 
  | 
 | 
| 80 | 
– | 
      RealType dVdtwist, dVdswingX, dVdswingY; | 
| 81 | 
– | 
      RealType dTwist, dSwingX, dSwingY; | 
| 80 | 
  | 
      RealType p; | 
| 81 | 
  | 
      Vector3d tTwist, tSwing; | 
| 82 | 
  | 
 | 
| 83 | 
  | 
      if (restType_ & rtTwist){ | 
| 84 | 
< | 
        dTwist = twistAngle - twist0_; | 
| 85 | 
< | 
        dVdtwist = kTwist_ * sin(dTwist); | 
| 84 | 
> | 
        RealType dTwist = twistAngle - twist0_; | 
| 85 | 
> | 
        RealType dVdtwist = kTwist_ * sin(dTwist); | 
| 86 | 
  | 
        p = kTwist_ * (1.0 - cos(dTwist) ); | 
| 87 | 
  | 
        pot_ += p; | 
| 88 | 
  | 
        tBody -= dVdtwist * V3Z; | 
| 90 | 
  | 
      } | 
| 91 | 
  | 
 | 
| 92 | 
  | 
      if (restType_ & rtSwingX){ | 
| 93 | 
< | 
        dSwingX = swingX - swingX0_; | 
| 94 | 
< | 
        dVdswingX = kSwingX_ * 0.5 * sin(2.0 * dSwingX); | 
| 93 | 
> | 
        RealType dSwingX = swingX - swingX0_; | 
| 94 | 
> | 
        RealType dVdswingX = kSwingX_ * 0.5 * sin(2.0 * dSwingX); | 
| 95 | 
  | 
        p = 0.25 * kSwingX_ * (1.0 - cos(2.0 * dSwingX)); | 
| 96 | 
  | 
        pot_ += p; | 
| 97 | 
  | 
        tBody -= dVdswingX * V3X; | 
| 99 | 
  | 
      } | 
| 100 | 
  | 
 | 
| 101 | 
  | 
      if (restType_ & rtSwingY){ | 
| 102 | 
< | 
        dSwingY = swingY - swingY0_; | 
| 103 | 
< | 
        dVdswingY = kSwingY_ * 0.5 * sin(2.0 * dSwingY); | 
| 102 | 
> | 
        RealType dSwingY = swingY - swingY0_; | 
| 103 | 
> | 
        RealType dVdswingY = kSwingY_ * 0.5 * sin(2.0 * dSwingY); | 
| 104 | 
  | 
        p = 0.25 * kSwingY_ * (1.0 - cos(2.0 * dSwingY)); | 
| 105 | 
  | 
        pot_ += p; | 
| 106 | 
  | 
        tBody -= dVdswingY * V3Y; |