Positional Constraints work, added Damping. Next: Rotational Constraints

Source/PBDRobotics/Joint.cpp

@@ -3,6 +3,81 @@
 
 #include "Joint.h"
 
+#include "Link.h"
+#include "util.h"
+#include "Kismet/KismetMathLibrary.h"
+
+Vector3d UJoint::GetFirstWorldPosition() const{
+	return FirstLink->Position + FirstLink->Orientation * FirstLocalPosition;
+}
+
+Vector3d UJoint::GetSecondWorldPosition() const{
+	return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition;
+}
+
+void UJoint::SolvePosition(const double H) const{
+	ULink* L1 = FirstLink;
+	ULink* L2 = SecondLink;
+
+	// Positional Constraints
+	const Vector3d R1 = GetFirstWorldPosition();
+	const Vector3d R2 = GetSecondWorldPosition();
+	const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated
+	const Vector3d N = D.normalized();
+	const double C = D.norm();
+	const Matrix3d I1 = L1->GetInertia();
+	const Matrix3d I2 = L2->GetInertia();
+	const double M1 = L1->Mass;
+	const double M2 = L2->Mass;
+	
+	const double W1 = L1->GetPositionalInverseMass(R1, N);
+	const double W2 = L2->GetPositionalInverseMass(R2, N);
+
+	const double A = 0.000001 / (H * H);
+	const Vector3d P = -C / (W1 + W2 + A) * N;
+	
+	L1->Position += P / M1;
+	L2->Position -= P / M2;
+		
+	Vector3d T1 = I1.inverse() * R1.cross(P);
+	Vector3d T2 = I2.inverse() * R2.cross(P);
+	L1->Orientation += Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
+	L2->Orientation -= Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
+
+	// Rotational Constraints
+}
+
+void UJoint::SolveVelocity(const double H) const{
+	ULink* L1 = FirstLink;
+	ULink* L2 = SecondLink;
+
+	// Damping
+	const Vector3d V1 = L1->Velocity;
+	const Vector3d V2 = L2->Velocity;
+
+	constexpr double MuLin = 100000;
+	const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1);
+
+	const Vector3d R1 = GetFirstWorldPosition();
+	const Vector3d R2 = GetSecondWorldPosition();
+	const Vector3d N = (R2 - R1).normalized();
+
+	const Matrix3d I1 = L1->GetInertia();
+	const Matrix3d I2 = L2->GetInertia();
+	
+	const double W1 = L1->GetPositionalInverseMass(R1, N);
+	const double W2 = L2->GetPositionalInverseMass(R2, N);
+	
+	const Vector3d P = DeltaV / (W1 + W2);
+
+	// UE_LOG(LogTemp, Log, TEXT("%f | %f"), (V2 - V1).norm(), MuLin * H);
+	
+	L1->Velocity += P / L1->Mass;
+	L2->Velocity -= P / L2->Mass;
+	L1->AngularVelocity += I1.inverse() * R1.cross(P);
+	L2->AngularVelocity -= I2.inverse() * R2.cross(P);
+}
+
 UJoint::UJoint()
 {
 }

Source/PBDRobotics/Joint.h

@@ -30,5 +30,11 @@ public:
 	Vector3d FirstLocalPosition;
 	Vector3d SecondLocalPosition;
 
+	Vector3d GetFirstWorldPosition() const;
+	Vector3d GetSecondWorldPosition() const;
+
+	void SolvePosition(const double H) const;
+	void SolveVelocity(const double H) const;
+	
 	UJoint();
 };

Source/PBDRobotics/Link.cpp

@@ -11,14 +11,17 @@
 
 using namespace Eigen;
 
