Question

我正在尝试构建刚体物理引擎。我使用glm库作为我的标准数学库。

我面临的问题如下，

当我计算相对于力的扭矩时（参见函数calculateTorque（）），身体变形并倾斜到旋转的程度，并且尺寸增大并且不再可见。但是当我移除力时，从我只是随机化的扭矩方程（参见calculateForces（））来看扭矩值就可以了。
当旋转时，我认为对于四元数和矩阵，身体都在倾斜并略微增大。（不确定惯性计算是否正确，主体是立方体）
我也遇到碰撞反应问题，身体逐渐穿透表面，并且相对于epsilon值没有正确反应。

P.S：AABB - 平面碰撞是在Christer Ericson的实时碰撞检测书的帮助下实现的，刚体物理学是在Braff实施的帮助下实现的。

这是我的代码。

RigidBody::RigidBody(void)
{
}

RigidBody::RigidBody(float height, float width, float depth, int n)
    : height(height), width(width), depth(depth), nrb(n)
{
    objPos = new glm::vec3[n];
    worldPos = new glm::vec3[n];
    initRB();
}

RigidBody::~RigidBody(void)
{
}

void RigidBody::initRB()
{
    dt = 0.01;

    // Mass
    m = 0.5;
    float m_inverse = 1/m;

    // Inertia
    IBody = glm::mat3();
    IWorld = glm::mat3();

    IBody[0][0] = 1.0 / 12.0 * (m * ((height * height) + (depth * depth)));
    IBody[1][1] = 1.0 / 12.0 * (m * ((width * width) + (depth * depth)));
    IBody[2][2] = 1.0 / 12.0 * (m * ((width * width) + (height * height)));

    IBody_inverse = glm::inverse(IBody);

    // CG
    x = glm::vec3(0.0, 0.0, 0.0);

    // Linear Momentum
    P = glm::vec3(0.0, 0.0, 0.0);

    // Linear Velocity
    v = P * m_inverse;

    // Orientation (Matrix)
    R = glm::mat3();

    // Orientation (Quaternion)
    QR = glm::fquat();

    // Use Quaternion Flag
    useQuat = true;

    // Angular Velocity
    w = glm::vec3(1.0, 0.0, 1.0);

    // Angular Momentum
    L = IBody * w;

    // Forces
    rbPhys.gravitational = 9.81;
    rbPhys.viscousdrag = 0.1;
    f = glm::vec3(0.0, -m * rbPhys.gravitational, 0.0);

    // Torque
    t = glm::vec3(0.0, 0.0, 0.0);

    // Collision Flag
    collided = false;
}

void RigidBody::setBodyPos(glm::vec3 *verts)
{
    for(int i=0; i<nrb; i++)
    {
        objPos[i] = verts[i];
    }
}

void RigidBody::calculateBodyToWorld()
{
    for(int i=0; i<nrb; i++){
        worldPos[i] = R * objPos[i] + x;
    }
}

void RigidBody::calculateAABB()
{
    aabb.FindMaxMin(worldPos, nrb);
    max = aabb.GetMaxVertices();
    min = aabb.GetMinVertices();
}

glm::mat3 RigidBody::makeSkewSymmetric(glm::vec3 v)
{
    glm::mat3 result;

    result = glm::mat3(0.0, -v.z, v.y,
                       v.z, 0.0, -v.x,
                       -v.y, v.x, 0.0);

    return result;
}

glm::mat3 RigidBody::orthonormalize(glm::mat3 r)
{
    glm::mat3 result;

    glm::vec3 v1 = glm::vec3(r[0][0], r[1][0], r[2][0]);
    glm::vec3 v2 = glm::vec3(r[0][1], r[1][1], r[2][1]);
    glm::vec3 v3 = glm::vec3(r[0][2], r[1][2], r[2][2]);

    glm::normalize(v1);
    v2 = glm::cross(v3, v1);
    glm::normalize(v2);
    v3 = glm::cross(v1, v2);
    glm::normalize(v3);

    result[0][0] = v1.x;    result[0][1] = v2.x;    result[0][2] = v3.x;
    result[1][0] = v1.y;    result[1][1] = v2.y;    result[1][2] = v3.y;
    result[2][0] = v1.z;    result[2][1] = v2.z;    result[2][2] = v3.z;

    return result;
}

