Поверните передние колеса автомобиля по траектории Безье.

У меня есть 3D-автомобиль, который следует заранее заданному 3D-пути Безье. Я хочу, чтобы вращение передних колес автомобиля соответствовало изменению направления движения автомобиля.

У меня возникла идея сопоставить ориентацию колеса с производной направления пути (трехмерным вектором), также известной как производная 2-й степени от пути Безье.

По какой-то причине это практически не работает. В какой-то момент вроде работает нормально, а в какой-то чертовски колесо крутится. Я заметил, что производная 2-й степени изменяется, даже если путь Безье является прямой: AFAIK в этом случае должен быть 0.

Итак, мой 1-й вопрос: верна ли моя идея согласовать вращение колеса со 2-й степенью. Если да, то мой второй вопрос: что, черт возьми, идет не так?

Вот мой код кривой Безье 3D:

package fanlib.math {

import flash.geom.Vector3D;

public class BezierCubic3D
{
    public const anchor1:Vector3D = new Vector3D();
    public const anchor2:Vector3D = new Vector3D();
    public const control1:Vector3D = new Vector3D();
    public const control2:Vector3D = new Vector3D();
    /**
     * Gets values from both 'getPointAt' and 'getDirectionAt'
     */
    public const result:Vector3D = new Vector3D();
    private const previous:Vector3D = new Vector3D(); // temporary (optimization)

    // normalization aka arc-parameterization
    public var arcLengths:Vector.<Number> = new Vector.<Number>;
    public var steps:Number = 100;

    private var _length:Number;

    public function BezierCubic3D()
    {
    }

    /**
     * To get a point between anchor1 and anchor2, pass value [0...1]
     * @param t
     */
    public function getPointAt(t:Number):Vector3D {
        const t2:Number = t*t;
        const t3:Number = t*t2;
        const threeT:Number = 3*t;
        const threeT2:Number = 3*t2;
        result.x = getPointAxisAt(anchor1.x, anchor2.x, control1.x, control2.x, t3, threeT, threeT2);
        result.y = getPointAxisAt(anchor1.y, anchor2.y, control1.y, control2.y, t3, threeT, threeT2);
        result.z = getPointAxisAt(anchor1.z, anchor2.z, control1.z, control2.z, t3, threeT, threeT2);
        return result;
    }
    public function getPointAxisAt(a1:Number,a2:Number,c1:Number,c2:Number, t3:Number, threeT:Number, threeT2:Number):Number {
        return  t3      * (a2+3*(c1-c2)-a1) +
                threeT2 * (a1-2*c1+c2) +
                threeT  * (c1-a1) +
                a1;
    }

    /**
     * @param t
     * @return Un-normalized Vector3D! 
     */
    public function getDirectionAt(t:Number):Vector3D {
        const threeT2:Number = 3 * t * t;
        const sixT:Number = 6 * t;
        result.x = getDirAxisAt(anchor1.x, anchor2.x, control1.x, control2.x, threeT2, sixT);
        result.y = getDirAxisAt(anchor1.y, anchor2.y, control1.y, control2.y, threeT2, sixT);
        result.z = getDirAxisAt(anchor1.z, anchor2.z, control1.z, control2.z, threeT2, sixT);
        return result;
    }
    public function getDirAxisAt(a1:Number,a2:Number,c1:Number,c2:Number, threeT2:Number, sixT:Number):Number {
        return  threeT2 * (a2+3*(c1-c2)-a1) +
                sixT    * (a1-2*c1+c2) +
                3       * (c1-a1);
    }

    public function getDirectionDerivativeAt(t:Number):Vector3D {
        const sixT:Number = 6 * t;
        result.x = getDirDerAxisAt(anchor1.x, anchor2.x, control1.x, control2.x, sixT);
        result.y = getDirDerAxisAt(anchor1.y, anchor2.y, control1.y, control2.y, sixT);
        result.z = getDirDerAxisAt(anchor1.z, anchor2.z, control1.z, control2.z, sixT);
        return result;
    }
    public function getDirDerAxisAt(a1:Number,a2:Number,c1:Number,c2:Number, sixT:Number):Number {
        return  sixT    * (a2+3*(c1-c2)-a1) +
                6       * (a1-2*c1+c2);
    }

    /**
     * Call this after any change to defining points and before accessing normalized points of curve.
     */
    public function recalc():void {
        arcLengths.length = steps + 1;
        arcLengths[0] = 0;
        const step:Number = 1 / steps;

        previous.copyFrom(getPointAt(0));
        _length = 0;
        for (var i:int = 1; i <= steps; ++i) {
            _length += Vector3D.distance(getPointAt(i * step), previous);
            arcLengths[i] = _length;
            previous.copyFrom(result);
        }
    }

    /**
     * 'recalc' must have already been called if any changes were made to any of the defining points 
     * @param u
     * @return u normalized/converted to t
     */
    public function normalizeT(u:Number):Number {
        var targetLength:Number = u * arcLengths[steps];
        var low:int = 0,
            high:int = steps,
            index:int; // TODO : have a look-up table of starting low/high indices for each step!
        while (low < high) {
            index = low + ((high - low) >>> 1);
            if (arcLengths[index] < targetLength) {
                low = index + 1;
            } else {
                high = index;
            }
        }
        if (this.arcLengths[index] > targetLength) {
            --index;
        }
        var lengthBefore:Number = arcLengths[index];
        if (lengthBefore === targetLength) {
            return index / steps;
        } else {
            return (index + (targetLength - lengthBefore) / (arcLengths[index + 1] - lengthBefore)) / steps;
        }
    }

    public function getNormalizedPointAt(u:Number):Vector3D {
        return getPointAt(normalizeT(u));
    }

    /**
     * "Normalized" goes for t, not the return Vector3D!!! 
     * @param u
     * @return Un-normalized Vector3D!
     */
    public function getNormalizedDirectionAt(u:Number):Vector3D {
        return getDirectionAt(normalizeT(u));
    }

    public function getNormalizedDirectionDerivativeAt(u:Number):Vector3D {
        return getDirectionDerivativeAt(normalizeT(u));
    }

    public function get length():Number
    {
        return _length;
    }

}
}

