如何从偏航角、俯仰角和翻滚角获取旋转矩阵。

Question

你好，我在我的Android应用中从加速度计和磁感应传感器中获得了偏航角、俯仰角和滚转角。现在我想根据这些角度围绕场景中的某一点旋转我的相机目标（min3d）。结果是能够通过移动Android设备来在3D场景中观察。我尝试了几天，几乎阅读了SO上所有相关的回答，但我就是无法让它工作。

我得到的运动完全没有意义。我验证过我的偏航角介于0-360之间且正确，俯仰角介于-90和90之间且正确，最后，滚转角介于-180和180之间且一致。

基本上我正在进行矩阵旋转，并通过我的目标和向上矢量进行乘法运算。

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;

最初我的目标点是(0,0,150)，我的向上向量为(0,1,0)。

感谢任何帮助。

- user5156016

---

找到了:) 更新了我的答案。 - Sung

Answer 1

在矩阵滚动后，目标向量必须保持一致，因为目标只瞄准轴线。

实际上，你的目标不是vec3(0,150,150)，而是vec3(0,0,150)。你旋转它，然后加上vec3(0,150,0)。所以想一想，如果你旋转z轴，vec3(0,0,150)始终是vec3(0,0,150)。

更新

是的，rotateM()将乘以之前设置的矩阵和现在设置的矩阵，所以逻辑上没有问题。

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

这个 Android 函数 rotateM() 是以下三个矩阵的组合版本。

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

- Sung

---

@JawadLeWywadi 好的，你在函数rotateM()中对矩阵进行了乘法运算，这一点我不知道。有两件事情让我感到奇怪。1. 你从未对上向量和目标向量进行归一化处理。2. 我没有看到你设置相机位置。3. 只需保留上向量而不进行乘法运算（可能没有必要，但我无法确定你在camera()函数中所做的事情）。 - Sung

camera() 包含三个向量，分别是 position、target 和 upVector。rotateM 是来自 Android OpenGL Matrix 类的函数，我没有它的实现。 - user5156016

为什么？你没有解释为什么，而且这也不是正在发生的事情。它进行了三次旋转，但最终结果与我应该得到的不一致。在我看来，以及文档中描述的方式是，当我在单位矩阵上调用rotateM时，旋转矩阵包含绕roll度z轴旋转，然后我将此矩阵绕x轴旋转...我没有将矩阵重置回单位矩阵，根据文档，旋转应该叠加。 - user5156016

让我试试这个。 - user5156016

@JawadLeWywadi 对不起，我的错误... rotateM() 不等同于 setrotateM()。我应该双重核对过。我已经更新了它。 - Sung

Answer 2

你在设置目标和upAxis值时出现了错误。你正在将角度值分配给目标，并使用相同的目标值更新upAxis。然而，目标是3D环境中的x、y、z点，而upAxis告诉相机以此上向量作为参考（更改upAxis会使相机滚动）。这是我的实现方式，通过加速度计和磁力计来实现，受到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;

    }
}

- ugur

---

这不起作用是因为当我们有旋转和/或俯仰时，相机的上向量也应该随之改变。 - user5156016