SpringyCamera

PlayerController.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class SpringAndMassEuler {
    float Stiffness;
    float Friction;
    Vector3 MassOffset;
    Vector3 MassVelocity;

    public SpringAndMassEuler() {
        Stiffness = 7.164850514373732f;
        Friction = 1.842068074395237f;
        MassOffset = new Vector3();
        MassVelocity = new Vector3();
    }

    public Vector3 Update(float dt, Vector3 offset) {
        MassOffset += offset * dt;
        MassVelocity -= dt * (Stiffness * MassOffset + Friction * MassVelocity);
        MassOffset += dt * MassVelocity;
        return MassOffset;
    }
};

public class PDController {

    private float kp;
    private float kd;
    private float _frequency = 3.0f;
    private float _damping = 1.0f;
    public Vector3 previous;
    public Vector3 current;

    private void updateConsts()
    {
        float wn = 2.0f * Mathf.PI * frequency;
        this.kp = wn * wn;
        this.kd = 2.0f * wn * damping;
        Debug.Log($"kp {this.kp}");
        Debug.Log($"kd {this.kd}");
    }
    public float frequency{
        get => _frequency;
        set
        {
            _frequency = value;
            updateConsts();
        }
    }
    public float damping{
        get => _damping;
        set
        {
            _damping = value;
            updateConsts();
        }
    }


    public PDController() {
        previous = new Vector3();
        updateConsts();
    }

    /**
    * 4th order Runge Kutta step
    * @param {Array} s0 current state vector
    * @param {Number} dt timestep
    * @param {Function} kinematics kinematics function
    * @return {Array} next state vector
    */
    private Vector2 rungeKutta4Step(Vector2 state, float target, float dt)
    {
        // dt = Mathf.Min(dt, 0.05f);
        Vector2 k0 = dynamics(state, target);
        Vector2 k1 = dynamics(state + (dt / 2.0f) * k0, target);
        Vector2 k2 = dynamics(state + (dt / 2.0f) * k1, target);
        Vector2 k3 = dynamics(state + (dt * k2), target);

       //s0 + dt/6*(k0 + 2*k1 + 2*k2 + k3)
       return state + ((dt / 6.0f) * (k0 + 2.0f * k1 + 2.0f * k2 + k3));
    }
    private Vector2 dynamics(Vector2 state, float target)
    {
        // scale movement to dt... just a guess on how to do this right
        // direction = direction.normalized * (direction.magnitude * dt);
        float x = state.x;
        float dx = state.y;
        float ddx = (this.kp * (target - x)) - (this.kd * dx);
        return new Vector2(dx, ddx);
    }

    public void update(Vector3 target, float deltaTime)
    {
        // deltaTime = Mathf.Min(deltaTime * 0.25f, 0.01f);
        Vector2 xs = rungeKutta4Step(new Vector2(current.x, previous.x), target.x, deltaTime);
        float x = xs.x;
        float dx = xs.y;

        Vector2 ys = rungeKutta4Step(new Vector2(current.y, previous.y), target.y, deltaTime);
        float y = ys.x;
        float dy = ys.y;

        Vector2 zs = rungeKutta4Step(new Vector2(current.z, previous.z), target.z, deltaTime);
        float z = zs.x;
        float dz = zs.y;

        previous = new Vector3(dx, dy, dz);
        current = new Vector3(x, y, z);

        // Debug.Log($"current {current} previous {previous} target: {target} deltaTime {deltaTime}");
    }

};




[RequireComponent (typeof (ConstantForce))]
public class PlayerController : MonoBehaviour
{
    ConstantForce constantforce;
    Rigidbody rigidBody;
    public GameObject mainCameraPivot;
    public GameObject mainCamera;
    public GameObject testControl;
    SpringAndMassEuler spring;
    public PDController pdController;
    Vector3 targetDirection;
    float targetDistance;
    float targetHeight;

    void Awake()
    {
        this.constantforce = GetComponent<ConstantForce> ();
        this.rigidBody = GetComponent<Rigidbody>();
        spring = new SpringAndMassEuler();
        pdController = new PDController();
        // this.camera = GameObject.FindWithTag("MainCamera");
    }

    // Start is called before the first frame update
    void Start()
    {
        Vector3 targetPosition = Vector3.ProjectOnPlane(
            this.mainCamera.transform.position - this.mainCameraPivot.transform.position,
            new Vector3(0, 1, 0));
        this.targetHeight = (this.mainCamera.transform.position - this.mainCameraPivot.transform.position).y;
        this.pdController.current = targetPosition;
        this.targetDistance = targetPosition.magnitude;
        this.targetDirection = targetPosition.normalized;
    }

    // Update is called once per frame
    void FixedUpdate()
    {
        var x = Input.GetAxis("Horizontal") * Time.fixedDeltaTime * 2000.0f;
        var z = Input.GetAxis("Vertical") * Time.fixedDeltaTime * 2000.0f;

        this.constantforce.torque = new Vector3(z, 0, -x);
        this.constantforce.force = new Vector3(x * 0.2f, 0, z * 0.2f);


        if (this.rigidBody.velocity.magnitude > 0.001f)
        {
            var planarVel = Vector3.ProjectOnPlane(-this.rigidBody.velocity, new Vector3(0, 1, 0));
            this.targetDirection = Vector3.RotateTowards(
                    this.targetDirection,
                    planarVel.normalized,
                    3.14f * Time.fixedDeltaTime * Mathf.Min(this.rigidBody.velocity.magnitude * 0.1f, 1f),
                    0.0f);
        }
        this.mainCameraPivot.transform.position = this.transform.position;

        var target = this.transform.position + this.targetDirection * this.targetDistance + new Vector3(0, this.targetHeight, 0);
        // Vector3 target = this.transform.position;
        pdController.update(
            target,
            Time.fixedDeltaTime);

        // this.testControl.transform.position = pdController.current;// + cameraHeight;
        this.mainCamera.transform.position = pdController.current;// + cameraHeight;
        this.mainCamera.transform.LookAt(this.transform);

        // Debug.DrawRay(transform.position, this.targetDirection, Color.green, 0, false);
    }

}