Катмулл-Рома

/// <summary>
/// Сплайн Катмулл-Рома.
/// http://www.gamedev.ru/code/tip/catmull_rom
/// </summary>
private class CatmullRomCurve
{
    private struct CurveSegment
    {
        public Vector2 A;
        public Vector2 B;
        public Vector2 C;
        public Vector2 D;

        public CurveSegment(Vector2 M0, Vector2 P0, Vector2 P1, Vector2 M1)
        {
            Vector2 V0 = Vector2.Zero;
            this.A = V0;
            this.B = V0;
            this.C = V0;
            this.D = V0;

            RecalculateFactors(M0, P0, P1, M1);
        }

        public void RecalculateFactors(Vector2 M0, Vector2 P0, Vector2 P1, Vector2 M1)
        {
            this.A = ((P0 - P1) * 1.5f) + ((M1 - M0) * 0.5f);
            this.B = (P1 * 2.0f) - (P0 * 2.5f) - (M1 * 0.5f) + M0;
            this.C = (P1 - M0) * 0.5f;
            this.D = P0;
        }
        public Vector2 CurveValue(float t)
        {
            return ((this.A * t + this.B) * t + this.C) * t + this.D;
        }
        public Vector2 CurveVelocity(float t)
        {
            return (this.A * 3 * t + this.B * 2) * t + this.C;
        }
    }

    private List<Vector2> mPoints = new List<Vector2>();
    private List<CurveSegment> mSeg = new List<CurveSegment>();

    public CatmullRomCurve(Vector2 P0, Vector2 P1)
    {
        mPoints.Add(P0);
        mPoints.Add(P1);

        RecalculateSegments();
    }
    public CatmullRomCurve(Vector2 P0, Vector2 P1, Vector2 P2, Vector2 P3)
    {
        mPoints.Add(P0);
        mPoints.Add(P1);
        mPoints.Add(P2);
        mPoints.Add(P3);

        RecalculateSegments();
    }

    private void RecalculateSegments()
    {
        mSeg.Clear();

        for (int i = 0; i < mPoints.Count - 1; i++)
        {
            mSeg.Add(new CurveSegment(PrevPoint(i), mPoints[i], NextPoint(i), NextPoint(i + 1)));
        }

        mNeedRecalcCurveLength = true;
    }

    private Vector2 NextPoint(int i)
    {
        if ((i + 1) >= mPoints.Count) return mPoints[mPoints.Count - 1];
        return mPoints[i + 1];
    }
    private Vector2 PrevPoint(int i)
    {
        if ((i - 1) < 0) return mPoints[0];
        return mPoints[i - 1];
    }

    private bool mNeedRecalcCurveLength = true;
    private float mCurveLength = 0f;
    private void RecalcCurveLength()
    {
        mCurveLength = 0;
        for (int i = 0; i < mPoints.Count - 1 ; i++)
        {
            mCurveLength += (mPoints[i] - mPoints[i + 1]).Length();
        }
        mNeedRecalcCurveLength = false;
    }

    /// <summary>
    /// Длина линии в сегментах.
    /// </summary>
    public float CurveLengthInSeg
    {
        get { return (float)(mSeg.Count); }
    }
    /// <summary>
    /// Линейная длина кривой.
    /// </summary>
    /// <returns>Длина.</returns>
    public float CurveLengthLin
    {
        get
        {
            if (mNeedRecalcCurveLength) RecalcCurveLength();
            return mCurveLength;
        }
    }
    /// <summary>
    /// Длина кривой по точкам заданым с шагом аргумента.
    /// </summary>
    /// <param name="Step">Шаг аргумента.</param>
    /// <returns>Длина.</returns>
    public float CurveLength(float Step)
    {
        float CurveLength = 0;
        float t = 0;
        Vector2 PreviosPoint = Evalute(t);

        while (t < CurveLengthInSeg)
        {
            t += Step;
            Vector2 CurrentPoint = Evalute(t);

            CurveLength += (PreviosPoint - CurrentPoint).Length();
            PreviosPoint = CurrentPoint;
        }

        return CurveLength;
    }

    private int mSegIndex = -1;
    private float mSegRem = -1;
    private void SetSegCurve(float t)
    {
        // обрезать t до допустимого диапазона
        t = Math.Min(t, mSeg.Count);
        t = Math.Max(t, 0);

        // кривоватэ
        if (t == mSeg.Count)
        {
            mSegIndex = mSeg.Count -1;
            mSegRem = 1;
            return;
        }

        mSegIndex = (int)Math.Floor(t);
        mSegRem = t - mSegIndex;
    }

