| 152 |
|
|
| 153 |
|
if (restType_ & rtTwist){ |
| 154 |
|
dTwist = twistAngle - twist0_; |
| 155 |
< |
dVdtwist = kTwist_ * sin(dTwist) ; |
| 156 |
< |
p = kTwist_ * (1.0 - cos(dTwist) ) ; |
| 155 |
> |
/// dVdtwist = kTwist_ * sin(dTwist) ; |
| 156 |
> |
/// p = kTwist_ * (1.0 - cos(dTwist) ) ; |
| 157 |
> |
dVdtwist = kTwist_ * dTwist; |
| 158 |
> |
p = 0.5 * kTwist_ * dTwist * dTwist; |
| 159 |
|
pot_ += p; |
| 160 |
|
tBody -= dVdtwist * V3Z; |
| 161 |
|
if (printRest_) restInfo_[rtTwist] = std::make_pair(twistAngle, p); |
| 162 |
|
} |
| 163 |
|
|
| 162 |
– |
// if (restType_ & rtSwing){ |
| 163 |
– |
// dSwing = swingAngle - swing0_; |
| 164 |
– |
// dVdswing = kSwing_ * 2.0 * sin(2.0 * dSwing); |
| 165 |
– |
// p = kSwing_ * (1.0 - cos(2.0 * dSwing)); |
| 166 |
– |
// pot_ += p; |
| 167 |
– |
// tBody -= dVdswing * swingAxis; |
| 168 |
– |
// restInfo_[rtSwing] = std::make_pair(swingAngle, p); |
| 169 |
– |
// } |
| 170 |
– |
|
| 164 |
|
if (restType_ & rtSwingX){ |
| 165 |
|
dSwingX = swingX - swingX0_; |
| 166 |
< |
dVdswingX = kSwingX_ * 2.0 * sin(2.0 * dSwingX); |
| 167 |
< |
p = kSwingX_ * (1.0 - cos(2.0 * dSwingX)); |
| 166 |
> |
/// dVdswingX = kSwingX_ * 2.0 * sin(2.0 * dSwingX); |
| 167 |
> |
/// p = kSwingX_ * (1.0 - cos(2.0 * dSwingX)); |
| 168 |
> |
dVdswingX = kSwingX_ * dSwingX; |
| 169 |
> |
p = 0.5 * kSwingX_ * dSwingX * dSwingX; |
| 170 |
|
pot_ += p; |
| 171 |
|
tBody -= dVdswingX * V3X; |
| 172 |
|
if (printRest_) restInfo_[rtSwingX] = std::make_pair(swingX, p); |
| 173 |
|
} |
| 174 |
|
if (restType_ & rtSwingY){ |
| 175 |
|
dSwingY = swingY - swingY0_; |
| 176 |
< |
dVdswingY = kSwingY_ * 2.0 * sin(2.0 * dSwingY); |
| 177 |
< |
p = kSwingY_ * (1.0 - cos(2.0 * dSwingY)); |
| 176 |
> |
/// dVdswingY = kSwingY_ * 2.0 * sin(2.0 * dSwingY); |
| 177 |
> |
/// p = kSwingY_ * (1.0 - cos(2.0 * dSwingY)); |
| 178 |
> |
dVdswingY = kSwingY_ * dSwingY; |
| 179 |
> |
p = 0.5 * kSwingX_ * dSwingY * dSwingY; |
| 180 |
|
pot_ += p; |
| 181 |
|
tBody -= dVdswingY * V3Y; |
| 182 |
|
if (printRest_) restInfo_[rtSwingY] = std::make_pair(swingY, p); |
