1 files changed, 46 insertions(+), 2 deletions(-)

M crates/unified/src/client/parts.rs

@@ 190,6 190,48 @@ const K1: f32 = ZETA/(PI*F);
 const K2: f32 = 1.0/((2.0*PI*F)*(2.0*PI*F));
 const K3: f32 = (R*ZETA)/(2.0*PI*F);
 
+fn smoothing_a(x: Vec3, x_dot: Vec3, r: Vec3, v: Vec3) -> Vec3 {
+    x/K2 + K3/K2*x_dot - r/K2 - K1/K2*v
+}
+
+// y + K1*y' + K2*y'' = x + K3*x'
+// K2*y'' = x - y + K3*x' - K1*y'
+// y''(t) = (x - y + K3*x' - K1*y')/K2
+// r(t) = y(t)
+// v(t) = y'(t)
+// r'(t) = y'(t) = v(t)
+// v'(t) = y''(t) = (x - y(t) + K3*x' - K1*y'(t))/K2
+//                = x/K2 + K3/K2*x' - r(t)/K2 - K1/K2*v(t)
+// [ r' ] = [ 0    1     ] * [ r(t) ] + [ 0 ]
+// [ v' ] = [ 1/K2 K1/K2 ] * [ v(t) ] + [ 1 ] * x/K2 + K3/K2*x'
+fn rk4_k_values(x: Vec3, x_dot:Vec3, r: Vec3, v: Vec3, dt: f32)
+    -> (Vec3, Vec3, Vec3, Vec3, Vec3, Vec3, Vec3, Vec3) {
+    let a = smoothing_a(x, x_dot, r, v);
+
+    let r_k1 = v;
+    let v_k1 = a;
+
+    let r_1 = r + r_k1*dt/2.0;
+    let v_1 = v + v_k1*dt/2.0;
+
+    let r_k2 = v_1;
+    let v_k2 = smoothing_a(x, x_dot, r_1, v_1);
+
+    let r_2 = r + r_k2*dt/2.0;
+    let v_2 = v + v_k2*dt/2.0;
+
+    let r_k3 = v_2;
+    let v_k3 = smoothing_a(x, x_dot, r_2, v_2);
+
+    let r_3 = r + r_k3*dt;
+    let v_3 = v + v_k3*dt;
+
+    let r_k4 = v_3;
+    let v_k4 = smoothing_a(x, x_dot, r_3, v_3);
+
+    (r_k1, v_k1, r_k2, v_k2, r_k3, v_k3, r_k4, v_k4)
+}
+
 /// !IMPORTANT!
 /// This function forms the bulk of the attachment system.
 /// PLEASE DO NOT MODIFY OR MOVE


@@ 359,11 401,13 @@ fn update_drag_ghosts(
     let target_vel = (best_target.translation - ghost_info.last_target_pos)
         /time.delta_secs();
 
+
     let a = (best_target.translation - ghost.translation
         + K3*target_vel - K1*ghost_info.vel)/K2;
+    let (r_k1, v_k1, r_k2, v_k2, r_k3, v_k3, r_k4, v_k4) = rk4_k_values(best_target.translation, target_vel, ghost.translation, ghost_info.vel, time.delta_secs());
 
-    ghost.translation += ghost_info.vel*time.delta_secs();
-    ghost_info.vel += a*time.delta_secs();
+    ghost.translation += (r_k1 + 2.0*r_k2 + 2.0*r_k3 + r_k4)/6.0*time.delta_secs();
+    ghost_info.vel += (v_k1 + 2.0*v_k2 + 2.0*v_k3 + v_k4)/6.0*time.delta_secs();
 
     ghost_info.last_target_pos = best_target.translation;