Персонажоконтроллер практически с нуля

[lawliet93]

Прив. Использую физ. библиотеку JBullet для рассчета позиций персонажей на стороне сервера в онлайн игре. Как вы поняли, сервер на Java пишется.
Сейчасная задача - написать контроллер для персонажа, т.е. код, который бы управлял объектом, который представляет персонажа. При этом персонаж должен уметь красиво ходить по горам, ямкам, падать с обрывов и все такое.
Для реализации данной задачи имеется собственно карта, с горками и прочим, и механизм пускания лучей. Это значит, что мы можем пустить луч из одной точки мира в другую, и проверить, попал ли этот луч в какой-то объект. После чего можем получить данные о объекте.
Блок-схему алгортма можете глянуть в прикрепленных файлах.
Добавлю к нему, что направление движения задается извне. То есть для того, чтобы персонаж начал двигатся, он должен:

1. Стоять на земле
2. Должно быть задано направление движения
3. Скорость движения не должна быть равна нулю

Т.е. нам нужно лишь вызвать метод, в который передать вектор направления и скорость. А для остановки нужно вызвать метод Stop(), который обнулит вектор направления.
Проблема в том, что персонаж может провалиться под землю во время поднятия на гору, если её наклон резко увеличивается.
Так же есть проблема с движением, если персонаж спускается с пологой горки. Потому что пускается луч в том направлени, в котором двигается персонаж, а это направление задается как-бы в двухмерном пространстве, то есть персонаж двигается сначала вперед, после чего оказывается в воздухе, и после этого срабатывает гравитация, которая просто тянет его вниз, таким образом спуск с горы кажется неестественным, что вы и можете наблюдать в видео.
https://youtu.be/Z1eg20Q5_24
Как же решить эту проблему? Чтобы персонаж плавно двигался по горкам и ямкам.
p.s. кстате, гора на японском будет - яма.

[lawliet93]

Код не влазит, поэтому вот ссылка на класс.
http://pastebin.com/c6dtd3gX
Все начинается с метода

public void updateAction(CollisionWorld collisionWorld, float deltaTime)

А вот еще класс MyUtil, чтобы было понятнее, че там делается

public class MyUtil {
    public static Vector3f MulVector(Vector3f vector3f, float f)
    {
        Vector3f v3 = new Vector3f();
        VectorUtil.mul(v3, vector3f, f);
        return v3;
    }

    public static Vector3f SubVector(Vector3f from, Vector3f v3)
    {
        Vector3f v = new Vector3f();
        VectorUtil.sub(v, from, v3);
        return v;
    }

    public static Vector3f AddVector(Vector3f v1, Vector3f v2)
    {
        Vector3f v = new Vector3f();
        VectorUtil.add(v, v1, v2);
        return v;
    }

    public static double ToDegree(float radians)
    {
        double d =Math.toDegrees(radians);
        d=(d%1)>0.5?Math.ceil(d):Math.floor(d);
        return (180.0f-d);
    }

    public static double Angle(CollisionWorld.ClosestRayResultCallback callback)
    {
        return ToDegree(callback.hitNormalWorld.angle(MyUtil.SubVector(callback.rayToWorld,callback.hitPointWorld)));
    }

    public static double AngleToY(CollisionWorld.ClosestRayResultCallback callback)
    {
        double d = Math.toDegrees(callback.hitNormalWorld.angle(new Vector3f(0, 1, 0)));
        return (d%1)>=0.5?Math.ceil(d):Math.floor(d);
    }

    public static Vector3f InvertXZ(Vector3f direction)
    {
        Vector3f v3 = new Vector3f(direction);
        v3.normalize();
        v3.x*=-1;
        v3.z*=-1;
        return v3;
    }

    public static Quat4f GetQuat4f(Vector3f v3, float angle)
    {
        angle=(float)Math.toRadians(angle);
        return new Quat4f((float)Math.sin(angle/2f)*v3.x,(float)Math.sin(angle/2f)*v3.y, (float)Math.sin(angle/2f)*v3.z, (float)Math.cos(angle/2f));
    }
}