void RigidBody::computeAuxillary()
{
    // Linear Velocity
    float m_inverse = 1/m;
    v = P * m_inverse;

    // Inertia Tensor (World)
    IWorld = R * IBody * glm::transpose(R);
    IWorld_inverse = R * IBody_inverse * glm::transpose(R);     

    // Angular Velocity
    w = IWorld_inverse * L; 

    if(useQuat)
    {
        // Store quaternion in Rotation Matrix
        R = glm::mat3_cast(QR);
    }
}

void RigidBody::calculateForces()
{
    // sine torque to get some spinning action

    t.x = 1.0 * sin(dt*0.9 + 0.5);
    t.y = 1.1 * sin(dt*0.5 + 0.4);
    t.z = 1.2 * sin(dt*0.7 + 0.9);

    // damping torque so we dont spin too fast

    t.x -= 0.2 * w.x;
    t.y -= 0.2 * w.y;
    t.z -= 0.2 * w.z;
}

void RigidBody::calculateTorque()
{
    // Torque about a point p acted on by a force f
    // r = cg + R * p, here p is taken for granted as the center of the cube
    glm::vec3 r = glm::vec3(0.0, 0.0, 0.0);
    glm::vec3 p = glm::vec3(0.0, 0.5 * height, 0.0);
    r = x + (R * p);
    t = glm::cross((r - x),f);
}

void RigidBody::resetForces()
{
    f = glm::vec3(0.0, -m * rbPhys.gravitational, 0.0);
    t = glm::vec3(0.0, 0.0, 0.0);
}

void RigidBody::calculateDerivatives()
{
    rbDerivative.dvdt.x = v.x;
    rbDerivative.dvdt.y = v.y;
    rbDerivative.dvdt.z = v.z;

    if(useQuat)
    {
        glm::fquat wQ;
        wQ.w = 0;
        wQ.x = w.x;
        wQ.y = w.y;
        wQ.z = w.z;
        rbDerivative.dQRdt = glm::normalize(wQ * QR);
    }
    else
    {
        glm::mat3 wR = makeSkewSymmetric(w);
        rbDerivative.dRdt = wR * R;
    }

    rbDerivative.dPdt.x = f.x;
    rbDerivative.dPdt.y = f.y;
    rbDerivative.dPdt.z = f.z;

    rbDerivative.dLdt.x = t.x;
    rbDerivative.dLdt.y = t.y;
    rbDerivative.dLdt.z = t.z;
}

void RigidBody::updateRB()
{
    calculateForces();
    calculateDerivatives();

    x.x += rbDerivative.dvdt.x * dt;
    x.y += rbDerivative.dvdt.y * dt;
    x.z += rbDerivative.dvdt.z * dt;

    if(useQuat)
    {
        QR = QR + rbDerivative.dQRdt * dt;
    }
    else
    {
        R += rbDerivative.dRdt * dt;
        //R = orthonormalize(R); orthornormalize wrong
    }

    P.x += rbDerivative.dPdt.x * dt;
    P.y += rbDerivative.dPdt.y * dt;
    P.z += rbDerivative.dPdt.z * dt;

    L.x += rbDerivative.dLdt.x * dt;
    L.y += rbDerivative.dLdt.y * dt;
    L.z += rbDerivative.dLdt.z * dt;

    computeAuxillary();

    calculateAABB();
}

void RigidBody::renderRB()
{
    calculateBodyToWorld();

    glPushMatrix();
        glColor3f(0.0, 0.0, 0.0);
        glBegin(GL_LINES);
            glVertex3f(max[0], max[1], max[2]);
            glVertex3f(max[0], min[1], max[2]);

            glVertex3f(max[0], max[1], max[2]);
            glVertex3f(min[0], max[1], max[2]); 

            glVertex3f(max[0], max[1], max[2]);
            glVertex3f(max[0], max[1], min[2]);

            glVertex3f(max[0], min[1], max[2]);
            glVertex3f(max[0], min[1], min[2]);

            glVertex3f(max[0], min[1], min[2]);
            glVertex3f(max[0], max[1], min[2]);

            glVertex3f(max[0], min[1], max[2]);
            glVertex3f(min[0], min[1], max[2]);

            glVertex3f(min[0], min[1], max[2]);
            glVertex3f(min[0], max[1], max[2]);

            glVertex3f(min[0], min[1], min[2]);
            glVertex3f(min[0], max[1], min[2]);

            glVertex3f(min[0], min[1], min[2]);
            glVertex3f(min[0], min[1], max[2]); 

            glVertex3f(min[0], min[1], min[2]);
            glVertex3f(max[0], min[1], min[2]);

            glVertex3f(min[0], max[1], min[2]);
            glVertex3f(max[0], max[1], min[2]);

            glVertex3f(min[0], max[1], min[2]);
            glVertex3f(min[0], max[1], max[2]);
        glEnd();

        if(isColliding())
            glColor3f(1.0, 0.0, 0.0);
        else
            glColor3f(0.0, 0.0, 1.0);

        glBegin(GL_QUADS);
            for(int i=0; i<nrb; i++)
            {
                glVertex3f(worldPos[i].x, worldPos[i].y, worldPos[i].z);
            }
        glEnd();
    glPopMatrix();
}

