Content/Models/arm.obj

@@ -1,26 +0,0 @@
-# Blender v2.76 (sub 0) OBJ File: ''
-# www.blender.org
-mtllib cube.mtl
-o Cube
-v 1.000000 -1.000000 -4.000000
-v 1.000000 -1.000000 4.000000
-v -1.000000 -1.000000 4.000000
-v -1.000000 -1.000000 -4.000000
-v 1.000000 1.000000 -4.000000
-v 1.000000 1.000000 4.00000
-v -1.000000 1.000000 4.000000
-v -1.000000 1.000000 -4.000000
-usemtl Material
-s off
-f 2/1/1 3/2/1 4/3/1
-f 8/1/2 7/4/2 6/5/2
-f 5/6/3 6/7/3 2/8/3
-f 6/8/4 7/5/4 3/4/4
-f 3/9/5 7/10/5 8/11/5
-f 1/12/6 4/13/6 8/11/6
-f 1/4/1 2/1/1 4/3/1
-f 5/14/2 8/1/2 6/5/2
-f 1/12/3 5/6/3 2/8/3
-f 2/12/4 6/8/4 3/4/4
-f 4/13/5 3/9/5 8/11/5
-f 5/6/6 1/12/6 8/11/6

Content/Models/cube.obj

@@ -1,26 +0,0 @@
-# Blender v2.76 (sub 0) OBJ File: ''
-# www.blender.org
-mtllib cube.mtl
-o Cube
-v 1.000000 -1.000000 -1.000000
-v 1.000000 -1.000000 1.000000
-v -1.000000 -1.000000 1.000000
-v -1.000000 -1.000000 -1.000000
-v 1.000000 1.000000 -1.000000
-v 1.000000 1.000000 1.000000
-v -1.000000 1.000000 1.000000
-v -1.000000 1.000000 -1.000000
-usemtl Material
-s off
-f 2/1/1 3/2/1 4/3/1
-f 8/1/2 7/4/2 6/5/2
-f 5/6/3 6/7/3 2/8/3
-f 6/8/4 7/5/4 3/4/4
-f 3/9/5 7/10/5 8/11/5
-f 1/12/6 4/13/6 8/11/6
-f 1/4/1 2/1/1 4/3/1
-f 5/14/2 8/1/2 6/5/2
-f 1/12/3 5/6/3 2/8/3
-f 2/12/4 6/8/4 3/4/4
-f 4/13/5 3/9/5 8/11/5
-f 5/6/6 1/12/6 8/11/6

Source/PBDRobotics/Joint.cpp

@@ -7,37 +7,30 @@
 #include "util.h"
 #include "Kismet/KismetMathLibrary.h"
 
