diff --git a/Content/Blueprints/BP_TestBot.uasset b/Content/Blueprints/BP_TestBot.uasset
index 998dcf1..d19ec8e 100644
Binary files a/Content/Blueprints/BP_TestBot.uasset and b/Content/Blueprints/BP_TestBot.uasset differ
diff --git a/Content/Blueprints/BP_TestCube.uasset b/Content/Blueprints/BP_TestCube.uasset
new file mode 100644
index 0000000..8afff8b
Binary files /dev/null and b/Content/Blueprints/BP_TestCube.uasset differ
diff --git a/Content/Levels/Main.umap b/Content/Levels/Main.umap
index 27accfe..f5f4353 100644
Binary files a/Content/Levels/Main.umap and b/Content/Levels/Main.umap differ
diff --git a/Content/Models/cube.obj b/Content/Models/cube.obj
new file mode 100644
index 0000000..e31cf14
--- /dev/null
+++ b/Content/Models/cube.obj
@@ -0,0 +1,26 @@
+# 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
diff --git a/Content/Models/test_base.uasset b/Content/Models/test_base.uasset
new file mode 100644
index 0000000..6ce86a9
Binary files /dev/null and b/Content/Models/test_base.uasset differ
diff --git a/Source/PBDRobotics/Joint.cpp b/Source/PBDRobotics/Joint.cpp
index 21619d8..baf9d22 100644
--- a/Source/PBDRobotics/Joint.cpp
+++ b/Source/PBDRobotics/Joint.cpp
@@ -7,30 +7,37 @@
 #include "util.h"
 #include "Kismet/KismetMathLibrary.h"
 
-Vector3d UJoint::GetFirstWorldPosition() const{
-	return FirstLink->Position + FirstLink->Orientation * FirstLocalPosition;
+Vector3d UJoint::GetFirstWorldDirection() const{
+	return FirstLink->Orientation * FirstLocalPosition;
 }
 
-Vector3d UJoint::GetSecondWorldPosition() const{
-	return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition;
+Vector3d UJoint::GetSecondWorldDirection() const{
+	return SecondLink->Orientation * SecondLocalPosition;
+}
+
+Vector3d UJoint::GetConnection() const{
+	return FirstLink->Position + GetFirstWorldDirection() - (SecondLink->Position + GetSecondWorldDirection());
 }
 
 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 = GetFirstWorldPosition();
-		const Vector3d R2 = GetSecondWorldPosition();
-		const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated
+		const Vector3d R1 = GetFirstWorldDirection();
+		const Vector3d R2 = GetSecondWorldDirection();
+		const Vector3d D = GetConnection();
 		const Vector3d N = D.normalized();
 		const double C = D.norm();
 		const Matrix3d I1 = L1->GetInertia();
@@ -41,7 +48,8 @@ void UJoint::SolvePosition(const double H) const{
 		const double W1 = L1->GetPositionalInverseMass(R1, N);
 		const double W2 = L2->GetPositionalInverseMass(R2, N);
 
-		constexpr double A = 1. / 1000000; // Compliance (inverse of stiffness)
+		constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness)
+		
 		const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
 	
 		L1->Position += P / M1;
@@ -49,20 +57,21 @@ void UJoint::SolvePosition(const double H) const{
 		
 		Vector3d T1 = I1.inverse() * R1.cross(P);
 		Vector3d T2 = I2.inverse() * R2.cross(P);
-		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());
+		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());
 	}
 	
 	// 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 = Q.norm();
+		const double Theta = UKismetMathLibrary::Asin(Q.norm());// * 180. / PI;
 
 		const Matrix3d I1 = L1->GetInertia();
 		const Matrix3d I2 = L2->GetInertia();
@@ -70,16 +79,16 @@ void UJoint::SolvePosition(const double H) const{
 		const double W1 = L1->GetRotationalInverseMass(N);
 		const double W2 = L2->GetRotationalInverseMass(N);
 
-		const double A = 0.000001 / (H * H);
-		const Vector3d P = -Theta / (W1 + W2 + A) * N;
+		constexpr double A = 0; //1. / 10000000;
+		const Vector3d P = Theta / (W1 + W2 + A / (H * H)) * N;
 		
 		Vector3d T1 = I1.inverse() * P;
 		Vector3d T2 = I2.inverse() * P;
-		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);
+		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());
 	}
 	
 }
@@ -92,11 +101,11 @@ void UJoint::SolveVelocity(const double H) const{
 	const Vector3d V1 = L1->Velocity;
 	const Vector3d V2 = L2->Velocity;
 	
-	constexpr double MuLin = 100000; // Damping strength
-	const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1);
+	constexpr double MuLin = 10000; // Damping strength
+	const Vector3d DeltaV = (V2 - V1) * (MuLin * H < 1 ? MuLin * H : 1);
 
