Spring Hovercar

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

public class HoverCar : MonoBehaviour
{

    public float movespeed;
    public float rotationSpeed;

    public Vector3[] hoverPoints;

    public Spring spring = new Spring(1f, .2f, .2f, Spring.SpringForceMode.PUSH);

    public float hoverHeight;
    //public float hoverStiffness;
    //public float hoverDamping;

    public LayerMask hitMask;

    private Rigidbody rb;

    // Start is called before the first frame update
    void OnEnable()
    {
        rb = GetComponent<Rigidbody>();
        //spring = new Spring(hoverHeight, hoverDamping, hoverStiffness, Spring.SpringForceMode.PUSH);
    }

    private void Update()
    {
        if (Input.GetKey(KeyCode.UpArrow))
        {
            rb.AddForce(transform.forward * movespeed * Time.deltaTime);
        }
        if (Input.GetKey(KeyCode.DownArrow))
        {
            rb.AddForce(-transform.forward * movespeed * Time.deltaTime);
        }
        if (Input.GetKey(KeyCode.LeftArrow))
        {
            rb.AddTorque(-transform.up * rotationSpeed * Time.deltaTime);
        }
        if (Input.GetKey(KeyCode.RightArrow))
        {
            rb.AddTorque(transform.up * rotationSpeed * Time.deltaTime);
        }
    }

    private void OnValidate()
    {
        spring.length = hoverHeight;
        //spring.stiffnessCoef = hoverStiffness;
        //spring.dampingCoef = hoverDamping;
    }

    // Update is called once per frame
    void FixedUpdate()
    {
        var mtx = transform.localToWorldMatrix;

        foreach (var hoverPoint in hoverPoints)
        {
            var pos = mtx.MultiplyPoint(hoverPoint);

            //var subMass = rb.mass / hoverPoints.Length;

            if (Physics.Raycast(pos, Vector3.down, out RaycastHit hit, hitMask ))
            {

                var worldDelta = pos - transform.position;
                var rvel = Vector3.Cross(-worldDelta, rb.angularVelocity) + rb.velocity;

                //Debug.DrawRay(pos, rvel);
                //Debug.DrawLine(pos, hit.point);

                var force = spring.GetSpringForce(pos, hit.point, rvel, Vector3.zero) * Time.deltaTime;

                rb.AddForceAtPosition(force / hoverPoints.Length, pos, ForceMode.Force);
                Debug.DrawRay(pos, force / 20f, new Color(1f, 0.92f, 0.02f, 0.25f));
            }
        }
    }

    private void OnDrawGizmos()
    {
        Gizmos.color = Color.red;

        for (int i = 0; i < hoverPoints.Length; i++)
        {
            var worldPos = transform.localToWorldMatrix.MultiplyPoint(hoverPoints[i]);

            Gizmos.DrawWireCube(worldPos, Vector3.one * .01f);
        }
    }
}


/////////SPRING CLASS
using UnityEngine;

[System.Serializable]
public class Spring {

    public float length = 1;

    public float dampingCoef = .5f;
    public float stiffnessCoef = .5f;

    public SpringForceMode forceMode;

    public Spring(float length, float dampingCoef, float stiffnessCoef, SpringForceMode forceMode)
    {
        this.length = length;
        this.dampingCoef = dampingCoef;
        this.stiffnessCoef = stiffnessCoef;
        this.forceMode = forceMode;
    }

    [System.Serializable]
    public enum SpringForceMode
    {
        PUSH,
        PULL,
        BOTH
    }

    public Vector3 GetSpringForce(Vector3 endPosition1, Vector3 endPosition2, Vector3 endVelocity1, Vector3 endVelocity2)
    {
        var delta = endPosition1 - endPosition2;

        var displacement = delta - (delta.normalized * length);
        var radialVel1 = (endVelocity1.magnitude * Mathf.Cos(Vector3.Angle(delta, endVelocity1) * Mathf.Deg2Rad)) * delta.normalized;
        var radialVel2 = (endVelocity2.magnitude * Mathf.Cos(Vector3.Angle(delta, endVelocity2) * Mathf.Deg2Rad)) * delta.normalized;

        //may need to remove this...this is meant to do damping based on a scaled value;
        var stiffness = Time.deltaTime * stiffnessCoef;
        var damping = Time.deltaTime * dampingCoef;

        //springForce = −m/Δt^2 * Ck * x − m/Δt Cd * v
        //var springForce = -stiffness * displacement - damping * (radialVel1 - radialVel2);

        //springForce = −m/Δt^2 * Ck * x − m/Δt Cd * v
        var springForce = -stiffnessCoef * displacement - dampingCoef * (radialVel1 - radialVel2);

        switch (forceMode)
        {
            case SpringForceMode.PUSH:
                if (delta.magnitude > length) springForce = Vector3.zero;
                break;
            case SpringForceMode.PULL:
                if (delta.magnitude < length) springForce = Vector3.zero;
                break;
            case SpringForceMode.BOTH: break;
            default:
                throw new System.ArgumentOutOfRangeException();
        }
        return springForce;
    }

    public void DrawGizmo(Vector3 p1, Vector3 p2, float blendRange = 1)
    {
        float d = Vector3.Distance(p1, p2);
        if (d > length)
            Gizmos.color = Color.Lerp(Color.white, Color.red, Mathf.Clamp(d - length, 0f, blendRange));
        else if (d < length)
            Gizmos.color = Color.Lerp(Color.white, Color.blue, Mathf.Clamp(Mathf.Abs(d - length), 0f, blendRange));
        else
            Gizmos.color = Color.white;

        Gizmos.DrawLine(p1, p2);
    }
}