    /// <summary>
    /// Значение кривой для t.
    /// </summary>
    /// <param name="t">t - от 0 до CurveLength(Point.Count-1)</param>
    /// <returns>Значение в аргументе.</returns>
    public Vector2 Evalute(float t)
    {
        SetSegCurve(t);
        return mSeg[mSegIndex].CurveValue(mSegRem);
    }
    /// <summary>
    /// Найти приращение t так чтобы идти по кривой с равномерной скоростью Vel.
    /// </summary>
    /// <param name="t">Исходный параметр t.</param>
    /// <param name="Vel">Требуемая скорость.</param>
    /// <returns>Приращение t для Vel.</returns>
    public float EvaluteParamUniform(float t, float Vel)
    {
        SetSegCurve(t);
        Vector2 CurveVel = mSeg[mSegIndex].CurveVelocity(mSegRem);
        // иногда скорость кривой считается неправильно
        // тогда можно проскочить очень много шагов
        // для устойчивости нужен минимальный шаг.
        return Math.Min(0.05f, Vel / CurveVel.Length());
    }

    #region Точки
    /// <summary>
    /// Кол-во точек в кривой.
    /// </summary>
    public int PointsCount
    {
        get { return mPoints.Count; }
    }
    /// <summary>
    /// Добавить точку в конец кривой.
    /// </summary>
    /// <param name="P">Новая точка</param>
    public void AddPoint(Vector2 P)
    {
        mPoints.Add(P);
        RecalculateSegments();
    }
    /// <summary>
    /// Добавить точку в произвольное место от 0 до (PointsCount - 1).
    /// </summary>
    /// <param name="Index">Индекс куда добавлять.</param>
    /// <param name="P">Новая точка</param>
    public void InsertPoint(int Index, Vector2 P)
    {
        mPoints.Insert(Index, P);
        RecalculateSegments();
    }
    /// <summary>
    /// Удалить точку по заданому индексу от 0 до (PointsCount - 1).
    /// Последние две точки НЕ УДОЛЯТСЯ.
    /// </summary>
    /// <param name="Index">Индекс.</param>
    public void RemovePoint(int Index)
    {
        if (mPoints.Count == 2) return;

        mPoints.RemoveAt(Index);
        RecalculateSegments();
    }
    /// <summary>
    /// Удолить все точки кроме первых двух.
    /// </summary>
    public void ClearPoints()
    {
        Vector2 P0 = mPoints[0];
        Vector2 P1 = mPoints[1];

        mPoints.Clear();

        mPoints.Add(P0);
        mPoints.Add(P1);

        RecalculateSegments();
    }
    /// <summary>
    /// Удолить все точки, первые две точки станут заданными.
    /// </summary>
    /// <param name="P0">Первая.</param>
    /// <param name="P1">Вторая.</param>
    public void ClearPoints(Vector2 P0, Vector2 P1)
    {
        mPoints.Clear();

        mPoints.Add(P0);
        mPoints.Add(P1);

        RecalculateSegments();
    }
    #endregion
}

--------------------------------------------------------------------------------------------------------------------------------------

/// <summary>
/// ХАВ ТУ ЮЗ? Прост)
/// </summary>
public TestCurve(Vector2 P0, Vector2 P1, float Vel)
{
    CatmullRomCurve CRC = new CatmullRomCurve(P0, P1);
    CRC.InsertPoint(1, new Vector2(40, 0));

    Console.WriteLine("CurveLen: " + CRC.CurveLengthLin.ToString());
    Console.WriteLine("CurveLen: " + CRC.CurveLength(0.1f).ToString() + " Step 0.1");
    Console.WriteLine("CurveLen: " + CRC.CurveLength(0.05f).ToString() + " Step 0.05");

    float t = 0;
    int InputKey = 0;
    while (t < CRC.CurveLengthInSeg)
    {
        Console.WriteLine("t=" + t.ToString() + " V=" + CRC.Evalute(t).ToString());
        t += CRC.EvaluteParamUniform(t, Vel);

        InputKey++;
        if (InputKey > 30)
        {
            Console.ReadKey();
            InputKey = 0;
        }
    }
}