void RigidBody::printDebug()
{
    std::cout<<" "<<std::endl;
    std::cout<<"RIGID BODY DEBUG:"<<std::endl;
    std::cout<<"Height :"<<height<<" "<<"Width :"<<width<<" "<<"Depth :"<<depth<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Mass :"<<m<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Inverse Mass :"<<1/m<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"CG :"<<x.x<<" "<<x.y<<" "<<x.z<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Linear Velocity :"<<v.x<<" "<<v.y<<" "<<v.z<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Linear Momentum :"<<P.x<<" "<<P.y<<" "<<P.z<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Angular Momentum :"<<L.x<<" "<<L.y<<" "<<L.z<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Angular Velocity :"<<w.x<<" "<<w.y<<" "<<w.z<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Torque :"<<t.x<<" "<<t.y<<" "<<t.z<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Force :"<<f.x<<" "<<f.y<<" "<<f.z<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Inertia Body:"<<IBody[0][0]<<" "<<IBody[0][1]<<" "<<IBody[0][2]<<std::endl;
    std::cout<<"             "<<IBody[1][0]<<" "<<IBody[1][1]<<" "<<IBody[1][2]<<std::endl;
    std::cout<<"             "<<IBody[2][0]<<" "<<IBody[2][1]<<" "<<IBody[2][2]<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Inertia Body (Inverse):"<<IBody_inverse[0][0]<<" "<<IBody_inverse[0][1]<<" "<<IBody_inverse[0][2]<<std::endl;
    std::cout<<"                       "<<IBody_inverse[1][0]<<" "<<IBody_inverse[1][1]<<" "<<IBody_inverse[1][2]<<std::endl;
    std::cout<<"                       "<<IBody_inverse[2][0]<<" "<<IBody_inverse[2][1]<<" "<<IBody_inverse[2][2]<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Inertia World:"<<IWorld[0][0]<<" "<<IWorld[0][1]<<" "<<IWorld[0][2]<<std::endl;
    std::cout<<"              "<<IWorld[1][0]<<" "<<IWorld[1][1]<<" "<<IWorld[1][2]<<std::endl;
    std::cout<<"              "<<IWorld[2][0]<<" "<<IWorld[2][1]<<" "<<IWorld[2][2]<<std::endl;
    std::cout<<" "<<std::endl;
    std::cout<<"Inertia World (Inverse):"<<IWorld_inverse[0][0]<<" "<<IWorld_inverse[0][1]<<" "<<IWorld_inverse[0][2]<<std::endl;
    std::cout<<"                        "<<IWorld_inverse[1][0]<<" "<<IWorld_inverse[1][1]<<" "<<IWorld_inverse[1][2]<<std::endl;
    std::cout<<"                        "<<IWorld_inverse[2][0]<<" "<<IWorld_inverse[2][1]<<" "<<IWorld_inverse[2][2]<<std::endl;
    std::cout<<" "<<std::endl;  
    std::cout<<"Rotation (Matrix) :"<<R[0][0]<<" "<<R[0][1]<<" "<<R[0][2]<<std::endl;
    std::cout<<"                 "<<R[1][0]<<" "<<R[1][1]<<" "<<R[1][2]<<std::endl;
    std::cout<<"                 "<<R[2][0]<<" "<<R[2][1]<<" "<<R[2][2]<<std::endl;
    std::cout<<" "<<std::endl;
    if(useQuat)
    {
        std::cout<<"Rotation (Quaternion) :"<<QR.w<<" "<<QR.x<<" "<<QR.y<<" "<<QR.z<<std::endl;
    }
}   

