|
|
@ -78,6 +78,7 @@ void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d |
|
|
|
Velocity += H * ExtTransForce / Mass; |
|
|
|
Velocity += H * ExtTransForce / Mass; |
|
|
|
Position += H * Velocity; |
|
|
|
Position += H * Velocity; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Orientation
|
|
|
|
Last_Orientation = Orientation; |
|
|
|
Last_Orientation = Orientation; |
|
|
|
Vector3d w = AngularVelocity; |
|
|
|
Vector3d w = AngularVelocity; |
|
|
|
AngularVelocity += H * GetInertia().inverse() * (ExtRotForce - w.cross(GetInertia() * w)); |
|
|
|
AngularVelocity += H * GetInertia().inverse() * (ExtRotForce - w.cross(GetInertia() * w)); |
|
|
@ -86,7 +87,7 @@ void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d |
|
|
|
Orientation.normalize(); |
|
|
|
Orientation.normalize(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void ULink::Integrate(const double H){ |
|
|
|
void ULink::UpdateVelocity(const double H){ |
|
|
|
Velocity = (Position - Last_Position) / H; |
|
|
|
Velocity = (Position - Last_Position) / H; |
|
|
|
Quaterniond DeltaOrientation = Orientation * Last_Orientation.inverse(); |
|
|
|
Quaterniond DeltaOrientation = Orientation * Last_Orientation.inverse(); |
|
|
|
AngularVelocity = 2 * DeltaOrientation.vec() / H; |
|
|
|
AngularVelocity = 2 * DeltaOrientation.vec() / H; |
|
|
|