Using Yaw and Pitch to create Rotation Matrix and Simulate FPS camera

Question

First of all, I must say that what I want to do here is all about Points, not objects or any thing else. And my use of OpenGL here is more for calculation.

So here is my problem. I want to show some 3d points in my 2d context (winform for example or image); For doing so I wrote this function: (It is in C# and using OpenTK (OpenGL wrapper) but I am sure that if you know OpenGL you can understand it regardless of your knowledge of C#)

    private Vector3 GetProjectedLocation(
        Vector3 pointPosition,
        Vector3 cameraPosition,
        Matrix4 cameraRotationMatrix)
    {
        float horizentalFov = MathHelper.DegreesToRadians(60.0f);
        float viewRatio = (float)this.RenderSize.Width / this.RenderSize.Height;
        Vector3 cameraReference = new Vector3(0, 0, -1);
        Vector3 transformedReference = Vector3.Transform(cameraReference, Matrix4.Invert(cameraRotationMatrix));
        Vector3 cameraLookatTarget = cameraPosition + transformedReference;
        Matrix4 modelMatrix = Matrix4.Identity;
        Matrix4 viewMatrix = Matrix4.LookAt(cameraPosition, cameraLookatTarget, new Vector3(0, 1, 0));
        Matrix4 projectionMatrix = Matrix4.CreatePerspectiveFieldOfView(horizentalFov, viewRatio, 0.1f, 100.0f);
        Matrix4 viewModelMatrix = viewMatrix * modelMatrix;
        return Helper.Project(
            pointPosition,
            viewModelMatrix,
            projectionMatrix,
            new Rectangle(new Point(), this.RenderSize));
    }

I also wrote another function named Project cuz I didnt find it in OpenTK:

    public static Vector3 Project(Vector3 point, Matrix4 viewmodel, Matrix4 projection, Rectangle viewport)
    {
        Vector4 point4 = new Vector4(point, 1.0f);
        Vector4 pointModel = Vector4.Transform(point4, viewmodel);
        Vector4 pointProjection = Vector4.Transform(pointModel, projection);
        pointProjection.W = (float)(1.0 / pointProjection.W);
        pointProjection.X *= pointProjection.W;
        pointProjection.Y *= pointProjection.W;
        pointProjection.Z *= pointProjection.W;
        return new Vector3
                   {
                       X = (float)((pointProjection.X * 0.5 + 0.5) * viewport.Width + viewport.X),
                       Y = (float)((pointProjection.Y * 0.5 + 0.5) * viewport.Height + viewport.Y),
                       Z = (float)((1.0 + pointProjection.Z) * 0.5)
                   };
    }

When all three values (yaw, pitch and roll) are zero, it seems that it works as intended. I can move the point horizontally by changing yaw and vertically using pitch values. Roll also move it the way it should be.

The problem shows itself when I set pitch to 90deg. In this case yaw instead of moving it horizontally, moves it vertically, and even worse, only moves it to down. No matter if I go to positive side or negative side (-20deg or +20deg) it always go down. Funny thing is that Roll has same affect when pitch is 90deg and it only move it vertically and to down just like yaw. Setting pitch to -90deg has same effect but Yaw and roll this time only moves it to up.

It seems like gimbal lock problem when two of my axis are parallel and I lost one direction which in this case is roll. I still don't understand why yaw works only in one direction though.

Any way, the problem is here that this lock happen when looking vertically and this is where my program look most of the times. I have read that I can move the gimbal lock to the top (less frequent place to look at) if I change my rotation order. I tried number of orders and couldn't find a good one. So here is my code, maybe I am doing it wrong:

    private Matrix4 CreateRotationMatrix(char axis, float radians)
    {
        float c = (float)Math.Cos(radians);
        float s = (float)Math.Sin(radians);

        if (axis == 'X')
        {
            return new Matrix4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, c, -s, 0.0f, 0.0f, s, c, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
        }
        if (axis == 'Y')
        {
            return new Matrix4(c, 0.0f, s, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, -s, 0.0f, c, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
        }
        return new Matrix4(c, -s, 0.0f, 0.0f, s, c, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
    }

    private Matrix4 MatrixFromEulerAngles(Vector3 euler, string order)
    {
        return CreateRotationMatrix(order[2], GetEulerAngle(order[2], euler))
               * CreateRotationMatrix(order[1], GetEulerAngle(order[1], euler))
               * CreateRotationMatrix(order[0], GetEulerAngle(order[0], euler));
    }

    private float GetEulerAngle(char angle, Vector3 euler)
    {
        if (angle == 'X') return euler.X;
        if (angle == 'Y') return euler.Y;
        return angle == 'Z' ? euler.Z : 0f;
    }

Do you have any idea?

Here are the orders I knew: "XYZ", "XZY", "YXZ", "YZX", "ZXY", "ZYX"

I can provide a sample program to see the problem by yourself.

Answer 1

I think part of the problem is coming from your use of the LookAt function in the first block of code. The way these functions work is to compute the local "up" vector by looking at the portion of the global "up" vector which is perpendicular to the direction that you're looking. In the case where you're looking up, the global "up" vector ( (0,1,0) in the above code) has no component which is perpendicular to the looking direction, and the result may be undefined or unstable. The same would apply when you're looking down.

Instead, try computing the "up" vector for the LookAt function as:

Vector3 localUpVec = Vector3.Transform(new Vector3(0, 1, 0), Matrix4.Invert(cameraRotationMatrix));
Matrix4 viewMatrix = Matrix4.LookAt(cameraPosition, cameraLookatTarget, localUpVec);

Or even better, you could bypass the LookAt function altogether and compute it yourself:

Matrix4 viewMatrix = Matrix4.Subtract( Matrix4.Transpose(cameraRotationMatrix), Matrix4.Translation(cameraPosition) );