-	const Vector3d R1 = GetFirstWorldPosition();
-	const Vector3d R2 = GetSecondWorldPosition();
+	const Vector3d R1 = GetFirstWorldDirection();
+	const Vector3d R2 = GetSecondWorldDirection();
 	const Vector3d N = (R2 - R1).normalized();
 
 	const Matrix3d I1 = L1->GetInertia();
@@ -107,10 +116,10 @@ void UJoint::SolveVelocity(const double H) const{
 	
 	const Vector3d P = DeltaV / (W1 + W2);
 
-	// UE_LOG(LogTemp, Log, TEXT("%f | %f"), (V2 - V1).norm(), MuLin * H);
+	//UE_LOG(LogTemp, Log, TEXT("%f | %f | %f | %f | %f"), P.norm(), MuLin * H, DeltaV.norm(), (V2 - V1).norm());
 	
-	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);
 }
diff --git a/Source/PBDRobotics/Joint.h b/Source/PBDRobotics/Joint.h
index d4ec465..d43c1cf 100644
--- a/Source/PBDRobotics/Joint.h
+++ b/Source/PBDRobotics/Joint.h
@@ -24,12 +24,22 @@ public:
 	UPROPERTY(BlueprintReadWrite)
 	ULink* SecondLink;
 
+	// First Connection point in local space
 	Vector3d FirstLocalPosition;
+
+	// Second Connection point in local space
 	Vector3d SecondLocalPosition;
 
-	Vector3d GetFirstWorldPosition() const;
-	Vector3d GetSecondWorldPosition() const;
+	// 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 FirstRotateAxis;
 	Vector3d SecondRotateAxis;
 
diff --git a/Source/PBDRobotics/Link.cpp b/Source/PBDRobotics/Link.cpp
index b84d99e..7564757 100644
--- a/Source/PBDRobotics/Link.cpp
+++ b/Source/PBDRobotics/Link.cpp
@@ -12,7 +12,8 @@
 using namespace Eigen;
 
 Matrix3d ULink::GetInertia() const {
-	return Orientation.toRotationMatrix() * Inertia_Tensor_Local;
+	const Matrix3d R = Orientation.toRotationMatrix();
+	return R * Inertia_Tensor_Local * R.transpose();
 }
 
 double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
@@ -31,25 +32,21 @@ void ULink::Setup(){
 }
 
 void ULink::SetupProperties(){
-	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
+	
+	Mass = CalculateMass();
 	Position = ToEigen(GetComponentLocation());
 	Velocity = Vector3d::Zero();
-	Mass = CalculateMass();
+	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
+	//Inertia_Tensor_Local = Matrix3d::Identity() * Mass;
 	Orientation = ToEigen(GetComponentQuat());
 	AngularVelocity = Vector3d::Zero();
 
 	if (IsBase){
-		Mass = 1e30;
-		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e30;
+		Mass = 1e50;
+		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e50;
 	}
 
-	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);
-	
+	Log(GetInertia());
 }
 
 void ULink::SetupJoints() const {
@@ -68,17 +65,23 @@ void ULink::SetupJoints() const {
 }
 
 void ULink::Update(const double H){
-	Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
+	
+	// Translation
+	Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 1;
 	if (IsBase)
 		ExtTransForce = Vector3d::Zero();
 	
 	Last_Position = Position;
 	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();
 }
diff --git a/Source/PBDRobotics/Robot.cpp b/Source/PBDRobotics/Robot.cpp
index b0aad8c..137db44 100644
--- a/Source/PBDRobotics/Robot.cpp
+++ b/Source/PBDRobotics/Robot.cpp
@@ -3,6 +3,8 @@
 
 #include "Robot.h"
 
+#include "util.h"
+
 
 ARobot::ARobot(){
 	PrimaryActorTick.bCanEverTick = true;
@@ -25,7 +27,7 @@ void ARobot::BeginPlay(){
 void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
 	
-	constexpr int SubSteps = 30;
+	constexpr int SubSteps = 10;
 	const double h = DeltaTime / SubSteps;
 	for (int i = 0; i < SubSteps; i++){
 		for (auto* Link : Parts)
diff --git a/Source/PBDRobotics/util.h b/Source/PBDRobotics/util.h
index 9b4f420..54a4d42 100644
--- a/Source/PBDRobotics/util.h
+++ b/Source/PBDRobotics/util.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Eigen/Dense"
+#include <string>
 
 inline Vector3d ToEigen(const FVector V){
 	return Vector3d(V.X, V.Y, V.Z);
@@ -30,6 +31,24 @@ 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;
+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));
+	}
+	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));
+}
+
+