/** * @param isActive True if you want to activate the body */ void CollisionBody::setIsActive(bool isActive) { // If the state does not change if (mIsActive == isActive) return; Body::setIsActive(isActive); // If we have to activate the body if (isActive) { // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { // Compute the world-space AABB of the new collision shape AABB aabb; shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform); // Add the proxy shape to the collision detection mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb); } } else { // If we have to deactivate the body // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { // Remove the proxy shape from the collision detection mWorld.mCollisionDetection.removeProxyCollisionShape(shape); } // Reset the contact manifold list of the body resetContactManifoldsList(); } }
/** * @param collisionShape The collision shape you want to add to the body * @param transform The transformation of the collision shape that transforms the * local-space of the collision shape into the local-space of the body * @param mass Mass (in kilograms) of the collision shape you want to add * @return A pointer to the proxy shape that has been created to link the body to * the new collision shape you have added. */ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, const Transform& transform, decimal mass) { // Create a new proxy collision shape to attach the collision shape to the body ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, mass, mWorld.mMemoryManager); #ifdef IS_PROFILING_ACTIVE // Set the profiler proxyShape->setProfiler(mProfiler); #endif #ifdef IS_LOGGING_ACTIVE // Set the logger proxyShape->setLogger(mLogger); #endif // Add it to the list of proxy collision shapes of the body if (mProxyCollisionShapes == nullptr) { mProxyCollisionShapes = proxyShape; } else { proxyShape->mNext = mProxyCollisionShapes; mProxyCollisionShapes = proxyShape; } // Compute the world-space AABB of the new collision shape AABB aabb; collisionShape->computeAABB(aabb, mTransform * transform); // Notify the collision detection about this new collision shape mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); mNbCollisionShapes++; // Recompute the center of mass, total mass and inertia tensor of the body with the new // collision shape recomputeMassInformation(); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mID) + ": Proxy shape " + std::to_string(proxyShape->getBroadPhaseId()) + " added to body"); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape, "ProxyShape " + std::to_string(proxyShape->getBroadPhaseId()) + ": collisionShape=" + proxyShape->getCollisionShape()->to_string()); // Return a pointer to the proxy collision shape return proxyShape; }
// Update the broad-phase state for this body (because it has moved for instance) void CollisionBody::updateBroadPhaseState() const { // For all the proxy collision shapes of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { // Recompute the world-space AABB of the collision shape AABB aabb; shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform()); // Update the broad-phase state for the proxy collision shape mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb); } }
/** * @return The axis-aligned bounding box (AABB) of the body in world-space coordinates */ AABB CollisionBody::getAABB() const { AABB bodyAABB; if (mProxyCollisionShapes == NULL) return bodyAABB; mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform()); // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) { // Compute the world-space AABB of the collision shape AABB aabb; shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform()); // Merge the proxy shape AABB with the current body AABB bodyAABB.mergeWithAABB(aabb); } return bodyAABB; }
// Update the broad-phase state for this body (because it has moved for instance) void RigidBody::updateBroadPhaseState() const { RP3D_PROFILE("RigidBody::updateBroadPhaseState()", mProfiler); DynamicsWorld& world = static_cast<DynamicsWorld&>(mWorld); const Vector3 displacement = world.mTimeStep * mLinearVelocity; // For all the proxy collision shapes of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { // If the proxy-shape shape is still part of the broad-phase if (shape->getBroadPhaseId() != -1) { // Recompute the world-space AABB of the collision shape AABB aabb; shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform()); // Update the broad-phase state for the proxy collision shape mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement); } } }
// Recompute the center of mass, total mass and inertia tensor of the body using all // the collision shapes attached to the body. void RigidBody::recomputeMassInformation() { mInitMass = decimal(0.0); mMassInverse = decimal(0.0); if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero(); if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero(); if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero(); Matrix3x3 inertiaTensorLocal; inertiaTensorLocal.setToZero(); // If it is a STATIC or a KINEMATIC body if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) { mCenterOfMassWorld = mTransform.getPosition(); return; } assert(mType == BodyType::DYNAMIC); // Compute the total mass of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { mInitMass += shape->getMass(); if (!mIsCenterOfMassSetByUser) { mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); } } if (mInitMass > decimal(0.0)) { mMassInverse = decimal(1.0) / mInitMass; } else { mCenterOfMassWorld = mTransform.getPosition(); return; } // Compute the center of mass const Vector3 oldCenterOfMass = mCenterOfMassWorld; if (!mIsCenterOfMassSetByUser) { mCenterOfMassLocal *= mMassInverse; } mCenterOfMassWorld = mTransform * mCenterOfMassLocal; if (!mIsInertiaTensorSetByUser) { // Compute the inertia tensor using all the collision shapes for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { // Get the inertia tensor of the collision shape in its local-space Matrix3x3 inertiaTensor; shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); // Convert the collision shape inertia tensor into the local-space of the body const Transform& shapeTransform = shape->getLocalToBodyTransform(); Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape // center into a inertia tensor w.r.t to the body origin. Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; decimal offsetSquare = offset.lengthSquare(); Matrix3x3 offsetMatrix; offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); offsetMatrix[0] += offset * (-offset.x); offsetMatrix[1] += offset * (-offset.y); offsetMatrix[2] += offset * (-offset.z); offsetMatrix *= shape->getMass(); inertiaTensorLocal += inertiaTensor + offsetMatrix; } // Compute the local inverse inertia tensor mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); } // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); }