AI Vehicle move

Hi,

I tried to implement vehicle based bot - to do so, I used AIPlayer class as my pattern and I modified Vehicle::ProcessTick by adding at the beginning:

Move aiMove;
if( !move && getAIMove( &aiMove ) ) move = &aiMove;

I prepared AIWheeledVehicle class with the same functionality as for AIPlayer.

Unfortunately getAIMove method doesn't work good for vehicles.
Instead going forward to the destination point, my car turn right and left in a slalom.

I added getRotation() method to Vehicle.cc:

Point3F Vehicle::getRotation() const
{
    QuatF rot = mRigid.state.angPosition;
    return Point3F( rot.x, rot.y, rot.z );
}

This is a code for getAIMove that I use for cars:

bool AIWheeledVehicle::getAIMove( Move *movePtr )
{
    *movePtr = NullMove;

    Point3F location = getPosition();
    Point3F rotation = getRotation();

    if( mAimObject || mAimLocationSet || mMoveState == ModeMove )
    {
        char buf[128];
        if( mAimObject ) mAimLocation = mAimObject->getPosition();

        F32 xDiff = mAimLocation.x - location.x;
        F32 yDiff = mAimLocation.y - location.y;
        if( !isZero( xDiff ) || !isZero( yDiff ) )
        {
            // First do Yaw
            // use the cur yaw between -Pi and Pi
            F32 curYaw = rotation.z;
            while( curYaw > M_2PI ) curYaw -= M_2PI;
            while( curYaw < -M_2PI ) curYaw += M_2PI;

            // find the yaw offset
            F32 newYaw = mAtan( xDiff, yDiff );
            F32 yawDiff = newYaw - curYaw;

            // make it between 0 and 2PI
            if( yawDiff < 0.0f ) yawDiff += M_2PI;
            else if( yawDiff >= M_2PI ) yawDiff -= M_2PI;

            // now make sure we take the short way around the circle
            if( yawDiff > M_PI ) yawDiff -= M_2PI;
            else if( yawDiff < -M_PI ) yawDiff += M_2PI;

            movePtr->yaw = yawDiff;
        }
    }

    // Move towards the destination
    if( mMoveState == ModeMove )
    {
        F32 xDiff = mMoveDestination.x - location.x;
        F32 yDiff = mMoveDestination.y - location.y;

        // Check if we should mMove, or if we are 'close enough'
        if( ( mFabs( xDiff ) < mMoveTolerance ) && 
            ( mFabs(yDiff) < mMoveTolerance ) )
        {
            mMoveState = ModeStop;
            throwCallback( "onReachDestination" );
        }
        else
        {
            // Build move direction in world space
            if( isZero( xDiff ) )
            {
                movePtr->y = ( location.y > mMoveDestination.y )? -1 : 1;
            }
            else
            {
                if( isZero( yDiff ) )
                {
                    movePtr->x = ( location.x > mMoveDestination.x )? -1 : 1;
                }
                else
                {
                    if( mFabs( xDiff ) > mFabs( yDiff ) )
                    {
                        F32 value = mFabs( yDiff / xDiff );
                        movePtr->y = ( location.y > mMoveDestination.y )? -value : value;
                        movePtr->x = ( location.x > mMoveDestination.x )? -1 : 1;
                    }
                    else
                    {
                        F32 value = mFabs( xDiff / yDiff );
                        movePtr->x = ( location.x > mMoveDestination.x )? -value : value;
                        movePtr->y = ( location.y > mMoveDestination.y )? -1 : 1;
                    }
                }
            }

            // Rotate the move into object space (this really only needs
            // a 2D matrix)
            Point3F newMove;
            MatrixF moveMatrix;

            moveMatrix.set( EulerF( 0, 0, -( rotation.z + movePtr->yaw ) ) );
            moveMatrix.mulV( Point3F( movePtr->x, movePtr->y, 0 ), &newMove );
            movePtr->x = newMove.x;
            movePtr->y = newMove.y;

            // Adjust speed
            movePtr->x *= mMoveSpeed;
            movePtr->y *= mMoveSpeed;

            // We should check to see if we are stuck...
            if( location == mLastLocation )
            {
                throwCallback( "onMoveStuck" );
                mMoveState = ModeStop;
            }
        }
    }

    // Test for target location in sight if it's an object. The LOS is
    // run from the eye position to the center of the object's bounding,
    // which is not very accurate.
    if( mAimObject )
    {
        MatrixF eyeMat;

        getEyeTransform( &eyeMat );
        eyeMat.getColumn( 3, &location );
        Point3F targetLoc = mAimObject->getBoxCenter();

        // This ray ignores non-static shapes. Cast Ray returns true
        // if it hit something.
        RayInfo dummy;
        if( getContainer()->castRay( location, targetLoc,
            InteriorObjectType | StaticShapeObjectType | StaticObjectType |
            TerrainObjectType, &dummy ) )
        {
            if( mTargetInLOS )
            {
                throwCallback( "onTargetExitLOS" );
                mTargetInLOS = false;
            }
        }
        else if( !mTargetInLOS )
        {
            throwCallback( "onTargetEnterLOS" );
            mTargetInLOS = true;
        }
    }

    // Replicate the trigger state into the move so that
    // triggers can be controlled from scripts.
    for( int i = 0; i < MaxTriggerKeys; i++ )
    {
        movePtr->trigger[i] = getImageTriggerState( i );
    }

   return true;
}

As I said, above code doesn't work :-(
Does anyone has an idea what's going on? Is it a correct way to implement AI for vehicles or do I need to rewrite something deeper?

Greetings