Snek solver

SnekSolver.cs

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

public class SnekSolver : MonoBehaviour {

	[Header("Joints")]
	public int totalJoints = 10;
	[Space]	
	public float maxLength = 1;
	public float minLength = .95f;
	[Space]
	public Transform targetT;
	[Header("Solver")]
	[Range(1,100)] public int iterations;
	[Space]
	public bool useTipAngle;
	public bool useBaseAngle;

	public enum BaseFixType { None, Reach, Stretch };
	[Space]
	public BaseFixType baseFixType;
	[Range(0,1)]public float pow=.2f;
	public enum JointAngleRestrictType { None, Slow, Instant };
	[Space]
	public JointAngleRestrictType restrict;
	
	[Range(0,180)] public float maxJointAngle = 90;
	[Range(0,1000)] public float angleRestrictSpeed;

	Vector2[] path;
	float pTime;

	
	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 = direction.angle();
					
					Vector2 prevDirection = path[p+1]-path[p];
					float prevAngle = prevDirection.angle();
					
					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.Slow:
						
							if(angleDiff>maxJointAngle)
								path[p-1] = path[p] - RotateVector2(new Vector2(0,length), dirAngle+angleRestrictSpeed*dt);
							else if(angleDiff<-maxJointAngle)
								path[p-1] = path[p] - RotateVector2(new Vector2(0,length), dirAngle-angleRestrictSpeed*dt);
						
						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), pow);
				}break;
			}
		}
	
		pTime = drawDebugs.time;
		
		// my own path drawer
		// drawDebugs.ddWideLine(path, .125f, Color.yellow);
		
		Gizmos.color = Color.yellow;
		for(int p=1;p<totalJoints;p++)
			Gizmos.DrawLine(path[p-1],path[p]);
	}
	
	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;
	}

}