unity 模拟车真实行驶的算法

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

//[ExecuteInEditMode]
public class DraveControl : MonoBehaviour {
  
    Transform StartTrans;
    public GameObject[] targetPos;
    public int Speed;
    Vector3 Center;
    float Radius;
    int CutIndex = 2;
    Vector3 CenterObj;
    Vector3 Direction;
    bool isRot;
    int NowIndex;
    float Dot;
    Vector3 startPos;
    LineRenderer LR;
    // Use this for initialization
    void Start () {
        StartTrans = this.transform;
        startPos = StartTrans.position;
        init();
 //       LR = this.GetComponent<LineRenderer>();//得到组件
 //       LR.SetVertexCount(targetPos.Length + 1);//设置线的段数
    }
    private void init()
    {
        Direction = targetPos[NowIndex].transform.forward;
        ConectRound(StartTrans.position, targetPos[0].transform.position, Direction);
    }
    // Update is called once per frame
    void Update () {
        if (isRot)
        {
            //目标点的半径和切线防线求法向量，从而获取汽车的旋转轴
            Vector3 Nor =  Vector3.Cross(Direction,(targetPos[NowIndex].transform.position - Center)).normalized;
            Nor = new Vector3(0, -Nor.y, 0);
            //围绕圆心旋转
            StartTrans.RotateAround(CenterObj, Nor, Time.deltaTime * (Speed/2) *100 / Radius);
            //车X轴方向朝向圆心，并且Z轴防线朝向旋转的方向
            StartTrans.transform.right = (Center - StartTrans.transform.position).normalized;
            //StartTrans.transform.right = Vector3.Lerp(StartTrans.transform.right, (Center - StartTrans.transform.position).normalized, a/10);
            StartTrans.transform.forward = StartTrans.transform.forward * Nor.y;

            if (Vector3.Distance(StartTrans.position, targetPos[NowIndex].transform.position) < 10f) {
                //下一个目标点
                NowIndex++;
                if (NowIndex < targetPos.Length )
                {
                    Direction = targetPos[NowIndex].transform.forward;
                    ConectRound(StartTrans.position, targetPos[NowIndex].transform.position, Direction);
                }
                else
                {
                    NowIndex = 0;
                    StartTrans.position = startPos;
                    init();
                }
            }
        }
        //startPos.transform.Rotate(Center, 10.0f);
        //startPos.transform.right = Center - startPos.transform.position;
        if (NowIndex < targetPos.Length) {
            Debug.DrawLine(Center, StartTrans.position, Color.blue);
            Debug.DrawLine(Center, targetPos[NowIndex].transform.position, Color.blue);
        }
       
        //Debug.DrawLine(targetPos[0].transform.position, startPos.position, Color.blue);
        //Debug.DrawLine(pos, Testpos, Color.blue);
        //Debug.DrawLine(targetPos[0].transform.position, Testpos2, Color.blue);
        //DrawLines(startPos.position, targetPos[0].transform.position, Direction);
    }
    //计算车轨道的圆心，半径
    //z坐标轴是车头方向
    void ConectRound(Vector3 startPoint , Vector3 target ,Vector3 targetDirection)
    {
        // 圆的方程和切线与半斤垂直得出
        if (((startPoint.z - target.z) / (target.x - startPoint.x) + targetDirection.z / targetDirection.x) != 0 && (target.x - startPoint.x) != 0 && targetDirection.x != 0
            && (startPoint.z - target.z) != 0 && targetDirection.z != 0)
        {

            Center.z = (((targetDirection.x * target.x + targetDirection.z * target.z) / targetDirection.x)
                  - ((target.x * target.x + target.z * target.z - startPoint.x * startPoint.x - startPoint.z * startPoint.z)) /( 2 * (target.x - startPoint.x)))
                  / ((startPoint.z - target.z) / (target.x - startPoint.x) + targetDirection.z / targetDirection.x);

            Center.x = (((targetDirection.x * target.x + targetDirection.z * target.z) / targetDirection.z)
                       - ((startPoint.x * startPoint.x + startPoint.z * startPoint.z - target.x * target.x - target.z * target.z) / (2 * (startPoint.z - target.z))))
                       / ((target.x - startPoint.x) / (startPoint.z - target.z) + targetDirection.x / targetDirection.z);
            Center.y = 0;
        }
        else
        {
            Debug.Log("目标不符合规范 ");
        }
        Radius = Vector3.Distance(Center, startPoint);
       
        CenterObj = new Vector3(Center.x, Center.y, Center.z);
        CalculationDriveData();
    }
    void CalculationDriveData()
    {
        //startPos.transform.parent = CenterObj.transform;
        isRot = true;
        Dot = StartTrans.forward.x * targetPos[NowIndex].transform.forward.x + StartTrans.forward.y * targetPos[NowIndex].transform.forward.y + StartTrans.forward.z * targetPos[NowIndex].transform.forward.z;

    }

    //void DrawCircular() {
    //    float x;
    //    float y;
    //    int n = targetPos.Length;
    //    //循环着取出36个点
    //    for (int i = 0; i < n + 1; i++)
    //    {
    //        x = Mathf.Sin((360f * i / n) * Mathf.Deg2Rad) * Radius;//横坐标
    //        y = Mathf.Cos((360f * i / n) * Mathf.Deg2Rad) * Radius;//纵坐标

    //        LR.SetPosition(i, new Vector3(x, y, 0));
    //    }
    //}
}