Added joints, TestBot, Setup and simulate functions added. TODO: SolvePositions, SolveVelocities

Models need to have Sockets "JointPrev" and "JointNext" for information about joint position and rotation axis.

Config/DefaultEngine.ini

@@ -19,3 +19,8 @@ AppliedDefaultGraphicsPerformance=Maximum
 [/Script/Engine.RendererSettings]
 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")

Source/PBDRobotics/Joint.cpp

@@ -0,0 +1,8 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "Joint.h"
+
+UJoint::UJoint()
+{
+}

Source/PBDRobotics/Joint.h

@@ -0,0 +1,34 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "Eigen/Dense"
+#include "CoreMinimal.h"
+#include "Joint.generated.h"
+
+using namespace Eigen;
+
+class ULink;
+/**
+ * 
+ */
+UCLASS(BlueprintType, ClassGroup=(Custom))
+class PBDROBOTICS_API UJoint : public UObject
+{
+	GENERATED_BODY()
+public:
+	
+	UPROPERTY(BlueprintReadWrite)
+	ULink* FirstLink;
+
+	UPROPERTY(BlueprintReadWrite)
+	ULink* SecondLink;
+
+	Vector3d FirstRotateAxis;
+	Vector3d SecondRotateAxis;
+
+	Vector3d FirstLocalPosition;
+	Vector3d SecondLocalPosition;
+	
+	UJoint();
+};

Source/PBDRobotics/Link.cpp

@@ -5,39 +5,77 @@
 
 #include <iostream>
 
+#include "util.h"
 #include "Eigen/Dense"
+#include "Engine/StaticMeshSocket.h"
 
