Small changes

Source/PBDRobotics/Joint.cpp

@@ -48,7 +48,7 @@ void UJoint::SolvePosition(const double H) const{
 		const double W1 = L1->GetPositionalInverseMass(R1, N);
 		const double W2 = L2->GetPositionalInverseMass(R2, N);
 
-		constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness)
+		constexpr double A = 0; //1. / 10000000; // Compliance (inverse of stiffness)
 		
 		const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
 	
@@ -61,7 +61,7 @@ void UJoint::SolvePosition(const double H) const{
 		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
 		L1->Orientation = (L1->Orientation + Add1).normalized();
 		L2->Orientation = (L2->Orientation - Add2).normalized();
-		UE_LOG(LogTemp, Log, TEXT("%f | %f | %f"), C, P.norm(), R2.cross(P).norm());
+		// UE_LOG(LogTemp, Log, TEXT("%f"), P.norm());
 	}
 	
 	// Rotational Constraints
@@ -79,16 +79,16 @@ void UJoint::SolvePosition(const double H) const{
 		const double W1 = L1->GetRotationalInverseMass(N);
 		const double W2 = L2->GetRotationalInverseMass(N);
 
-		constexpr double A = 0; //1. / 10000000;
+		constexpr double A = 0; //1. / 100000000;
 		const Vector3d P = Theta / (W1 + W2 + A / (H * H)) * N;
 		
 		Vector3d T1 = I1.inverse() * P;
 		Vector3d T2 = I2.inverse() * P;
 		const Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
 		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
-		//L1->Orientation = (L1->Orientation + Add1).normalized();
+		L1->Orientation = (L1->Orientation + Add1).normalized();
-		//L2->Orientation = (L2->Orientation - Add2).normalized();
+		L2->Orientation = (L2->Orientation - Add2).normalized();
-		//UE_LOG(LogTemp, Log, TEXT("%f | %f | %f | %f"), N.norm(), W1, W2, Q.norm());
+		// UE_LOG(LogTemp, Log, TEXT("%f"), Add2.norm());
 	}
 	
 }

Source/PBDRobotics/Joint.h

@@ -39,11 +39,13 @@ public:
 	// First - Second in World-Space 
 	Vector3d GetConnection() const;
 	
-	// 
 	Vector3d FirstRotateAxis;
 	Vector3d SecondRotateAxis;
 
+	// World Axis, around which the First Link wants to rotate
 	Vector3d GetFirstWorldAxis() const;
+
+	// World Axis, around which the Second Link wants to rotate
 	Vector3d GetSecondWorldAxis() const;
 
 	void SolvePosition(const double H) const;

Source/PBDRobotics/Robot.cpp

@@ -27,7 +27,7 @@ void ARobot::BeginPlay(){
 void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
 	
-	constexpr int SubSteps = 10;
+	constexpr int SubSteps = 30;
 	const double h = DeltaTime / SubSteps;
 	for (int i = 0; i < SubSteps; i++){
 		for (auto* Link : Parts)