Before Input

Source/PBDRobotics/Link.cpp

@@ -2,15 +2,18 @@
 
 
 #include "Link.h"
-
-#include <iostream>
-
 #include "util.h"
 #include "Eigen/Dense"
 #include "Engine/StaticMeshSocket.h"
 
 using namespace Eigen;
 
+Vector3d ULink::GetGravityForce() const{
+	if (IsBase)
+			return Vector3d::Zero();
+	return Vector3d(0, 0, -9.81 * 100) * Mass;
+}
+
 Matrix3d ULink::GetInertia() const {
 	const Matrix3d R = Orientation.toRotationMatrix();
 	return R * Inertia_Tensor_Local * R.transpose();

Source/PBDRobotics/Link.h

@@ -51,9 +51,11 @@ public:
 	UPROPERTY(EditAnywhere)
 	bool IsEffector = false;
 
+	Vector3d GetGravityForce() const;
+private:
 	UPROPERTY(EditAnywhere)
 	bool IsBase = false;
-private:
+	
 	Matrix3d Inertia_Tensor_Local;
 	
 	void SetupJoints() const;

Source/PBDRobotics/Robot.cpp

@@ -3,6 +3,8 @@
 
 #include "Robot.h"
 
+#include "util.h"
+
 ARobot::ARobot(){
 	PrimaryActorTick.bCanEverTick = true;
 }
@@ -17,12 +19,9 @@ void ARobot::BeginPlay(){
 
 void ARobot::ResetPositionAndOrientation(){
 	for (const auto* Joint : Joints){
-		const ULink* L1 = Joint->FirstLink;
-		ULink* L2 = Joint->SecondLink;
-
-		Vector3d ConnectPosL1 = L1->Position + Joint->GetFirstWorldDirection();
-		const Vector3d Correction = ConnectPosL1 - Joint->GetSecondWorldDirection();
-		L2->Position = Correction;
+		const Vector3d ConnectPosL1 = Joint->FirstLink->Position + Joint->GetFirstWorldDirection();
+		Joint->SecondLink->Position = ConnectPosL1 - Joint->GetSecondWorldDirection();
+		Joint->SecondLink->Orientation = Quaterniond::Identity();
 	}
 }
 
@@ -45,11 +44,13 @@ void ARobot::Tick(const float DeltaTime){
 	const double h = DeltaTime / SubSteps;
 	for (int i = 0; i < SubSteps; i++){
 		for (auto* Link : Links){
-			Vector3d Gravity = Vector3d::Zero();
-			if (HasGravity && !Link->IsBase)
-				Gravity = Vector3d(0, 0, -9.81 * 100) * Link->Mass;
+			Vector3d TransForce = Vector3d::Zero();
+			if (HasGravity)
+				TransForce += Link->GetGravityForce();
+			if (Link->IsEffector)
+				TransForce += Link->Mass * ToEigen(EffectorMove);
 			const Vector3d RotForce = Vector3d::Zero();
-			Link->Update(h, Gravity, RotForce);
+			Link->Update(h, TransForce, RotForce);
 		}
 		for (const auto* Joint : Joints)
 			Joint->SolvePosition(h);

Source/PBDRobotics/Robot.h

@@ -29,7 +29,11 @@ protected:
 	UPROPERTY(BlueprintReadWrite)
 	bool HasGravity = false;
 
+	UPROPERTY(BlueprintReadWrite)
+	FVector EffectorMove = FVector::ZeroVector;
+
 private:
+	// Assumes First Link to have Identity Orientation and Joints Array to be in order
 	void ResetPositionAndOrientation();
 public:	
 	virtual void Tick(float DeltaTime) override;

Source/PBDRobotics/util.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include "Eigen/Dense"
-#include <string>
 
 inline Vector3d ToEigen(const FVector V){
 	return Vector3d(V.X, V.Y, V.Z);
@@ -11,27 +10,27 @@ inline Quaterniond ToEigen(const FQuat Q){
 	return Quaterniond(Q.W, Q.X, Q.Y, Q.Z);
 }
 
-inline FVector FromEigen(const Vector3d V){
+inline FVector FromEigen(const Vector3d& V){
 	return FVector(V.x(), V.y(), V.z());
 }
 
-inline FQuat FromEigen(const Quaterniond Q){
+inline FQuat FromEigen(const Quaterniond& Q){
 	return FQuat(Q.x(), Q.y(), Q.z(), Q.w());
 }
 
-inline Quaterniond operator* (Quaterniond Q, double D){
+inline Quaterniond operator* (const Quaterniond& Q, const double D){
 	return Quaterniond(Q.w() * D, Q.x() * D, Q.y() * D, Q.z() * D);
 }
 
-inline Quaterniond operator+ (const Quaterniond Q1, const Quaterniond 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){
+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 void Log(MatrixXd M){
+inline void Log(const MatrixXd& M){
 	assert(M.rows() == 3);
 	if (M.cols() == 3){
 		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0, 0), M(0, 1), M(0, 2));