Added Gravity toggle Hotkey G (default off)

Source/PBDRobotics/Link.cpp

@@ -16,12 +16,12 @@ Matrix3d ULink::GetInertia() const {
 	return R * Inertia_Tensor_Local * R.transpose();
 }
 
-double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
+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 Vector3d N) const{
+double ULink::GetRotationalInverseMass(const Vector3d& N) const{
 	const Matrix3d I = GetInertia();
 	return N.transpose() * I.inverse() * N;
 }
@@ -62,19 +62,12 @@ void ULink::SetupJoints() const {
 	}
 }
 
-void ULink::Update(const double H){
+void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce){
 	
 	// 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;
 	Vector3d w = AngularVelocity;

Source/PBDRobotics/Link.h

@@ -36,24 +36,24 @@ public:
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
 	UJoint* NextJoint;
 	
-	void Update(const double H);
+	void Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce);
 	void Integrate(const double H);
 	void UpdateInternalTransform();
 
 	// Inertia tensor in world space
 	Matrix3d GetInertia() const;
 	
-	double GetPositionalInverseMass(const Vector3d R, const Vector3d N) const;
+	double GetPositionalInverseMass(const Vector3d& R, const Vector3d& N) const;
-	double GetRotationalInverseMass(const Vector3d N) const;
+	double GetRotationalInverseMass(const Vector3d& N) const;
 	
 	void Setup();
 
 	UPROPERTY(EditAnywhere)
 	bool IsEffector = false;
-private:
+
 	UPROPERTY(EditAnywhere)
 	bool IsBase = false;
-	
+private:
 	Matrix3d Inertia_Tensor_Local;
 	
 	void SetupJoints() const;

Source/PBDRobotics/Robot.cpp

@@ -41,12 +41,16 @@ void ARobot::Connect(ULink* L1, ULink* L2){
 
 void ARobot::Tick(const float DeltaTime){
 	Super::Tick(DeltaTime);
-	
 	constexpr int SubSteps = 50;
 	const double h = DeltaTime / SubSteps;
 	for (int i = 0; i < SubSteps; i++){
-		for (auto* Link : Links)
+		for (auto* Link : Links){
-			Link->Update(h);
+			Vector3d Gravity = Vector3d::Zero();
+			if (HasGravity && !Link->IsBase)
+				Gravity = Vector3d(0, 0, -9.81 * 100) * Link->Mass;
+			const Vector3d RotForce = Vector3d::Zero();
+			Link->Update(h, Gravity, RotForce);
+		}
 		for (const auto* Joint : Joints)
 			Joint->SolvePosition(h);
 		for (auto* Link : Links)

Source/PBDRobotics/Robot.h

@@ -26,6 +26,9 @@ protected:
 	UFUNCTION(BlueprintCallable)
 	void Connect(ULink* L1, ULink* L2);
 
+	UPROPERTY(BlueprintReadWrite)
+	bool HasGravity = false;
+
 private:
 	void ResetPositionAndOrientation();
 public: