基于Unity3D的路径平滑实现

《一》相关说明
本部分将基于博士的沙漏所介绍的平滑轨迹方法，其中博主已经实现了Matlab版本和Python版本。在本文章中将利用C#语言在Unity3D中实现所介绍的平滑轨迹方法。
《二》线性插值
相关理论介绍见博士的沙漏
下面是实现的代码和效果：

1. 线性插值类：LinearInterpolation.cs

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

namespace DTUAV.Algorithm.Interpolation
{
    public struct MotionInfo
    {
        public float X;
        public float Vec;
        public float Acc;
    }
    public class LinearInterpolation
    {
        private List<float> _time;
        private List<float> _value;

        public MotionInfo GetPosition(float time)
        {
            MotionInfo ret = new MotionInfo();
            if (time < _time[0] || time > _time[_time.Count - 1])
            {
                Debug.LogError("The specific time error, time ranges error!!!!");
                ret.X = float.NaN;
                ret.Vec = float.NaN;
                ret.Acc = float.NaN;
            }
            else
            {
                int j = _time.FindIndex(_time => _time >= time);
              //  Debug.Log("j= " + j);
                int i = 0;
                if(j==0)
                {
                    i = 0;
                    j = 1;
                }
                else
                {
                    i = j - 1;
                }
                ret = Linear(_value[i], _value[j], _time[i], _time[j]);
                ret.X = _value[i] + ret.Vec * (time - _time[i]);
            }
            return ret;
        }
        private MotionInfo Linear(float value_0, float value_1, float time_0,float time_1)
        {
            MotionInfo ret = new MotionInfo();
            if(Mathf.Abs(time_0-time_1)<10e-6)
            {
                Debug.LogError("The time_0 and time_1 must be different!!!!");
                ret.X = -1;
                ret.Vec = -1;
                ret.Acc = -1;
            }
            else
            {
                ret.X = value_0;
                ret.Vec = (value_1 - value_0) / (time_1 - time_0);
                ret.Acc = 0;
            }
            return ret;
        }

        public LinearInterpolation() { }
        public LinearInterpolation(List<float> time,List<float> value)
        {
            _time = time;
            _value = value;
        }

    }
}

2. 类的实例化使用：LinearInterpolationNode.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using DTUAV.Plot.Line;
namespace DTUAV.Algorithm.Interpolation
{
    public class LinearInterpolationNode : MonoBehaviour
    {
        
        [Header("LinearInterpolation")]
        public List<float> Time;
        public List<float> Value;
        public float testTime;

        public Material PositionLineMat;//The Material of Draw Line for position.
        public Color PositionLineColor;//The Color of Draw Line for position.

        public Material VelocityLineMat;//The Material of Draw Line for velocity.
        public Color VelocityLineColor;//The Color of Draw Line for velocity.

        public Material AccLineMat;//The Material of Draw Line for acceleration.
        public Color AccLineColor;//The Color of Draw Line for acceleration.

        public float LineWidthX = 0.06f;//The Width X of Draw Line.
        public float LinewidthY = 0.06f;//The Width Y of Draw Line.
        public Vector3 LastPosition;//The Start Position of Draw Line.
        public Vector3 CurrentPosition;//The End Position of Draw Line.
        public bool IsPlot;//The Flag to Draw Line.

        private PlotLine _plotPositionLine;
        private PlotLine _plotVelocityLine;
        private PlotLine _plotAccLine;


        private LinearInterpolation _linear;

        [System.Obsolete]
        void Start()
        {
            _linear = new LinearInterpolation(Time, Value);
            LastPosition = new Vector3(Time[0], Value[0], 0);
            _plotPositionLine = new PlotLine(PositionLineMat, PositionLineColor, LineWidthX, LinewidthY, LastPosition,IsPlot);
            LastPosition.y = (Value[1] - Value[0]) / (Time[1] - Time[0]);
            _plotVelocityLine = new PlotLine(VelocityLineMat, VelocityLineColor, LineWidthX, LinewidthY, LastPosition, IsPlot);
            LastPosition.y = 0;
            _plotAccLine = new PlotLine(AccLineMat, AccLineColor, LineWidthX, LinewidthY, LastPosition, IsPlot);

            for (int i=0;i<10;i++)
            {
                LastPosition.x = i;
                LastPosition.z = 0;
                LastPosition.y = _linear.GetPosition(i).X;
                _plotPositionLine.SetCurrentPosition(LastPosition);
                _plotPositionLine.DrawLine();
                LastPosition.y = _linear.GetPosition(i).Vec;
                _plotVelocityLine.SetCurrentPosition(LastPosition);
                _plotVelocityLine.DrawLine();
                LastPosition.y = _linear.GetPosition(i).Acc;
                _plotAccLine.SetCurrentPosition(LastPosition);
                _plotAccLine.DrawLine();
            }

            Debug.Log("value: " + _linear.GetPosition(testTime).X);

        }
    }
}

3. Unity软件Demo配置
   
4. 实现效果：红色为位置，绿色是速度，蓝色是加速度。