87 RealType swingX, swingY;
89 quat.toTwistSwing(twistAngle, swingX, swingY);
94 if (restType_ & rtTwist) {
95 RealType dTwist = twistAngle - twist0_;
98 RealType dVdtwist = kTwist_ * dTwist;
99 p = 0.5 * kTwist_ * dTwist * dTwist;
101 tBody -= dVdtwist * V3Z;
103 if (printRest_) restInfo_[rtTwist] = std::make_pair(twistAngle, p);
106 if (restType_ & rtSwingX) {
107 RealType dSwingX = swingX - swingX0_;
110 RealType dVdswingX = kSwingX_ * dSwingX;
111 p = 0.5 * kSwingX_ * dSwingX * dSwingX;
113 tBody -= dVdswingX * V3X;
114 if (printRest_) restInfo_[rtSwingX] = std::make_pair(swingX, p);
117 if (restType_ & rtSwingY) {
118 RealType dSwingY = swingY - swingY0_;
121 RealType dVdswingY = kSwingY_ * dSwingY;
122 p = 0.5 * kSwingX_ * dSwingY * dSwingY;
124 tBody -= dVdswingY * V3Y;
125 if (printRest_) restInfo_[rtSwingY] = std::make_pair(swingY, p);
128 Vector3d tLab = A.transpose() * tBody;
129 torque_ = tLab * scaleFactor_;