3D физика твердого тела (OpenGL)

Я пытаюсь построить физический двигатель твердого тела. Я использую библиотеку glm в качестве моей стандартной математической библиотеки.

Проблемы, с которыми я сталкиваюсь, заключаются в следующем,

• Когда я вычисляю крутящий момент по отношению к силе (см. Функцию calcTorque ()), тело искажается и искажается до такой степени, что оно вращается и увеличивается в размерах и больше не видно. Но когда я убираю силу, из уравнения крутящего момента, которое я просто рандомизирую (см. CalculateForces ()), значения крутящего момента работают нормально.
• Я думаю, что при вращении тело слегка искажается и увеличивается в размерах как для кватернионов, так и для матриц. (Не уверен, что вычисление инерции верно, тело — это куб)
• У меня также возникают проблемы с реакцией на столкновение, тело постепенно проникает сквозь поверхность и не реагирует должным образом на величину эпсилона.

П.С .: Столкновение AABB — плоскость реализовано с помощью книги Кристера Эриксона «Обнаружение столкновений в реальном времени», а физика твердого тела — с помощью реализации Брэффа.

Вот мой код

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