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