r2mods/ilspy_dump/ror2_csproj/QuaternionPID.cs

67 lines
1.6 KiB
C#
Raw Normal View History

2024-10-04 07:26:37 +00:00
using UnityEngine;
using UnityEngine.Serialization;
public class QuaternionPID : MonoBehaviour
{
[Tooltip("Just a field for user naming. Doesn't do anything.")]
[FormerlySerializedAs("name")]
public string customName;
[Tooltip("PID Constants.")]
public Vector3 PID = new Vector3(1f, 0f, 0f);
[Tooltip("The quaternion we are currently at.")]
public Quaternion inputQuat = Quaternion.identity;
[Tooltip("The quaternion we want to be at.")]
public Quaternion targetQuat = Quaternion.identity;
[Tooltip("Vector output from PID controller; what we read.")]
[HideInInspector]
public Vector3 outputVector = Vector3.zero;
public float gain = 1f;
private Vector3 errorSum = Vector3.zero;
private Vector3 deltaError = Vector3.zero;
private Vector3 lastError = Vector3.zero;
private float lastUpdateTime;
private void Start()
{
gain *= 60f * Time.fixedDeltaTime;
lastUpdateTime = Time.time;
}
public Vector3 UpdatePID()
{
float time = Time.time;
float num = time - lastUpdateTime;
lastUpdateTime = time;
if (num != 0f)
{
Quaternion quaternion = targetQuat * Quaternion.Inverse(inputQuat);
if (quaternion.w < 0f)
{
quaternion.x *= -1f;
quaternion.y *= -1f;
quaternion.z *= -1f;
quaternion.w *= -1f;
}
Vector3 vector = default(Vector3);
vector.x = quaternion.x;
vector.y = quaternion.y;
vector.z = quaternion.z;
errorSum += vector * num;
deltaError = (vector - lastError) / num;
lastError = vector;
outputVector = vector * PID.x + errorSum * PID.y + deltaError * PID.z;
return outputVector * gain;
}
return Vector3.zero;
}
}