Added reset of position and orientation at the start

Config/DefaultEngine.ini

@@ -23,4 +23,5 @@ r.DefaultFeature.AutoExposure=False
 [CoreRedirects]
 +ClassRedirects=(OldName="/Script/PBDRobotics.Joint",NewName="/Script/PBDRobotics.Joint")
 +PropertyRedirects=(OldName="/Script/PBDRobotics.Joint.PrevLink",NewName="/Script/PBDRobotics.Joint.FirstLink")
-+PropertyRedirects=(OldName="/Script/PBDRobotics.Joint.NextLink",NewName="/Script/PBDRobotics.Joint.SecondLink")
++PropertyRedirects=(OldName="/Script/PBDRobotics.Joint.NextLink",NewName="/Script/PBDRobotics.Joint.SecondLink")
++PropertyRedirects=(OldName="/Script/PBDRobotics.Robot.Parts",NewName="/Script/PBDRobotics.Robot.Links")

Source/PBDRobotics/Joint.cpp

@@ -72,7 +72,7 @@ void UJoint::SolvePosition(const double H) const{
 		Vector3d Q;
 		if (L2->IsEffector){
 			const Quaterniond Tq = L1->Orientation * L2->Orientation.inverse();
-			Q = 2 * Tq.vec();
+			Q = -2 * Tq.vec();
 		} else{
 			const Vector3d A1 = GetFirstWorldAxis();
 			const Vector3d A2 = GetSecondWorldAxis();

Source/PBDRobotics/Link.cpp

@@ -45,8 +45,6 @@ void ULink::SetupProperties(){
 		Mass = 1e50;
 		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50;
 	}
-
-	Log(GetInertia());
 }
 
 void ULink::SetupJoints() const {

Source/PBDRobotics/Robot.cpp

@@ -3,42 +3,57 @@
 
 #include "Robot.h"
 
-#include "util.h"
-
-
 ARobot::ARobot(){
 	PrimaryActorTick.bCanEverTick = true;
 }
 
 void ARobot::BeginPlay(){
 	Super::BeginPlay();
-	
-	GetComponents<ULink>(Parts);
-	for (auto* Link : Parts){
+
+	for (auto* Link : Links)
 		Link->Setup();
-		if (Link->PrevJoint)
-			Joints.Add(Link->PrevJoint);
+	ResetPositionAndOrientation();
+}
+
+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;
 	}
+}
 
-	// Example: 3 Parts are connected with 2 Joints
-	assert(Parts.Num() == Joints.Num() + 1);
+
+void ARobot::Connect(ULink* L1, ULink* L2){
+	Links.AddUnique(L1);
+	Links.Add(L2);
+	UJoint* Joint = NewObject<UJoint>(this);
+	Joint->FirstLink = L1;
+	Joint->SecondLink = L2;
+	L1->NextJoint = Joint;
+	L2->PrevJoint = Joint;
+	Joints.Add(Joint);
 }
 
+
 void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
 	
-	constexpr int SubSteps = 30;
+	constexpr int SubSteps = 50;
 	const double h = DeltaTime / SubSteps;
 	for (int i = 0; i < SubSteps; i++){
-		for (auto* Link : Parts)
+		for (auto* Link : Links)
 			Link->Update(h);
 		for (const auto* Joint : Joints)
 			Joint->SolvePosition(h);
-		for (auto* Link : Parts)
+		for (auto* Link : Links)
 			Link->Integrate(h);
 		for (const auto* Joint : Joints)
 			Joint->SolveVelocity(h);
 	}
-	for (auto* Link : Parts)
+	for (auto* Link : Links)
 		Link->UpdateInternalTransform();
 }

Source/PBDRobotics/Robot.h

@@ -12,21 +12,22 @@ class PBDROBOTICS_API ARobot : public AActor
 {
 	GENERATED_BODY()
 public:
-	// Sets default values for this actor's properties
 	ARobot();
 
 protected:
-	// Called when the game starts or when spawned
 	virtual void BeginPlay() override;
 
-public:	
-	// Called every frame
-	virtual void Tick(float DeltaTime) override;
+	UPROPERTY(BlueprintReadWrite)
+	TArray<ULink*> Links = TArray<ULink*>();
 
-private:
-	UPROPERTY()
-	TArray<ULink*> Parts;
-
-	UPROPERTY()
+	UPROPERTY(BlueprintReadWrite)
 	TArray<UJoint*> Joints = TArray<UJoint*>();
+
+	UFUNCTION(BlueprintCallable)
+	void Connect(ULink* L1, ULink* L2);
+
+private:
+	void ResetPositionAndOrientation();
+public:	
+	virtual void Tick(float DeltaTime) override;
 };