46 |
|
void ObjectRestraint::calcForce(Vector3d struc) { |
47 |
|
|
48 |
|
pot_ = 0.0; |
49 |
– |
|
49 |
|
if (restType_ & rtDisplacement) { |
50 |
|
Vector3d del = struc - refPos_; |
51 |
< |
RealType r = del.length(); |
51 |
> |
RealType r = del.length(); |
52 |
|
Vector3d frc = -kDisp_ * del; |
53 |
|
RealType p = 0.5 * kDisp_ * del.lengthSquare(); |
54 |
< |
pot_ += p; |
54 |
> |
pot_ = p; |
55 |
|
force_ = frc * scaleFactor_; |
56 |
|
restInfo_[rtDisplacement] = std::make_pair(r,p); |
57 |
|
} |
72 |
|
|
73 |
|
RealType twistAngle, swingAngle; |
74 |
|
Vector3d swingAxis; |
75 |
< |
RealType tw, swingX, swingY; |
75 |
> |
RealType swingX, swingY; |
76 |
|
|
77 |
< |
quat.getTwistSwingAxisAngle(twistAngle, swingAngle, swingAxis); |
78 |
< |
quat.toSwingTwist(swingX, swingY, tw); |
79 |
< |
|
77 |
> |
quat.toSwingTwist(swingX, swingY, twistAngle); |
78 |
> |
|
79 |
> |
|
80 |
|
RealType dVdtwist, dVdswingX, dVdswingY; |
81 |
|
RealType dTwist, dSwingX, dSwingY; |
82 |
|
RealType p; |