3D刚体物理(OpenGL）

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

我面临的问题如下，

• 当我计算相对于力的扭矩时(请参见函数calculateTorque())，物体会扭曲和偏斜到一定程度，在这种程度上，它会旋转并增大尺寸，不再可见。但是，当我去掉力时，从扭矩方程中，我只是随机化(参见calculateForces())扭矩值，它工作得很好
• 当旋转时，我认为无论是四元数还是矩阵，物体都会倾斜并略微增大。(不确定惯性计算是否正确，物体是一个立方体)
• Am在碰撞响应方面也存在问题，物体逐渐穿透表面，并且对ε的值没有做出正确的反应

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