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