А вот код, который применяет производную ориентацию 2-й степени к колесам автомобиля:

            const dirDer:Vector3D = bezier.getDirectionDerivativeAt(time);
            dirDer.negate(); // negate vector's values; for some reason, this gives better results
            for each (wheel in dirWheels) {
                wheel.setRotation(0,0,0); // must nullify before below line
                const localDirDer:Vector3D = wheel.globalToLocalVector(dirDer); // convert dirDer vector to wheel's local axis; wheel translation does NOT affect conversion
                wheel.setOrientation(localDirDer); // orients the object in a specific direction; Up-vector's default value = (0,1,0) 
            }

Я даже пробовал (безуспешно):

            for each (wheel in dirWheels) {
                const localDirDer:Vector3D = wheel.parent.globalToLocalVector(dirDer); // convert dirDer vector to wheel's local axis; wheel translation does NOT affect conversion
                wheel.setOrientation(localDirDer); // orients the object in a specific direction; Up-vector's default value = (0,1,0) 
            }

Один наглядный пример того, что что-то не так: даже когда автомобиль движется по прямой, колесо изначально не вращается (как и должно), но после того, как автомобиль пересекает центральную точку линии, колесо поворачивается на 180 градусов!

РЕДАКТИРОВАТЬ: Вот пример, где Безье вырожден в прямую линию (все 4 точки принадлежат прямой)! Поскольку в случае прямой линии направление f '(t) постоянно, не должна ли его производная f' '(t) быть всегда нулевой?

Например, для якоря1, якоря2, элемента управления1, элемента управления2 соответственно:

Vector3D(-4.01,0.00,-1.90) Vector3D(4.01,0.00,-1.90)
Vector3D(-2.01,0.00,-1.90) Vector3D(2.01,0.00,-1.90)

я получил