-Eigen::Matrix3d ULink::GetMomentOfInertia() const {
-	const FVector FI = this->GetInertiaTensor();
+using namespace Eigen;
 
-	// Inertial tensor of rotation
-	const Eigen::Vector3d I(FI.X, FI.Y, FI.Z);
+// Inertia tensor in world space
+Matrix3d ULink::GetMomentOfInertia() const {
+	return GetRotationMatrix() * Inertia_Tensor_Local;
+}
 
-	// Moment of inertia
-	const Eigen::Matrix3d I0;// = R.transpose() * I * R;
+Matrix3d ULink::GetRotationMatrix() const{
+	const FQuat Q = this->GetRelativeRotation().Quaternion();
+	return Quaterniond(Q.W, Q.X, Q.Y, Q.Z).toRotationMatrix();
+}
 
-	return I0;
+void ULink::Setup(){
+	SetupJoints();
+	SetupProperties();
+}
+
+void ULink::SetupProperties(){
+	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
+	
+	CenterOfMass = ToEigen(GetCenterOfMass());
+	Position = ToEigen(GetComponentLocation());
+	Velocity = Vector3d::Zero();
+	Mass = IsBase ? INFINITY : CalculateMass();
+	Orientation = Quaterniond::Identity();
+	AngularVelocity = Vector3d::Zero();
+	Inertia = GetMomentOfInertia();
 }
 
-Eigen::Matrix3d ULink::GetRotationMatrix() const{
-	const FQuat fq = this->GetRelativeRotation().Quaternion();
-	const Eigen::Vector3d q(fq.X, fq.Y, fq.Z);
-	const double w = fq.W;
-	Eigen::Matrix3d q_hat;
-	q_hat << 0, -q.z(), q.y(),
-			 q.z(), 0, -q.x(),
-			 -q.y(), q.x(), 0;
-	const Eigen::Matrix3d R = (w * w - q.dot(q)) * Eigen::Matrix3d::Identity() + 2 * q * q.transpose() + 2 * w * q_hat;
-	return R;
+void ULink::SetupJoints() const {
+	if (const UStaticMeshSocket* Socket = GetSocketByName("JointPrev")){
+		if (PrevJoint){
+			PrevJoint->SecondLocalPosition = ToEigen(Socket->RelativeLocation);
+			PrevJoint->SecondRotateAxis = ToEigen(Socket->RelativeRotation.Vector());
+		}
+	}
+	if (const UStaticMeshSocket* Socket = GetSocketByName("JointNext")){
+		if (NextJoint){
+			NextJoint->FirstLocalPosition = ToEigen(Socket->RelativeLocation);
+			NextJoint->FirstRotateAxis = ToEigen(Socket->RelativeRotation.Vector());
+		}
+	}
 }
 
-void ULink::BeginPlay(){
-	const FVector t = this->GetInertiaTensor();
-	inertia_tensor = Eigen::Vector3d(t.X, t.Y, t.Z);
+
+void ULink::Update(const double H){
+	const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass;
 	
-	const double M = this->CalculateMass();
-	Eigen::Vector3d v;
-	v << 3, 5, 7;
+	Last_Position = Position;
+	Velocity += H * ExtTransForce / Mass;
+	Position += H * Velocity;
 	
-	UE_LOG(LogTemp, Log, TEXT("%f"), M);
+	Last_Orientation = Orientation;
+	// Angle Velocity has no external contributor
+	Vector3d w = AngularVelocity;
+	Orientation += Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H;
+	Orientation.normalize();
+}
+
+void ULink::Integrate(const double H){
+	Velocity = (Position - Last_Position) / H;
+	Quaterniond DeltaOrientation = Orientation * Last_Orientation.inverse();
+	AngularVelocity = 2 * DeltaOrientation.vec() / H;
+	AngularVelocity = DeltaOrientation.w() >= 0 ? AngularVelocity : -AngularVelocity;
+}
+
+void ULink::UpdateVisualTransform(){
+	SetWorldLocation(FromEigen(Position));
+	SetWorldRotation(FromEigen(Orientation));
 }

Source/PBDRobotics/Link.h

@@ -3,10 +3,13 @@
 #pragma once
 
 #include "CoreMinimal.h"
+#include "Joint.h"
 #include "Components/StaticMeshComponent.h"
 #include "Eigen/Dense"
 #include "Link.generated.h"
 
+using namespace Eigen;
+
 /**
  * 
  */
@@ -14,21 +17,41 @@ UCLASS(ClassGroup=(Custom), meta=(BlueprintSpawnableComponent))
 class PBDROBOTICS_API ULink : public UStaticMeshComponent
 {
 	GENERATED_BODY()
-
 public:
-	Eigen::Vector3d CenterOfMass;
-	Eigen::Vector3d Position;
-	Eigen::Vector3d Velocity;
+	
+	// Properties in World space
+	
+	Vector3d CenterOfMass;
+	Vector3d Position;
+	Vector3d Velocity;
 	double Mass;
-	Eigen::Quaterniond Orientation;
-	Eigen::Vector3d AngleVelocity;
-	Eigen::Matrix3d Inertia;
+	Quaterniond Orientation;
+	Vector3d AngularVelocity;
+	Matrix3d Inertia;
+
+	Vector3d Last_Position;
+	Quaterniond Last_Orientation;
+	
+	UPROPERTY(EditAnywhere, BlueprintReadWrite)
+	UJoint* PrevJoint;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite)
+	UJoint* NextJoint;
+	
+	void Update(const double H);
+	void Integrate(const double H);
+	void UpdateVisualTransform();
 
+	Matrix3d GetRotationMatrix() const;
+	
+	void Setup();
 private:
-	Eigen::Vector3d inertia_tensor;
-	Eigen::Matrix3d GetMomentOfInertia() const;
-	Eigen::Matrix3d GetRotationMatrix() const;
+	UPROPERTY(EditAnywhere)
+	bool IsBase = false;
+	
+	Matrix3d Inertia_Tensor_Local;
+	Matrix3d GetMomentOfInertia() const;
 	
-protected:
-	virtual void BeginPlay() override;
+	void SetupJoints() const;
+	void SetupProperties();
 };

Source/PBDRobotics/Robot.cpp

@@ -3,25 +3,38 @@
 
 #include "Robot.h"
 
-// Sets default values
-ARobot::ARobot()
-{
- 	// Set this actor to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
+ARobot::ARobot(){
 	PrimaryActorTick.bCanEverTick = true;
-
 }
 
-// Called when the game starts or when spawned
-void ARobot::BeginPlay()
-{
+void ARobot::BeginPlay(){
 	Super::BeginPlay();
 	
+	GetComponents<ULink>(Parts);
+	for (auto* Link : Parts) Link->Setup(); 
 }
 
-// Called every frame
-void ARobot::Tick(float DeltaTime)
-{
+void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
+	
+	constexpr int SubSteps = 10;
+	const double h = DeltaTime / SubSteps;
+	for (int i = 0; i < SubSteps; i++){
+		for (auto* Link : Parts) Link->Update(h);
+		SolvePositions();
+		for (auto* Link : Parts) Link->Integrate(h);
+		SolveVelocities();
+	}
+
+	for (auto* Link : Parts) Link->UpdateVisualTransform();
+}
+
+void ARobot::SolvePositions(){
 
+	
+	
 }
 
+void ARobot::SolveVelocities(){
+	
+}

Source/PBDRobotics/Robot.h

@@ -3,6 +3,7 @@
 #pragma once
 
 #include "CoreMinimal.h"
+#include "Link.h"
 #include "GameFramework/Actor.h"
 #include "Robot.generated.h"
 
@@ -10,7 +11,6 @@ UCLASS()
 class PBDROBOTICS_API ARobot : public AActor
 {
 	GENERATED_BODY()
-	
 public:	
 	// Sets default values for this actor's properties
 	ARobot();
@@ -23,4 +23,10 @@ public:
 	// Called every frame
 	virtual void Tick(float DeltaTime) override;
 
+private:
+	UPROPERTY()
+	TArray<ULink*> Parts;
+
+	void SolvePositions();
+	void SolveVelocities();
 };

Source/PBDRobotics/util.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include "Eigen/Dense"
+
+inline Vector3d ToEigen(const FVector V){
+	return Vector3d(V.X, V.Y, V.Z);
+}
+
+inline FVector FromEigen(const Vector3d V){
+	return FVector(V.x(), V.y(), V.z());
+}
+
+inline FQuat FromEigen(const Quaterniond Q){
+	return FQuat(Q.x(), Q.y(), Q.z(), Q.w());
+}
+
+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){
+	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){
+	return Q1 + Q2;
+}