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 PhysicsWorld::integrateVelocity( RigidBody *b, float dt )
{
if(b->inverseMass == 0.0f)
return;
b->position += b->velocity * dt;
b->rotation += b->angularVelocity * dt;
b->setRotation( b->rotation );
integrateForces( b, dt );
}
void NP_World::integrateVelocity(NP_Object* obj, float deltaTime)
{
// Position
// pos += velocity * dt
obj->getBody()->m_position += obj->getBody()->m_velocity * deltaTime;
// Orientation
// orient += angularVelocity * dt
obj->getBody()->m_orientation += obj->getBody()->m_angularVelocity * deltaTime;
obj->getBody()->setOrient(obj->getBody()->m_orientation);
//IntegrateForces(obj, deltatime)
integrateForces(obj, deltaTime);
}
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);
}
}
}
}
