diff --git a/Content/Blueprints/BP_Puma.uasset b/Content/Blueprints/BP_Puma.uasset index 8e8ca69..e28a1ca 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 0a262c8..2762e6d 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 532bc84..938892a 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 e37b182..29def97 100644 --- a/Source/PBDRobotics/Joint.cpp +++ b/Source/PBDRobotics/Joint.cpp @@ -109,32 +109,48 @@ void UJoint::SolvePosition(const double H) const{ 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 = 10000; // Damping strength - const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1); - - const Vector3d R1 = GetFirstWorldDirection(); - const Vector3d R2 = GetSecondWorldDirection(); - const Vector3d N = (R2 - R1).normalized(); + // Positional Damping + { + const Vector3d V1 = L1->Velocity; + const Vector3d V2 = L2->Velocity; - const Matrix3d I1 = L1->GetInertia(); - const Matrix3d I2 = L2->GetInertia(); + const Vector3d R1 = GetFirstWorldDirection(); + const Vector3d R2 = GetSecondWorldDirection(); - const double W1 = L1->GetPositionalInverseMass(R1, N); - const double W2 = L2->GetPositionalInverseMass(R2, N); + constexpr double MuLin = 1000; // Damping strength + const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1); + + const Vector3d N = GetConnection().normalized(); - const Vector3d P = DeltaV / (W1 + W2); - - //UE_LOG(LogTemp, Log, TEXT("%f | %f | %f | %f | %f"), P.norm(), MuLin * H, DeltaV.norm(), (V2 - V1).norm()); + const double W1 = L1->GetPositionalInverseMass(R1, N); + const double W2 = L2->GetPositionalInverseMass(R2, N); + + const Vector3d P = DeltaV / (W1 + W2); - //L1->Velocity += P / L1->Mass; - //L2->Velocity -= P / L2->Mass; - //L1->AngularVelocity += I1.inverse() * R1.cross(P); - //L2->AngularVelocity -= I2.inverse() * R2.cross(P); + //L1->Velocity += P / L1->Mass; + //L2->Velocity -= P / L2->Mass; + + const Matrix3d I1 = L1->GetInertia(); + const Matrix3d I2 = L2->GetInertia(); + + //L1->AngularVelocity += I1.inverse() * R1.cross(P); + //L2->AngularVelocity -= I2.inverse() * R2.cross(P); + + constexpr double Damp = 1 - 1. / 1000; + + L1->Velocity *= Damp; + L2->Velocity *= Damp; + L1->AngularVelocity *= Damp; + L2->AngularVelocity *= Damp; + } + // Rotational Damping + { + //constexpr double MuAng = 10000; + //const Vector3d DeltaWV = (WV2 - WV1) * (MuAng * H < 1 ? MuAng * H : 1); + + + } } UJoint::UJoint()