-// Inertia tensor in world space
-Matrix3d ULink::GetMomentOfInertia() const {
-	return GetRotationMatrix() * Inertia_Tensor_Local;
+Matrix3d ULink::GetInertia() const {
+	return Orientation.toRotationMatrix() * Inertia_Tensor_Local;
 }
 
-Matrix3d ULink::GetRotationMatrix() const{
-	const FQuat Q = this->GetRelativeRotation().Quaternion();
-	return Quaterniond(Q.W, Q.X, Q.Y, Q.Z).toRotationMatrix();
+double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
+	const double M = Mass;
+	const Matrix3d I = GetInertia();
+	return 1 / M + R.cross(N).transpose() * I.inverse() * R.cross(N);
+}
+double ULink::GetRotationalInverseMass() const{
+	return 0;
 }
 
 void ULink::Setup(){
@@ -28,14 +31,11 @@ void ULink::Setup(){
 
 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();
 }
 
 void ULink::SetupJoints() const {
@@ -53,9 +53,8 @@ void ULink::SetupJoints() const {
 	}
 }
 
-
 void ULink::Update(const double H){
-	const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass;
+	const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
 	
 	Last_Position = Position;
 	Velocity += H * ExtTransForce / Mass;
@@ -75,7 +74,7 @@ void ULink::Integrate(const double H){
 	AngularVelocity = DeltaOrientation.w() >= 0 ? AngularVelocity : -AngularVelocity;
 }
 
-void ULink::UpdateVisualTransform(){
+void ULink::UpdateInternalTransform(){
 	SetWorldLocation(FromEigen(Position));
 	SetWorldRotation(FromEigen(Orientation));
 }

Source/PBDRobotics/Link.h

@@ -21,13 +21,11 @@ public:
 	
 	// Properties in World space
 	
-	Vector3d CenterOfMass;
 	Vector3d Position;
 	Vector3d Velocity;
 	double Mass;
 	Quaterniond Orientation;
 	Vector3d AngularVelocity;
-	Matrix3d Inertia;
 
 	Vector3d Last_Position;
 	Quaterniond Last_Orientation;
@@ -40,9 +38,13 @@ public:
 	
 	void Update(const double H);
 	void Integrate(const double H);
-	void UpdateVisualTransform();
+	void UpdateInternalTransform();
 
-	Matrix3d GetRotationMatrix() const;
+	// Inertia tensor in world space
+	Matrix3d GetInertia() const;
+	
+	double GetPositionalInverseMass(const Vector3d R, const Vector3d N) const;
+	double GetRotationalInverseMass() const;
 	
 	void Setup();
 private:
@@ -50,7 +52,6 @@ private:
 	bool IsBase = false;
 	
 	Matrix3d Inertia_Tensor_Local;
-	Matrix3d GetMomentOfInertia() const;
 	
 	void SetupJoints() const;
 	void SetupProperties();

Source/PBDRobotics/Robot.cpp

@@ -3,6 +3,7 @@
 
 #include "Robot.h"
 
+
 ARobot::ARobot(){
 	PrimaryActorTick.bCanEverTick = true;
 }
@@ -11,30 +12,31 @@ void ARobot::BeginPlay(){
 	Super::BeginPlay();
 	
 	GetComponents<ULink>(Parts);
-	for (auto* Link : Parts) Link->Setup(); 
+	for (auto* Link : Parts){
+		Link->Setup();
+		if (Link->PrevJoint)
+			Joints.Add(Link->PrevJoint);
+	}
+
+	// Example: 3 Parts are connected with 2 Joints
+	assert(Parts.Num() == Joints.Num() + 1);
 }
 
 void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
 	
-	constexpr int SubSteps = 10;
+	constexpr int SubSteps = 100;
 	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->Update(h);
+		for (const auto* Joint : Joints)
+			Joint->SolvePosition(h);
+		for (auto* Link : Parts)
+			Link->Integrate(h);
+		for (const auto* Joint : Joints)
+			Joint->SolveVelocity(h);
 	}
-
-	for (auto* Link : Parts) Link->UpdateVisualTransform();
-}
-
-void ARobot::SolvePositions(){
-
-	
-	
-}
-
-void ARobot::SolveVelocities(){
-	
+	for (auto* Link : Parts)
+		Link->UpdateInternalTransform();
 }

Source/PBDRobotics/Robot.h

@@ -27,6 +27,6 @@ private:
 	UPROPERTY()
 	TArray<ULink*> Parts;
 
-	void SolvePositions();
-	void SolveVelocities();
+	UPROPERTY()
+	TArray<UJoint*> Joints = TArray<UJoint*>();
 };

Source/PBDRobotics/util.h

@@ -14,7 +14,7 @@ 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){
+inline Quaterniond operator* (Quaterniond Q, double D){
 	return Quaterniond(Q.w() * D, Q.x() * D, Q.y() * D, Q.z() * D);
 }
 
@@ -25,3 +25,11 @@ inline Quaterniond operator+ (const Quaterniond Q1, const Quaterniond Q2){
 inline Quaterniond operator+= (const Quaterniond Q1, const Quaterniond Q2){
 	return Q1 + 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){
+	return Q1 - Q2;
+}