diff --git a/Source/PBDRobotics/Link.cpp b/Source/PBDRobotics/Link.cpp index d4443fb..0146359 100644 --- a/Source/PBDRobotics/Link.cpp +++ b/Source/PBDRobotics/Link.cpp @@ -77,7 +77,8 @@ void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d Last_Position = Position; Velocity += H * ExtTransForce / Mass; Position += H * Velocity; - + + // Orientation Last_Orientation = Orientation; Vector3d w = AngularVelocity; 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(); } -void ULink::Integrate(const double H){ +void ULink::UpdateVelocity(const double H){ Velocity = (Position - Last_Position) / H; Quaterniond DeltaOrientation = Orientation * Last_Orientation.inverse(); AngularVelocity = 2 * DeltaOrientation.vec() / H; diff --git a/Source/PBDRobotics/Link.h b/Source/PBDRobotics/Link.h index 8c27f20..d49fee6 100644 --- a/Source/PBDRobotics/Link.h +++ b/Source/PBDRobotics/Link.h @@ -44,7 +44,7 @@ public: void Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce); - void Integrate(const double H); + void UpdateVelocity(const double H); void UpdateInternalTransform(); // Inertia tensor in world space diff --git a/Source/PBDRobotics/Robot.cpp b/Source/PBDRobotics/Robot.cpp index 6f1d258..abbce24 100644 --- a/Source/PBDRobotics/Robot.cpp +++ b/Source/PBDRobotics/Robot.cpp @@ -60,7 +60,7 @@ void ARobot::Tick(const float DeltaTime){ for (const auto* Joint : Joints) Joint->SolvePosition(h); for (auto* Link : Links) - Link->Integrate(h); + Link->UpdateVelocity(h); for (const auto* Joint : Joints) Joint->SolveVelocity(h); }