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

@ -77,7 +77,8 @@ void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d
Last_Position = Position; Last_Position = Position;
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;

@ -44,7 +44,7 @@ public:
void Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce); void Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce);
void Integrate(const double H); void UpdateVelocity(const double H);
void UpdateInternalTransform(); void UpdateInternalTransform();
// Inertia tensor in world space // Inertia tensor in world space

@ -60,7 +60,7 @@ void ARobot::Tick(const float DeltaTime){
for (const auto* Joint : Joints) for (const auto* Joint : Joints)
Joint->SolvePosition(h); Joint->SolvePosition(h);
for (auto* Link : Links) for (auto* Link : Links)
Link->Integrate(h); Link->UpdateVelocity(h);
for (const auto* Joint : Joints) for (const auto* Joint : Joints)
Joint->SolveVelocity(h); Joint->SolveVelocity(h);
} }

Loading…
Cancel
Save