Can follow orientation, if needed (but doesnt work correctly)

Source/PBDRobotics/Joint.cpp

@@ -137,13 +137,15 @@ void UJoint::SolveVelocity(const double H) const{
 		//L1->AngularVelocity += I1.inverse() * R1.cross(P);
 		//L2->AngularVelocity -= I2.inverse() * R2.cross(P);
 
-		constexpr double Damp = 1 - 1. / 1000;
+		constexpr double A = 7;
+		const double Damp = 1 - A * H;
 
 		L1->Velocity *= Damp;
 		L2->Velocity *= Damp;
 		L1->AngularVelocity *= Damp;
 		L2->AngularVelocity *= Damp;
 	}
+	
 	// Rotational Damping
 	{
 		//constexpr double MuAng = 10000;

Source/PBDRobotics/Link.cpp

@@ -40,13 +40,12 @@ void ULink::SetupProperties(){
 	Position = ToEigen(GetComponentLocation());
 	Velocity = Vector3d::Zero();
 	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
-	//Inertia_Tensor_Local = Matrix3d::Identity() * Mass;
 	Orientation = ToEigen(GetComponentQuat());
 	AngularVelocity = Vector3d::Zero();
 
 	if (IsBase){
 		Mass = 1e50;
-		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50;
+		Inertia_Tensor_Local = Matrix3d::Identity() * 1e50;
 	}
 }
 
@@ -65,6 +64,13 @@ void ULink::SetupJoints() const {
 	}
 }
 
+FVector ULink::GetPosition() const{
+	return FromEigen(Position);
+}
+FQuat ULink::GetOrientation() const{
+	return FromEigen(Orientation);
+}
+
 void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce){
 	
 	// Translation

Source/PBDRobotics/Link.h

@@ -35,6 +35,13 @@ public:
 
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
 	UJoint* NextJoint;
+
+	UFUNCTION(BlueprintCallable)
+	FVector GetPosition() const;
+
+	UFUNCTION(BlueprintCallable)
+	FQuat GetOrientation() const;
+
 	
 	void Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce);
 	void Integrate(const double H);
@@ -52,12 +59,11 @@ public:
 	bool IsEffector = false;
 
 	Vector3d GetGravityForce() const;
+	Matrix3d Inertia_Tensor_Local;
 private:
 	UPROPERTY(EditAnywhere)
 	bool IsBase = false;
 	
-	Matrix3d Inertia_Tensor_Local;
-	
 	void SetupJoints() const;
 	void SetupProperties();
 };

Source/PBDRobotics/Robot.cpp

@@ -45,11 +45,16 @@ void ARobot::Tick(const float DeltaTime){
 	for (int i = 0; i < SubSteps; i++){
 		for (auto* Link : Links){
 			Vector3d TransForce = Vector3d::Zero();
+			Vector3d RotForce = Vector3d::Zero();
 			if (HasGravity)
 				TransForce += Link->GetGravityForce();
-			if (Link->IsEffector)
-				TransForce += Link->Mass * ToEigen(EffectorMove);
-			const Vector3d RotForce = Vector3d::Zero();
+			if (Link->IsEffector){
+				FVector Translation, Rotation;
+				GetEffectorForce(Translation, Rotation);
+				Log(ToEigen(Rotation));
+				TransForce += Link->Mass * ToEigen(Translation);
+				RotForce += Link->Mass * ToEigen(Rotation);
+			}
 			Link->Update(h, TransForce, RotForce);
 		}
 		for (const auto* Joint : Joints)
@@ -69,3 +74,5 @@ ULink* ARobot::GetEffector() const{
 			return Link;
 	return nullptr;
 }
+
+void ARobot::GetEffectorForce_Implementation(FVector& Translation, FVector& Rotation){}

Source/PBDRobotics/Robot.h

@@ -29,12 +29,12 @@ protected:
 	UPROPERTY(BlueprintReadWrite)
 	bool HasGravity = false;
 
-	UPROPERTY(BlueprintReadWrite)
-	FVector EffectorMove = FVector::ZeroVector;
-
 	UFUNCTION(BlueprintCallable)
 	ULink* GetEffector() const;
 
+	UFUNCTION(BlueprintNativeEvent)
+	void GetEffectorForce(FVector& Translation, FVector& Rotation);
+
 private:
 	// Assumes First Link to have Identity Orientation and Joints Array to be in order
 	void ResetPositionAndOrientation();

Source/PBDRobotics/util.h

@@ -50,4 +50,8 @@ inline void Log(const char* S){
 	UE_LOG(LogTemp, Log, TEXT("%s"), *FString(S));
 }
 
+inline void Log(const Quaterniond& Q){
+	UE_LOG(LogTemp, Log, TEXT("%f %f %f | %f"), Q.x(), Q.y(), Q.z(), Q.w());
+}
+