/******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * * Copyright (c) 2010-2018 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * * In no event will the authors be held liable for any damages arising from the * * use of this software. * * * * Permission is granted to anyone to use this software for any purpose, * * including commercial applications, and to alter it and redistribute it * * freely, subject to the following restrictions: * * * * 1. The origin of this software must not be misrepresented; you must not claim * * that you wrote the original software. If you use this software in a * * product, an acknowledgment in the product documentation would be * * appreciated but is not required. * * * * 2. Altered source versions must be plainly marked as such, and must not be * * misrepresented as being the original software. * * * * 3. This notice may not be removed or altered from any source distribution. * * * ********************************************************************************/ // Libraries #include "DynamicsWorld.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" #include "constraint/FixedJoint.h" #include "utils/Profiler.h" #include "engine/EventListener.h" #include "engine/Island.h" #include "engine/Islands.h" #include "collision/ContactManifold.h" #include "containers/Stack.h" // Namespaces using namespace reactphysics3d; using namespace std; // Constructor /** * @param gravity Gravity vector in the world (in meters per second squared) * @param worldSettings The settings of the world * @param logger Pointer to the logger * @param profiler Pointer to the profiler */ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler) : CollisionWorld(worldSettings, logger, profiler), mIslands(mMemoryManager.getSingleFrameAllocator()), mContactSolver(mMemoryManager, mIslands, mBodyComponents, mDynamicsComponents, mProxyShapesComponents, mConfig), mConstraintSolver(mIslands, mDynamicsComponents), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) { #ifdef IS_PROFILING_ACTIVE // Set the profiler mConstraintSolver.setProfiler(mProfiler); mContactSolver.setProfiler(mProfiler); #endif RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::World, "Dynamics World: Dynamics world " + mName + " has been created"); } // Destructor DynamicsWorld::~DynamicsWorld() { // Destroy all the joints that have not been removed for (int i=mJoints.size() - 1; i >= 0; i--) { destroyJoint(mJoints[i]); } // Destroy all the rigid bodies that have not been removed for (int i=mRigidBodies.size() - 1; i >= 0; i--) { destroyRigidBody(mRigidBodies[i]); } assert(mJoints.size() == 0); assert(mRigidBodies.size() == 0); #ifdef IS_PROFILING_ACTIVE // Print the profiling report into the destinations mProfiler->printReport(); #endif RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::World, "Dynamics World: Dynamics world " + mName + " has been destroyed"); } // Update the physics simulation /** * @param timeStep The amount of time to step the simulation by (in seconds) */ void DynamicsWorld::update(decimal timeStep) { #ifdef IS_PROFILING_ACTIVE // Increment the frame counter of the profiler mProfiler->incrementFrameCounter(); #endif RP3D_PROFILE("DynamicsWorld::update()", mProfiler); mTimeStep = timeStep; // Notify the event listener about the beginning of an internal tick if (mEventListener != nullptr) mEventListener->beginInternalTick(); // Compute the collision detection mCollisionDetection.computeCollisionDetection(); // Create the islands createIslands(); // Create the actual narrow-phase contacts mCollisionDetection.createContacts(); // Integrate the velocities integrateRigidBodiesVelocities(); // Solve the contacts and constraints solveContactsAndConstraints(); // Integrate the position and orientation of each body integrateRigidBodiesPositions(); // Solve the position correction for constraints solvePositionCorrection(); // Update the state (positions and velocities) of the bodies updateBodiesState(); if (mIsSleepingEnabled) updateSleepingBodies(); // Notify the event listener about the end of an internal tick if (mEventListener != nullptr) mEventListener->endInternalTick(); // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); // Reset the islands mIslands.clear(); // Reset the single frame memory allocator mMemoryManager.resetFrameAllocator(); } // Integrate position and orientation of the rigid bodies. /// The positions and orientations of the bodies are integrated using /// the sympletic Euler time stepping scheme. void DynamicsWorld::integrateRigidBodiesPositions() { RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler); // TODO : We should loop over non-sleeping dynamic components here and not over islands // For each island of the world for (uint i=0; i < mIslands.getNbIslands(); i++) { // For each body of the island for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) { const Entity bodyEntity = mIslands.bodyEntities[i][b]; RigidBody* body = static_cast(mBodyComponents.getBody(bodyEntity)); // Get the constrained velocity Vector3 newLinVelocity = mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity); Vector3 newAngVelocity = mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity); // TODO : Remove this Vector3 testLinVel = newLinVelocity; // Add the split impulse velocity from Contact Solver (only used // to update the position) if (mContactSolver.isSplitImpulseActive()) { newLinVelocity += mDynamicsComponents.getSplitLinearVelocity(bodyEntity); newAngVelocity += mDynamicsComponents.getSplitAngularVelocity(bodyEntity); } // Get current position and orientation of the body const Vector3& currentPosition = body->mCenterOfMassWorld; const Quaternion& currentOrientation = mTransformComponents.getTransform(body->getEntity()).getOrientation(); // Update the new constrained position and orientation of the body mDynamicsComponents.setConstrainedPosition(bodyEntity, currentPosition + newLinVelocity * mTimeStep); mDynamicsComponents.setConstrainedOrientation(bodyEntity, currentOrientation + Quaternion(0, newAngVelocity) * currentOrientation * decimal(0.5) * mTimeStep); } } } // Update the postion/orientation of the bodies void DynamicsWorld::updateBodiesState() { RP3D_PROFILE("DynamicsWorld::updateBodiesState()", mProfiler); // TODO : Make sure we compute this in a system // TODO : We should loop over non-sleeping dynamic components here and not over islands // For each island of the world for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) { // For each body of the island for (uint b=0; b < mIslands.bodyEntities[islandIndex].size(); b++) { Entity bodyEntity = mIslands.bodyEntities[islandIndex][b]; RigidBody* body = static_cast(mBodyComponents.getBody(bodyEntity)); uint index = body->mArrayIndex; // Update the linear and angular velocity of the body mDynamicsComponents.setLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity)); mDynamicsComponents.setAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity)); // Update the position of the center of mass of the body body->mCenterOfMassWorld = mDynamicsComponents.getConstrainedPosition(bodyEntity); // Update the orientation of the body const Quaternion& constrainedOrientation = mDynamicsComponents.getConstrainedOrientation(bodyEntity); mTransformComponents.getTransform(bodyEntity).setOrientation(constrainedOrientation.getUnit()); // Update the transform of the body (using the new center of mass and new orientation) body->updateTransformWithCenterOfMass(); // Update the world inverse inertia tensor of the body body->updateInertiaTensorInverseWorld(); } } // Update the proxy-shapes components mCollisionDetection.updateProxyShapes(); } // Initialize the bodies velocities arrays for the next simulation step. void DynamicsWorld::initVelocityArrays() { RP3D_PROFILE("DynamicsWorld::initVelocityArrays()", mProfiler); // Allocate memory for the bodies velocity arrays uint nbBodies = mRigidBodies.size(); assert(mDynamicsComponents.getNbComponents() == nbBodies); // Initialize the map of body indexes in the velocity arrays uint i = 0; for (List::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { (*it)->mArrayIndex = i++; } } // Reset the split velocities of the bodies void DynamicsWorld::resetSplitVelocities() { for(uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { mDynamicsComponents.mSplitLinearVelocities[i].setToZero(); mDynamicsComponents.mSplitAngularVelocities[i].setToZero(); } } // Integrate the velocities of rigid bodies. /// This method only set the temporary velocities but does not update /// the actual velocitiy of the bodies. The velocities updated in this method /// might violate the constraints and will be corrected in the constraint and /// contact solver. void DynamicsWorld::integrateRigidBodiesVelocities() { RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler); // Initialize the bodies velocity arrays initVelocityArrays(); // Reset the split velocities of the bodies resetSplitVelocities(); // Integration component velocities using force/torque for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { assert(mDynamicsComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0)); assert(mDynamicsComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0)); // Integrate the external force to get the new velocity of the body mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mLinearVelocities[i] + mTimeStep * mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mExternalForces[i]; mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mAngularVelocities[i] + mTimeStep * mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i]; } // Apply gravity force for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { // If the gravity has to be applied to this rigid body if (mDynamicsComponents.mIsGravityEnabled[i] && mIsGravityEnabled) { // Integrate the gravity force mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] + mTimeStep * mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mInitMasses[i] * mGravity; } } // Apply the velocity damping // Damping force : F_c = -c' * v (c=damping factor) // Equation : m * dv/dt = -c' * v // => dv/dt = -c * v (with c=c'/m) // => dv/dt + c * v = 0 // Solution : v(t) = v0 * e^(-c * t) // => v(t + dt) = v0 * e^(-c(t + dt)) // = v0 * e^(-ct) * e^(-c * dt) // = v(t) * e^(-c * dt) // => v2 = v1 * e^(-c * dt) // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... // => e^(-x) ~ 1 - x // => v2 = v1 * (1 - c * dt) for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { const decimal linDampingFactor = mDynamicsComponents.mLinearDampings[i]; const decimal angDampingFactor = mDynamicsComponents.mAngularDampings[i]; const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep); const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep); mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] * linearDamping; mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i] * angularDamping; } } // Solve the contacts and constraints void DynamicsWorld::solveContactsAndConstraints() { RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler); // ---------- Solve velocity constraints for joints and contacts ---------- // // Initialize the contact solver mContactSolver.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, mTimeStep); // For each island of the world for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) { // If there are constraints to solve if (mIslands.joints[islandIndex].size() > 0) { // Initialize the constraint solver mConstraintSolver.initializeForIsland(mTimeStep, islandIndex); } } // For each iteration of the velocity solver for (uint i=0; i 0) { mConstraintSolver.solveVelocityConstraints(islandIndex); } } mContactSolver.solve(); } mContactSolver.storeImpulses(); } // Solve the position error correction of the constraints void DynamicsWorld::solvePositionCorrection() { RP3D_PROFILE("DynamicsWorld::solvePositionCorrection()", mProfiler); // Do not continue if there is no constraints if (mJoints.size() == 0) return; // For each island of the world for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) { // ---------- Solve the position error correction for the constraints ---------- // if (mIslands.joints[islandIndex].size() > 0) { // For each iteration of the position (error correction) solver for (uint i=0; i::max()); mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform)); mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(Vector3::zero(), Vector3::zero())); // Create the rigid body RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(RigidBody))) RigidBody(transform, *this, entity, bodyID); assert(rigidBody != nullptr); BodyComponents::BodyComponent bodyComponent(rigidBody); mBodyComponents.addComponent(entity, false, bodyComponent); // Add the rigid body to the physics world mBodies.add(rigidBody); mRigidBodies.add(rigidBody); #ifdef IS_PROFILING_ACTIVE rigidBody->setProfiler(mProfiler); #endif #ifdef IS_LOGGING_ACTIVE rigidBody->setLogger(mLogger); #endif RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(bodyID) + ": New collision body created"); // Return the pointer to the rigid body return rigidBody; } // Destroy a rigid body and all the joints which it belongs /** * @param rigidBody Pointer to the body you want to destroy */ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(rigidBody->getId()) + ": rigid body destroyed"); // Remove all the collision shapes of the body rigidBody->removeAllCollisionShapes(); // Add the body ID to the list of free IDs mFreeBodiesIds.add(rigidBody->getId()); // Destroy all the joints in which the rigid body to be destroyed is involved JointListElement* element; for (element = rigidBody->mJointsList; element != nullptr; element = element->next) { destroyJoint(element->joint); } // Destroy the corresponding entity and its components mBodyComponents.removeComponent(rigidBody->getEntity()); mTransformComponents.removeComponent(rigidBody->getEntity()); mEntityManager.destroyEntity(rigidBody->getEntity()); // Call the destructor of the rigid body rigidBody->~RigidBody(); // Remove the rigid body from the list of rigid bodies mBodies.remove(rigidBody); mRigidBodies.remove(rigidBody); // Free the object from the memory allocator mMemoryManager.release(MemoryManager::AllocationType::Pool, rigidBody, sizeof(RigidBody)); } // Create a joint between two bodies in the world and return a pointer to the new joint /** * @param jointInfo The information that is necessary to create the joint * @return A pointer to the joint that has been created in the world */ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { Joint* newJoint = nullptr; // Get the next available joint ID uint jointId = computeNextAvailableJointId(); // Allocate memory to create the new joint switch(jointInfo.type) { // Ball-and-Socket joint case JointType::BALLSOCKETJOINT: { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); newJoint = new (allocatedMemory) BallAndSocketJoint(jointId, info); break; } // Slider joint case JointType::SLIDERJOINT: { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) SliderJoint(jointId, info); break; } // Hinge joint case JointType::HINGEJOINT: { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) HingeJoint(jointId, info); break; } // Fixed joint case JointType::FIXEDJOINT: { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) FixedJoint(jointId, info); break; } default: { assert(false); return nullptr; } } // If the collision between the two bodies of the constraint is disabled if (!jointInfo.isCollisionEnabled) { // Add the pair of bodies in the set of body pairs that cannot collide with each other mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2); } // Add the joint into the world mJoints.add(newJoint); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint, "Joint " + std::to_string(newJoint->getId()) + ": New joint created"); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint, "Joint " + std::to_string(newJoint->getId()) + ": " + newJoint->to_string()); // Add the joint into the joint list of the bodies involved in the joint addJointToBody(newJoint); // Return the pointer to the created joint return newJoint; } // Destroy a joint /** * @param joint Pointer to the joint you want to destroy */ void DynamicsWorld::destroyJoint(Joint* joint) { assert(joint != nullptr); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint, "Joint " + std::to_string(joint->getId()) + ": joint destroyed"); // If the collision between the two bodies of the constraint was disabled if (!joint->isCollisionEnabled()) { // Remove the pair of bodies from the set of body pairs that cannot collide with each other mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2()); } // Wake up the two bodies of the joint joint->getBody1()->setIsSleeping(false); joint->getBody2()->setIsSleeping(false); // Remove the joint from the world mJoints.remove(joint); // Remove the joint from the joint list of the bodies involved in the joint joint->mBody1->removeJointFromJointsList(mMemoryManager, joint); joint->mBody2->removeJointFromJointsList(mMemoryManager, joint); size_t nbBytes = joint->getSizeInBytes(); // Add the joint ID to the list of free IDs mFreeJointsIDs.add(joint->getId()); // Call the destructor of the joint joint->~Joint(); // Add the joint ID to the list of free IDs mFreeJointsIDs.add(joint->getId()); // Release the allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, joint, nbBytes); } // Add the joint to the list of joints of the two bodies involved in the joint void DynamicsWorld::addJointToBody(Joint* joint) { assert(joint != nullptr); // Add the joint at the beginning of the linked list of joints of the first body void* allocatedMemory1 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(JointListElement)); JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, joint->mBody1->mJointsList); joint->mBody1->mJointsList = jointListElement1; RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(joint->mBody1->getId()) + ": Joint " + std::to_string(joint->getId()) + " added to body"); // Add the joint at the beginning of the linked list of joints of the second body void* allocatedMemory2 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(JointListElement)); JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, joint->mBody2->mJointsList); joint->mBody2->mJointsList = jointListElement2; RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(joint->mBody2->getId()) + ": Joint " + std::to_string(joint->getId()) + " added to body"); } // Return the next available joint Id uint DynamicsWorld::computeNextAvailableJointId() { // Compute the joint ID uint jointId; if (mFreeJointsIDs.size() != 0) { jointId = mFreeJointsIDs[mFreeJointsIDs.size() - 1]; mFreeJointsIDs.removeAt(mFreeJointsIDs.size() - 1); } else { jointId = mCurrentJointId; mCurrentJointId++; } return jointId; } // Compute the islands using potential contacts and joints /// We compute the islands before creating the actual contacts here because we want all /// the contact manifolds and contact points of the same island /// to be packed together into linear arrays of manifolds and contacts for better caching. /// An island is an isolated group of rigid bodies that have constraints (joints or contacts) /// between each other. This method computes the islands at each time step as follows: For each /// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body /// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to /// find all the bodies that are connected with it (the bodies that share joints or contacts with /// it). Then, we create an island with this group of connected bodies. void DynamicsWorld::createIslands() { // TODO : Check if we handle kinematic bodies correctly in islands creation // list of contact pairs involving a non-rigid body List nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator()); RP3D_PROFILE("DynamicsWorld::createIslands()", mProfiler); // Reset all the isAlreadyInIsland variables of bodies and joints for (uint b=0; b < mDynamicsComponents.getNbComponents(); b++) { mDynamicsComponents.mIsAlreadyInIsland[b] = false; } for (List::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) { (*it)->mIsAlreadyInIsland = false; } // Create a stack for the bodies to visit during the Depth First Search Stack bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator()); uint nbTotalManifolds = 0; // For each dynamic component // TODO : Here we iterate on dynamic component where we can have static, kinematic and dynamic bodies. Maybe we should // not use a dynamic component for a static body. for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) { // If the body has already been added to an island, we go to the next body if (mDynamicsComponents.mIsAlreadyInIsland[b]) continue; // If the body is static, we go to the next body // TODO : Do not use pointer to rigid body here (maybe move getType() into a component) CollisionBody* body = static_cast(mBodyComponents.getBody(mDynamicsComponents.mBodies[b])); if (body->getType() == BodyType::STATIC) continue; // Reset the stack of bodies to visit bodyEntityIndicesToVisit.clear(); // Add the body into the stack of bodies to visit mDynamicsComponents.mIsAlreadyInIsland[b] = true; bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]); // Create the new island uint32 islandIndex = mIslands.addIsland(nbTotalManifolds); // While there are still some bodies to visit in the stack while (bodyEntityIndicesToVisit.size() > 0) { // Get the next body to visit from the stack const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop(); // Add the body into the island mIslands.bodyEntities[islandIndex].add(bodyToVisitEntity); // If the current body is static, we do not want to perform the DFS // search across that body // TODO : Do not use pointer to rigid body here (maybe move getType() into a component) RigidBody* rigidBodyToVisit = static_cast(mBodyComponents.getBody(bodyToVisitEntity)); // Awake the body if it is sleeping rigidBodyToVisit->setIsSleeping(false); if (rigidBodyToVisit->getType() == BodyType::STATIC) continue; // If the body is involved in contacts with other bodies auto itBodyContactPairs = mCollisionDetection.mMapBodyToContactPairs.find(bodyToVisitEntity); if (itBodyContactPairs != mCollisionDetection.mMapBodyToContactPairs.end()) { // For each contact pair in which the current body is involded List& contactPairs = itBodyContactPairs->second; for (uint p=0; p < contactPairs.size(); p++) { ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]]; assert(pair.potentialContactManifoldsIndices.size() > 0); // Check if the current contact pair has already been added into an island if (pair.isAlreadyInIsland) continue; // Get the other body of the contact manifold // TODO : Maybe avoid those casts here RigidBody* body1 = dynamic_cast(mBodyComponents.getBody(pair.body1Entity)); RigidBody* body2 = dynamic_cast(mBodyComponents.getBody(pair.body2Entity)); // If the colliding body is a RigidBody (and not a CollisionBody instead) if (body1 != nullptr && body2 != nullptr) { nbTotalManifolds += pair.potentialContactManifoldsIndices.size(); // Add the pair into the list of pair to process to create contacts mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex); // Add the contact manifold into the island mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size(); pair.isAlreadyInIsland = true; const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity; // Check if the other body has already been added to the island if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; // Insert the other body into the stack of bodies to visit bodyEntityIndicesToVisit.push(otherBodyEntity); mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true); } else { // Add the contact pair index in the list of contact pairs that won't be part of islands nonRigidBodiesContactPairs.add(pair.contactPairIndex); pair.isAlreadyInIsland = true; } } } // For each joint in which the current body is involved JointListElement* jointElement; for (jointElement = rigidBodyToVisit->mJointsList; jointElement != nullptr; jointElement = jointElement->next) { Joint* joint = jointElement->joint; // Check if the current joint has already been added into an island if (joint->isAlreadyInIsland()) continue; // Add the joint into the island mIslands.joints[islandIndex].add(joint); joint->mIsAlreadyInIsland = true; const Entity body1Entity = joint->getBody1()->getEntity(); const Entity body2Entity = joint->getBody2()->getEntity(); const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity; // Check if the other body has already been added to the island if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue; // Insert the other body into the stack of bodies to visit bodyEntityIndicesToVisit.push(otherBodyEntity); mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true); } } // Reset the isAlreadyIsland variable of the static bodies so that they // can also be included in the other islands for (uint j=0; j < mDynamicsComponents.getNbEnabledComponents(); j++) { // If the body is static, we go to the next body // TODO : Do not use pointer to rigid body here (maybe move getType() into a component) CollisionBody* body = static_cast(mBodyComponents.getBody(mDynamicsComponents.mBodies[j])); if (body->getType() == BodyType::STATIC) { mDynamicsComponents.mIsAlreadyInIsland[j] = false; } } } // Add the contact pairs that are not part of islands at the end of the array of pairs for contacts creations mCollisionDetection.mContactPairsIndicesOrderingForContacts.addRange(nonRigidBodiesContactPairs); assert(mCollisionDetection.mCurrentContactPairs->size() == mCollisionDetection.mContactPairsIndicesOrderingForContacts.size()); mCollisionDetection.mMapBodyToContactPairs.clear(true); } // Put bodies to sleep if needed. /// For each island, if all the bodies have been almost still for a long enough period of /// time, we put all the bodies of the island to sleep. void DynamicsWorld::updateSleepingBodies() { RP3D_PROFILE("DynamicsWorld::updateSleepingBodies()", mProfiler); const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; // For each island of the world for (uint i=0; i(mBodyComponents.getBody(bodyEntity)); // Skip static bodies if (body->getType() == BodyType::STATIC) continue; // If the body is velocity is large enough to stay awake if (mDynamicsComponents.getLinearVelocity(bodyEntity).lengthSquare() > sleepLinearVelocitySquare || mDynamicsComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare || !body->isAllowedToSleep()) { // Reset the sleep time of the body body->mSleepTime = decimal(0.0); minSleepTime = decimal(0.0); } else { // If the body velocity is below the sleeping velocity threshold // Increase the sleep time body->mSleepTime += mTimeStep; if (body->mSleepTime < minSleepTime) { minSleepTime = body->mSleepTime; } } } // If the velocity of all the bodies of the island is under the // sleeping velocity threshold for a period of time larger than // the time required to become a sleeping body if (minSleepTime >= mTimeBeforeSleep) { // Put all the bodies of the island to sleep for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) { const Entity bodyEntity = mIslands.bodyEntities[i][b]; CollisionBody* body = static_cast(mBodyComponents.getBody(bodyEntity)); body->setIsSleeping(true); } } } } // Enable/Disable the sleeping technique. /// The sleeping technique is used to put bodies that are not moving into sleep /// to speed up the simulation. /** * @param isSleepingEnabled True if you want to enable the sleeping technique * and false otherwise */ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { mIsSleepingEnabled = isSleepingEnabled; if (!mIsSleepingEnabled) { // For each body of the world List::Iterator it; for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { // Wake up the rigid body (*it)->setIsSleeping(false); } } RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::World, "Dynamics World: isSleepingEnabled=" + (isSleepingEnabled ? std::string("true") : std::string("false")) ); } // Return the list of all contacts of the world /** * @return A pointer to the first contact manifold in the linked-list of manifolds */ List DynamicsWorld::getContactsList() { List contactManifolds(mMemoryManager.getPoolAllocator()); // TODO : Rework how to report contacts /* // For each currently overlapping pair of bodies for (auto it = mCollisionDetection.mOverlappingPairs.begin(); it != mCollisionDetection.mOverlappingPairs.end(); ++it) { OverlappingPair* pair = it->second; // For each contact manifold of the pair const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); ContactManifold* manifold = manifoldSet.getContactManifolds(); while (manifold != nullptr) { // Get the contact manifold contactManifolds.add(manifold); manifold = manifold->getNext(); } } */ // Return all the contact manifold return contactManifolds; }