Improvised Damping, looking good

Source/PBDRobotics/Joint.cpp

@@ -110,31 +110,47 @@ void UJoint::SolveVelocity(const double H) const{
 	ULink* L1 = FirstLink;
 	ULink* L2 = SecondLink;
 	
-	// Damping
+	// Positional 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();
 	
-	const Matrix3d I1 = L1->GetInertia();
-	const Matrix3d I2 = L2->GetInertia();
+		constexpr double MuLin = 1000; // Damping strength
+		const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1);
+		
+		const Vector3d N = GetConnection().normalized();
 	
 		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 | %f | %f | %f"), P.norm(), MuLin * H, DeltaV.norm(), (V2 - V1).norm());
-	
 		//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()