diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 642264fe..67806187 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -39,8 +39,8 @@ using namespace reactphysics3d; * @param world The physics world where the body is created * @param id ID of the body */ -CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id) - : Body(entity, id), mType(BodyType::DYNAMIC), mTransform(transform), mProxyCollisionShapes(nullptr), +CollisionBody::CollisionBody(CollisionWorld& world, Entity entity, bodyindex id) + : Body(entity, id), mType(BodyType::DYNAMIC), mProxyCollisionShapes(nullptr), mNbCollisionShapes(0), mContactManifoldsList(nullptr), mWorld(world) { #ifdef IS_PROFILING_ACTIVE @@ -105,7 +105,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, // Compute the world-space AABB of the new collision shape AABB aabb; - collisionShape->computeAABB(aabb, mTransform * transform); + collisionShape->computeAABB(aabb, mWorld.mTransformComponents.getTransform(mEntity) * transform); // Notify the collision detection about this new collision shape mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); @@ -218,6 +218,18 @@ void CollisionBody::resetContactManifoldsList() { mContactManifoldsList = nullptr; } +// Return the current position and orientation +/** + * @return The current transformation of the body that transforms the local-space + * of the body into world-space + */ +const Transform& CollisionBody::getTransform() const { + + // TODO : Make sure we do not call this method from the internal physics engine + + return mWorld.mTransformComponents.getTransform(mEntity); +} + // Update the broad-phase state for this body (because it has moved for instance) void CollisionBody::updateBroadPhaseState() const { @@ -232,11 +244,13 @@ void CollisionBody::updateBroadPhaseState() const { // Update the broad-phase state of a proxy collision shape of the body void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const { + const Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); + if (proxyShape->getBroadPhaseId() != -1) { // Recompute the world-space AABB of the collision shape AABB aabb; - proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + proxyShape->getCollisionShape()->computeAABB(aabb, transform * proxyShape->getLocalToBodyTransform()); // Update the broad-phase state for the proxy collision shape mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert) ; @@ -257,12 +271,14 @@ void CollisionBody::setIsActive(bool isActive) { // If we have to activate the body if (isActive) { + const Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); + // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { // Compute the world-space AABB of the new collision shape AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform); + shape->getCollisionShape()->computeAABB(aabb, transform * shape->mLocalToBodyTransform); // Add the proxy shape to the collision detection mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb); @@ -377,14 +393,18 @@ AABB CollisionBody::getAABB() const { if (mProxyCollisionShapes == nullptr) return bodyAABB; - mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform()); + // TODO : Make sure we compute this in a system + + const Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); + + mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, transform * mProxyCollisionShapes->getLocalToBodyTransform()); // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != nullptr; shape = shape->mNext) { // Compute the world-space AABB of the collision shape AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform()); + shape->getCollisionShape()->computeAABB(aabb, transform * shape->getLocalToBodyTransform()); // Merge the proxy shape AABB with the current body AABB bodyAABB.mergeWithAABB(aabb); @@ -401,16 +421,52 @@ AABB CollisionBody::getAABB() const { void CollisionBody::setTransform(const Transform& transform) { // Update the transform of the body - mTransform = transform; + mWorld.mTransformComponents.setTransform(mEntity, transform); // Update the broad-phase state of the body updateBroadPhaseState(); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(mID) + ": Set transform=" + mTransform.to_string()); + "Body " + std::to_string(mID) + ": Set transform=" + transform.to_string()); } +// Return the world-space coordinates of a point given the local-space coordinates of the body +/** +* @param localPoint A point in the local-space coordinates of the body +* @return The point in world-space coordinates +*/ +Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const { + return mWorld.mTransformComponents.getTransform(mEntity) * localPoint; +} + +// Return the world-space vector of a vector given in local-space coordinates of the body +/** +* @param localVector A vector in the local-space coordinates of the body +* @return The vector in world-space coordinates +*/ +Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const { + return mWorld.mTransformComponents.getTransform(mEntity).getOrientation() * localVector; +} + +// Return the body local-space coordinates of a point given in the world-space coordinates +/** +* @param worldPoint A point in world-space coordinates +* @return The point in the local-space coordinates of the body +*/ +Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const { + return mWorld.mTransformComponents.getTransform(mEntity).getInverse() * worldPoint; +} + +// Return the body local-space coordinates of a vector given in the world-space coordinates +/** +* @param worldVector A vector in world-space coordinates +* @return The vector in the local-space coordinates of the body +*/ +Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const { + return mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getInverse() * worldVector; +} + // Set the type of the body /// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow: /// STATIC : A static body has infinite mass, zero velocity but the position can be diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 19f55594..c298999d 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -70,9 +70,6 @@ class CollisionBody : public Body { /// Type of body (static, kinematic or dynamic) BodyType mType; - /// Position and orientation of the body - Transform mTransform; - /// First element of the linked list of proxy collision shapes of this body ProxyShape* mProxyCollisionShapes; @@ -118,7 +115,7 @@ class CollisionBody : public Body { // -------------------- Methods -------------------- // /// Constructor - CollisionBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id); + CollisionBody(CollisionWorld& world, Entity entity, bodyindex id); /// Destructor virtual ~CollisionBody() override; @@ -209,15 +206,6 @@ inline BodyType CollisionBody::getType() const { return mType; } -// Return the current position and orientation -/** - * @return The current transformation of the body that transforms the local-space - * of the body into world-space - */ -inline const Transform& CollisionBody::getTransform() const { - return mTransform; -} - // Return the first element of the linked list of contact manifolds involving this body /** * @return A pointer to the first element of the linked-list with the contact @@ -245,42 +233,6 @@ inline const ProxyShape* CollisionBody::getProxyShapesList() const { return mProxyCollisionShapes; } -// Return the world-space coordinates of a point given the local-space coordinates of the body -/** -* @param localPoint A point in the local-space coordinates of the body -* @return The point in world-space coordinates -*/ -inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const { - return mTransform * localPoint; -} - -// Return the world-space vector of a vector given in local-space coordinates of the body -/** -* @param localVector A vector in the local-space coordinates of the body -* @return The vector in world-space coordinates -*/ -inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const { - return mTransform.getOrientation() * localVector; -} - -// Return the body local-space coordinates of a point given in the world-space coordinates -/** -* @param worldPoint A point in world-space coordinates -* @return The point in the local-space coordinates of the body -*/ -inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const { - return mTransform.getInverse() * worldPoint; -} - -// Return the body local-space coordinates of a vector given in the world-space coordinates -/** -* @param worldVector A vector in world-space coordinates -* @return The vector in the local-space coordinates of the body -*/ -inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const { - return mTransform.getOrientation().getInverse() * worldVector; -} - /// Test if the collision body overlaps with a given AABB /** * @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index a9d31746..92eabbf0 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -40,7 +40,7 @@ using namespace reactphysics3d; * @param id The ID of the body */ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id) - : CollisionBody(transform, world, entity, id), mArrayIndex(0), mInitMass(decimal(1.0)), + : CollisionBody(world, entity, id), mArrayIndex(0), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mMaterial(world.mConfig), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { @@ -186,7 +186,7 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { mCenterOfMassLocal = centerOfMassLocal; // Compute the center of mass in world-space coordinates - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + mCenterOfMassWorld = mWorld.mTransformComponents.getTransform(mEntity) * mCenterOfMassLocal; // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); @@ -247,6 +247,20 @@ void RigidBody::removeJointFromJointsList(MemoryManager& memoryManager, const Jo } } +// Update the world inverse inertia tensor of the body +/// The inertia tensor I_w in world coordinates is computed with the +/// local inverse inertia tensor I_b^-1 in body coordinates +/// by I_w = R * I_b^-1 * R^T +/// where R is the rotation matrix (and R^T its transpose) of the +/// current orientation quaternion of the body +void RigidBody::updateInertiaTensorInverseWorld() { + + // TODO : Make sure we do this in a system + + Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix(); + mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose(); +} + // Add a collision shape to the body. /// When you add a collision shape to the body, an internal copy of this /// collision shape will be created internally. Therefore, you can delete it @@ -297,7 +311,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, // Compute the world-space AABB of the new collision shape AABB aabb; - collisionShape->computeAABB(aabb, mTransform * transform); + collisionShape->computeAABB(aabb, mWorld.mTransformComponents.getTransform(mEntity) * transform); // Notify the collision detection about this new collision shape mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); @@ -373,6 +387,16 @@ void RigidBody::setAngularDamping(decimal angularDamping) { "Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(mAngularDamping)); } +/// Update the transform of the body after a change of the center of mass +void RigidBody::updateTransformWithCenterOfMass() { + + // TODO : Make sure we compute this in a system + + // Translate the body according to the translation of the center of mass position + Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); + transform.setPosition(mCenterOfMassWorld - transform.getOrientation() * mCenterOfMassLocal); +} + // Set a new material for this rigid body /** * @param material The material you want to set to the body @@ -434,12 +458,12 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { void RigidBody::setTransform(const Transform& transform) { // Update the transform of the body - mTransform = transform; + mWorld.mTransformComponents.setTransform(mEntity, transform); const Vector3 oldCenterOfMass = mCenterOfMassWorld; // Compute the new center of mass in world-space coordinates - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + mCenterOfMassWorld = transform * mCenterOfMassLocal; // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); @@ -451,7 +475,7 @@ void RigidBody::setTransform(const Transform& transform) { updateBroadPhaseState(); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(mID) + ": Set transform=" + mTransform.to_string()); + "Body " + std::to_string(mID) + ": Set transform=" + transform.to_string()); } // Recompute the center of mass, total mass and inertia tensor of the body using all @@ -466,9 +490,11 @@ void RigidBody::recomputeMassInformation() { Matrix3x3 inertiaTensorLocal; inertiaTensorLocal.setToZero(); + const Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); + // If it is a STATIC or a KINEMATIC body if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) { - mCenterOfMassWorld = mTransform.getPosition(); + mCenterOfMassWorld = transform.getPosition(); return; } @@ -487,7 +513,7 @@ void RigidBody::recomputeMassInformation() { mMassInverse = decimal(1.0) / mInitMass; } else { - mCenterOfMassWorld = mTransform.getPosition(); + mCenterOfMassWorld = transform.getPosition(); return; } @@ -498,7 +524,7 @@ void RigidBody::recomputeMassInformation() { mCenterOfMassLocal *= mMassInverse; } - mCenterOfMassWorld = mTransform * mCenterOfMassLocal; + mCenterOfMassWorld = transform * mCenterOfMassLocal; if (!mIsInertiaTensorSetByUser) { @@ -546,6 +572,10 @@ void RigidBody::updateBroadPhaseState() const { RP3D_PROFILE("RigidBody::updateBroadPhaseState()", mProfiler); + // TODO : Make sure we compute this in a system + + const Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); + DynamicsWorld& world = static_cast(mWorld); const Vector3 displacement = world.mTimeStep * mLinearVelocity; @@ -554,7 +584,7 @@ void RigidBody::updateBroadPhaseState() const { // Recompute the world-space AABB of the collision shape AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform()); + shape->getCollisionShape()->computeAABB(aabb, transform * shape->getLocalToBodyTransform()); // Update the broad-phase state for the proxy collision shape mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 8ab06b13..9e0b9829 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -300,17 +300,6 @@ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { return mInertiaTensorInverseWorld; } -// Update the world inverse inertia tensor of the body -/// The inertia tensor I_w in world coordinates is computed with the -/// local inverse inertia tensor I_b^-1 in body coordinates -/// by I_w = R * I_b^-1 * R^T -/// where R is the rotation matrix (and R^T its transpose) of the -/// current orientation quaternion of the body -inline void RigidBody::updateInertiaTensorInverseWorld() { - Matrix3x3 orientation = mTransform.getOrientation().getMatrix(); - mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose(); -} - // Return true if the gravity needs to be applied to this rigid body /** * @return True if the gravity is applied to the body @@ -442,13 +431,6 @@ inline void RigidBody::applyTorque(const Vector3& torque) { mExternalTorque += torque; } -/// Update the transform of the body after a change of the center of mass -inline void RigidBody::updateTransformWithCenterOfMass() { - - // Translate the body according to the translation of the center of mass position - mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal); -} - } #endif diff --git a/src/collision/CollisionDetection.sync-conflict-20180707-081346-ARAT43F.cpp b/src/collision/CollisionDetection.sync-conflict-20180707-081346-ARAT43F.cpp deleted file mode 100644 index 7dfb2f72..00000000 --- a/src/collision/CollisionDetection.sync-conflict-20180707-081346-ARAT43F.cpp +++ /dev/null @@ -1,1084 +0,0 @@ -/******************************************************************************** -* 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 "CollisionDetection.h" -#include "engine/CollisionWorld.h" -#include "collision/OverlapCallback.h" -#include "body/Body.h" -#include "collision/shapes/BoxShape.h" -#include "collision/shapes/ConcaveShape.h" -#include "body/RigidBody.h" -#include "configuration.h" -#include "collision/CollisionCallback.h" -#include "collision/MiddlePhaseTriangleCallback.h" -#include "collision/OverlapCallback.h" -#include "collision/NarrowPhaseInfo.h" -#include "collision/ContactManifold.h" -#include "collision/ContactManifoldInfo.h" -#include "utils/Profiler.h" -#include "engine/EventListener.h" -#include "collision/RaycastInfo.h" -#include - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; -using namespace std; - - -// Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager) - : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), - mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this), - mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false) { - - // Set the default collision dispatch configuration - setCollisionDispatch(&mDefaultCollisionDispatch); - - // Fill-in the collision detection matrix with algorithms - fillInCollisionMatrix(); - -#ifdef IS_PROFILING_ACTIVE - - mProfiler = nullptr; - -#endif - -} - -// Compute the collision detection -void CollisionDetection::computeCollisionDetection() { - - RP3D_PROFILE("CollisionDetection::computeCollisionDetection()", mProfiler); - - // Compute the broad-phase collision detection - computeBroadPhase(); - - // Compute the middle-phase collision detection - computeMiddlePhase(); - - // Compute the narrow-phase collision detection - computeNarrowPhase(); - - // Reset the linked list of narrow-phase info - mNarrowPhaseInfoList = nullptr; -} - -// Compute the broad-phase collision detection -void CollisionDetection::computeBroadPhase() { - - RP3D_PROFILE("CollisionDetection::computeBroadPhase()", mProfiler); - - // If new collision shapes have been added to bodies - if (mIsCollisionShapesAdded) { - - // Ask the broad-phase to recompute the overlapping pairs of collision - // shapes. This call can only add new overlapping pairs in the collision - // detection. - mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryManager); - } -} - -// Compute the middle-phase collision detection -void CollisionDetection::computeMiddlePhase() { - - RP3D_PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler); - - // For each possible collision pair of bodies - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - - OverlappingPair* pair = it->second; - - // Make all the contact manifolds and contact points of the pair obsolete - pair->makeContactsObsolete(); - - // Make all the last frame collision info obsolete - pair->makeLastFrameCollisionInfosObsolete(); - - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - - assert(shape1->getBroadPhaseId() != -1); - assert(shape2->getBroadPhaseId() != -1); - assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId()); - - // Check if the two shapes are still overlapping. Otherwise, we destroy the - // overlapping pair - if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - - // Destroy the overlapping pair - it->second->~OverlappingPair(); - - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); - it = mOverlappingPairs.remove(it); - continue; - } - else { - ++it; - } - - // Check if the collision filtering allows collision between the two shapes - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) { - - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); - - // Check that at least one body is awake and not static - bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC; - bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC; - if (!isBody1Active && !isBody2Active) continue; - - // Check if the bodies are in the set of bodies that cannot collide between each other - bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; - - bool isShape1Convex = shape1->getCollisionShape()->isConvex(); - bool isShape2Convex = shape2->getCollisionShape()->isConvex(); - - // If both shapes are convex - if (isShape1Convex && isShape2Convex) { - - // No middle-phase is necessary, simply create a narrow phase info - // for the narrow-phase collision detection - NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; - mNarrowPhaseInfoList = new (mMemoryManager.allocate(MemoryManager::AllocationType::Frame, sizeof(NarrowPhaseInfo))) - NarrowPhaseInfo(pair, shape1->getCollisionShape(), - shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), mMemoryManager.getSingleFrameAllocator()); - mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; - - } - // Concave vs Convex algorithm - else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { - - NarrowPhaseInfo* narrowPhaseInfo = nullptr; - computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), &narrowPhaseInfo); - - // Add all the narrow-phase info object reported by the callback into the - // list of all the narrow-phase info object - while (narrowPhaseInfo != nullptr) { - NarrowPhaseInfo* next = narrowPhaseInfo->next; - narrowPhaseInfo->next = mNarrowPhaseInfoList; - mNarrowPhaseInfoList = narrowPhaseInfo; - - narrowPhaseInfo = next; - } - } - // Concave vs Concave shape - else { - // Not handled - continue; - } - - // Remove the obsolete last frame collision infos - pair->clearObsoleteLastFrameCollisionInfos(); - } - } -} - -// Compute the concave vs convex middle-phase algorithm for a given pair of bodies -void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, - NarrowPhaseInfo** firstNarrowPhaseInfo) { - - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - - ProxyShape* convexProxyShape; - ProxyShape* concaveProxyShape; - const ConvexShape* convexShape; - const ConcaveShape* concaveShape; - - // Collision shape 1 is convex, collision shape 2 is concave - if (shape1->getCollisionShape()->isConvex()) { - convexProxyShape = shape1; - convexShape = static_cast(shape1->getCollisionShape()); - concaveProxyShape = shape2; - concaveShape = static_cast(shape2->getCollisionShape()); - } - else { // Collision shape 2 is convex, collision shape 1 is concave - convexProxyShape = shape2; - convexShape = static_cast(shape2->getCollisionShape()); - concaveProxyShape = shape1; - concaveShape = static_cast(shape1->getCollisionShape()); - } - - // Set the parameters of the callback object - MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape, - concaveShape, allocator); - -#ifdef IS_PROFILING_ACTIVE - - // Set the profiler - middlePhaseCallback.setProfiler(mProfiler); - -#endif - - // Compute the convex shape AABB in the local-space of the convex shape - const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() * - convexProxyShape->getLocalToWorldTransform(); - AABB aabb; - convexShape->computeAABB(aabb, convexToConcaveTransform); - - // Call the convex vs triangle callback for each triangle of the concave shape - concaveShape->testAllTriangles(middlePhaseCallback, aabb); - - // Add all the narrow-phase info object reported by the callback into the - // list of all the narrow-phase info object - *firstNarrowPhaseInfo = middlePhaseCallback.narrowPhaseInfoList; -} - -// Compute the narrow-phase collision detection -void CollisionDetection::computeNarrowPhase() { - - RP3D_PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler); - - List narrowPhaseInfos(mMemoryManager.getSingleFrameAllocator()); - - NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; - while (currentNarrowPhaseInfo != nullptr) { - - // Select the narrow phase algorithm to use according to the two collision shapes - const CollisionShapeType shape1Type = currentNarrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = currentNarrowPhaseInfo->collisionShape2->getType(); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is no collision algorithm between those two kinds of shapes, skip it - if (narrowPhaseAlgorithm != nullptr) { - - LastFrameCollisionInfo* lastCollisionFrameInfo = currentNarrowPhaseInfo->getLastFrameCollisionInfo(); - - narrowPhaseInfos.add(currentNarrowPhaseInfo); - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mMemoryManager.getSingleFrameAllocator())) { - - lastCollisionFrameInfo->wasColliding = true; - } - else { - lastCollisionFrameInfo->wasColliding = false; - } - - // The previous frame collision info is now valid - lastCollisionFrameInfo->isValid = true; - } - - currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; - } - - // Convert the potential contact into actual contacts - processAllPotentialContacts(narrowPhaseInfos, mMemoryManager.getSingleFrameAllocator()); - - // Add all the contact manifolds (between colliding bodies) to the bodies - addAllContactManifoldsToBodies(); - - // Report contacts to the user - reportAllContacts(); - - // Destroy the narrow phase infos - for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { - - NarrowPhaseInfo* narrowPhaseInfo = *it; - - // Call the destructor - narrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory for the narrow phase info - mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } -} - -// Allow the broadphase to notify the collision detection about an overlapping pair. -/// This method is called by the broad-phase collision detection algorithm -void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { - - assert(shape1->getBroadPhaseId() != -1); - assert(shape2->getBroadPhaseId() != -1); - assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId()); - - // Check if the collision filtering allows collision between the two shapes - if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; - - // Compute the overlapping pair ID - Pair pairID = OverlappingPair::computeID(shape1, shape2); - - // Check if the overlapping pair already exists - if (mOverlappingPairs.containsKey(pairID)) return; - - // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(), - mMemoryManager.getSingleFrameAllocator(), mWorld->mConfig); - assert(newPair != nullptr); - - mOverlappingPairs.add(Pair, OverlappingPair*>(pairID, newPair)); - - // Wake up the two bodies - shape1->getBody()->setIsSleeping(false); - shape2->getBody()->setIsSleeping(false); -} - -// Remove a body from the collision detection -void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { - - assert(proxyShape->getBroadPhaseId() != -1); - - // Remove all the overlapping pairs involving this proxy shape - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - if (it->second->getShape1()->getBroadPhaseId() == proxyShape->getBroadPhaseId()|| - it->second->getShape2()->getBroadPhaseId() == proxyShape->getBroadPhaseId()) { - - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - - // Destroy the overlapping pair - it->second->~OverlappingPair(); - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); - it = mOverlappingPairs.remove(it); - } - else { - ++it; - } - } - - // Remove the body from the broad-phase - mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); -} - -void CollisionDetection::addAllContactManifoldsToBodies() { - - RP3D_PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler); - - // For each overlapping pairs in contact during the narrow-phase - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - - // Add all the contact manifolds of the pair into the list of contact manifolds - // of the two bodies involved in the contact - addContactManifoldToBody(it->second); - } -} - -// Ray casting method -void CollisionDetection::raycast(RaycastCallback* raycastCallback, - const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const { - - RP3D_PROFILE("CollisionDetection::raycast()", mProfiler); - - RaycastTest rayCastTest(raycastCallback); - - // Ask the broad-phase algorithm to call the testRaycastAgainstShape() - // callback method for each proxy shape hit by the ray in the broad-phase - mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); -} - -// Add a contact manifold to the linked list of contact manifolds of the two bodies involved -// in the corresponding contact -void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { - - assert(pair != nullptr); - - CollisionBody* body1 = pair->getShape1()->getBody(); - CollisionBody* body2 = pair->getShape2()->getBody(); - const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - - // For each contact manifold in the set of manifolds in the pair - ContactManifold* contactManifold = manifoldSet.getContactManifolds(); - while (contactManifold != nullptr) { - - assert(contactManifold->getNbContactPoints() > 0); - - // Add the contact manifold at the beginning of the linked - // list of contact manifolds of the first body - ContactManifoldListElement* listElement1 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, - sizeof(ContactManifoldListElement))) - ContactManifoldListElement(contactManifold, - body1->mContactManifoldsList); - body1->mContactManifoldsList = listElement1; - - // Add the contact manifold at the beginning of the linked - // list of the contact manifolds of the second body - ContactManifoldListElement* listElement2 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, - sizeof(ContactManifoldListElement))) - ContactManifoldListElement(contactManifold, - body2->mContactManifoldsList); - body2->mContactManifoldsList = listElement2; - - contactManifold = contactManifold->getNext(); - } -} - -/// Convert the potential contact into actual contacts -void CollisionDetection::processAllPotentialContacts(const List& narrowPhaseInfos, - MemoryAllocator& tempMemoryAllocator) { - - RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); - - Set collidingPairs(tempMemoryAllocator); - - // For each narrow phase info object - for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { - - NarrowPhaseInfo* narrowPhaseInfo = *it; - - assert(narrowPhaseInfo != nullptr); - - if (narrowPhaseInfo->contactPoints != nullptr) { - - // Transfer the contact points from the narrow phase info to the overlapping pair - narrowPhaseInfo->overlappingPair->addPotentialContactPoints(narrowPhaseInfo); - - // Store the colliding pair - collidingPairs.add(narrowPhaseInfo->overlappingPair); - } - } - - // For each overlapping pairs in contact during the narrow-phase - for (auto it = collidingPairs.begin(); it != collidingPairs.end(); ++it) { - - OverlappingPair* pair = *it; - - // Clear the obsolete contact manifolds and contact points - pair->clearObsoleteManifoldsAndContactPoints(); - - // Reduce the contact manifolds and contact points if there are too many of them - pair->reduceContactManifolds(); - - // Reset the potential contacts of the pair - pair->clearPotentialContactManifolds(); - } -} - -// Report contacts for all the colliding overlapping pairs -void CollisionDetection::reportAllContacts() { - - RP3D_PROFILE("CollisionDetection::reportAllContacts()", mProfiler); - - // For each overlapping pairs in contact during the narrow-phase - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - - // If there is a user callback - if (mWorld->mEventListener != nullptr && it->second->hasContacts()) { - - CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mMemoryManager); - - // Trigger a callback event to report the new contact to the user - mWorld->mEventListener->newContact(collisionInfo); - } - } -} - -// Compute the middle-phase collision detection between two proxy shapes -NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) { - - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - - // ------------------------------------------------------- - - const bool isShape1Convex = shape1->getCollisionShape()->isConvex(); - const bool isShape2Convex = shape2->getCollisionShape()->isConvex(); - - NarrowPhaseInfo* narrowPhaseInfo = nullptr; - - pair->makeLastFrameCollisionInfosObsolete(); - - // If both shapes are convex - if ((isShape1Convex && isShape2Convex)) { - - // No middle-phase is necessary, simply create a narrow phase info - // for the narrow-phase collision detection - narrowPhaseInfo = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, - sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), - shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), mMemoryManager.getPoolAllocator()); - - } - // Concave vs Convex algorithm - else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { - - // Run the middle-phase collision detection algorithm to find the triangles of the concave - // shape we need to use during the narrow-phase collision detection - computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), &narrowPhaseInfo); - } - - pair->clearObsoleteLastFrameCollisionInfos(); - - return narrowPhaseInfo; -} - -// Report all the bodies that overlap with the aabb in parameter -void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, - unsigned short categoryMaskBits) { - assert(overlapCallback != nullptr); - - Set reportedBodies(mMemoryManager.getPoolAllocator()); - - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); - - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { - - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - - CollisionBody* overlapBody = proxyShape->getBody(); - - // If the proxy shape is from a body that we have not already reported collision - if (reportedBodies.find(overlapBody->getId()) == reportedBodies.end()) { - - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - - // Add the body into the set of reported bodies - reportedBodies.add(overlapBody->getId()); - - // Notify the overlap to the user - overlapCallback->notifyOverlap(overlapBody); - } - } - - // Go to the next overlapping proxy shape - element = element->next; - } -} - -// Return true if two bodies overlap -bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) { - - // For each proxy shape proxy shape of the first body - ProxyShape* body1ProxyShape = body1->getProxyShapesList(); - while (body1ProxyShape != nullptr) { - - AABB aabb1 = body1ProxyShape->getWorldAABB(); - - // For each proxy shape of the second body - ProxyShape* body2ProxyShape = body2->getProxyShapesList(); - while (body2ProxyShape != nullptr) { - - AABB aabb2 = body2ProxyShape->getWorldAABB(); - - // Test if the AABBs of the two proxy shapes overlap - if (aabb1.testCollision(aabb2)) { - - // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); - - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - - bool isColliding = false; - - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { - - // If we have not found a collision yet - if (!isColliding) { - - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); - } - } - - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; - - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory - mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } - - // Return if we have found a narrow-phase collision - if (isColliding) return true; - } - - // Go to the next proxy shape - body2ProxyShape = body2ProxyShape->getNext(); - } - - // Go to the next proxy shape - body1ProxyShape = body1ProxyShape->getNext(); - } - - // No overlap has been found - return false; -} - -// Report all the bodies that overlap with the body in parameter -void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, - unsigned short categoryMaskBits) { - - assert(overlapCallback != nullptr); - - Set reportedBodies(mMemoryManager.getPoolAllocator()); - - // For each proxy shape proxy shape of the body - ProxyShape* bodyProxyShape = body->getProxyShapesList(); - while (bodyProxyShape != nullptr) { - - if (bodyProxyShape->getBroadPhaseId() != -1) { - - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->getBroadPhaseId()); - - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - - const bodyindex bodyId = body->getId(); - - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { - - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - - // If the proxy shape is from a body that we have not already reported collision and the - // two proxy collision shapes are not from the same body - if (reportedBodies.find(proxyShape->getBody()->getId()) == reportedBodies.end() && - proxyShape->getBody()->getId() != bodyId) { - - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - - // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); - - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - - bool isColliding = false; - - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { - - // If we have not found a collision yet - if (!isColliding) { - - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); - } - } - - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; - - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory - mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } - - // Return if we have found a narrow-phase collision - if (isColliding) { - - CollisionBody* overlapBody = proxyShape->getBody(); - - // Add the body into the set of reported bodies - reportedBodies.add(overlapBody->getId()); - - // Notify the overlap to the user - overlapCallback->notifyOverlap(overlapBody); - } - } - } - - // Go to the next overlapping proxy shape - element = element->next; - } - } - - // Go to the next proxy shape - bodyProxyShape = bodyProxyShape->getNext(); - } -} - -// Test and report collisions between two bodies -void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* collisionCallback) { - - assert(collisionCallback != nullptr); - - List collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); - Set collidingPairs(mMemoryManager.getPoolAllocator()); - - // For each proxy shape proxy shape of the first body - ProxyShape* body1ProxyShape = body1->getProxyShapesList(); - while (body1ProxyShape != nullptr) { - - AABB aabb1 = body1ProxyShape->getWorldAABB(); - - // For each proxy shape of the second body - ProxyShape* body2ProxyShape = body2->getProxyShapesList(); - while (body2ProxyShape != nullptr) { - - AABB aabb2 = body2ProxyShape->getWorldAABB(); - - // Test if the AABBs of the two proxy shapes overlap - if (aabb1.testCollision(aabb2)) { - - // Create a temporary overlapping pair - OverlappingPair* pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) - OverlappingPair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); - - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* allNarrowPhaseInfos = computeMiddlePhaseForProxyShapes(pair); - - // For each narrow-phase info object - NarrowPhaseInfo* narrowPhaseInfo = allNarrowPhaseInfos; - while (narrowPhaseInfo != nullptr) { - - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { - - collidingNarrowPhaseInfos.add(narrowPhaseInfo); - collidingPairs.add(pair); - } - } - - narrowPhaseInfo = narrowPhaseInfo->next; - } - } - - // Go to the next proxy shape - body2ProxyShape = body2ProxyShape->getNext(); - } - - // Go to the next proxy shape - body1ProxyShape = body1ProxyShape->getNext(); - } - - // Process the potential contacts - processAllPotentialContacts(collidingNarrowPhaseInfos, mMemoryManager.getPoolAllocator()); - - // For each colliding pair - for (auto it = collidingPairs.begin(); it != collidingPairs.end(); ++it) { - - OverlappingPair* pair = *it; - - assert(pair->hasContacts()); - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); - collisionCallback->notifyContact(collisionInfo); - - // Destroy the temporary overlapping pair - pair->~OverlappingPair(); - mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); - } - - // Destroy the narrow phase infos - NarrowPhaseInfo* narrowPhaseInfo = mNarrowPhaseInfoList; - while (narrowPhaseInfo != nullptr) { - - NarrowPhaseInfo* narrowPhaseInfoToDelete = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; - - // Call the destructor - narrowPhaseInfoToDelete->~NarrowPhaseInfo(); - - // Release the allocated memory for the narrow phase info - mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); - } -} - -// Test and report collisions between a body and all the others bodies of the world -void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) { - - assert(callback != nullptr); - - List narrowPhaseInfos(mMemoryManager.getPoolAllocator()); - Set overlappingPairs(mMemoryManager.getPoolAllocator()); - - // For each proxy shape proxy shape of the body - ProxyShape* bodyProxyShape = body->getProxyShapesList(); - while (bodyProxyShape != nullptr) { - - if (bodyProxyShape->getBroadPhaseId() != -1) { - - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->getBroadPhaseId()); - - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - - const bodyindex bodyId = body->getId(); - - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { - - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - - // If the two proxy collision shapes are not from the same body - if (proxyShape->getBody()->getId() != bodyId) { - - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - - // Create a temporary overlapping pair - OverlappingPair* pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) - OverlappingPair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); - - overlappingPairs.add(pair); - - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); - - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { - - narrowPhaseInfos.add(narrowPhaseInfo); - - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator()); - } - - narrowPhaseInfo = narrowPhaseInfo->next; - } - } - } - - // Go to the next overlapping proxy shape - element = element->next; - } - - // Go to the next proxy shape - bodyProxyShape = bodyProxyShape->getNext(); - } - } - - // Process the potential contacts - processAllPotentialContacts(narrowPhaseInfos, mMemoryManager.getPoolAllocator()); - - // For each overlapping pair - for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { - - OverlappingPair* pair = *it; - - if (pair->hasContacts()) { - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); - callback->notifyContact(collisionInfo); - } - - // Destroy the temporary overlapping pair - pair->~OverlappingPair(); - mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); - } - - // Destroy the narrow phase infos - for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { - - NarrowPhaseInfo* narrowPhaseInfo = *it; - - // Call the destructor - narrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory for the narrow phase info - mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } -} - -// Test and report collisions between all shapes of the world -void CollisionDetection::testCollision(CollisionCallback* callback) { - - assert(callback != nullptr); - - // Compute the broad-phase collision detection - computeBroadPhase(); - - List narrowPhaseInfos(mMemoryManager.getPoolAllocator()); - Set overlappingPairs(mMemoryManager.getPoolAllocator()); - - // For each possible collision pair of bodies - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - - OverlappingPair* originalPair = it->second; - - // Create a temporary overlapping pair - OverlappingPair* pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) - OverlappingPair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); - overlappingPairs.add(pair); - - ProxyShape* shape1 = pair.getShape1(); - ProxyShape* shape2 = pair.getShape2(); - - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0) && - mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { - - const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); - const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { - - narrowPhaseInfos.add(narrowPhaseInfo); - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision. If a collision occurs, the - // notifyContact() callback method will be called. - narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator()); - } - - narrowPhaseInfo = narrowPhaseInfo->next; - } - } - } - - // Process the potential contacts - processAllPotentialContacts(narrowPhaseInfos, mMemoryManager.getPoolAllocator()); - - // For each overlapping pair - for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { - - OverlappingPair* pair = *it; - - if (pair->hasContacts()) { - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); - callback->notifyContact(collisionInfo); - } - - // Destroy the temporary overlapping pair - pair->~OverlappingPair(); - mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); - } - - // Destroy the narrow phase infos - for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { - - NarrowPhaseInfo* narrowPhaseInfo = *it; - - // Call the destructor - narrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory for the narrow phase info - mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } -} - -// Fill-in the collision detection matrix -void CollisionDetection::fillInCollisionMatrix() { - - // For each possible type of collision shape - for (int i=0; iselectAlgorithm(i, j); - } - } -} - -// Return the world event listener -EventListener* CollisionDetection::getWorldEventListener() { - return mWorld->mEventListener; -} - -// Return the world-space AABB of a given proxy shape -const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const { - assert(proxyShape->getBroadPhaseId() > -1); - return mBroadPhaseAlgorithm.getFatAABB(proxyShape->getBroadPhaseId()); -} diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index f83da44b..66bc0540 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -28,6 +28,7 @@ #include "utils/Logger.h" #include "collision/RaycastInfo.h" #include "memory/MemoryManager.h" +#include "engine/CollisionWorld.h" using namespace reactphysics3d; @@ -55,7 +56,7 @@ ProxyShape::~ProxyShape() { * @return True if the point is inside the collision shape */ bool ProxyShape::testPointInside(const Vector3& worldPoint) { - const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform; + const Transform localToWorld = mBody->mWorld.mTransformComponents.getTransform(mBody->getEntity()) * mLocalToBodyTransform; const Vector3 localPoint = localToWorld.getInverse() * worldPoint; return mCollisionShape->testPointInside(localPoint, this); } @@ -127,3 +128,13 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { return isHit; } + +// Return the local to world transform +/** + * @return The transformation that transforms the local-space of the collision + * shape to the world-space + */ +const Transform ProxyShape::getLocalToWorldTransform() const { + return mBody->mWorld.mTransformComponents.getTransform(mBody->getEntity()) * mLocalToBodyTransform; +} + diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 17bbebbf..48e82565 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -267,15 +267,6 @@ inline const Transform& ProxyShape::getLocalToBodyTransform() const { return mLocalToBodyTransform; } -// Return the local to world transform -/** - * @return The transformation that transforms the local-space of the collision - * shape to the world-space - */ -inline const Transform ProxyShape::getLocalToWorldTransform() const { - return mBody->mTransform * mLocalToBodyTransform; -} - // Return the AABB of the proxy shape in world-space /** * @return The AABB of the proxy shape in world-space diff --git a/src/components/TransformComponents.cpp b/src/components/TransformComponents.cpp index 3c702ead..57652fa7 100644 --- a/src/components/TransformComponents.cpp +++ b/src/components/TransformComponents.cpp @@ -74,10 +74,10 @@ void TransformComponents::allocate(uint32 nbComponentsToAllocate) { // New pointers to components data Entity* newEntities = static_cast(newBuffer); - Transform* newTransforms = reinterpret_cast(newEntities + mNbAllocatedComponents); + Transform* newTransforms = reinterpret_cast(newEntities + nbComponentsToAllocate); // If there was already components before - if (mNbAllocatedComponents > 0) { + if (mNbComponents > 0) { // Copy component data from the previous buffer to the new one memcpy(newTransforms, mTransforms, mNbComponents * sizeof(Transform)); diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 15530583..72d5a9f0 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -155,7 +155,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { // Create the collision body CollisionBody* collisionBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(CollisionBody))) - CollisionBody(transform, *this, entity, bodyID); + CollisionBody(*this, entity, bodyID); assert(collisionBody != nullptr); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index a9615d51..e53df768 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -185,7 +185,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { // Get current position and orientation of the body const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld; - const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); + const Quaternion& currentOrientation = mTransformComponents.getTransform(bodies[b]->getEntity()).getOrientation(); // Update the new constrained position and orientation of the body mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep; @@ -201,6 +201,8 @@ void DynamicsWorld::updateBodiesState() { RP3D_PROFILE("DynamicsWorld::updateBodiesState()", mProfiler); + // TODO : Make sure we compute this in a system + // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { @@ -219,7 +221,7 @@ void DynamicsWorld::updateBodiesState() { bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index]; // Update the orientation of the body - bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit()); + mTransformComponents.getTransform(bodies[b]->getEntity()).setOrientation(mConstrainedOrientations[index].getUnit()); // Update the transform of the body (using the new center of mass and new orientation) bodies[b]->updateTransformWithCenterOfMass();