How to get RotationMatrix from yaw, pitch and roll

Question

Hi I managed to find yaw, pitch and roll from acceleratoin and magnetic sensor in my Android app. I now want to rotate my Camera Target(min3d) around a point in my scene according to does angles. The result is to be able to look in a 3d scene by moving the android device around. I've been trying for days almost read all related answers in SO but I just can't have it work.

The movement I get has absolutely no sens. I verified that my yaw is between 0-360 and is correct, pitch is between -90 and 90 and is correct and finally the roll is between -180 and 180 and is consistent.

Basically I am doing matrix rotation and multiplication by my target and up vector.

float[] rotation = new float[16];   
Matrix.setIdentityM(rotation, 0);
Matrix.rotateM(rotation, 0, (float) Math.toDegrees(roll), 0, 0, 1);         
Matrix.rotateM(rotation, 0, (float) Math.toDegrees(pitch)+90f, 1, 0, 0);        
Matrix.rotateM(rotation, 0, (float) Math.toDegrees(-azimut), 0, 1, 0);      

float[] target = new float[4];
float[] source = new float[]{0,0,150,0};    
Matrix.multiplyMV(target, 0,rotation, 0, source, 0);
float targetX = target[0] + 0;
float targetY = target[1] + 150;
float targetZ = -target[2] + 0;

target = new float[4];
source = new float[]{0,1,0,0};
Matrix.multiplyMV(target, 0,rotation, 0, source, 0);
float upX = target[0];
float upY = target[1];
float upZ = target[2];

scene.camera().target.x = targetX;              
scene.camera().target.y = targetY;
scene.camera().target.z = targetZ;      
scene.camera().upAxis.x = upX;
scene.camera().upAxis.y = upY;
scene.camera().upAxis.z = upZ;

Initially my target is at (0,0,150) and my up vector is (0,1,0).

Thank you for any help.

Answer 1

After the roll matrix, the target vector must be consistent because the target only aim axis.

Actually, your target is not vec3(0,150,150) the target is vec3(0,0,150) indeed. You rotate it and then add vec3(0,150,0). So think about it, vec3(0,0,150) is always vec3(0,0,150) if you rotate z-axis.

Update

