| 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(tw, swingX, swingY); |
| 79 |
< |
|
| 77 |
> |
quat.toSwingTwist(swingX, swingY, twistAngle); |
| 78 |
> |
|
| 79 |
> |
|
| 80 |
|
RealType dVdtwist, dVdswingX, dVdswingY; |
| 81 |
|
RealType dTwist, dSwingX, dSwingY; |
| 82 |
|
RealType p; |