1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
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);
}
}
|
