- v3_muladds( player->rb.w, axis, (Fs+Fd) * s->substep_delta,
- player->rb.w );
- }
+ v3f p1;
+ v3_muladds( player->rb.co, plane_z, 1.5f, p1 );
+ vg_line( player->rb.co, p1, VG__GREEN );
+
+ v3f refdir;
+ v3_muls( player->rb.to_world[2], -(float)s->state.manual_direction,
+ refdir );
+
+ float a = v3_dot( plane_z, refdir );
+ a = acosf( vg_clampf( a, -1.0f, 1.0f ) );
+
+ v3f axis;
+ v3_cross( plane_z, refdir, axis );
+
+ float Fs = -a * k_manul_spring,
+ Fd = -v3_dot( player->rb.w, axis ) * k_manul_dampener;
+
+ v3_muladds( player->rb.w, axis, (Fs+Fd) * s->substep_delta,
+ player->rb.w );