Yes, rotateM() multiplies the matrix you set before and a matrix you set now, so logically there is no problem there.

 public static void rotateM(float[] rm, int rmOffset,
        float[] m, int mOffset,
        float a, float x, float y, float z) {
    synchronized(sTemp) {
        setRotateM(sTemp, 0, a, x, y, z);
        multiplyMM(rm, rmOffset, m, mOffset, sTemp, 0);
    }

public static void setRotateM(float[] rm, int rmOffset,
        float a, float x, float y, float z) {
    rm[rmOffset + 3] = 0;
    rm[rmOffset + 7] = 0;
    rm[rmOffset + 11]= 0;
    rm[rmOffset + 12]= 0;
    rm[rmOffset + 13]= 0;
    rm[rmOffset + 14]= 0;
    rm[rmOffset + 15]= 1;
    a *= (float) (Math.PI / 180.0f);
    float s = (float) Math.sin(a);
    float c = (float) Math.cos(a);
    if (1.0f == x && 0.0f == y && 0.0f == z) {
        rm[rmOffset + 5] = c;   rm[rmOffset + 10]= c;
        rm[rmOffset + 6] = s;   rm[rmOffset + 9] = -s;
        rm[rmOffset + 1] = 0;   rm[rmOffset + 2] = 0;
        rm[rmOffset + 4] = 0;   rm[rmOffset + 8] = 0;
        rm[rmOffset + 0] = 1;
    } else if (0.0f == x && 1.0f == y && 0.0f == z) {
        rm[rmOffset + 0] = c;   rm[rmOffset + 10]= c;
        rm[rmOffset + 8] = s;   rm[rmOffset + 2] = -s;
        rm[rmOffset + 1] = 0;   rm[rmOffset + 4] = 0;
        rm[rmOffset + 6] = 0;   rm[rmOffset + 9] = 0;
        rm[rmOffset + 5] = 1;
    } else if (0.0f == x && 0.0f == y && 1.0f == z) {
        rm[rmOffset + 0] = c;   rm[rmOffset + 5] = c;
        rm[rmOffset + 1] = s;   rm[rmOffset + 4] = -s;
        rm[rmOffset + 2] = 0;   rm[rmOffset + 6] = 0;
        rm[rmOffset + 8] = 0;   rm[rmOffset + 9] = 0;
        rm[rmOffset + 10]= 1;
    } else {
        float len = length(x, y, z);
        if (1.0f != len) {
            float recipLen = 1.0f / len;
            x *= recipLen;
            y *= recipLen;
            z *= recipLen;
        }
        float nc = 1.0f - c;
        float xy = x * y;
        float yz = y * z;
        float zx = z * x;
        float xs = x * s;
        float ys = y * s;
        float zs = z * s;
        rm[rmOffset +  0] = x*x*nc +  c;
        rm[rmOffset +  4] =  xy*nc - zs;
        rm[rmOffset +  8] =  zx*nc + ys;
        rm[rmOffset +  1] =  xy*nc + zs;
        rm[rmOffset +  5] = y*y*nc +  c;
        rm[rmOffset +  9] =  yz*nc - xs;
        rm[rmOffset +  2] =  zx*nc - ys;
        rm[rmOffset +  6] =  yz*nc + xs;
        rm[rmOffset + 10] = z*z*nc +  c;
    }
}

This android function rotateM() is a combined version of these three matrices below

void Matrix_Rotation_X(Matrix   &out_M,const float angle)
{
    float COS = (float)cos(angle);
    float SIN = (float)sin(angle);
    out_M.s[_0x0_]=  1.f;       out_M.s[_0x1_]= 0.f;    out_M.s[_0x2_]= 0.f;    out_M.s[_0x3_]= 0.f;
    out_M.s[_1x0_]=  0.f;       out_M.s[_1x1_]= COS;    out_M.s[_1x2_]= SIN;    out_M.s[_1x3_]= 0.f;
    out_M.s[_2x0_]=  0.f;       out_M.s[_2x1_]=-SIN;    out_M.s[_2x2_]= COS;    out_M.s[_2x3_]= 0.f;
    out_M.s[_3x0_]=  0.f;       out_M.s[_3x1_]= 0.f;    out_M.s[_3x2_]= 0.f;    out_M.s[_3x3_]= 1.f;

}

void Matrix_Rotation_Y(Matrix   &out_M, const float angle)
{
    float COS = (float)cos(angle);
    float SIN = (float)sin(angle);
    out_M.s[_0x0_]=  COS;       out_M.s[_0x1_]= 0.f;    out_M.s[_0x2_]=-SIN;    out_M.s[_0x3_]= 0.f;
    out_M.s[_1x0_]=  0.f;       out_M.s[_1x1_]= 1.f;    out_M.s[_1x2_]= 0.f;    out_M.s[_1x3_]= 0.f;
    out_M.s[_2x0_]=  SIN;       out_M.s[_2x1_]= 0.f;    out_M.s[_2x2_]= COS;    out_M.s[_2x3_]= 0.f;
    out_M.s[_3x0_]=  0.f;       out_M.s[_3x1_]= 0.f;    out_M.s[_3x2_]= 0.f;    out_M.s[_3x3_]= 1.f;
}

void Matrix_Rotation_Z( Matrix  &out_M, const float angle)
{
    float COS = (float)cos(angle);
    float SIN = (float)sin(angle);
    out_M.s[_0x0_]=  COS;       out_M.s[_0x1_]= SIN;    out_M.s[_0x2_]= 0.f;    out_M.s[_0x3_]= 0.f;
    out_M.s[_1x0_]=  -SIN;      out_M.s[_1x1_]= COS;    out_M.s[_1x2_]= 0.f;    out_M.s[_1x3_]= 0.f;
    out_M.s[_2x0_]=  0.f;       out_M.s[_2x1_]= 0.f;    out_M.s[_2x2_]= 1.f;    out_M.s[_2x3_]= 0.f;
    out_M.s[_3x0_]=  0.f;       out_M.s[_3x1_]= 0.f;    out_M.s[_3x2_]= 0.f;    out_M.s[_3x3_]= 1.f;
}

https://github.com/sunglab/StarEngine/blob/master/math/Matrix.cpp

Answer 2

You are doing wrong while setting target and upAxis values. you are assigning angle values to target and update upAxis with same target values. However target is ax,y,z points in a 3d environment and upAxis tells the camera to take this up vector as a reference (changing upAxis rolls a camera). Here is my implementation via accels and mags motivated from ExampleAccelerometer.java

public class ExampleAccelerometer extends RendererActivity implements SensorEventListener {

private SkyBox mSkyBox;
private final float[] mAccelerometerReading = new float[3];
private final float[] mMagnetometerReading = new float[3];

private final float[] mRotationMatrix = new float[16];
private final float[] mOrientation = new float[3];
private SensorManager mSensorManager;
private Sensor mAccel, mMag;
public Number3d upAxis;

@Override
public void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
    setRequestedOrientation(ActivityInfo.SCREEN_ORIENTATION_LANDSCAPE);
    mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
    mAccel = mSensorManager.getDefaultSensor(
            Sensor.TYPE_ACCELEROMETER);
    mMag = mSensorManager.getDefaultSensor(
            Sensor.TYPE_MAGNETIC_FIELD);
    Matrix.setIdentityM(mRotationMatrix, 0);

}

public void initScene()
{
    scene.lights().add(new Light());

    mSkyBox = new SkyBox(5.0f, 2);
    mSkyBox.addTexture(SkyBox.Face.North,   R.drawable.wood_back,   "north");
    mSkyBox.addTexture(SkyBox.Face.East,    R.drawable.wood_right,  "east");
    mSkyBox.addTexture(SkyBox.Face.South,   R.drawable.wood_back,   "south");
    mSkyBox.addTexture(SkyBox.Face.West,    R.drawable.wood_left,   "west");
    mSkyBox.addTexture(SkyBox.Face.Up,      R.drawable.ceiling,     "up");
    mSkyBox.addTexture(SkyBox.Face.Down,    R.drawable.floor,       "down");
    mSkyBox.scale().y = 0.8f;
    mSkyBox.scale().z = 2.0f;
    scene.addChild(mSkyBox);

    //Initial upAxis vector
    upAxis = new Number3d(-1,0,0);
    scene.camera().upAxis = upAxis;

    mSensorManager.registerListener(this, mAccel, SensorManager.SENSOR_DELAY_UI);
    mSensorManager.registerListener(this, mMag,SensorManager.SENSOR_DELAY_UI);
}

@Override
protected void onDestroy() {
    super.onDestroy();

    // Don't receive any more updates from either sensor.
    mSensorManager.unregisterListener(this);
}

@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
    // TODO Auto-generated method stub
}

