SnekSolver

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

public class SnekSolver : MonoBehaviour {

    public Transform targetT;
    [Header("Joints")]
    public int totalJoints = 10;
    [Space]
    public float maxLength = 1;
    public float minLength = .95f;
    [Header("Solver")]
    [Range(1,100)] public int iterations = 1;
    [Space]
    public bool useTipAngle;
    public bool useBaseAngle;
    [Space]
    public BaseFixType baseFixType;
    [Range(0,1)]public float bodyStretch=.8f;
    [Space]
    public JointAngleRestrictType restrict = JointAngleRestrictType.Adaptive;

    [Range(0,180)] public float maxJointAngle = 90;
    [Range(0,60)] public float angleRestrictSpeed;

    Vector2[] path;
    float pTime;
    public enum BaseFixType { None, Reach, Stretch };
    public enum JointAngleRestrictType { None, Dumb, Adaptive, Instant };


    void OnDrawGizmos() {

        if(targetT==null){
            GameObject targetGO = new GameObject("target");
            targetT = targetGO.transform;
            targetT.position = (Vector2)transform.position + Random.insideUnitCircle*5;
        }

        Vector2 target=targetT.position;
        Vector2 start=transform.position;

        if(path==null || path.Length!=totalJoints){
            path = new Vector2[totalJoints];
            for(int p=0;p<totalJoints;p++){
                path[p]=Vector2.Lerp( start,target,(float)p/totalJoints);
            }
        }

        float dt = Mathf.Clamp(time-pTime, .0166f,.04f);

        for(int i=0;i<iterations;i++){

            path[totalJoints-1] = target;

            for(int p=totalJoints-1;p>0;p--){

                Vector2 direction = path[p]-path[p-1];
                float length = direction.magnitude;

                if(useTipAngle && p==totalJoints-1){
                    path[p-1] = path[p] - RotateVector2(new Vector2(0,length), targetT.eulerAngles.z);
                    direction = (path[p]-path[p-1]).normalized;
                }

                if(useBaseAngle && p==1){
                    path[p-1] = path[p] - RotateVector2(new Vector2(0,length), transform.eulerAngles.z);
                    direction = (path[p]-path[p-1]).normalized;
                }

                direction.Normalize();

                if(restrict!=JointAngleRestrictType.None && p<totalJoints-1){

                    float dirAngle = angle(direction);

                    Vector2 prevDirection = path[p+1]-path[p];
                    float prevAngle = angle(prevDirection);

                    float angleDiff = Mathf.DeltaAngle(dirAngle, prevAngle);

                    switch(restrict){

                        case JointAngleRestrictType.Instant:

                            if(angleDiff>maxJointAngle)
                                path[p-1] = path[p] - RotateVector2(new Vector2(0,length), prevAngle-maxJointAngle);
                            else if(angleDiff<-maxJointAngle)
                                path[p-1] = path[p] - RotateVector2(new Vector2(0,length), prevAngle+maxJointAngle);

                        break;

                        case JointAngleRestrictType.Dumb:

                            if(angleDiff>maxJointAngle)
                                path[p-1] = path[p] - RotateVector2(new Vector2(0,length), dirAngle+angleRestrictSpeed*dt*360);
                            else if(angleDiff<-maxJointAngle)
                                path[p-1] = path[p] - RotateVector2(new Vector2(0,length), dirAngle-angleRestrictSpeed*dt*360);

                        break;

                        case JointAngleRestrictType.Adaptive:

                            if(angleDiff>maxJointAngle)
                                path[p-1] = path[p] - RotateVector2(new Vector2(0,length), dirAngle+angleRestrictSpeed*dt*Mathf.Clamp(angleDiff-maxJointAngle,-90,90));
                            else if(angleDiff<-maxJointAngle)
                                path[p-1] = path[p] - RotateVector2(new Vector2(0,length), dirAngle+angleRestrictSpeed*dt*Mathf.Clamp(maxJointAngle+angleDiff,-90,90));

                        break;
                    }

                    direction = (path[p]-path[p-1]).normalized;
                }

                if(length<minLength)
                    path[p-1] = path[p] - direction*minLength;
                else if(length>maxLength)
                    path[p-1] = path[p] - direction*maxLength;

            }

            switch(baseFixType){
                case BaseFixType.Reach:{
                    Vector2 od = start-path[0];
                    for(int p=0;p<totalJoints;p++) path[p] += od;
                }break;

                case BaseFixType.Stretch:{
                    Vector2 od = start-path[0];
                    for(int p=0;p<totalJoints;p++)
                        path[p] += od * Mathf.Pow(1-(float)p/(totalJoints-1), bodyStretch);
                }break;
            }
        }

        pTime = time;


        // drawDebugs.ddPath(path, Color.white, type:drawDebugs.ddPathType.Capsules);

        Gizmos.color = Color.yellow;
        for(int p=1;p<totalJoints;p++)
            Gizmos.DrawLine(path[p-1],path[p]);
    }

    void OnValidate(){
        maxLength = Mathf.Max(maxLength, 0);
        minLength = Mathf.Clamp(minLength, 0, maxLength);
    }

    public static float time {get{
        #if UNITY_EDITOR
        if(!Application.isPlaying)
            return (float)UnityEditor.EditorApplication.timeSinceStartup;
        #endif
        return Time.time;
    }}


    Vector2 RotateVector2(Vector2 v, float degrees) {

        if(degrees<0 || degrees>=360) degrees = Mathf.Repeat(degrees, 360);

        if(degrees.Equals(0)) return v;
        else if(degrees.Equals(90)) return new Vector2(-v.y,v.x);
        else if(degrees.Equals(180)) return new Vector2(-v.x, -v.y);
        else if(degrees.Equals(270)) return new Vector2(v.y,-v.x);

        float sin = Mathf.Sin(degrees * Mathf.Deg2Rad);
        float cos = Mathf.Cos(degrees * Mathf.Deg2Rad);

        float tx = v.x;
        float ty = v.y;
        v.x = (cos * tx) - (sin * ty);
        v.y = (sin * tx) + (cos * ty);

        return v;
    }

    float angle(Vector2 v){
        return Mathf.Atan2(v.y,v.x)*Mathf.Rad2Deg - 90f;
    }

}