88 lines
2.6 KiB
C#
88 lines
2.6 KiB
C#
// Based on: allenchou.net/2015/04/game-math-precise-control-over-numeric-springing/
|
|
|
|
using System.Diagnostics;
|
|
using Godot;
|
|
|
|
public class SpringDamper {
|
|
public float omega = 1;
|
|
public float zeta = 1;
|
|
|
|
public SpringDamper(float osc_freq = 1.0f, float osc_red = 0.1f, float osc_red_h = 1.0f) {
|
|
Debug.Assert(osc_red > 0.001 && osc_red < 0.999);
|
|
omega = osc_freq * 2 * Mathf.Pi;
|
|
zeta = Mathf.Log(1.0f - osc_red) / (-omega * osc_red_h);
|
|
}
|
|
|
|
public (float, float) Calc(float x, float v, float xt, float h) {
|
|
float f = 1 + 2 * h * zeta * omega;
|
|
float oo = omega * omega;
|
|
float hoo = oo * h;
|
|
float hhoo = hoo * h;
|
|
|
|
float det_inv = 1.0f / (f + hhoo);
|
|
float det_x = f * x + h * v + hhoo * xt;
|
|
float det_v = v + hoo * (xt - x);
|
|
|
|
return (det_x * det_inv, det_v * det_inv);
|
|
}
|
|
|
|
public (Vector3, Vector3) Calc(Vector3 x, Vector3 v, Vector3 xt, float h) {
|
|
float f = 1 + 2 * h * zeta * omega;
|
|
float oo = omega * omega;
|
|
float hoo = oo * h;
|
|
float hhoo = hoo * h;
|
|
|
|
float det_inv = 1.0f / (f + hhoo);
|
|
Vector3 det_x = f * x + h * v + hhoo * xt;
|
|
Vector3 det_v = v + hoo * (xt - x);
|
|
|
|
return (det_x * det_inv, det_v * det_inv);
|
|
}
|
|
|
|
public (Vector3, Vector3) CalcClampedSpeed(Vector3 x, Vector3 v, Vector3 xt, float h, float speedMax) {
|
|
(Vector3, Vector3) defaultResult = Calc(x, v, xt, h);
|
|
|
|
Vector3 x_new = defaultResult.Item1;
|
|
Vector3 vel_new = (x_new - x) / h;
|
|
float speed_new = vel_new.Length();
|
|
|
|
if (speed_new > speedMax) {
|
|
vel_new = vel_new / speed_new * speedMax;
|
|
x_new = x + vel_new * h;
|
|
}
|
|
|
|
return (x_new, vel_new);
|
|
}
|
|
|
|
public (Vector3, Vector3) CalcClampedSpeedXZ(Vector3 x, Vector3 v, Vector3 xt, float h, float speedMax) {
|
|
(float, float) result_x = Calc(x.x, v.x, xt.x, h);
|
|
(float, float) result_z = Calc(x.z, v.z, xt.z, h);
|
|
|
|
Vector3 x_new = x;
|
|
Vector3 v_new = v;
|
|
|
|
x_new.x = result_x.Item1;
|
|
v_new.x = result_x.Item2;
|
|
|
|
x_new.z = result_z.Item1;
|
|
v_new.z = result_z.Item2;
|
|
|
|
Vector3 result_v_xz = new(
|
|
(result_x.Item1 - x.x) / h,
|
|
0,
|
|
(result_z.Item1 - x.z) / h);
|
|
|
|
float speed_new = result_v_xz.Length();
|
|
|
|
if (speed_new > speedMax) {
|
|
result_v_xz = result_v_xz / speed_new * speedMax;
|
|
x_new.x = x.x + result_v_xz.x * h;
|
|
x_new.z = x.z + result_v_xz.z * h;
|
|
}
|
|
|
|
v.x = result_v_xz.x;
|
|
v.z = result_v_xz.z;
|
|
|
|
return (x_new, v_new);
|
|
}
|
|
} |