Turn the front wheels of a car according to the car's bezier path

I have a 3D car which follows a predefined 3D Bezier path. I want the car's front wheels' rotation to match the car's changing direction.

I had the idea to match the wheel's orientation to the derivative of the path's direction (3D vector), aka the 2nd degree derivative of the Bezier path.

For some reason, this barely works. At some point it seems to work fine, while at others the wheel spins like hell. I noted that the 2nd degree derivative changes even when the Bezier path is a straight line: AFAIK in this case it should be 0.

So, my 1st question is if my idea to match the wheel's rotation to the 2nd degree is the way to go. If yes, my 2nd question is what on earth is going wrong?

Here is my Bezier 3D curve code:

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;
    }

}
}

And here is the code that applies the 2nd degree derivative orientation to the car's wheels:

            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) 
            }

I even tried (to no avail):

            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) 
            }

One clear example that something is wrong: even when the car is on a straight line, the wheel originally is non-rotated (as it should), but after the car passes the center point of the line, the wheel rotates 180 degrees! 1st OK 2nd wrong

EDIT: Here is an example where the Bezier is degenerated to a straight line (all 4 points belonging to a straight line)! Since, in the case of a straight line, the direction f'(t) is constant, shouldn't its derivative f''(t) be always zero?

For example, for anchor1, anchor2, control1, control2 respectively:

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)

I get

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)

Solution

The angle of the wheels relative to the direction of the car is related to the signed curvature of the path, usually denoted by \kappa. For arc-length parameterized curves, |\kappa| = length of vector dT/ds where T is the unit tangent vector and dT/ds is its derivative with respect to the arc length parameter. The sign of \kappa depends on the orientation of the curve, but once you've figured out whether left or right is positive at one location, you should be good for the rest of the scenario.

Bezier curves are not arc-length parameterized (unless you have done something ultra-magical), so you'll have to use a more complicated expression. For a path on the plane you should use \kappa = (x'y''-y'x'')/(x'^2+y'^2)^{3/2}. That's nice because you don't need arc length parameterization and also it's signed, but you still have to figure out which sign means left or right.

You also have to figure out the relationship between the angle of the wheels and the curvature. For that, you might find formulas based on the radius of curvature R = 1/\kappa. The radius of curvature has a nice geometrical meaning (related to the "osculating circle" of the path), but it becomes infinite when the path is a straight line.

Here's one approximate formula I found in physics literature for the relationship between wheel angle and radius of curvature: R = s/sqrt(2-2cos(2A)) where s is the wheel base (distance between the centers of the front and rear wheels) and A is the angle of the wheels. You can solve that formula for A like so: (s/R)^2/2 = 1-cos(2A), (s/(2R))^2 = sin^2(A), s\kappa/2 = sin(A), A = arcsin(s\kappa/2). That nicely avoids the singularity at 0 angle. As usual, you'll have to check whether the sign makes sense and reverse it if necessary.

Another formula I've seen is A=arcsin(s\kappa). Clearly both formulas can't be right. I'm not sure which one is right. Just try them both, or find a good treatment in the physics literature.

One more thing you have to think about: at what point along the car to measure the curvature. Again, there are (at least) two choices, at front wheel or at back wheels, and I'm not sure which is right. I think back wheels.

If none of those choices works out, I've probably made a mistake. Let me know and I'll check my work.