void RigidBody::findContacts(float ifps) {
static vec3 old_velicity;
static vec3 old_angularMomentum;
static vec3 old_angularVelocity;
static Position old_pos;
static mat3 old_orientation;
static mat3 old_iWorldInertiaTensor;
old_velicity = velocity;
old_angularMomentum = angularMomentum;
old_angularVelocity = angularVelocity;
old_pos = pos;
old_orientation = orienation;
old_iWorldInertiaTensor = iWorldInertiaTensor;
// predicted new position
integrateVelocity(ifps);
integratePos(ifps);
// find conact points
if(collide_type == COLLIDE_MESH) collide->collide(object);
else if(collide_type == COLLIDE_SPHERE) collide->collide(object,object->pos,object->getRadius());
velocity = old_velicity; // restore old values
angularMomentum = old_angularMomentum;
angularVelocity = old_angularVelocity;
pos = old_pos;
orienation = old_orientation;
iWorldInertiaTensor = old_iWorldInertiaTensor;
}
void NP_World::update(float deltaTime)
{
for (size_t i = 0; i < m_objects.size(); ++i)
m_objects[i]->update(deltaTime);
//TODO:
// FOR EACH STEP DO THESE THINGS:
//Generate collision info
contacts.clear();
for (size_t i = 0; i < m_objects.size(); ++i)
{
NP_Body* A = m_objects[i]->getBody();
for (size_t j = 0; j < m_objects.size(); ++j)
{
NP_Body* B = m_objects[j]->getBody();
if (A->inverseMass == 0 && B->inverseMass == 0)
continue;
NP_CollisionInfo cI(A, B);
updateOrientation(m_objects[i]);
updateOrientation(m_objects[j]);
cI.Solve(); //Do collision check
if (cI.contact_count)
contacts.emplace_back(cI);
}
}
// Integrate forces
// - Go through objects - calc forces (calc acceleration)
for (size_t i = 0; i < m_objects.size(); ++i)
integrateForces(m_objects[i], deltaTime);
// Init collision
//for (size_t i = 0; i < contacts.size(); ++i)
//{
// contacts[i].Initialize();
//}
//Solve collisions - apply impulse
for (size_t i = 0; i < contacts.size(); ++i)
{
contacts[i].ApplyImpulse();
}
// Integrate velocities
for (size_t i = 0; i < m_objects.size(); ++i)
integrateVelocity(m_objects[i], deltaTime);
for (size_t i = 0; i < m_objects.size(); ++i)
{
m_objects[i]->getBody()->m_force = glm::vec2(0, 0);
m_objects[i]->getBody()->m_torque = 0;
}
}
void stepSimulation(double deltaTime)
{
integrateVelocity(deltaTime);
}
void ftPhysicsSystem::step(real dt)
{
integrateVelocity(dt);
auto updateColSystem = [this](ftBody *body) -> void {
for (auto collider = body->colliders; collider != nullptr; collider = collider->next)
{
this->m_collisionSystem.moveShape(collider->collisionHandle, body->transform * collider->transform);
}
};
m_dynamicBodies.forEach(std::cref(updateColSystem));
m_kinematicBodies.forEach(std::cref(updateColSystem));
auto colFilter = [](void *userdataA, void *userdataB) -> bool {
ftCollider *colliderA = (ftCollider *)userdataA;
ftCollider *colliderB = (ftCollider *)userdataB;
ftBody *bodyA = colliderA->body;
ftBody *bodyB = colliderB->body;
if (bodyA == bodyB)
return false;
if (colliderA->group != 0 && colliderA->group == colliderB->group)
return false;
int maskA = colliderA->mask;
int maskB = colliderB->mask;
int categoryA = colliderA->category;
int categoryB = colliderB->category;
if (!(maskA & categoryB))
return false;
if (!(maskB & categoryA))
return false;
return true;
};
ftCollisionCallback callback;
callback.beginContact = &beginContactListener;
callback.updateContact = &updateContactListener;
callback.endContact = &endContactListener;
callback.data = &m_islandSystem;
m_collisionSystem.updateContacts(colFilter, callback);
auto processIsland =
[this, dt](const ftIsland &island) -> void {
bool allSleeping = true;
const ftChunkArray<ftBody *> *bodies = &(island.bodies);
for (uint32 i = 0; i < island.bodies.getSize(); ++i)
{
ftBody *body = (*bodies)[i];
bool isStatic = body->bodyType == STATIC;
bool isKinematic = body->bodyType == KINEMATIC;
bool wantSleep = body->sleepTimer > m_sleepTimeLimit;
if (isKinematic || (!isStatic && !wantSleep))
{
allSleeping = false;
break;
}
}
if (allSleeping)
{
for (uint32 i = 0; i < island.bodies.getSize(); ++i)
{
ftBody *body = (*bodies)[i];
if (body->bodyType == DYNAMIC && body->activationState != SLEEP)
{
body->activationState = SLEEP;
body->velocity.setZero();
body->angularVelocity = 0;
this->m_dynamicBodies.unlink(body);
this->m_sleepingBodies.insert(body);
}
}
}
else
{
for (uint32 i = 0; i < island.bodies.getSize(); ++i)
{
ftBody *body = (*bodies)[i];
if (body->activationState == SLEEP)
{
body->activationState = ACTIVE;
body->sleepTimer = 0;
this->m_sleepingBodies.unlink(body);
this->m_dynamicBodies.insert(body);
}
}
this->m_contactSolver.createConstraints(island);
this->m_contactSolver.preSolve(dt);
this->m_contactSolver.warmStart();
this->m_contactSolver.solve(dt);
this->m_contactSolver.clearConstraints();
}
};
m_islandSystem.buildAndProcessIsland(std::cref(processIsland));
updateBodiesActivation(dt);
integratePosition(dt);
}
void PhysicsWorld::step()
{
int typeCount = 0;
for (auto b : bodies)
{
if (b->getType() > typeCount)
typeCount = b->getType();
}
for (int curType = 0; curType <= typeCount; curType++)
{
for (auto b : bodies)
{
if (b->getType() >= curType)
continue;
b->velocity_ = b->velocity;
b->velocity.x = b->velocity.y = 0.f;
b->angularVelocity_ = b->angularVelocity;
b->angularVelocity = 0.f;
b->mass = 0.f;
b->inverseMass = 0.f;
b->inertia = 0.f;
b->inverseInertia = 0.f;
}
// Generate new collision info
contacts.clear();
for(sf::Uint32 i = 0; i < bodies.size(); ++i)
{
if (bodies[i]->getType() > curType)
continue;
RigidBody* A = bodies[i];
sf::FloatRect ABounds = A->getShape()->getLocalBounds();
ABounds.left += A->position.x;
ABounds.top += A->position.y;
A->oldPosition = A->position;
for(sf::Uint32 j = i + 1; j < bodies.size(); ++j)
{
if (bodies[j]->getType() > curType)
continue;
RigidBody* B = bodies[j];
sf::FloatRect BBounds = B->getShape()->getLocalBounds();
BBounds.left += B->position.x;
BBounds.top += B->position.y;
if (!ABounds.intersects(BBounds))
continue;
if(A->inverseMass == 0 && B->inverseMass == 0)
continue;
Collision c(A, B, dt, sf::Vector2f(0, 40));
c.solve();
if(c.hasCollision())
{
contacts.emplace_back(c);
}
}
}
// Integrate forces
for(auto b : bodies)
{
if (b->getType() > curType)
continue;
integrateForces(b, dt);
}
// Initialize collision
for(auto& contact : contacts)
{
contact.initialize();
}
// Solve collisions
for(sf::Uint32 j = 0; j < iterations; ++j)
for(auto& contact : contacts)
contact.applyImpulse();
// Solve constraints
for (auto constraint : constraints)
constraint->solve(dt);
// Integrate velocities
for(auto b : bodies)
{
if (b->getType() > curType)
continue;
integrateVelocity(b, dt);
}
// Correct positions
for(auto& contact : contacts)
contact.positionalCorrection();
// Clear all forces
for(auto b : bodies)
{
if (b->getType() > curType)
continue;
b->force.x = b->force.y = 0;
b->torque = 0;
}
for(auto b : bodies)
{
if (b->getType() >= curType)
continue;
b->velocity = b->velocity_;
b->angularVelocity = b->angularVelocity_;
b->mass = b->mass_;
b->inverseMass = b->inverseMass_;
b->inertia = b->inertia_;
b->inverseInertia = b->inverseInertia_;
}
for (auto b : bodies)
{
if (b->getType() != curType)
continue;
std::stack<RigidBody*> pstack;
for (auto child : b->children)
pstack.push(child);
while (!pstack.empty())
{
RigidBody* current = pstack.top();
pstack.pop();
current->position += b->position-b->oldPosition;
for (auto child : current->children)
pstack.push(child);
}
}
}
}
void ElysiumEngine::RigidBody::updateAndIntegrate(float dt)
{
updateAcceleration();
integrateVelocity(dt);
}
