start of rotational constraints, behind comments

Source/PBDRobotics/Joint.cpp

@@ -15,36 +15,73 @@ 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 = 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 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 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;
+		constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness)
+		const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
 	
-	L1->Position += P / M1;
-	L2->Position -= P / M2;
+		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;
-
+		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());
+	}
+	
 	// Rotational Constraints
+	{
+		const Vector3d A1 = GetFirstWorldAxis();
+		const Vector3d A2 = GetSecondWorldAxis();
+		const Vector3d Q = A1.cross(A2);
+		const Vector3d N = Q.normalized();
+		const double Theta = Q.norm();
+
+		const Matrix3d I1 = L1->GetInertia();
+		const Matrix3d I2 = L2->GetInertia();
+
+		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;
+		
+		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);
+	}
+	
 }
 
 void UJoint::SolveVelocity(const double H) const{
@@ -54,8 +91,8 @@ void UJoint::SolveVelocity(const double H) const{
 	// Damping
 	const Vector3d V1 = L1->Velocity;
 	const Vector3d V2 = L2->Velocity;
-
-	constexpr double MuLin = 100000;
+	
+	constexpr double MuLin = 100000; // Damping strength
 	const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1);
 
 	const Vector3d R1 = GetFirstWorldPosition();
@@ -74,8 +111,8 @@ void UJoint::SolveVelocity(const double H) const{
 	
 	L1->Velocity += P / L1->Mass;
 	L2->Velocity -= P / L2->Mass;
-	L1->AngularVelocity += I1.inverse() * R1.cross(P);
-	L2->AngularVelocity -= I2.inverse() * R2.cross(P);
+	//L1->AngularVelocity += I1.inverse() * R1.cross(P);
+	//L2->AngularVelocity -= I2.inverse() * R2.cross(P);
 }
 
 UJoint::UJoint()

Source/PBDRobotics/Joint.h

@@ -24,15 +24,18 @@ public:
 	UPROPERTY(BlueprintReadWrite)
 	ULink* SecondLink;
 
-	Vector3d FirstRotateAxis;
-	Vector3d SecondRotateAxis;
-
 	Vector3d FirstLocalPosition;
 	Vector3d SecondLocalPosition;
 
 	Vector3d GetFirstWorldPosition() const;
 	Vector3d GetSecondWorldPosition() const;
 
+	Vector3d FirstRotateAxis;
+	Vector3d SecondRotateAxis;
+
+	Vector3d GetFirstWorldAxis() const;
+	Vector3d GetSecondWorldAxis() const;
+
 	void SolvePosition(const double H) const;
 	void SolveVelocity(const double H) const;
 	

Source/PBDRobotics/Link.cpp

@@ -20,8 +20,9 @@ double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const
 	const Matrix3d I = GetInertia();
 	return 1 / M + R.cross(N).transpose() * I.inverse() * R.cross(N);
 }
-double ULink::GetRotationalInverseMass() const{
-	return 0;
+double ULink::GetRotationalInverseMass(const Vector3d N) const{
+	const Matrix3d I = GetInertia();
+	return N.transpose() * I.inverse() * N;
 }
 
 void ULink::Setup(){
@@ -33,9 +34,22 @@ void ULink::SetupProperties(){
 	Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
 	Position = ToEigen(GetComponentLocation());
 	Velocity = Vector3d::Zero();
-	Mass = IsBase ? INFINITY : CalculateMass();
-	Orientation = Quaterniond::Identity();
+	Mass = CalculateMass();
+	Orientation = ToEigen(GetComponentQuat());
 	AngularVelocity = Vector3d::Zero();
+
+	if (IsBase){
+		Mass = 1e30;
+		Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e30;
+	}
+
+	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 {
@@ -54,7 +68,9 @@ void ULink::SetupJoints() const {
 }
 
 void ULink::Update(const double H){
-	const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
+	Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
+	if (IsBase)
+		ExtTransForce = Vector3d::Zero();
 	
 	Last_Position = Position;
 	Velocity += H * ExtTransForce / Mass;
@@ -63,7 +79,7 @@ void ULink::Update(const double H){
 	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 = Orientation + Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H;
 	Orientation.normalize();
 }
 

Source/PBDRobotics/Link.h

@@ -44,7 +44,7 @@ public:
 	Matrix3d GetInertia() const;
 	
 	double GetPositionalInverseMass(const Vector3d R, const Vector3d N) const;
-	double GetRotationalInverseMass() const;
+	double GetRotationalInverseMass(const Vector3d N) const;
 	
 	void Setup();
 private:

Source/PBDRobotics/Robot.cpp

@@ -25,7 +25,7 @@ void ARobot::BeginPlay(){
 void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
 	
-	constexpr int SubSteps = 100;
+	constexpr int SubSteps = 30;
 	const double h = DeltaTime / SubSteps;
 	for (int i = 0; i < SubSteps; i++){
 		for (auto* Link : Parts)

Source/PBDRobotics/util.h

@@ -6,6 +6,10 @@ inline Vector3d ToEigen(const FVector V){
 	return Vector3d(V.X, V.Y, V.Z);
 }
 
+inline Quaterniond ToEigen(const FQuat Q){
+	return Quaterniond(Q.W, Q.X, Q.Y, Q.Z);
+}
+
 inline FVector FromEigen(const Vector3d V){
 	return FVector(V.x(), V.y(), V.z());
 }
@@ -22,10 +26,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 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());
 }