/**
* @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 worldPoint The point to test (in world-space coordinates)
* @return True if the point is inside the body
*/
bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the point is inside the collision shape
if (shape->testPointInside(worldPoint)) return true;
}
return false;
}
// Set the profiler
void RigidBody::setProfiler(Profiler* profiler) {
CollisionBody::setProfiler(profiler);
// Set the profiler for each proxy shape
ProxyShape* proxyShape = getProxyShapesList();
while (proxyShape != nullptr) {
proxyShape->setProfiler(profiler);
proxyShape = proxyShape->getNext();
}
}
// 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);
}
}
/**
* @param ray The ray used to raycast agains the body
* @param[out] raycastInfo Structure that contains the result of the raycasting
* (valid only if the method returned true)
* @return True if the ray hit the body and false otherwise
*/
bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the body is not active, it cannot be hit by rays
if (!mIsActive) return false;
bool isHit = false;
Ray rayTemp(ray);
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the ray hits the collision shape
if (shape->raycast(rayTemp, raycastInfo)) {
rayTemp.maxFraction = raycastInfo.hitFraction;
isHit = true;
}
}
return isHit;
}
/**
* @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);
}
}
}
/**
* @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;
}
// 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);
}