f'(0.08)=Vector3D(-1.00,0.00,0.00) f''(0.08)=Vector3D(10.14,0.00,0.00)
f'(0.11)=Vector3D(-1.00,0.00,0.00) f''(0.11)=Vector3D(9.42,0.00,0.00)
f'(0.15)=Vector3D(-1.00,0.00,0.00) f''(0.15)=Vector3D(8.44,0.00,0.00)
f'(0.18)=Vector3D(-1.00,0.00,0.00) f''(0.18)=Vector3D(7.69,0.00,0.00)
f'(0.21)=Vector3D(-1.00,0.00,0.00) f''(0.21)=Vector3D(6.87,0.00,0.00)
f'(0.24)=Vector3D(-1.00,0.00,0.00) f''(0.24)=Vector3D(6.16,0.00,0.00)
f'(0.27)=Vector3D(-1.00,0.00,0.00) f''(0.27)=Vector3D(5.47,0.00,0.00)
f'(0.30)=Vector3D(-1.00,0.00,0.00) f''(0.30)=Vector3D(4.70,0.00,0.00)
f'(0.33)=Vector3D(-1.00,0.00,0.00) f''(0.33)=Vector3D(4.03,0.00,0.00)
f'(0.36)=Vector3D(-1.00,0.00,0.00) f''(0.36)=Vector3D(3.37,0.00,0.00)
f'(0.39)=Vector3D(-1.00,0.00,0.00) f''(0.39)=Vector3D(2.63,0.00,0.00)
f'(0.42)=Vector3D(-1.00,0.00,0.00) f''(0.42)=Vector3D(1.99,0.00,0.00)
f'(0.44)=Vector3D(-1.00,0.00,0.00) f''(0.44)=Vector3D(1.34,0.00,0.00)
f'(0.47)=Vector3D(-1.00,0.00,0.00) f''(0.47)=Vector3D(0.62,0.00,0.00)
f'(0.50)=Vector3D(-1.00,0.00,0.00) f''(0.50)=Vector3D(-0.02,0.00,0.00)
f'(0.53)=Vector3D(-1.00,0.00,0.00) f''(0.53)=Vector3D(-0.74,0.00,0.00)
f'(0.56)=Vector3D(-1.00,0.00,0.00) f''(0.56)=Vector3D(-1.38,0.00,0.00)
f'(0.58)=Vector3D(-1.00,0.00,0.00) f''(0.58)=Vector3D(-2.03,0.00,0.00)
f'(0.61)=Vector3D(-1.00,0.00,0.00) f''(0.61)=Vector3D(-2.67,0.00,0.00)
f'(0.64)=Vector3D(-1.00,0.00,0.00) f''(0.64)=Vector3D(-3.41,0.00,0.00)
f'(0.67)=Vector3D(-1.00,0.00,0.00) f''(0.67)=Vector3D(-4.07,0.00,0.00)
f'(0.70)=Vector3D(-1.00,0.00,0.00) f''(0.70)=Vector3D(-4.74,0.00,0.00)
f'(0.73)=Vector3D(-1.00,0.00,0.00) f''(0.73)=Vector3D(-5.51,0.00,0.00)
f'(0.76)=Vector3D(-1.00,0.00,0.00) f''(0.76)=Vector3D(-6.20,0.00,0.00)
f'(0.79)=Vector3D(-1.00,0.00,0.00) f''(0.79)=Vector3D(-6.91,0.00,0.00)
f'(0.82)=Vector3D(-1.00,0.00,0.00) f''(0.82)=Vector3D(-7.74,0.00,0.00)
f'(0.85)=Vector3D(-1.00,0.00,0.00) f''(0.85)=Vector3D(-8.49,0.00,0.00)
f'(0.89)=Vector3D(-1.00,0.00,0.00) f''(0.89)=Vector3D(-9.27,0.00,0.00)
f'(0.92)=Vector3D(-1.00,0.00,0.00) f''(0.92)=Vector3D(-10.19,0.00,0.00)
f'(0.96)=Vector3D(-1.00,0.00,0.00) f''(0.96)=Vector3D(-11.06,0.00,0.00)
f'(1.00)=Vector3D(-1.00,0.00,0.00) f''(1.00)=Vector3D(-11.98,0.00,0.00)