From 6ce8b9e22fc13be34b442c7b6af48b42cd44275a Mon Sep 17 00:00:00 2001
From: chai <215380520@qq.com>
Date: Wed, 13 Mar 2024 11:00:58 +0800
Subject: +init

---
 PID.cs | 104 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 104 insertions(+)
 create mode 100644 PID.cs

(limited to 'PID.cs')

diff --git a/PID.cs b/PID.cs
new file mode 100644
index 0000000..9403232
--- /dev/null
+++ b/PID.cs
@@ -0,0 +1,104 @@
+using UnityEngine;
+
+public class PID : MonoBehaviour
+{
+	public float maxForce;
+
+	public float proportionalGain;
+
+	public float differentialGain;
+
+	private Vector3 lastError = Vector3.zero;
+
+	private Vector3 currentForce = Vector3.zero;
+
+	public float maxTorque;
+
+	public float proportionalGainTorque;
+
+	public float differentialGainTorque;
+
+	private Vector3 lastErrorTorque = Vector3.zero;
+
+	private Vector3 currentTorque = Vector3.zero;
+
+	private Vector3 lastErrorTorqueUp = Vector3.zero;
+
+	private Vector3 currentTorqueUp = Vector3.zero;
+
+	public float currentMultiplier = 1f;
+
+	private Rigidbody rig;
+
+	private SetInertiaTension inertia;
+
+	public Transform addForcePoint;
+
+	private Transform forcePoint;
+
+	private void Start()
+	{
+		rig = GetComponent<Rigidbody>();
+		inertia = GetComponent<SetInertiaTension>();
+	}
+
+	private void Update()
+	{
+		if ((bool)inertia)
+		{
+			currentMultiplier = 1f - Mathf.Clamp(inertia.collisionValue, 0f, 0.7f);
+		}
+	}
+
+	private void FixedUpdate()
+	{
+	}
+
+	public void DoPID(Vector3 targetPosition, Vector3 targetRotation, Vector3 targetRotationUp, float multiplier = 1f)
+	{
+		AddForce(targetPosition, currentMultiplier * multiplier);
+		if (proportionalGainTorque != 0f)
+		{
+			AddTorque(targetRotation, currentMultiplier * multiplier);
+			AddTorqueUp(targetRotationUp, currentMultiplier * multiplier);
+		}
+	}
+
+	public void AddForce(Vector3 targetPosition, float multiplier = 1f)
+	{
+		forcePoint = ((!(addForcePoint == null)) ? addForcePoint : base.transform);
+		Vector3 vector = targetPosition - forcePoint.position;
+		Vector3 vector2 = (vector - lastError) / Time.fixedDeltaTime;
+		lastError = vector;
+		currentForce = vector * proportionalGain + vector2 * differentialGain;
+		currentForce = Vector3.ClampMagnitude(currentForce, maxForce);
+		if ((bool)addForcePoint)
+		{
+			rig.AddForceAtPosition(currentForce * Time.fixedDeltaTime * 60f * multiplier, addForcePoint.position, ForceMode.Acceleration);
+		}
+		else
+		{
+			rig.AddForce(currentForce * Time.fixedDeltaTime * 60f * multiplier, ForceMode.Acceleration);
+		}
+	}
+
+	public void AddTorque(Vector3 targetRotation, float multiplier = 1f)
+	{
+		Vector3 vector = Mathf.Clamp(Vector3.Angle(targetRotation, base.transform.forward), 0f, 3f) * Vector3.Cross(targetRotation, base.transform.forward);
+		Vector3 vector2 = (vector - lastErrorTorque) / Time.fixedDeltaTime;
+		lastErrorTorque = vector;
+		currentTorque = vector * proportionalGainTorque + vector2 * differentialGainTorque;
+		currentTorque = Vector3.ClampMagnitude(currentTorque, maxTorque);
+		rig.AddTorque(-currentTorque * Time.fixedDeltaTime * 60f * multiplier, ForceMode.Acceleration);
+	}
+
+	public void AddTorqueUp(Vector3 targetRotation, float multiplier = 1f)
+	{
+		Vector3 vector = Mathf.Clamp(Vector3.Angle(targetRotation, base.transform.up), 0f, 3f) * Vector3.Cross(targetRotation, base.transform.up);
+		Vector3 vector2 = (vector - lastErrorTorqueUp) / Time.fixedDeltaTime;
+		lastErrorTorqueUp = vector;
+		currentTorqueUp = vector * proportionalGainTorque + vector2 * differentialGainTorque;
+		currentTorqueUp = Vector3.ClampMagnitude(currentTorqueUp, maxTorque);
+		rig.AddTorque(-currentTorqueUp * Time.fixedDeltaTime * 60f * multiplier, ForceMode.Acceleration);
+	}
+}
-- 
cgit v1.1-26-g67d0