diff --git a/Content/Blueprints/BP_Robot.uasset b/Content/Blueprints/BP_Robot.uasset index b878317..87117cc 100644 Binary files a/Content/Blueprints/BP_Robot.uasset and b/Content/Blueprints/BP_Robot.uasset differ diff --git a/Content/Blueprints/BP_TestBot.uasset b/Content/Blueprints/BP_TestBot.uasset index 397d475..268db87 100644 Binary files a/Content/Blueprints/BP_TestBot.uasset and b/Content/Blueprints/BP_TestBot.uasset differ diff --git a/Content/Levels/Main.umap b/Content/Levels/Main.umap index c4e4769..00d1eb6 100644 Binary files a/Content/Levels/Main.umap and b/Content/Levels/Main.umap differ diff --git a/Source/PBDRobotics/Joint.cpp b/Source/PBDRobotics/Joint.cpp index 2e969ea..a972715 100644 --- a/Source/PBDRobotics/Joint.cpp +++ b/Source/PBDRobotics/Joint.cpp @@ -3,6 +3,81 @@ #include "Joint.h" +#include "Link.h" +#include "util.h" +#include "Kismet/KismetMathLibrary.h" + +Vector3d UJoint::GetFirstWorldPosition() const{ + return FirstLink->Position + FirstLink->Orientation * FirstLocalPosition; +} + +Vector3d UJoint::GetSecondWorldPosition() const{ + return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition; +} + +void UJoint::SolvePosition(const double H) const{ + ULink* L1 = FirstLink; + ULink* L2 = SecondLink; + + // Positional Constraints + const Vector3d R1 = GetFirstWorldPosition(); + const Vector3d R2 = GetSecondWorldPosition(); + const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated + const Vector3d N = D.normalized(); + const double C = D.norm(); + const Matrix3d I1 = L1->GetInertia(); + const Matrix3d I2 = L2->GetInertia(); + const double M1 = L1->Mass; + const double M2 = L2->Mass; + + const double W1 = L1->GetPositionalInverseMass(R1, N); + const double W2 = L2->GetPositionalInverseMass(R2, N); + + const double A = 0.000001 / (H * H); + const Vector3d P = -C / (W1 + W2 + A) * N; + + L1->Position += P / M1; + L2->Position -= P / M2; + + Vector3d T1 = I1.inverse() * R1.cross(P); + Vector3d T2 = I2.inverse() * R2.cross(P); + L1->Orientation += Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5; + L2->Orientation -= Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5; + + // Rotational Constraints +} + +void UJoint::SolveVelocity(const double H) const{ + ULink* L1 = FirstLink; + ULink* L2 = SecondLink; + + // Damping + const Vector3d V1 = L1->Velocity; + const Vector3d V2 = L2->Velocity; + + constexpr double MuLin = 100000; + const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1); + + const Vector3d R1 = GetFirstWorldPosition(); + const Vector3d R2 = GetSecondWorldPosition(); + const Vector3d N = (R2 - R1).normalized(); + + const Matrix3d I1 = L1->GetInertia(); + const Matrix3d I2 = L2->GetInertia(); + + const double W1 = L1->GetPositionalInverseMass(R1, N); + const double W2 = L2->GetPositionalInverseMass(R2, N); + + const Vector3d P = DeltaV / (W1 + W2); + + // UE_LOG(LogTemp, Log, TEXT("%f | %f"), (V2 - V1).norm(), MuLin * H); + + L1->Velocity += P / L1->Mass; + L2->Velocity -= P / L2->Mass; + L1->AngularVelocity += I1.inverse() * R1.cross(P); + L2->AngularVelocity -= I2.inverse() * R2.cross(P); +} + UJoint::UJoint() { } diff --git a/Source/PBDRobotics/Joint.h b/Source/PBDRobotics/Joint.h index 0a4eafb..fc4c7dd 100644 --- a/Source/PBDRobotics/Joint.h +++ b/Source/PBDRobotics/Joint.h @@ -29,6 +29,12 @@ public: Vector3d FirstLocalPosition; Vector3d SecondLocalPosition; + + Vector3d GetFirstWorldPosition() const; + Vector3d GetSecondWorldPosition() const; + + void SolvePosition(const double H) const; + void SolveVelocity(const double H) const; UJoint(); }; diff --git a/Source/PBDRobotics/Link.cpp b/Source/PBDRobotics/Link.cpp index 41465a9..5c60304 100644 --- a/Source/PBDRobotics/Link.cpp +++ b/Source/PBDRobotics/Link.cpp @@ -11,14 +11,17 @@ using namespace Eigen; -// Inertia tensor in world space -Matrix3d ULink::GetMomentOfInertia() const { - return GetRotationMatrix() * Inertia_Tensor_Local; +Matrix3d ULink::GetInertia() const { + return Orientation.toRotationMatrix() * Inertia_Tensor_Local; } -Matrix3d ULink::GetRotationMatrix() const{ - const FQuat Q = this->GetRelativeRotation().Quaternion(); - return Quaterniond(Q.W, Q.X, Q.Y, Q.Z).toRotationMatrix(); +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{ + return 0; } void ULink::Setup(){ @@ -28,14 +31,11 @@ void ULink::Setup(){ void ULink::SetupProperties(){ Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal(); - - CenterOfMass = ToEigen(GetCenterOfMass()); Position = ToEigen(GetComponentLocation()); Velocity = Vector3d::Zero(); Mass = IsBase ? INFINITY : CalculateMass(); Orientation = Quaterniond::Identity(); AngularVelocity = Vector3d::Zero(); - Inertia = GetMomentOfInertia(); } void ULink::SetupJoints() const { @@ -53,9 +53,8 @@ void ULink::SetupJoints() const { } } - void ULink::Update(const double H){ - const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass; + const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0; Last_Position = Position; Velocity += H * ExtTransForce / Mass; @@ -75,7 +74,7 @@ void ULink::Integrate(const double H){ AngularVelocity = DeltaOrientation.w() >= 0 ? AngularVelocity : -AngularVelocity; } -void ULink::UpdateVisualTransform(){ +void ULink::UpdateInternalTransform(){ SetWorldLocation(FromEigen(Position)); SetWorldRotation(FromEigen(Orientation)); } diff --git a/Source/PBDRobotics/Link.h b/Source/PBDRobotics/Link.h index 879ab91..a4f0f18 100644 --- a/Source/PBDRobotics/Link.h +++ b/Source/PBDRobotics/Link.h @@ -21,13 +21,11 @@ public: // Properties in World space - Vector3d CenterOfMass; Vector3d Position; Vector3d Velocity; double Mass; Quaterniond Orientation; Vector3d AngularVelocity; - Matrix3d Inertia; Vector3d Last_Position; Quaterniond Last_Orientation; @@ -40,9 +38,13 @@ public: void Update(const double H); void Integrate(const double H); - void UpdateVisualTransform(); + void UpdateInternalTransform(); - Matrix3d GetRotationMatrix() const; + // Inertia tensor in world space + Matrix3d GetInertia() const; + + double GetPositionalInverseMass(const Vector3d R, const Vector3d N) const; + double GetRotationalInverseMass() const; void Setup(); private: @@ -50,7 +52,6 @@ private: bool IsBase = false; Matrix3d Inertia_Tensor_Local; - Matrix3d GetMomentOfInertia() const; void SetupJoints() const; void SetupProperties(); diff --git a/Source/PBDRobotics/Robot.cpp b/Source/PBDRobotics/Robot.cpp index a0eed51..1d878b6 100644 --- a/Source/PBDRobotics/Robot.cpp +++ b/Source/PBDRobotics/Robot.cpp @@ -3,6 +3,7 @@ #include "Robot.h" + ARobot::ARobot(){ PrimaryActorTick.bCanEverTick = true; } @@ -11,30 +12,31 @@ void ARobot::BeginPlay(){ Super::BeginPlay(); GetComponents(Parts); - for (auto* Link : Parts) Link->Setup(); + for (auto* Link : Parts){ + Link->Setup(); + if (Link->PrevJoint) + Joints.Add(Link->PrevJoint); + } + + // Example: 3 Parts are connected with 2 Joints + assert(Parts.Num() == Joints.Num() + 1); } void ARobot::Tick(const float DeltaTime){ Super::Tick(DeltaTime); - constexpr int SubSteps = 10; + constexpr int SubSteps = 100; const double h = DeltaTime / SubSteps; for (int i = 0; i < SubSteps; i++){ - for (auto* Link : Parts) Link->Update(h); - SolvePositions(); - for (auto* Link : Parts) Link->Integrate(h); - SolveVelocities(); + for (auto* Link : Parts) + Link->Update(h); + for (const auto* Joint : Joints) + Joint->SolvePosition(h); + for (auto* Link : Parts) + Link->Integrate(h); + for (const auto* Joint : Joints) + Joint->SolveVelocity(h); } - - for (auto* Link : Parts) Link->UpdateVisualTransform(); -} - -void ARobot::SolvePositions(){ - - - -} - -void ARobot::SolveVelocities(){ - + for (auto* Link : Parts) + Link->UpdateInternalTransform(); } diff --git a/Source/PBDRobotics/Robot.h b/Source/PBDRobotics/Robot.h index a48ba49..6f1c32e 100644 --- a/Source/PBDRobotics/Robot.h +++ b/Source/PBDRobotics/Robot.h @@ -11,7 +11,7 @@ UCLASS() class PBDROBOTICS_API ARobot : public AActor { GENERATED_BODY() -public: +public: // Sets default values for this actor's properties ARobot(); @@ -27,6 +27,6 @@ private: UPROPERTY() TArray Parts; - void SolvePositions(); - void SolveVelocities(); + UPROPERTY() + TArray Joints = TArray(); }; diff --git a/Source/PBDRobotics/util.h b/Source/PBDRobotics/util.h index 84b4fc2..b5707a9 100644 --- a/Source/PBDRobotics/util.h +++ b/Source/PBDRobotics/util.h @@ -14,7 +14,7 @@ inline FQuat FromEigen(const Quaterniond Q){ return FQuat(Q.x(), Q.y(), Q.z(), Q.w()); } -inline Quaterniond operator* (const Quaterniond Q, const double D){ +inline Quaterniond operator* (Quaterniond Q, double D){ return Quaterniond(Q.w() * D, Q.x() * D, Q.y() * D, Q.z() * D); } @@ -25,3 +25,11 @@ inline Quaterniond operator+ (const Quaterniond Q1, const Quaterniond Q2){ inline Quaterniond operator+= (const Quaterniond Q1, const Quaterniond Q2){ return Q1 + Q2; } + +inline Quaterniond operator- (const Quaterniond Q1, const Quaterniond Q2){ + return Quaterniond(Q1.w() - Q2.w(), Q1.x() - Q2.x(), Q1.y() - Q2.y(), Q1.z() - Q2.z()); +} + +inline Quaterniond operator-= (const Quaterniond Q1, const Quaterniond Q2){ + return Q1 - Q2; +}