0

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

我面临的问题如下，

  • 当我计算相对于力的扭矩时（参见函数calculateTorque()），身体会扭曲和倾斜到一定程度，它会旋转并增大尺寸并且不再可见。但是，当我从扭矩方程中移除力时，我只是随机化（参见 calculateForces()）扭矩值，它工作正常。
  • 旋转时，我认为对于四元数和矩阵来说，身体会发生倾斜并且尺寸会略微增加。（不确定惯性计算是否正确，body是立方体）
  • 我的碰撞响应也有问题，身体逐渐穿透表面，并且相对于 epsilon 的值没有适当的反应。

PS：AABB - 平面碰撞是借助 Christer Ericson 的 Real Time Collision Detection 一书实现的，刚体物理是借助 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);
}
4

1 回答 1

0

根据您对问题的描述，听起来您没有对表示刚体旋转的任何矩阵或四元数进行归一化。您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;
}

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

（在我的项目中，我改用四元数并去掉了这个函数。）

于 2012-10-06T17:21:15.420 回答