bool AABB::IntersectsPlane(Plane p)
{
    glm::vec3 q;
    float t = 0.0;

    center = max + min;
    center /= 2.0;

    extents = max - center;

    glm::vec3 N = p.getPlaneNormal();

    float r = extents.x * abs(N.x) + extents.y * abs(N.y) + extents.z * abs(N.z);

    float d = glm::dot(N,center) - glm::dot(N,p.getVertex());

    if(abs(d) <= r)
    {
        t = 0.0;
        collisionPoint = center - r * N;
        return true;
    }
    else
    {
        if(glm::dot(N,p.getVertex()) >= 0.0)
        {
            return false;
        }
        else
        {
            float tempR = d > 0.0 ? r : -r;
            t = (tempR + d) - glm::dot(N,center) / glm::dot(N,p.getVertex());
            collisionPoint = center + t * p.getVertex()  - r * N;
            return true;
        }
    }
}

void CollisionManager::CheckPlaneRBCollision(RigidBody *rb, Plane *p, int n) 
{
    AABB aabb;
    glm::vec3 N;

    aabb = rb->aabb;

    for(int j=0; j<6; j++)
    {
        N = p[j].getPlaneNormal();

        if(aabb.IntersectsPlane(p[j]))
        {
            std::cout<<"Collision - Plane "<<j<<std::endl;
            rb->setCollisionFlag(true);
            rb->setCollisionPoint(aabb.GetAABBCollisionPoint());
            rb->setCollisionNormal(glm::normalize(rb->getCollisionPoint()));
            ApplyPlaneRBImpulse(rb);
        }
        else
        {
            rb->setCollisionFlag(false);
        }
    }
}

void CollisionManager::ApplyPlaneRBImpulse(RigidBody *rb)
{
    glm::vec3 j;
    glm::vec3 JN;

    float m_inverse = 1 / rb->getMass();
    glm::vec3 x = rb->getCG();
    glm::mat3 I_inv = rb->getInvInertiaWorldMat();
    glm::vec3 collP = rb->getCollisionPoint();
    glm::vec3 N = rb->getCollisionNormal(); 

    glm::vec3 pa = getCollisionPointVelocity(rb);
    glm::vec3 ra = collP - x;
    glm::vec3 vrel = N * pa;
    glm::vec3 numerator = -(1 + epsilon) * vrel;

    glm::vec3 term1 = glm::vec3(m_inverse, m_inverse, m_inverse);
    glm::vec3 term2 = N * (glm::cross(I_inv * glm::cross(ra,N),ra));
    glm::vec3 denominator = term1 + term2;

    j = numerator / denominator;

    JN = j * N;

    //std::cout<<"Impulse Vector "<<JN.x<<" "<<JN.y<<" "<<JN.z<<std::endl;

    // Apply Impulse
    glm::vec3 P = rb->getLMomentum();
    glm::vec3 L = rb->getAMomentum();

    P += JN;
    L += glm::cross(ra,JN);

    rb->setLMomentum(P);
    rb->setAMomentum(L);
}

Answer 1

根据您对问题的描述，听起来您没有规范任何矩阵或四元数代表刚体的旋转。您定义了orthonormalize()函数，但我看到您注释掉了调用它的代码。你将需要调用该函数，你需要使它工作，否则会产生小的数值误差，并导致你的对象随着矩阵变得越来越正常而增长。

我不知道对矩阵进行标准化的最佳方法是什么，但我的方法（使用Eigen C ++库）以前工作得很好。它是这样的：

matrix normalize(const matrix & matrix)
{
    matrix result;
    result.row(0) = matrix.row(0) + matrix.row(1).cross(matrix.row(2))/10;
    result.row(1) = matrix.row(1) + matrix.row(2).cross(matrix.row(0))/10;
    result.row(2) = matrix.row(2) + matrix.row(0).cross(matrix.row(1))/10;
    result.row(0).normalize();
    result.row(1).normalize();
    result.row(2).normalize();
    return result;
}

因此，如果您的orthonormalize函数不起作用，您可以将其转换为GLM并尝试。我的好处是它平等地对待所有轴，但是蹩脚的是它有这个任意数字“10”没有数学上的理由。此外，结果将只是近似正交，但如果你的增量旋转足够小，你在每次旋转后调用此函数应该不是问题。

（在我的项目中，我切换到使用四元数并摆脱了这个功能。）

3D刚体物理（OpenGL）

1 个答案: