惯性张量（OpenGL）

Question

我一直在研究刚性身体力学系统一周，我不确定我做错了什么。我有一个旋转矩阵，它以yxz的顺序计算旋转。基于先前的旋转和更新的偏移来计算所有旋转。然后每当我使用旋转矩阵检测到碰撞时计算惯性张量，即I = Rot * I0 * RotNOT。当所有旋转与主轴对齐时，I0是恒定惯性框架。最终，我试图计算碰撞后物体的合成角速度和线速度。我下面的代码很乱，但是评论它基于这个维基百科页面http://en.wikipedia.org/wiki/Collision_response，用于标量jr计算以及角度和线性速度计算。

//Calculate inertia tensor based on rotation matrix
    Matrix RotMat = rectangle1->GetRotMat();
    Matrix TRotMat = RotMat.Transpose();
    I1 = RotMat*I1*TRotMat;

    //Inverse of I1
    //Determinant of I1
    float det1 = I1.GetDeterminant();

    //Transpose of I1
    Matrix I1T = I1.Transpose();

    //Cofactors
    float *A = I1T.GetMatrix();
    Matrix I1NOT(3,3);
    I1NOT.SetMatrix((A[4]*A[8] - A[5]*A[7]), -(A[3]*A[8] - A[5]*A[6]), (A[3]*A[7] - A[4]*A[6]),
        -(A[1]*A[8] - A[2]*A[7]), (A[0]*A[8] - A[2]*A[6]), -(A[0]*A[7] - A[1]*A[6]),
        (A[1]*A[5] - A[2]*A[4]), -(A[0]*A[5] - A[2]*A[3]), (A[0]*A[4] - A[1]*A[3]));

    //Scale by determinant to find inverse
    I1NOT = I1NOT/det1;

    //Compute impluse
    float e = 1.0f;
    Vector r1 = hitPoint - rectangle1->GetLinPos(); 
    Vector r2 = hitPoint - rectangle2->GetLinPos(); 
    Vector n = tempNorm;
    n = n*-1.0f;
    float jrNumer = ((rectangle2Velocity-rectangle1Velocity)*-(1.0f + e)).DotProduct(n);


    //Compute (INOT*(r x n) x r)
    Vector tr1 = r1;
    // tr1 = (r x n)
    tr1.CrossProduct(n);
    float *I1A = I1NOT.GetMatrix();
    //(I1Ir1n = INOT * tr1)
    Vector I1Ir1n(I1A[0]*tr1.getx()+I1A[1]*tr1.gety()+I1A[2]*tr1.getz(),
        I1A[3]*tr1.getx()+I1A[4]*tr1.gety()+I1A[5]*tr1.getz(),
        I1A[6]*tr1.getx()+I1A[7]*tr1.gety()+I1A[8]*tr1.getz());
    //(I1Ir1n x r)
    I1Ir1n.CrossProduct(r1);
    //Compute (INOT*(r x n) x r)dot(n) + 1/m1
    float jrDenom = I1Ir1n.DotProduct(n) + (1.0f/m1);

    //jr magnitude of impulse
    float jr = jrNumer/jrDenom;
    Vector v1 = rectangle1->GetLinVel();
    Vector v1prime = v1 - n*(jr/m1);

    //Calculate angular velocity
    Vector R1 = r1;
    //R1 = (r x n)
    R1.CrossProduct(n);
    //IR1 = (INOT * R1)
    Vector IR1(I1A[0]*R1.getx()+I1A[1]*R1.gety()+I1A[2]*R1.getz(),
        I1A[3]*R1.getx()+I1A[4]*R1.gety()+I1A[5]*R1.getz(),
        I1A[6]*R1.getx()+I1A[7]*R1.gety()+I1A[8]*R1.getz());

    Vector w1 = rectangle1->GetAngVel();
    //w1' = w1 - jr*INOT
    Vector w1prime = w1 - IR1*jr;