Я пытаюсь построить физический двигатель твердого тела. Я использую библиотеку glm в качестве моей стандартной математической библиотеки.
Проблемы, с которыми я сталкиваюсь, заключаются в следующем,
П.С .: Столкновение 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);
}
Из вашего описания проблемы звучит, как будто вы не нормализуете матрицу или кватернион, представляющие вращение твердого тела. У тебя есть 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 и попробовать, если ваша функция orthonormalize не работает. Приятно, что у меня все одинаково обрабатывается по всем осям, но, к сожалению, у него есть произвольное число «10», которое не имеет математического обоснования. Кроме того, результат будет только приблизительно ортонормированным, но если ваши дополнительные вращения достаточно малы, и вы вызываете эту функцию после каждого вращения, что не должно быть проблемой.
(В моем проекте я переключился на использование кватернионов и избавился от этой функции.)
Других решений пока нет …