@Override
public void onSensorChanged(SensorEvent event) {
    if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
        System.arraycopy(event.values, 0, mAccelerometerReading,
                0, mAccelerometerReading.length);
    }
    else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
        System.arraycopy(event.values, 0, mMagnetometerReading,
                0, mMagnetometerReading.length);
    }

    SensorManager.getRotationMatrix(mRotationMatrix, null,
            mAccelerometerReading, mMagnetometerReading);

    SensorManager.getOrientation(mRotationMatrix, mOrientation);

    //Camera Position
    scene.camera().position.x = 0;
    scene.camera().position.y = 0;
    scene.camera().position.z = 0;

    //Camera target position (where camera looks at i.e. position of interest)
    scene.camera().target.x = 0;
        scene.camera().target.y = 0;
        scene.camera().target.z = 50;

        //To multiply target with rotation create a temp vector with 4 parameter
        float[] p = new float[4];
        p[0] = scene.camera().target.x;
        p[1] = scene.camera().target.y;
        p[2] = scene.camera().target.z;
        p[3] = 1;

        //Rotate target according to the rotation matrix derived via accel and mag sensor
        float[] target = new float[4];
        Matrix.multiplyMV(target, 0, mRotationMatrix, 0, p, 0);

//      set rotated camera target (this creates 2d rotation with roll and pitch)
        scene.camera().target.x = target[0];
        scene.camera().target.y = target[1];
        scene.camera().target.z = target[2];

//      change up axis for rolling with yaw parameter
        upAxis.x = (float)(Math.cos(mOrientation[0]));
        upAxis.y = (float) (Math.sin(mOrientation[0]));
        upAxis.z = 0;
        scene.camera().upAxis = upAxis;

    }
}