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; |