-Vector3d UJoint::GetFirstWorldDirection() const{
-	return FirstLink->Orientation * FirstLocalPosition;
+Vector3d UJoint::GetFirstWorldPosition() const{
+	return FirstLink->Position + FirstLink->Orientation * FirstLocalPosition;
 }
 
-Vector3d UJoint::GetSecondWorldDirection() const{
-	return SecondLink->Orientation * SecondLocalPosition;
-}
-
-Vector3d UJoint::GetConnection() const{
-	return FirstLink->Position + GetFirstWorldDirection() - (SecondLink->Position + GetSecondWorldDirection());
+Vector3d UJoint::GetSecondWorldPosition() const{
+	return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition;
 }
 
 Vector3d UJoint::GetFirstWorldAxis() const{
 	return FirstLink->Orientation * FirstRotateAxis;
 }
-
 Vector3d UJoint::GetSecondWorldAxis() const{
 	return SecondLink->Orientation * SecondRotateAxis;
 }
 
-
-
 void UJoint::SolvePosition(const double H) const{
 	ULink* L1 = FirstLink;
 	ULink* L2 = SecondLink;
 
 	// Positional Constraints
 	{
-		const Vector3d R1 = GetFirstWorldDirection();
-		const Vector3d R2 = GetSecondWorldDirection();
-		const Vector3d D = GetConnection();
+		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();
@@ -49,7 +42,6 @@ void UJoint::SolvePosition(const double H) const{
 		const double W2 = L2->GetPositionalInverseMass(R2, N);
 
 		constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness)
-		
 		const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
 	
 		L1->Position += P / M1;
@@ -57,21 +49,20 @@ void UJoint::SolvePosition(const double H) const{
 		
 		Vector3d T1 = I1.inverse() * R1.cross(P);
 		Vector3d T2 = I2.inverse() * R2.cross(P);
-		const Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
-		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
-		L1->Orientation = (L1->Orientation + Add1).normalized();
-		L2->Orientation = (L2->Orientation - Add2).normalized();
-		UE_LOG(LogTemp, Log, TEXT("%f | %f | %f"), C, P.norm(), R2.cross(P).norm());
+		Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
+		Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
+		//L1->Orientation = L1->Orientation + Add1;
+		//L2->Orientation = L2->Orientation- Add2;
+		//UE_LOG(LogTemp, Log, TEXT("%f %f"), (T1).norm(), T2.norm());
 	}
 	
 	// Rotational Constraints
 	{
 		const Vector3d A1 = GetFirstWorldAxis();
 		const Vector3d A2 = GetSecondWorldAxis();
-		assert(A1.norm() == A2.norm() == 1);
 		const Vector3d Q = A1.cross(A2);
 		const Vector3d N = Q.normalized();
-		const double Theta = UKismetMathLibrary::Asin(Q.norm());// * 180. / PI;
+		const double Theta = Q.norm();
 
 		const Matrix3d I1 = L1->GetInertia();
 		const Matrix3d I2 = L2->GetInertia();
@@ -79,16 +70,16 @@ void UJoint::SolvePosition(const double H) const{
 		const double W1 = L1->GetRotationalInverseMass(N);
 		const double W2 = L2->GetRotationalInverseMass(N);
 
-		constexpr double A = 0; //1. / 10000000;
-		const Vector3d P = Theta / (W1 + W2 + A / (H * H)) * N;
+		const double A = 0.000001 / (H * H);
+		const Vector3d P = -Theta / (W1 + W2 + A) * N;
 		
 		Vector3d T1 = I1.inverse() * P;
 		Vector3d T2 = I2.inverse() * P;
-		const Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
-		const Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
-		//L1->Orientation = (L1->Orientation + Add1).normalized();
-		//L2->Orientation = (L2->Orientation - Add2).normalized();
-		//UE_LOG(LogTemp, Log, TEXT("%f | %f | %f | %f"), N.norm(), W1, W2, Q.norm());
+		T1 = T2 = 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;
+
+		// UE_LOG(LogTemp, Log, TEXT("%f | %f"), W1, W2);
 	}
 	
 }
@@ -101,11 +92,11 @@ void UJoint::SolveVelocity(const double H) const{
 	const Vector3d V1 = L1->Velocity;
 	const Vector3d V2 = L2->Velocity;
 	
-	constexpr double MuLin = 10000; // Damping strength
-	const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1);
+	constexpr double MuLin = 100000; // Damping strength
+	const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1);
 
-	const Vector3d R1 = GetFirstWorldDirection();
-	const Vector3d R2 = GetSecondWorldDirection();
+	const Vector3d R1 = GetFirstWorldPosition();
+	const Vector3d R2 = GetSecondWorldPosition();
 	const Vector3d N = (R2 - R1).normalized();
 
 	const Matrix3d I1 = L1->GetInertia();
@@ -116,10 +107,10 @@ void UJoint::SolveVelocity(const double H) const{
 	
 	const Vector3d P = DeltaV / (W1 + W2);
 
-	//UE_LOG(LogTemp, Log, TEXT("%f | %f | %f | %f | %f"), P.norm(), MuLin * H, DeltaV.norm(), (V2 - V1).norm());
+	// UE_LOG(LogTemp, Log, TEXT("%f | %f"), (V2 - V1).norm(), MuLin * H);
 	
-	//L1->Velocity += P / L1->Mass;
-	//L2->Velocity -= P / L2->Mass;
+	L1->Velocity += P / L1->Mass;
+	L2->Velocity -= P / L2->Mass;
 	//L1->AngularVelocity += I1.inverse() * R1.cross(P);
 	//L2->AngularVelocity -= I2.inverse() * R2.cross(P);
 }

Source/PBDRobotics/Joint.h

@@ -24,22 +24,12 @@ public:
 	UPROPERTY(BlueprintReadWrite)
 	ULink* SecondLink;
 
-	// First Connection point in local space
 	Vector3d FirstLocalPosition;
-
-	// Second Connection point in local space
 	Vector3d SecondLocalPosition;
 
-	// Direction from CenterOfMass to First Connection point
-	Vector3d GetFirstWorldDirection() const;
-
-	// Direction from CenterOfMass to Second Connection point
-	Vector3d GetSecondWorldDirection() const;
-
-	// First - Second in World-Space 
-	Vector3d GetConnection() const;
+	Vector3d GetFirstWorldPosition() const;
+	Vector3d GetSecondWorldPosition() const;
 
-	// 
 	Vector3d FirstRotateAxis;
 	Vector3d SecondRotateAxis;
 

Source/PBDRobotics/Link.cpp

@@ -12,8 +12,7 @@
 using namespace Eigen;
 
 Matrix3d ULink::GetInertia() const {
-	const Matrix3d R = Orientation.toRotationMatrix();
-	return R * Inertia_Tensor_Local * R.transpose();
+	return Orientation.toRotationMatrix() * Inertia_Tensor_Local;
 }
 
 double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
@@ -32,21 +31,25 @@ void ULink::Setup(){
 }
 
 void ULink::SetupProperties(){
-	
-	Mass = CalculateMass();
+	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
 	Position = ToEigen(GetComponentLocation());
 	Velocity = Vector3d::Zero();
-	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
-	//Inertia_Tensor_Local = Matrix3d::Identity() * Mass;
+	Mass = CalculateMass();
 	Orientation = ToEigen(GetComponentQuat());
 	AngularVelocity = Vector3d::Zero();
 
 	if (IsBase){
-		Mass = 1e50;
-		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50;
+		Mass = 1e30;
+		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e30;
 	}
 
-	Log(GetInertia());
+	const Matrix3d I = GetInertia().inverse();
+	UE_LOG(LogTemp, Log, TEXT("%s"), *GetName());
+	auto log = [this, I](int col){
+		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), I(col, 0), I(col, 1), I(col, 2));
+	};
+	log(0);log(1);log(2);
+	
 }
 
 void ULink::SetupJoints() const {
@@ -65,9 +68,7 @@ void ULink::SetupJoints() const {
 }
 
 void ULink::Update(const double H){
-	
-	// Translation
-	Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 1;
+	Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
 	if (IsBase)
 		ExtTransForce = Vector3d::Zero();
 	
@@ -75,13 +76,9 @@ void ULink::Update(const double H){
 	Velocity += H * ExtTransForce / Mass;
 	Position += H * Velocity;
 	
-	// Rotation
-	const Vector3d ExtRotForce = Vector3d::Zero();
-	
 	Last_Orientation = Orientation;
+	// Angle Velocity has no external contributor
 	Vector3d w = AngularVelocity;
-	AngularVelocity += H * GetInertia().inverse() * (ExtRotForce - w.cross(GetInertia() * w));
-	w = AngularVelocity;
 	Orientation = Orientation + Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H;
 	Orientation.normalize();
 }

Source/PBDRobotics/Robot.cpp

@@ -3,8 +3,6 @@
 
 #include "Robot.h"
 
-#include "util.h"
-
 
 ARobot::ARobot(){
 	PrimaryActorTick.bCanEverTick = true;
@@ -27,7 +25,7 @@ void ARobot::BeginPlay(){
 void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
 	
-	constexpr int SubSteps = 10;
+	constexpr int SubSteps = 30;
 	const double h = DeltaTime / SubSteps;
 	for (int i = 0; i < SubSteps; i++){
 		for (auto* Link : Parts)

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);
@@ -31,24 +30,6 @@ 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){
-	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));
-		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(1, 0), M(1, 1), M(1, 2));
-		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(2, 0), M(2, 1), M(2, 2));
+inline Quaterniond operator-= (const Quaterniond Q1, const Quaterniond Q2){
+	return Q1 - Q2;
 }
-	if (M.cols() == 1){
-		UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0), M(1), M(2));
-	}
-}
-
-inline void Log(const double V){
-	UE_LOG(LogTemp, Log, TEXT("%f"), V);
-}
-
-inline void Log(const char* S){
-	UE_LOG(LogTemp, Log, TEXT("%s"), *FString(S));
-}
-
-