diff --git a/Content/Blueprints/BP_Puma.uasset b/Content/Blueprints/BP_Puma.uasset index d35a0a9..5ae8bc2 100644 Binary files a/Content/Blueprints/BP_Puma.uasset and b/Content/Blueprints/BP_Puma.uasset differ diff --git a/Content/Blueprints/BP_Robot.uasset b/Content/Blueprints/BP_Robot.uasset index 7dd3c98..6ba2ab3 100644 Binary files a/Content/Blueprints/BP_Robot.uasset and b/Content/Blueprints/BP_Robot.uasset differ diff --git a/Content/Levels/Main.umap b/Content/Levels/Main.umap index 7adfc03..a25ece9 100644 Binary files a/Content/Levels/Main.umap and b/Content/Levels/Main.umap differ diff --git a/Source/PBDRobotics/Link.cpp b/Source/PBDRobotics/Link.cpp index 3c21d02..0e4a5fa 100644 --- a/Source/PBDRobotics/Link.cpp +++ b/Source/PBDRobotics/Link.cpp @@ -16,12 +16,12 @@ Matrix3d ULink::GetInertia() const { return R * Inertia_Tensor_Local * R.transpose(); } -double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{ +double ULink::GetPositionalInverseMass(const Vector3d& R, const Vector3d& N) const{ const double M = Mass; const Matrix3d I = GetInertia(); return 1 / M + R.cross(N).transpose() * I.inverse() * R.cross(N); } -double ULink::GetRotationalInverseMass(const Vector3d N) const{ +double ULink::GetRotationalInverseMass(const Vector3d& N) const{ const Matrix3d I = GetInertia(); return N.transpose() * I.inverse() * N; } @@ -62,19 +62,12 @@ void ULink::SetupJoints() const { } } -void ULink::Update(const double H){ +void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce){ // Translation - Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 1; - if (IsBase) - ExtTransForce = Vector3d::Zero(); - Last_Position = Position; Velocity += H * ExtTransForce / Mass; Position += H * Velocity; - - // Rotation - const Vector3d ExtRotForce = Vector3d::Zero(); Last_Orientation = Orientation; Vector3d w = AngularVelocity; diff --git a/Source/PBDRobotics/Link.h b/Source/PBDRobotics/Link.h index ee886bf..a4765bc 100644 --- a/Source/PBDRobotics/Link.h +++ b/Source/PBDRobotics/Link.h @@ -36,24 +36,24 @@ public: UPROPERTY(EditAnywhere, BlueprintReadWrite) UJoint* NextJoint; - void Update(const double H); + void Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce); void Integrate(const double H); void UpdateInternalTransform(); // Inertia tensor in world space Matrix3d GetInertia() const; - double GetPositionalInverseMass(const Vector3d R, const Vector3d N) const; - double GetRotationalInverseMass(const Vector3d N) const; + double GetPositionalInverseMass(const Vector3d& R, const Vector3d& N) const; + double GetRotationalInverseMass(const Vector3d& N) const; void Setup(); UPROPERTY(EditAnywhere) bool IsEffector = false; -private: + UPROPERTY(EditAnywhere) bool IsBase = false; - +private: Matrix3d Inertia_Tensor_Local; void SetupJoints() const; diff --git a/Source/PBDRobotics/Robot.cpp b/Source/PBDRobotics/Robot.cpp index d77fff4..2d430ee 100644 --- a/Source/PBDRobotics/Robot.cpp +++ b/Source/PBDRobotics/Robot.cpp @@ -41,12 +41,16 @@ void ARobot::Connect(ULink* L1, ULink* L2){ void ARobot::Tick(const float DeltaTime){ Super::Tick(DeltaTime); - constexpr int SubSteps = 50; const double h = DeltaTime / SubSteps; for (int i = 0; i < SubSteps; i++){ - for (auto* Link : Links) - Link->Update(h); + for (auto* Link : Links){ + Vector3d Gravity = Vector3d::Zero(); + if (HasGravity && !Link->IsBase) + Gravity = Vector3d(0, 0, -9.81 * 100) * Link->Mass; + const Vector3d RotForce = Vector3d::Zero(); + Link->Update(h, Gravity, RotForce); + } for (const auto* Joint : Joints) Joint->SolvePosition(h); for (auto* Link : Links) diff --git a/Source/PBDRobotics/Robot.h b/Source/PBDRobotics/Robot.h index 90bf5e1..a5324a9 100644 --- a/Source/PBDRobotics/Robot.h +++ b/Source/PBDRobotics/Robot.h @@ -26,6 +26,9 @@ protected: UFUNCTION(BlueprintCallable) void Connect(ULink* L1, ULink* L2); + UPROPERTY(BlueprintReadWrite) + bool HasGravity = false; + private: void ResetPositionAndOrientation(); public: