| 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 |  | if (printRest_) restInfo_[rtTwist] = std::make_pair(twistAngle, p); | 
| 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 |  | if (printRest_) restInfo_[rtSwingX] = std::make_pair(swingX, p); | 
| 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 |  | if (printRest_) restInfo_[rtSwingY] = std::make_pair(swingY, p); |