main
Benjamin Kraft 2 years ago
parent da416ba80c
commit 48322ec0c6
  1. 3
      Source/PBDRobotics/Link.cpp
  2. 2
      Source/PBDRobotics/Link.h
  3. 2
      Source/PBDRobotics/Robot.cpp

@ -78,6 +78,7 @@ void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d
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;

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

@ -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);
}

Loading…
Cancel
Save