diff --git a/CMakeLists.txt b/CMakeLists.txt index b3d37ed1..169476c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,9 @@ SET (REACTPHYSICS3D_SOURCES "src/body/CollisionBody.cpp" "src/body/RigidBody.h" "src/body/RigidBody.cpp" + "src/collision/ContactPointInfo.h" + "src/collision/ContactManifoldInfo.h" + "src/collision/ContactManifoldInfo.cpp" "src/collision/broadphase/BroadPhaseAlgorithm.h" "src/collision/broadphase/BroadPhaseAlgorithm.cpp" "src/collision/broadphase/DynamicAABBTree.h" @@ -67,27 +70,31 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/CollisionDispatch.h" "src/collision/narrowphase/DefaultCollisionDispatch.h" "src/collision/narrowphase/DefaultCollisionDispatch.cpp" - "src/collision/narrowphase/EPA/EdgeEPA.h" - "src/collision/narrowphase/EPA/EdgeEPA.cpp" - "src/collision/narrowphase/EPA/EPAAlgorithm.h" - "src/collision/narrowphase/EPA/EPAAlgorithm.cpp" - "src/collision/narrowphase/EPA/TriangleEPA.h" - "src/collision/narrowphase/EPA/TriangleEPA.cpp" - "src/collision/narrowphase/EPA/TrianglesStore.h" - "src/collision/narrowphase/EPA/TrianglesStore.cpp" "src/collision/narrowphase/GJK/VoronoiSimplex.h" "src/collision/narrowphase/GJK/VoronoiSimplex.cpp" "src/collision/narrowphase/GJK/GJKAlgorithm.h" "src/collision/narrowphase/GJK/GJKAlgorithm.cpp" + "src/collision/narrowphase/SAT/SATAlgorithm.h" + "src/collision/narrowphase/SAT/SATAlgorithm.cpp" "src/collision/narrowphase/NarrowPhaseAlgorithm.h" "src/collision/narrowphase/SphereVsSphereAlgorithm.h" "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp" - "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h" - "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp" + "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h" + "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp" + "src/collision/narrowphase/SphereVsCapsuleAlgorithm.h" + "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp" + "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" + "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp" + "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp" "src/collision/shapes/AABB.h" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.h" "src/collision/shapes/ConvexShape.cpp" + "src/collision/shapes/ConvexPolyhedronShape.h" + "src/collision/shapes/ConvexPolyhedronShape.cpp" "src/collision/shapes/ConcaveShape.h" "src/collision/shapes/ConcaveShape.cpp" "src/collision/shapes/BoxShape.h" @@ -96,12 +103,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/shapes/CapsuleShape.cpp" "src/collision/shapes/CollisionShape.h" "src/collision/shapes/CollisionShape.cpp" - "src/collision/shapes/ConeShape.h" - "src/collision/shapes/ConeShape.cpp" "src/collision/shapes/ConvexMeshShape.h" "src/collision/shapes/ConvexMeshShape.cpp" - "src/collision/shapes/CylinderShape.h" - "src/collision/shapes/CylinderShape.cpp" "src/collision/shapes/SphereShape.h" "src/collision/shapes/SphereShape.cpp" "src/collision/shapes/TriangleShape.h" @@ -116,15 +119,24 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/ProxyShape.cpp" "src/collision/TriangleVertexArray.h" "src/collision/TriangleVertexArray.cpp" + "src/collision/PolygonVertexArray.h" + "src/collision/PolygonVertexArray.cpp" "src/collision/TriangleMesh.h" "src/collision/TriangleMesh.cpp" + "src/collision/PolyhedronMesh.h" + "src/collision/PolyhedronMesh.cpp" + "src/collision/HalfEdgeStructure.h" + "src/collision/HalfEdgeStructure.cpp" "src/collision/CollisionDetection.h" "src/collision/CollisionDetection.cpp" "src/collision/NarrowPhaseInfo.h" + "src/collision/NarrowPhaseInfo.cpp" "src/collision/ContactManifold.h" "src/collision/ContactManifold.cpp" "src/collision/ContactManifoldSet.h" "src/collision/ContactManifoldSet.cpp" + "src/collision/MiddlePhaseTriangleCallback.h" + "src/collision/MiddlePhaseTriangleCallback.cpp" "src/constraint/BallAndSocketJoint.h" "src/constraint/BallAndSocketJoint.cpp" "src/constraint/ContactPoint.h" @@ -156,6 +168,9 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/Profiler.cpp" "src/engine/Timer.h" "src/engine/Timer.cpp" + "src/collision/CollisionCallback.h" + "src/collision/CollisionCallback.cpp" + "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" "src/mathematics/mathematics_functions.h" "src/mathematics/mathematics_functions.cpp" @@ -172,13 +187,18 @@ SET (REACTPHYSICS3D_SOURCES "src/mathematics/Vector3.h" "src/mathematics/Ray.h" "src/mathematics/Vector3.cpp" - "src/memory/Allocator.h" + "src/memory/MemoryAllocator.h" "src/memory/PoolAllocator.h" "src/memory/PoolAllocator.cpp" "src/memory/SingleFrameAllocator.h" "src/memory/SingleFrameAllocator.cpp" + "src/memory/DefaultAllocator.h" + "src/memory/MemoryManager.h" + "src/memory/MemoryManager.cpp" "src/containers/Stack.h" "src/containers/LinkedList.h" + "src/containers/List.h" + "src/containers/Map.h" ) # Create the library diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index db22f606..bb8b0408 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -70,9 +70,16 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, const Transform& transform) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, decimal(1)); + transform, decimal(1), mWorld.mMemoryManager); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + proxyShape->setProfiler(mProfiler); + +#endif // Add it to the list of proxy collision shapes of the body if (mProxyCollisionShapes == nullptr) { @@ -111,12 +118,12 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { if (current == proxyShape) { mProxyCollisionShapes = current->mNext; - if (mIsActive) { + if (mIsActive && proxyShape->mBroadPhaseID != -1) { mWorld.mCollisionDetection.removeProxyCollisionShape(current); } current->~ProxyShape(); - mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -131,12 +138,12 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { ProxyShape* elementToRemove = current->mNext; current->mNext = elementToRemove->mNext; - if (mIsActive) { + if (mIsActive && proxyShape->mBroadPhaseID != -1) { mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove); } elementToRemove->~ProxyShape(); - mWorld.mPoolAllocator.release(elementToRemove, sizeof(ProxyShape)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, elementToRemove, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -157,12 +164,12 @@ void CollisionBody::removeAllCollisionShapes() { // Remove the proxy collision shape ProxyShape* nextElement = current->mNext; - if (mIsActive) { + if (mIsActive && current->mBroadPhaseID != -1) { mWorld.mCollisionDetection.removeProxyCollisionShape(current); } current->~ProxyShape(); - mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape)); // Get the next element in the list current = nextElement; @@ -177,11 +184,11 @@ void CollisionBody::resetContactManifoldsList() { // Delete the linked list of contact manifolds of that body ContactManifoldListElement* currentElement = mContactManifoldsList; while (currentElement != nullptr) { - ContactManifoldListElement* nextElement = currentElement->next; + ContactManifoldListElement* nextElement = currentElement->getNext(); // Delete the current element currentElement->~ContactManifoldListElement(); - mWorld.mPoolAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, currentElement, sizeof(ContactManifoldListElement)); currentElement = nextElement; } @@ -202,12 +209,15 @@ 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 { - // Recompute the world-space AABB of the collision shape - AABB aabb; - proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + if (proxyShape->mBroadPhaseID != -1) { - // Update the broad-phase state for the proxy collision shape - mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); + // Recompute the world-space AABB of the collision shape + AABB aabb; + proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); + + // Update the broad-phase state for the proxy collision shape + mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert) ; + } } // Set whether or not the body is active @@ -240,8 +250,11 @@ void CollisionBody::setIsActive(bool isActive) { // For each proxy shape of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { - // Remove the proxy shape from the collision detection - mWorld.mCollisionDetection.removeProxyCollisionShape(shape); + if (shape->mBroadPhaseID != -1) { + + // Remove the proxy shape from the collision detection + mWorld.mCollisionDetection.removeProxyCollisionShape(shape); + } } // Reset the contact manifold list of the body @@ -272,8 +285,8 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { // this body ContactManifoldListElement* currentElement = mContactManifoldsList; while (currentElement != nullptr) { - currentElement->contactManifold->mIsAlreadyInIsland = false; - currentElement = currentElement->next; + currentElement->getContactManifold()->mIsAlreadyInIsland = false; + currentElement = currentElement->getNext(); nbManifolds++; } diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 2d5101b9..9943dc86 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -36,6 +36,7 @@ #include "collision/RaycastInfo.h" #include "memory/PoolAllocator.h" #include "configuration.h" +#include "engine/Profiler.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -85,6 +86,13 @@ class CollisionBody : public Body { /// Reference to the world the body belongs to CollisionWorld& mWorld; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Reset the contact manifold lists @@ -177,6 +185,13 @@ class CollisionBody : public Body { /// Return the body local-space coordinates of a vector given in the world-space coordinates Vector3 getLocalVector(const Vector3& worldVector) const; +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler); + +#endif + // -------------------- Friendship -------------------- // friend class CollisionWorld; @@ -313,6 +328,15 @@ inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const { return worldAABB.testCollision(getAABB()); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionBody::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 6e6a55e6..36c362dc 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -39,7 +39,7 @@ using namespace reactphysics3d; * @param id The ID of the body */ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id) - : CollisionBody(transform, world, id), mInitMass(decimal(1.0)), + : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(nullptr) { @@ -176,7 +176,7 @@ void RigidBody::setMass(decimal mass) { } // Remove a joint from the joints list -void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint) { +void RigidBody::removeJointFromJointsList(MemoryManager& memoryManager, const Joint* joint) { assert(joint != nullptr); assert(mJointsList != nullptr); @@ -186,7 +186,8 @@ void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const JointListElement* elementToRemove = mJointsList; mJointsList = elementToRemove->next; elementToRemove->~JointListElement(); - memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + memoryManager.release(MemoryManager::AllocationType::Pool, + elementToRemove, sizeof(JointListElement)); } else { // If the element to remove is not the first one in the list JointListElement* currentElement = mJointsList; @@ -195,7 +196,8 @@ void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const JointListElement* elementToRemove = currentElement->next; currentElement->next = elementToRemove->next; elementToRemove->~JointListElement(); - memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + memoryManager.release(MemoryManager::AllocationType::Pool, + elementToRemove, sizeof(JointListElement)); break; } currentElement = currentElement->next; @@ -224,9 +226,16 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, decimal mass) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(ProxyShape))) ProxyShape(this, collisionShape, - transform, mass); + transform, mass, mWorld.mMemoryManager); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + proxyShape->setProfiler(mProfiler); + +#endif // Add it to the list of proxy collision shapes of the body if (mProxyCollisionShapes == nullptr) { @@ -410,7 +419,7 @@ void RigidBody::recomputeMassInformation() { // Update the broad-phase state for this body (because it has moved for instance) void RigidBody::updateBroadPhaseState() const { - PROFILE("RigidBody::updateBroadPhaseState()"); + PROFILE("RigidBody::updateBroadPhaseState()", mProfiler); DynamicsWorld& world = static_cast(mWorld); const Vector3 displacement = world.mTimeStep * mLinearVelocity; @@ -427,3 +436,22 @@ void RigidBody::updateBroadPhaseState() const { } } +#ifdef IS_PROFILING_ACTIVE + +// 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(); + } +} + +#endif + diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index a38b53d5..23603bda 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -31,7 +31,7 @@ #include "CollisionBody.h" #include "engine/Material.h" #include "mathematics/mathematics.h" -#include "memory/PoolAllocator.h" +#include "memory/MemoryManager.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -50,6 +50,11 @@ class DynamicsWorld; */ class RigidBody : public CollisionBody { + private : + + /// Index of the body in arrays for contact/constraint solver + uint mArrayIndex; + protected : // -------------------- Attributes -------------------- // @@ -102,12 +107,12 @@ class RigidBody : public CollisionBody { decimal mAngularDamping; /// First element of the linked list of joints involving this body - JointListElement* mJointsList; + JointListElement* mJointsList; // -------------------- Methods -------------------- // /// Remove a joint from the joints list - void removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint); + void removeJointFromJointsList(reactphysics3d::MemoryManager& memoryManager, const Joint* joint); /// Update the transform of the body after a change of the center of mass void updateTransformWithCenterOfMass(); @@ -227,6 +232,13 @@ class RigidBody : public CollisionBody { /// the collision shapes attached to the body. void recomputeMassInformation(); +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) override; + +#endif + // -------------------- Friendship -------------------- // friend class DynamicsWorld; diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp new file mode 100644 index 00000000..1702e40a --- /dev/null +++ b/src/collision/CollisionCallback.cpp @@ -0,0 +1,82 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "collision/CollisionCallback.h" +#include "engine/OverlappingPair.h" +#include "memory/MemoryAllocator.h" +#include "collision/ContactManifold.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager) : + contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()), + body2(pair->getShape2()->getBody()), + proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()), + mMemoryManager(memoryManager) { + + assert(pair != nullptr); + + const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); + + // For each contact manifold in the set of manifolds in the pair + ContactManifold* contactManifold = manifoldSet.getContactManifolds(); + assert(contactManifold != nullptr); + 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* element = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(ContactManifoldListElement))) + ContactManifoldListElement(contactManifold, + contactManifoldElements); + contactManifoldElements = element; + + contactManifold = contactManifold->getNext(); + } +} + +// Destructor +CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() { + + // Release memory allocator for the contact manifold list elements + ContactManifoldListElement* element = contactManifoldElements; + while (element != nullptr) { + + ContactManifoldListElement* nextElement = element->getNext(); + + // Delete and release memory + element->~ContactManifoldListElement(); + mMemoryManager.release(MemoryManager::AllocationType::Pool, element, + sizeof(ContactManifoldListElement)); + + element = nextElement; + } +} + diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h new file mode 100644 index 00000000..57ae0f04 --- /dev/null +++ b/src/collision/CollisionCallback.h @@ -0,0 +1,91 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H +#define REACTPHYSICS3D_COLLISION_CALLBACK_H + +// Libraries +#include "collision/ContactManifold.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +class OverlappingPair; + +// Class CollisionCallback +/** + * This class can be used to register a callback for collision test queries. + * You should implement your own class inherited from this one and implement + * the notifyContact() method. This method will called each time a contact + * point is reported. + */ +class CollisionCallback { + + public: + + // Structure CollisionCallbackInfo + /** + * This structure represents the contact information between two collision + * shapes of two bodies + */ + struct CollisionCallbackInfo { + + public: + + /// Pointer to the first element of the linked-list that contains contact manifolds + ContactManifoldListElement* contactManifoldElements; + + /// Pointer to the first body of the contact + CollisionBody* body1; + + /// Pointer to the second body of the contact + CollisionBody* body2; + + /// Pointer to the proxy shape of first body + const ProxyShape* proxyShape1; + + /// Pointer to the proxy shape of second body + const ProxyShape* proxyShape2; + + /// Memory manager + MemoryManager& mMemoryManager; + + // Constructor + CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager); + + // Destructor + ~CollisionCallbackInfo(); + }; + + /// Destructor + virtual ~CollisionCallback() = default; + + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0; +}; + +} + +#endif diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index be17e4f7..66cf1eb5 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -26,10 +26,15 @@ // 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 #include #include @@ -42,9 +47,8 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator) - : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator), - mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), +CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager) + : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { // Set the default collision dispatch configuration @@ -52,12 +56,19 @@ CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& mem // Fill-in the collision detection matrix with algorithms fillInCollisionMatrix(); + +#ifdef IS_PROFILING_ACTIVE + + mProfiler = nullptr; + +#endif + } // Compute the collision detection void CollisionDetection::computeCollisionDetection() { - PROFILE("CollisionDetection::computeCollisionDetection()"); + PROFILE("CollisionDetection::computeCollisionDetection()", mProfiler); // Compute the broad-phase collision detection computeBroadPhase(); @@ -66,76 +77,16 @@ void CollisionDetection::computeCollisionDetection() { computeMiddlePhase(); // Compute the narrow-phase collision detection - computeNarrowPhase(); + computeNarrowPhase(); // Reset the linked list of narrow-phase info mNarrowPhaseInfoList = nullptr; } -// TODO : Remove this method -// Report collision between two sets of shapes -/* -void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - - // For each possible collision pair of bodies - map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - - OverlappingPair* pair = it->second; - - const ProxyShape* shape1 = pair->getShape1(); - const ProxyShape* shape2 = pair->getShape2(); - - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - - // If both shapes1 and shapes2 sets are non-empty, we check that - // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && - (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { - continue; - } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) - { - continue; - } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) - { - continue; - } - - // For each contact manifold set of the overlapping pair - const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - for (int j=0; jgetNbContactPoints(); i++) { - - ContactPoint* contactPoint = manifold->getContactPoint(i); - - // Create the contact info object for the contact - ContactPointInfo contactInfo; - contactInfo.init(contactPoint->getNormal(), contactPoint->getPenetrationDepth(), - contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2()); - - // Notify the collision callback about this new contact - if (callback != nullptr) callback->notifyContact(contactInfo); - } - } - } -} -*/ - // Compute the broad-phase collision detection void CollisionDetection::computeBroadPhase() { - PROFILE("CollisionDetection::computeBroadPhase()"); + PROFILE("CollisionDetection::computeBroadPhase()", mProfiler); // If new collision shapes have been added to bodies if (mIsCollisionShapesAdded) { @@ -143,17 +94,14 @@ void CollisionDetection::computeBroadPhase() { // 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(mMemoryAllocator); + mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryManager); } } // Compute the middle-phase collision detection void CollisionDetection::computeMiddlePhase() { - PROFILE("CollisionDetection::computeMiddlePhase()"); - - // Clear the set of overlapping pairs in narrow-phase contact - mContactOverlappingPairs.clear(); + PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler); // For each possible collision pair of bodies map::iterator it; @@ -161,26 +109,30 @@ void CollisionDetection::computeMiddlePhase() { 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->mBroadPhaseID != -1); + assert(shape2->mBroadPhaseID != -1); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. Otherwise, we destroy the + // Check if the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || - !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { std::map::iterator itToRemove = it; ++it; - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -188,65 +140,68 @@ void CollisionDetection::computeMiddlePhase() { ++it; } - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); + // Check if the collision filtering allows collision between the two shapes + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) { - // Update the contact cache of the overlapping pair - pair->update(); + 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 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.count(bodiesIndex) > 0) 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.count(bodiesIndex) > 0) continue; - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + bool isShape2Convex = shape2->getCollisionShape()->isConvex(); - // If both shapes are convex - if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + // 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 (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo))) - NarrowPhaseInfo(pair, shape1->getCollisionShape(), - shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData()); - mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; + // 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 ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { - - NarrowPhaseInfo* narrowPhaseInfo = nullptr; - computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &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; + // 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, Allocator& allocator, +void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, NarrowPhaseInfo** firstNarrowPhaseInfo) { ProxyShape* shape1 = pair->getShape1(); @@ -275,11 +230,18 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape, concaveShape, allocator); - // Compute the convex shape AABB in the local-space of the convex shape - AABB aabb; - convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); +#ifdef IS_PROFILING_ACTIVE - // TODO : Implement smooth concave mesh collision somewhere + // 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); @@ -292,170 +254,65 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair // Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhase() { - PROFILE("CollisionDetection::computeNarrowPhase()"); + PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler); - const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; + 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(); - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + 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(); + // 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. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactPointInfo)) { + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mMemoryManager.getSingleFrameAllocator())) { - // If it is the first contact since the pairs are overlapping - if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) { + // Add the contact points as a potential contact manifold into the pair + currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - // Trigger a callback event - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactPointInfo); - } - - // Create a new contact - ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint))) - ContactPoint(contactPointInfo); - - contact->updateWorldContactPoints(currentNarrowPhaseInfo->shape1ToWorldTransform, - currentNarrowPhaseInfo->shape2ToWorldTransform); - - // Add the contact to the contact manifold set of the corresponding overlapping pair - currentNarrowPhaseInfo->overlappingPair->addContact(contact); - - // Add the overlapping pair into the set of pairs in contact during narrow-phase - overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(), - currentNarrowPhaseInfo->overlappingPair->getShape2()); - mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair; - - // Trigger a callback event for the new contact - if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactPointInfo); + lastCollisionFrameInfo->wasColliding = true; } + else { + lastCollisionFrameInfo->wasColliding = false; + } + + // The previous frame collision info is now valid + lastCollisionFrameInfo->isValid = true; } + NarrowPhaseInfo* narrowPhaseInfoToDelete = currentNarrowPhaseInfo; currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; + + // Call the destructor + narrowPhaseInfoToDelete->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); } + // Convert the potential contact into actual contacts + processAllPotentialContacts(); + // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); + + // Report contacts to the user + reportAllContacts(); } -// TODO : Remove this method -// Compute the narrow-phase collision detection -/* -void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - - mContactOverlappingPairs.clear(); - - // For each possible collision pair of bodies - map::iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - - OverlappingPair* pair = it->second; - - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - - assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); - - // If both shapes1 and shapes2 sets are non-empty, we check that - // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) && - (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) { - ++it; - continue; - } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0) - { - ++it; - continue; - } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0) - { - ++it; - continue; - } - - // Check if the collision filtering allows collision between the two shapes and - // that the two shapes are still overlapping. Otherwise, we destroy the - // overlapping pair - if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || - !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - - std::map::iterator itToRemove = it; - ++it; - - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - - // Destroy the overlapping pair - itToRemove->second->~OverlappingPair(); - mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); - mOverlappingPairs.erase(itToRemove); - continue; - } - else { - ++it; - } - - CollisionBody* const body1 = shape1->getBody(); - CollisionBody* const body2 = shape2->getBody(); - - // Update the contact cache of the overlapping pair - pair->update(); - - // Check if the two bodies are allowed to collide, otherwise, we do not test for collision - if (body1->getType() != BodyType::DYNAMIC && body2->getType() != BodyType::DYNAMIC) continue; - bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); - if (mNoCollisionPairs.count(bodiesIndex) > 0) continue; - - // Check if the two bodies are sleeping, if so, we do no test collision between them - if (body1->isSleeping() && body2->isSleeping()) continue; - - // Select the narrow phase algorithm to use according to the two collision shapes - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; - - // If there is no collision algorithm between those two kinds of shapes - if (narrowPhaseAlgorithm == nullptr) continue; - - // Create the CollisionShapeInfo objects - CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), - pair, shape1->getCachedCollisionData()); - CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), - pair, shape2->getCachedCollisionData()); - - TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); - - // Use the narrow-phase collision detection algorithm to check - // if there really is a collision - narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback); - } - - // Add all the contact manifolds (between colliding bodies) to the bodies - addAllContactManifoldsToBodies(); -} -*/ - // 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->mBroadPhaseID != -1); + assert(shape2->mBroadPhaseID != -1); assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID); // Check if the collision filtering allows collision between the two shapes @@ -468,13 +325,10 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro // Check if the overlapping pair already exists if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return; - // Compute the maximum number of contact manifolds for this pair - int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), - shape2->getCollisionShape()->getType()); - // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator); + OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(), + mMemoryManager.getSingleFrameAllocator()); assert(newPair != nullptr); #ifndef NDEBUG @@ -491,6 +345,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro // Remove a body from the collision detection void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { + assert(proxyShape->mBroadPhaseID != -1); + // Remove all the overlapping pairs involving this proxy shape std::map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { @@ -503,7 +359,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); } else { @@ -517,9 +373,11 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { void CollisionDetection::addAllContactManifoldsToBodies() { + PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler); + // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; - for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++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 @@ -538,73 +396,126 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); // For each contact manifold in the set of manifolds in the pair - for (int i=0; igetNbContactPoints() > 0); // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement1 = new (allocatedMemory1) + 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 - void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); - ContactManifoldListElement* listElement2 = new (allocatedMemory2) + ContactManifoldListElement* listElement2 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(ContactManifoldListElement))) ContactManifoldListElement(contactManifold, body2->mContactManifoldsList); body2->mContactManifoldsList = listElement2; + + contactManifold = contactManifold->getNext(); } } -// Delete all the contact points in the currently overlapping pairs -void CollisionDetection::clearContactPoints() { +/// Convert the potential contact into actual contacts +void CollisionDetection::processAllPotentialContacts() { - // For each overlapping pair + PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); + + // For each overlapping pairs in contact during the narrow-phase std::map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - it->second->clearContactPoints(); + + // Process the potential contacts of the overlapping pair + processPotentialContacts(it->second); + } +} + +// Process the potential contact manifold of a pair to create actual contact manifold +void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { + + // Reduce the number of contact points of the manifold + pair->reducePotentialContactManifolds(); + + // Add all the potential contact manifolds as actual contact manifolds to the pair + ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); + while (potentialManifold != nullptr) { + + pair->addContactManifold(potentialManifold); + + potentialManifold = potentialManifold->mNext; + } + + // 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() { + + PROFILE("CollisionDetection::reportAllContacts()", mProfiler); + + // For each overlapping pairs in contact during the narrow-phase + std::map::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(ProxyShape* shape1, ProxyShape* shape2) { +NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) { - // Create a temporary overlapping pair - OverlappingPair pair(shape1, shape2, 0, mMemoryAllocator); + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); // ------------------------------------------------------- - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + const bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + const bool isShape2Convex = shape2->getCollisionShape()->isConvex(); NarrowPhaseInfo* narrowPhaseInfo = nullptr; + pair->makeLastFrameCollisionInfosObsolete(); + // If both shapes are convex - if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) { + if ((isShape1Convex && isShape2Convex)) { // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(&pair, shape1->getCollisionShape(), + narrowPhaseInfo = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), - shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(), - shape2->getCachedCollisionData()); + shape2->getLocalToWorldTransform(), mMemoryManager.getPoolAllocator()); } // Concave vs Convex algorithm - else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) || - (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) { + 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, mMemoryAllocator, &narrowPhaseInfo); + computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), &narrowPhaseInfo); } + pair->clearObsoleteLastFrameCollisionInfos(); + return narrowPhaseInfo; } @@ -616,7 +527,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over std::unordered_set reportedBodies; // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); // For each overlaping proxy shape @@ -666,11 +577,12 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // Test if the AABBs of the two proxy shapes overlap if (aabb1.testCollision(aabb2)) { - const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); + // Create a temporary overlapping pair + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); bool isColliding = false; @@ -680,10 +592,11 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // 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 - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { @@ -691,16 +604,18 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) // 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. - ContactPointInfo contactPointInfo; - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo); + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } // Return if we have found a narrow-phase collision @@ -731,84 +646,91 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl ProxyShape* bodyProxyShape = body->getProxyShapesList(); while (bodyProxyShape != nullptr) { - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + if (bodyProxyShape->mBroadPhaseID != -1) { - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); - const bodyindex bodyId = body->getID(); + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + const bodyindex bodyId = body->getID(); - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { - // 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) { + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + // 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) { - const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); - bool isColliding = false; + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { + bool isColliding = false; - // If we have not found a collision yet - if (!isColliding) { + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { - // Select the narrow phase algorithm to use according to the two collision shapes - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + // If we have not found a collision yet + if (!isColliding) { - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - // 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. - ContactPointInfo contactPointInfo; - isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo); + // 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)); } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; + // Return if we have found a narrow-phase collision + if (isColliding) { - // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } + CollisionBody* overlapBody = proxyShape->getBody(); - // Return if we have found a narrow-phase collision - if (isColliding) { + // Add the body into the set of reported bodies + reportedBodies.insert(overlapBody->getID()); - CollisionBody* overlapBody = proxyShape->getBody(); - - // Add the body into the set of reported bodies - reportedBodies.insert(overlapBody->getID()); - - // Notify the overlap to the user - overlapCallback->notifyOverlap(overlapBody); + // Notify the overlap to the user + overlapCallback->notifyOverlap(overlapBody); + } } } - } - // Go to the next overlapping proxy shape - element = element->next; + // Go to the next overlapping proxy shape + element = element->next; + } } // Go to the next proxy shape @@ -836,19 +758,21 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Test if the AABBs of the two proxy shapes overlap if (aabb1.testCollision(aabb2)) { - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape); + // Create a temporary overlapping pair + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); - const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType(); + // 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 - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { @@ -856,23 +780,32 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // 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. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { - CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body1, body2, - body1ProxyShape, body2ProxyShape); - - // Report the contact to the user - collisionCallback->notifyContact(collisionInfo); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); + + if (pair.hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); + collisionCallback->notifyContact(collisionInfo); + } } // Go to the next proxy shape @@ -893,76 +826,89 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c ProxyShape* bodyProxyShape = body->getProxyShapesList(); while (bodyProxyShape != nullptr) { - // Get the AABB of the shape - const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); + if (bodyProxyShape->mBroadPhaseID != -1) { - // Ask the broad-phase to get all the overlapping shapes - LinkedList overlappingNodes(mMemoryAllocator); - mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID); - const bodyindex bodyId = body->getID(); + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); - // For each overlaping proxy shape - LinkedList::ListElement* element = overlappingNodes.getListHead(); - while (element != nullptr) { + const bodyindex bodyId = body->getID(); - // Get the overlapping proxy shape - int broadPhaseId = element->data; - ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { - // If the two proxy collision shapes are not from the same body - if (proxyShape->getBody()->getID() != bodyId) { + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); - // Check if the collision filtering allows collision between the two shapes - if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + // If the two proxy collision shapes are not from the same body + if (proxyShape->getBody()->getID() != bodyId) { - const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType(); + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape); + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); - // For each narrow-phase info object - while (narrowPhaseInfo != nullptr) { + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); - // Select the narrow phase algorithm to use according to the two collision shapes - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { - // If there is a collision algorithm for those two kinds of shapes - if (narrowPhaseAlgorithm != nullptr) { + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); - // 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. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); - CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body, - proxyShape->getBody(), bodyProxyShape, - proxyShape); + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { - // Report the contact to the user - callback->notifyContact(collisionInfo); + // 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())) { + + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + } } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; - narrowPhaseInfo = narrowPhaseInfo->next; + // Process the potential contacts + processPotentialContacts(&pair); - // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + if (pair.hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); + callback->notifyContact(collisionInfo); + } } } + + // Go to the next overlapping proxy shape + element = element->next; } - // Go to the next overlapping proxy shape - element = element->next; + // Go to the next proxy shape + bodyProxyShape = bodyProxyShape->getNext(); } - - // Go to the next proxy shape - bodyProxyShape = bodyProxyShape->getNext(); } } @@ -978,10 +924,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { map::iterator it; for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { - OverlappingPair* pair = it->second; + OverlappingPair* originalPair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); + // Create a new overlapping pair so that we do not work on the original one + OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator()); + + 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. @@ -990,18 +940,16 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(shape1, shape2); - - const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + 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 - const int shape1Index = static_cast(shape1Type); - const int shape2Index = static_cast(shape2Type); - NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index]; + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); // If there is a collision algorithm for those two kinds of shapes if (narrowPhaseAlgorithm != nullptr) { @@ -1009,23 +957,32 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // 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. - ContactPointInfo contactPointInfo; - if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) { + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { - CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, shape1->getBody(), - shape2->getBody(), shape1, shape2); - - // Report the contact to the user - callback->notifyContact(collisionInfo); + // Add the contact points as a potential contact manifold into the pair + narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); } } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + // Release the allocated memory - mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } + + // Process the potential contacts + processPotentialContacts(&pair); + + if (pair.hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); + callback->notifyContact(collisionInfo); + } } } } @@ -1046,7 +1003,8 @@ EventListener* CollisionDetection::getWorldEventListener() { return mWorld->mEventListener; } -/// Return a reference to the world memory allocator -PoolAllocator& CollisionDetection::getWorldMemoryAllocator() { - return mWorld->mPoolAllocator; +// Return the world-space AABB of a given proxy shape +const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const { + assert(proxyShape->mBroadPhaseID > -1); + return mBroadPhaseAlgorithm.getFatAABB(proxyShape->mBroadPhaseID); } diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 3bb27366..6aeac709 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -32,8 +32,7 @@ #include "engine/OverlappingPair.h" #include "engine/EventListener.h" #include "narrowphase/DefaultCollisionDispatch.h" -#include "memory/PoolAllocator.h" -#include "memory/SingleFrameAllocator.h" +#include "memory/MemoryManager.h" #include "constraint/ContactPoint.h" #include #include @@ -62,6 +61,9 @@ class CollisionDetection { // -------------------- Attributes -------------------- // + /// Memory manager + MemoryManager& mMemoryManager; + /// Collision Detection Dispatch configuration CollisionDispatch* mCollisionDispatch; @@ -71,12 +73,6 @@ class CollisionDetection { /// Collision detection matrix (algorithms to use) NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; - /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; - - /// Reference to the single frame memory allocator - SingleFrameAllocator& mSingleFrameAllocator; - /// Pointer to the physics world CollisionWorld* mWorld; @@ -86,9 +82,6 @@ class CollisionDetection { /// Broad-phase overlapping pairs std::map mOverlappingPairs; - /// Overlapping pairs in contact (during the current Narrow-phase collision detection) - std::map mContactOverlappingPairs; - /// Broad-phase algorithm BroadPhaseAlgorithm mBroadPhaseAlgorithm; @@ -103,6 +96,13 @@ class CollisionDetection { /// True if some collision shapes have been added previously bool mIsCollisionShapesAdded; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Compute the broad-phase collision detection @@ -118,28 +118,41 @@ class CollisionDetection { /// involed in the corresponding contact. void addContactManifoldToBody(OverlappingPair* pair); - /// Delete all the contact points in the currently overlapping pairs - void clearContactPoints(); - /// Fill-in the collision detection matrix void fillInCollisionMatrix(); + /// Return the corresponding narrow-phase algorithm + NarrowPhaseAlgorithm* selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, + const CollisionShapeType& shape2Type) const; + /// Add all the contact manifold of colliding pairs to their bodies void addAllContactManifoldsToBodies(); /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies - void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator, + void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, NarrowPhaseInfo** firstNarrowPhaseInfo); /// Compute the middle-phase collision detection between two proxy shapes - NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2); + NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair); + + /// Convert the potential contact into actual contacts + void processAllPotentialContacts(); + + /// Process the potential contact manifold of a pair to create actual contact manifold + void processPotentialContacts(OverlappingPair* pair); + + /// Report contacts for all the colliding overlapping pairs + void reportAllContacts(); + + /// Process the potential contacts where one collion is a concave shape + void processSmoothMeshContacts(OverlappingPair* pair); public : // -------------------- Methods -------------------- // /// Constructor - CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator); + CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager); /// Destructor ~CollisionDetection() = default; @@ -153,10 +166,6 @@ class CollisionDetection { /// Set the collision dispatch configuration void setCollisionDispatch(CollisionDispatch* collisionDispatch); - /// Return the Narrow-phase collision detection algorithm to use between two types of shapes - NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type, - CollisionShapeType shape2Type) const; - /// Add a proxy collision shape to the collision detection void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); @@ -179,12 +188,6 @@ class CollisionDetection { /// Compute the collision detection void computeCollisionDetection(); - // TODO : Remove this method - /// Report collision between two sets of shapes - //void reportCollisionBetweenShapes(CollisionCallback* callback, - // const std::set& shapes1, - // const std::set& shapes2) ; - /// Ray casting method void raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const; @@ -210,20 +213,21 @@ class CollisionDetection { /// Allow the broadphase to notify the collision detection about an overlapping pair. void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); - // TODO : Remove this method - /// Compute the narrow-phase collision detection - //void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - // const std::set& shapes1, - // const std::set& shapes2); - /// Return a pointer to the world CollisionWorld* getWorld(); /// Return the world event listener EventListener* getWorldEventListener(); - /// Return a reference to the world memory allocator - PoolAllocator& getWorldMemoryAllocator(); +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + + /// Return the world-space AABB of a given proxy shape + const AABB getWorldAABB(const ProxyShape* proxyShape) const; // -------------------- Friendship -------------------- // @@ -231,18 +235,20 @@ class CollisionDetection { friend class ConvexMeshShape; }; -// Return the Narrow-phase collision detection algorithm to use between two types of shapes -inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, - CollisionShapeType shape2Type) const { - return mCollisionMatrix[static_cast(shape1Type)][static_cast(shape2Type)]; -} - // Set the collision dispatch configuration inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { mCollisionDispatch = collisionDispatch; // Fill-in the collision matrix with the new algorithms to use fillInCollisionMatrix(); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + mCollisionDispatch->setProfiler(mProfiler); + +#endif + } // Add a body to the collision detection @@ -271,7 +277,10 @@ inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, /// We simply put the shape in the list of collision shape that have moved in the /// previous frame so that it is tested for collision again in the broad-phase. inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { - mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); + + if (shape->mBroadPhaseID != -1) { + mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID); + } } // Update a proxy collision shape (that has moved for instance) @@ -280,12 +289,31 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } +// Return the corresponding narrow-phase algorithm +inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type, + const CollisionShapeType& shape2Type) const { + + uint shape1Index = static_cast(shape1Type); + uint shape2Index = static_cast(shape2Type); + + // Swap the shape types if necessary + if (shape1Index > shape2Index) { + const uint tempIndex = shape1Index; + shape1Index = shape2Index; + shape2Index = tempIndex; + } + + assert(shape1Index <= shape2Index); + + return mCollisionMatrix[shape1Index][shape2Index]; +} + // Ray casting method inline void CollisionDetection::raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const { - PROFILE("CollisionDetection::raycast()"); + PROFILE("CollisionDetection::raycast()", mProfiler); RaycastTest rayCastTest(raycastCallback); @@ -299,6 +327,17 @@ inline CollisionWorld* CollisionDetection::getWorld() { return mWorld; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionDetection::setProfiler(Profiler* profiler) { + mProfiler = profiler; + mBroadPhaseAlgorithm.setProfiler(profiler); + mCollisionDispatch->setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index dc40774b..ba1bcb1e 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -30,231 +30,159 @@ using namespace reactphysics3d; // Constructor -ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, short normalDirectionId) - : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), +ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator) + : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), - mMemoryAllocator(memoryAllocator) { + mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) { + // For each contact point info in the manifold + const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo(); + while(pointInfo != nullptr) { + + // Add the new contact point + addContactPoint(pointInfo); + + pointInfo = pointInfo->next; + } + + assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); + assert(mNbContactPoints > 0); } // Destructor ContactManifold::~ContactManifold() { - clear(); + + // Delete all the contact points + ContactPoint* contactPoint = mContactPoints; + while(contactPoint != nullptr) { + + ContactPoint* nextContactPoint = contactPoint->getNext(); + + // Delete the contact point + contactPoint->~ContactPoint(); + mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); + + contactPoint = nextContactPoint; + } } -// Add a contact point in the manifold -void ContactManifold::addContactPoint(ContactPoint* contact) { - - // For contact already in the manifold - for (uint i=0; igetWorldPointOnBody1() - - contact->getWorldPointOnBody1()).lengthSquare(); - if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { - - // Delete the new contact - contact->~ContactPoint(); - mMemoryAllocator.release(contact, sizeof(ContactPoint)); - - assert(mNbContactPoints > 0); - - return; - } - } - - // If the contact manifold is full - if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { - int indexMaxPenetration = getIndexOfDeepestPenetration(contact); - int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); - removeContactPoint(indexToRemove); - } - - // Add the new contact point in the manifold - mContactPoints[mNbContactPoints] = contact; - mNbContactPoints++; +// Remove a contact point +void ContactManifold::removeContactPoint(ContactPoint* contactPoint) { assert(mNbContactPoints > 0); -} + assert(mContactPoints != nullptr); + assert(contactPoint != nullptr); -// Remove a contact point from the manifold -void ContactManifold::removeContactPoint(uint index) { - assert(index < mNbContactPoints); - assert(mNbContactPoints > 0); - - // Call the destructor explicitly and tell the memory allocator that - // the corresponding memory block is now free - mContactPoints[index]->~ContactPoint(); - mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint)); - - // If we don't remove the last index - if (index < mNbContactPoints - 1) { - mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; - } + ContactPoint* previous = contactPoint->getPrevious(); + ContactPoint* next = contactPoint->getNext(); - mNbContactPoints--; -} - -// Update the contact manifold -/// First the world space coordinates of the current contacts in the manifold are recomputed from -/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts -/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also -/// the contacts with a too large distance between the contact points in the plane orthogonal to the -/// contact normal. -void ContactManifold::update(const Transform& transform1, const Transform& transform2) { - - if (mNbContactPoints == 0) return; - - // Update the world coordinates and penetration depth of the contact points in the manifold - for (uint i=0; iupdateWorldContactPoints(transform1, transform2); - mContactPoints[i]->updatePenetrationDepth(); - } - - const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * - PERSISTENT_CONTACT_DIST_THRESHOLD; - - // Remove the contact points that don't represent very well the contact manifold - for (int i=static_cast(mNbContactPoints)-1; i>=0; i--) { - assert(i < static_cast(mNbContactPoints)); - - // Compute the distance between contact points in the normal direction - decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); - - // If the contacts points are too far from each other in the normal direction - if (distanceNormal > squarePersistentContactThreshold) { - removeContactPoint(i); - } - else { - // Compute the distance of the two contact points in the plane - // orthogonal to the contact normal - Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + - mContactPoints[i]->getNormal() * distanceNormal; - Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; - - // If the orthogonal distance is larger than the valid distance - // threshold, we remove the contact - if (projDifference.lengthSquare() > squarePersistentContactThreshold) { - removeContactPoint(i); - } - } - } -} - -// Return the index of the contact point with the larger penetration depth. -/// This corresponding contact will be kept in the cache. The method returns -1 is -/// the new contact is the deepest. -int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { - assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); - int indexMaxPenetrationDepth = -1; - decimal maxPenetrationDepth = newContact->getPenetrationDepth(); - - // For each contact in the cache - for (uint i=0; igetPenetrationDepth() > maxPenetrationDepth) { - maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); - indexMaxPenetrationDepth = i; - } - } - - // Return the index of largest penetration depth - return indexMaxPenetrationDepth; -} - -// Return the index that will be removed. -/// The index of the contact point with the larger penetration -/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute -/// the different area and we want to keep the contacts with the largest area. The new point is also -/// kept. In order to compute the area of a quadrilateral, we use the formula : -/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that -/// when we compute this area, we do not calculate it exactly but we -/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, -/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library -/// by Erwin Coumans (http://wwww.bulletphysics.org). -int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { - - assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); - - decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint - decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint - decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint - decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint - - if (indexMaxPenetration != 0) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area0 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 1) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area1 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 2) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area2 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 3) { - // Compute the area - Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area3 = crossProduct.lengthSquare(); - } - - // Return the index of the contact to remove - return getMaxArea(area0, area1, area2, area3); -} - -// Return the index of maximum area -int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { - if (area0 < area1) { - if (area1 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area1 < area3) return 3; - else return 1; - } + if (previous != nullptr) { + previous->setNext(next); } else { - if (area0 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area0 < area3) return 3; - else return 0; - } + mContactPoints = next; } + + if (next != nullptr) { + next->setPrevious(previous); + } + + // Delete the contact point + contactPoint->~ContactPoint(); + mMemoryAllocator.release(contactPoint, sizeof(ContactPoint)); + + mNbContactPoints--; + assert(mNbContactPoints >= 0); } -// Clear the contact manifold -void ContactManifold::clear() { - for (uint i=0; i~ContactPoint(); - mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint)); +// Add a contact point +void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) { + + assert(contactPointInfo != nullptr); + + // Create the new contact point + ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo); + + // Add the new contact point into the manifold + contactPoint->setNext(mContactPoints); + contactPoint->setPrevious(nullptr); + if (mContactPoints != nullptr) { + mContactPoints->setPrevious(contactPoint); } - mNbContactPoints = 0; + mContactPoints = contactPoint; + + mNbContactPoints++; +} + +// Clear the obsolete contact points +void ContactManifold::clearObsoleteContactPoints() { + + assert(mContactPoints != nullptr); + + // For each contact point of the manifold + ContactPoint* contactPoint = mContactPoints; + while (contactPoint != nullptr) { + + ContactPoint* nextContactPoint = contactPoint->getNext(); + + // If the contact point is obsolete + if (contactPoint->getIsObsolete()) { + + // Remove the contact point + removeContactPoint(contactPoint); + } + + contactPoint = nextContactPoint; + } + + assert(mNbContactPoints > 0); + assert(mContactPoints != nullptr); +} + +// Make sure we do not have too much contact points by keeping only the best +// contact points of the manifold (with largest penetration depth) +void ContactManifold::reduce() { + + assert(mContactPoints != nullptr); + + // Remove contact points while there is too much contact points + while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { + removeNonOptimalContactPoint(); + } + + assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0); + assert(mContactPoints != nullptr); +} + +// Remove a contact point that is not optimal (with a small penetration depth) +void ContactManifold::removeNonOptimalContactPoint() { + + assert(mContactPoints != nullptr); + assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD); + + // Get the contact point with the minimum penetration depth among all points + ContactPoint* contactPoint = mContactPoints; + ContactPoint* minContactPoint = nullptr; + decimal minPenetrationDepth = DECIMAL_LARGEST; + while (contactPoint != nullptr) { + + ContactPoint* nextContactPoint = contactPoint->getNext(); + + if (contactPoint->getPenetrationDepth() < minPenetrationDepth) { + + minContactPoint = contactPoint; + minPenetrationDepth = contactPoint->getPenetrationDepth(); + } + + contactPoint = nextContactPoint; + } + + assert(minContactPoint != nullptr); + + // Remove the non optimal contact point + removeContactPoint(minContactPoint); + + assert(mNbContactPoints > 0); + assert(mContactPoints != nullptr); } diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index c36c1476..f513ad82 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -31,14 +31,12 @@ #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" #include "memory/PoolAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { -// Constants -const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold - // Class declarations class ContactManifold; @@ -48,40 +46,48 @@ class ContactManifold; */ struct ContactManifoldListElement { - public: + private: // -------------------- Attributes -------------------- // - /// Pointer to the actual contact manifold - ContactManifold* contactManifold; + /// Pointer to a contact manifold with contact points + ContactManifold* mContactManifold; /// Next element of the list - ContactManifoldListElement* next; + ContactManifoldListElement* mNext; + + public: // -------------------- Methods -------------------- // /// Constructor - ContactManifoldListElement(ContactManifold* initContactManifold, - ContactManifoldListElement* initNext) - :contactManifold(initContactManifold), next(initNext) { + ContactManifoldListElement(ContactManifold* contactManifold, + ContactManifoldListElement* next) + :mContactManifold(contactManifold), mNext(next) { } + + /// Return the contact manifold + ContactManifold* getContactManifold() { + return mContactManifold; + } + + /// Return the next element in the linked-list + ContactManifoldListElement* getNext() { + return mNext; + } }; // Class ContactManifold /** - * This class represents the set of contact points between two bodies. + * This class represents a set of contact points between two bodies that + * all have a similar contact normal direction. Usually, there is a single + * contact manifold when two convex shapes are in contact. However, when + * a convex shape collides with a concave shape, there can be several + * contact manifolds with different normal directions. * The contact manifold is implemented in a way to cache the contact - * points among the frames for better stability following the - * "Contact Generation" presentation of Erwin Coumans at GDC 2010 - * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf). - * Some code of this class is based on the implementation of the - * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org). - * The contacts between two bodies are added one after the other in the cache. - * When the cache is full, we have to remove one point. The idea is to keep - * the point with the deepest penetration depth and also to keep the - * points producing the larger area (for a more stable contact manifold). - * The new added point is always kept. + * points among the frames for better stability (warm starting of the + * contact solver) */ class ContactManifold { @@ -96,10 +102,7 @@ class ContactManifold { ProxyShape* mShape2; /// Contact points in the manifold - ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; - - /// Normal direction Id (Unique Id representing the normal direction) - short int mNormalDirectionId; + ContactPoint* mContactPoints; /// Number of contacts in the cache int8 mNbContactPoints; @@ -126,32 +129,95 @@ class ContactManifold { bool mIsAlreadyInIsland; /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + MemoryAllocator& mMemoryAllocator; + + /// Pointer to the next contact manifold in the linked-list + ContactManifold* mNext; + + /// Pointer to the previous contact manifold in linked-list + ContactManifold* mPrevious; + + /// True if the contact manifold is obsolete + bool mIsObsolete; // -------------------- Methods -------------------- // - /// Return the index of maximum area - int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; - - /// Return the index of the contact with the larger penetration depth. - int getIndexOfDeepestPenetration(ContactPoint* newContact) const; - - /// Return the index that will be removed. - int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; - - /// Remove a contact point from the manifold - void removeContactPoint(uint index); - /// Return true if the contact manifold has already been added into an island bool isAlreadyInIsland() const; - + + /// Set the pointer to the next element in the linked-list + void setNext(ContactManifold* nextManifold); + + /// Return true if the manifold is obsolete + bool getIsObsolete() const; + + /// Set to true to make the manifold obsolete + void setIsObsolete(bool isObselete, bool setContactPoints); + + /// Clear the obsolete contact points + void clearObsoleteContactPoints(); + + /// Return the contact normal direction Id of the manifold + short getContactNormalId() const; + + /// Return the largest depth of all the contact points + decimal getLargestContactDepth() const; + + /// set the first friction vector at the center of the contact manifold + void setFrictionVector1(const Vector3& mFrictionVector1); + + /// set the second friction vector at the center of the contact manifold + void setFrictionVector2(const Vector3& mFrictionVector2); + + /// Set the first friction accumulated impulse + void setFrictionImpulse1(decimal frictionImpulse1); + + /// Set the second friction accumulated impulse + void setFrictionImpulse2(decimal frictionImpulse2); + + /// Add a contact point + void addContactPoint(const ContactPointInfo* contactPointInfo); + + /// Make sure we do not have too much contact points by keeping only the best ones + void reduce(); + + /// Remove a contact point that is not optimal (with a small penetration depth) + void removeNonOptimalContactPoint(); + + /// Remove a contact point + void removeContactPoint(ContactPoint* contactPoint); + + /// Set the friction twist accumulated impulse + void setFrictionTwistImpulse(decimal frictionTwistImpulse); + + /// Set the accumulated rolling resistance impulse + void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); + + /// Set the pointer to the previous element in the linked-list + void setPrevious(ContactManifold* previousManifold); + + /// Return the first friction vector at the center of the contact manifold + const Vector3& getFrictionVector1() const; + + /// Return the second friction vector at the center of the contact manifold + const Vector3& getFrictionVector2() const; + + /// Return the first friction accumulated impulse + decimal getFrictionImpulse1() const; + + /// Return the second friction accumulated impulse + decimal getFrictionImpulse2() const; + + /// Return the friction twist accumulated impulse + decimal getFrictionTwistImpulse() const; + public: // -------------------- Methods -------------------- // /// Constructor - ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, short int normalDirectionId); + ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, + MemoryAllocator& memoryAllocator); /// Destructor ~ContactManifold(); @@ -174,68 +240,25 @@ class ContactManifold { /// Return a pointer to the second body of the contact manifold CollisionBody* getBody2() const; - /// Return the normal direction Id - short int getNormalDirectionId() const; - - /// Add a contact point to the manifold - void addContactPoint(ContactPoint* contact); - - /// Update the contact manifold. - void update(const Transform& transform1, const Transform& transform2); - - /// Clear the contact manifold - void clear(); - /// Return the number of contact points in the manifold int8 getNbContactPoints() const; - /// Return the first friction vector at the center of the contact manifold - const Vector3& getFrictionVector1() const; + /// Return a pointer to the first contact point of the manifold + ContactPoint* getContactPoints() const; - /// set the first friction vector at the center of the contact manifold - void setFrictionVector1(const Vector3& mFrictionVector1); + /// Return a pointer to the previous element in the linked-list + ContactManifold* getPrevious() const; - /// Return the second friction vector at the center of the contact manifold - const Vector3& getFrictionVector2() const; - - /// set the second friction vector at the center of the contact manifold - void setFrictionVector2(const Vector3& mFrictionVector2); - - /// Return the first friction accumulated impulse - decimal getFrictionImpulse1() const; - - /// Set the first friction accumulated impulse - void setFrictionImpulse1(decimal frictionImpulse1); - - /// Return the second friction accumulated impulse - decimal getFrictionImpulse2() const; - - /// Set the second friction accumulated impulse - void setFrictionImpulse2(decimal frictionImpulse2); - - /// Return the friction twist accumulated impulse - decimal getFrictionTwistImpulse() const; - - /// Set the friction twist accumulated impulse - void setFrictionTwistImpulse(decimal frictionTwistImpulse); - - /// Set the accumulated rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse); - - /// Return a contact point of the manifold - ContactPoint* getContactPoint(uint index) const; - - /// Return the normalized averaged normal vector - Vector3 getAverageContactNormal() const; - - /// Return the largest depth of all the contact points - decimal getLargestContactDepth() const; + /// Return a pointer to the next element in the linked-list + ContactManifold* getNext() const; // -------------------- Friendship -------------------- // friend class DynamicsWorld; friend class Island; friend class CollisionBody; + friend class ContactManifoldSet; + friend class ContactSolver; }; // Return a pointer to the first proxy shape of the contact @@ -258,11 +281,6 @@ inline CollisionBody* ContactManifold::getBody2() const { return mShape2->getBody(); } -// Return the normal direction Id -inline short int ContactManifold::getNormalDirectionId() const { - return mNormalDirectionId; -} - // Return the number of contact points in the manifold inline int8 ContactManifold::getNbContactPoints() const { return mNbContactPoints; @@ -323,10 +341,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR mRollingResistanceImpulse = rollingResistanceImpulse; } -// Return a contact point of the manifold -inline ContactPoint* ContactManifold::getContactPoint(uint index) const { - assert(index < mNbContactPoints); - return mContactPoints[index]; +// Return a pointer to the first contact point of the manifold +inline ContactPoint* ContactManifold::getContactPoints() const { + return mContactPoints; } // Return true if the contact manifold has already been added into an island @@ -334,31 +351,65 @@ inline bool ContactManifold::isAlreadyInIsland() const { return mIsAlreadyInIsland; } -// Return the normalized averaged normal vector -inline Vector3 ContactManifold::getAverageContactNormal() const { - Vector3 averageNormal; - - for (uint i=0; igetNormal(); - } - - return averageNormal.getUnit(); -} - // Return the largest depth of all the contact points inline decimal ContactManifold::getLargestContactDepth() const { decimal largestDepth = 0.0f; - for (uint i=0; igetPenetrationDepth(); + assert(mNbContactPoints > 0); + + ContactPoint* contactPoint = mContactPoints; + while(contactPoint != nullptr){ + decimal depth = contactPoint->getPenetrationDepth(); if (depth > largestDepth) { largestDepth = depth; } + + contactPoint = contactPoint->getNext(); } return largestDepth; } +// Return a pointer to the previous element in the linked-list +inline ContactManifold* ContactManifold::getPrevious() const { + return mPrevious; } + +// Set the pointer to the previous element in the linked-list +inline void ContactManifold::setPrevious(ContactManifold* previousManifold) { + mPrevious = previousManifold; +} + +// Return a pointer to the next element in the linked-list +inline ContactManifold* ContactManifold::getNext() const { + return mNext; +} + +// Set the pointer to the next element in the linked-list +inline void ContactManifold::setNext(ContactManifold* nextManifold) { + mNext = nextManifold; +} + +// Return true if the manifold is obsolete +inline bool ContactManifold::getIsObsolete() const { + return mIsObsolete; +} + +// Set to true to make the manifold obsolete +inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoints) { + mIsObsolete = isObsolete; + + if (setContactPoints) { + ContactPoint* contactPoint = mContactPoints; + while (contactPoint != nullptr) { + contactPoint->setIsObsolete(isObsolete); + + contactPoint = contactPoint->getNext(); + } + } +} + +} + #endif diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp new file mode 100644 index 00000000..a28e3817 --- /dev/null +++ b/src/collision/ContactManifoldInfo.cpp @@ -0,0 +1,280 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "ContactManifoldInfo.h" + +using namespace reactphysics3d; + +// Constructor +ContactManifoldInfo::ContactManifoldInfo(MemoryAllocator& allocator) + : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) { + +} + +// Destructor +ContactManifoldInfo::~ContactManifoldInfo() { + + // Remove all the contact points + reset(); +} + +// Add a new contact point into the manifold +void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo) { + + assert(contactPointInfo->penetrationDepth > decimal(0.0)); + + // Add it into the linked list of contact points + contactPointInfo->next = mContactPointsList; + mContactPointsList = contactPointInfo; + + mNbContactPoints++; +} + +// Remove all the contact points +void ContactManifoldInfo::reset() { + + // Delete all the contact points in the linked list + ContactPointInfo* element = mContactPointsList; + while(element != nullptr) { + ContactPointInfo* elementToDelete = element; + element = element->next; + + // Call the constructor + elementToDelete->~ContactPointInfo(); + + // Delete the current element + mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + mContactPointsList = nullptr; + mNbContactPoints = 0; +} + +// Return the largest penetration depth among its contact points +decimal ContactManifoldInfo::getLargestPenetrationDepth() const { + + ContactPointInfo* contactPoint = mContactPointsList; + assert(contactPoint != nullptr); + decimal maxDepth = decimal(0.0); + while (contactPoint != nullptr) { + + if (contactPoint->penetrationDepth > maxDepth) { + maxDepth = contactPoint->penetrationDepth; + } + + contactPoint = contactPoint->next; + } + + return maxDepth; +} + +// Reduce the number of contact points of the currently computed manifold +// This is based on the technique described by Dirk Gregorius in his +// "Contacts Creation" GDC presentation +void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { + + assert(mContactPointsList != nullptr); + + // The following algorithm only works to reduce to 4 contact points + assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); + + // If there are too many contact points in the manifold + if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { + + ContactPointInfo* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD]; + + // Compute the initial contact point we need to keep. + // The first point we keep is always the point in a given + // constant direction (in order to always have same contact points + // between frames for better stability) + + const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse(); + + // Compute the contact normal of the manifold (we use the first contact point) + // in the local-space of the first collision shape + const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mContactPointsList->normal; + + // Compute a search direction + const Vector3 searchDirection(1, 1, 1); + ContactPointInfo* element = mContactPointsList; + pointsToKeep[0] = element; + decimal maxDotProduct = searchDirection.dot(element->localPoint1); + element = element->next; + while(element != nullptr) { + + decimal dotProduct = searchDirection.dot(element->localPoint1); + if (dotProduct > maxDotProduct) { + maxDotProduct = dotProduct; + pointsToKeep[0] = element; + } + element = element->next; + } + + // Compute the second contact point we need to keep. + // The second point we keep is the one farthest away from the first point. + + decimal maxDistance = decimal(0.0); + element = mContactPointsList; + while(element != nullptr) { + + if (element == pointsToKeep[0]) { + element = element->next; + continue; + } + + decimal distance = (pointsToKeep[0]->localPoint1 - element->localPoint1).lengthSquare(); + if (distance >= maxDistance) { + maxDistance = distance; + pointsToKeep[1] = element; + } + element = element->next; + } + assert(pointsToKeep[1] != nullptr); + + // Compute the third contact point we need to keep. + // The second point is the one producing the triangle with the larger area + // with first and second point. + + // We compute the most positive or most negative triangle area (depending on winding) + ContactPointInfo* thirdPointMaxArea = nullptr; + ContactPointInfo* thirdPointMinArea = nullptr; + decimal minArea = decimal(0.0); + decimal maxArea = decimal(0.0); + bool isPreviousAreaPositive = true; + element = mContactPointsList; + while(element != nullptr) { + + if (element == pointsToKeep[0] || element == pointsToKeep[1]) { + element = element->next; + continue; + } + + const Vector3 newToFirst = pointsToKeep[0]->localPoint1 - element->localPoint1; + const Vector3 newToSecond = pointsToKeep[1]->localPoint1 - element->localPoint1; + + // Compute the triangle area + decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); + + if (area >= maxArea) { + maxArea = area; + thirdPointMaxArea = element; + } + if (area <= minArea) { + minArea = area; + thirdPointMinArea = element; + } + element = element->next; + } + assert(minArea <= decimal(0.0)); + assert(maxArea >= decimal(0.0)); + if (maxArea > (-minArea)) { + isPreviousAreaPositive = true; + pointsToKeep[2] = thirdPointMaxArea; + } + else { + isPreviousAreaPositive = false; + pointsToKeep[2] = thirdPointMinArea; + } + assert(pointsToKeep[2] != nullptr); + + // Compute the 4th point by choosing the triangle that add the most + // triangle area to the previous triangle and has opposite sign area (opposite winding) + + decimal largestArea = decimal(0.0); // Largest area (positive or negative) + element = mContactPointsList; + + // For each remaining point + while(element != nullptr) { + + if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) { + element = element->next; + continue; + } + + // For each edge of the triangle made by the first three points + for (uint i=0; i<3; i++) { + + uint edgeVertex1Index = i; + uint edgeVertex2Index = i < 2 ? i + 1 : 0; + + const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->localPoint1 - element->localPoint1; + const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->localPoint1 - element->localPoint1; + + // Compute the triangle area + decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); + + // We are looking at the triangle with maximal area (positive or negative). + // If the previous area is positive, we are looking at negative area now. + // If the previous area is negative, we are looking at the positive area now. + if (isPreviousAreaPositive && area < largestArea) { + largestArea = area; + pointsToKeep[3] = element; + } + else if (!isPreviousAreaPositive && area > largestArea) { + largestArea = area; + pointsToKeep[3] = element; + } + } + + element = element->next; + } + assert(pointsToKeep[3] != nullptr); + + // Delete the contact points we do not want to keep from the linked list + element = mContactPointsList; + ContactPointInfo* previousElement = nullptr; + while(element != nullptr) { + + // Skip the points we want to keep + if (element == pointsToKeep[0] || element == pointsToKeep[1] || + element == pointsToKeep[2] || element == pointsToKeep[3]) { + + previousElement = element; + element = element->next; + continue; + } + + ContactPointInfo* elementToDelete = element; + if (previousElement != nullptr) { + previousElement->next = elementToDelete->next; + } + else { + mContactPointsList = elementToDelete->next; + } + element = element->next; + + // Call the destructor + elementToDelete->~ContactPointInfo(); + + // Delete the current element + mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + mNbContactPoints = 4; + } +} + + diff --git a/src/collision/narrowphase/EPA/TrianglesStore.h b/src/collision/ContactManifoldInfo.h similarity index 50% rename from src/collision/narrowphase/EPA/TrianglesStore.h rename to src/collision/ContactManifoldInfo.h index d21ccd35..b2687da5 100644 --- a/src/collision/narrowphase/EPA/TrianglesStore.h +++ b/src/collision/ContactManifoldInfo.h @@ -23,117 +23,91 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_TRIANGLES_STORE_H -#define REACTPHYSICS3D_TRIANGLES_STORE_H - -#include "TriangleEPA.h" - +#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H +#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H // Libraries -#include +#include "collision/ContactPointInfo.h" +#include "memory/MemoryAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { // Constants -constexpr unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles +const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold -// Class TriangleStore +// Class ContactManifoldInfo /** - * This class stores several triangles of the polytope in the EPA algorithm. + * This class is used to collect the list of ContactPointInfo that come + * from a collision test between two shapes. */ -class TrianglesStore { +class ContactManifoldInfo { private: // -------------------- Attributes -------------------- // - /// Triangles - TriangleEPA mTriangles[MAX_TRIANGLES]; + /// Linked list with all the contact points + ContactPointInfo* mContactPointsList; + + /// Number of contact points in the manifold + uint mNbContactPoints; + + /// Next element in the linked-list of contact manifold info + ContactManifoldInfo* mNext; + + /// Reference the the memory allocator where the contact point infos have been allocated + MemoryAllocator& mAllocator; - /// Number of triangles - int mNbTriangles; - public: // -------------------- Methods -------------------- // /// Constructor - TrianglesStore(); + ContactManifoldInfo(MemoryAllocator& allocator); /// Destructor - ~TrianglesStore() = default; + ~ContactManifoldInfo(); - /// Deleted copy-constructor - TrianglesStore(const TrianglesStore& triangleStore) = delete; + /// Deleted Copy-constructor + ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete; /// Deleted assignment operator - TrianglesStore& operator=(const TrianglesStore& triangleStore) = delete; + ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete; - /// Clear all the storage - void clear(); + /// Add a new contact point into the manifold + void addContactPoint(ContactPointInfo* contactPointInfo); - /// Return the number of triangles - int getNbTriangles() const; + /// Remove all the contact points + void reset(); - /// Set the number of triangles - void setNbTriangles(int backup); + /// Get the first contact point info of the linked list of contact points + ContactPointInfo* getFirstContactPointInfo() const; - /// Return the last triangle - TriangleEPA& last(); + /// Return the largest penetration depth among its contact points + decimal getLargestPenetrationDepth() const; - /// Create a new triangle - TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2); + /// Return the pointer to the next manifold info in the linked-list + ContactManifoldInfo* getNext(); - /// Access operator - TriangleEPA& operator[](int i); + /// Reduce the number of contact points of the currently computed manifold + void reduce(const Transform& shape1ToWorldTransform); + + // Friendship + friend class OverlappingPair; + friend class CollisionDetection; }; -// Clear all the storage -inline void TrianglesStore::clear() { - mNbTriangles = 0; +// Get the first contact point info of the linked list of contact points +inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const { + return mContactPointsList; } -// Return the number of triangles -inline int TrianglesStore::getNbTriangles() const { - return mNbTriangles; -} - - -inline void TrianglesStore::setNbTriangles(int backup) { - mNbTriangles = backup; -} - -// Return the last triangle -inline TriangleEPA& TrianglesStore::last() { - assert(mNbTriangles > 0); - return mTriangles[mNbTriangles - 1]; -} - -// Create a new triangle -inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, - uint v0,uint v1, uint v2) { - TriangleEPA* newTriangle = nullptr; - - // If we have not reached the maximum number of triangles - if (mNbTriangles != MAX_TRIANGLES) { - newTriangle = &mTriangles[mNbTriangles++]; - new (newTriangle) TriangleEPA(v0, v1, v2); - if (!newTriangle->computeClosestPoint(vertices)) { - mNbTriangles--; - newTriangle = nullptr; - } - } - - // Return the new triangle - return newTriangle; -} - -// Access operator -inline TriangleEPA& TrianglesStore::operator[](int i) { - return mTriangles[i]; +// Return the pointer to the next manifold info in the linked-list +inline ContactManifoldInfo* ContactManifoldInfo::getNext() { + return mNext; } } - #endif + diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index a7dc9d2e..c63e6a11 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -30,10 +30,12 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, int nbMaxManifolds) - : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), - mShape2(shape2), mMemoryAllocator(memoryAllocator) { + MemoryAllocator& memoryAllocator) + : mNbManifolds(0), mShape1(shape1), + mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) { + // Compute the maximum number of manifolds allowed between the two shapes + mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape()); } // Destructor @@ -43,194 +45,239 @@ ContactManifoldSet::~ContactManifoldSet() { clear(); } -// Add a contact point to the manifold set -void ContactManifoldSet::addContactPoint(ContactPoint* contact) { +void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { - // Compute an Id corresponding to the normal direction (using a cubemap) - short int normalDirectionId = computeCubemapNormalId(contact->getNormal()); + assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr); - // If there is no contact manifold yet - if (mNbManifolds == 0) { - - createManifold(normalDirectionId); - mManifolds[0]->addContactPoint(contact); - assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); - for (int i=0; igetNbContactPoints() > 0); - } - - return; - } - - // Select the manifold with the most similar normal (if exists) - int similarManifoldIndex = 0; - if (mNbMaxManifolds > 1) { - similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); - } + // Try to find an existing contact manifold with similar contact normal + ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo); // If a similar manifold has been found - if (similarManifoldIndex != -1) { + if (similarManifold != nullptr) { - // Add the contact point to that similar manifold - mManifolds[similarManifoldIndex]->addContactPoint(contact); - assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0); - - return; - } - - // If the maximum number of manifold has not been reached yet - if (mNbManifolds < mNbMaxManifolds) { - - // Create a new manifold for the contact point - createManifold(normalDirectionId); - mManifolds[mNbManifolds-1]->addContactPoint(contact); - for (int i=0; igetNbContactPoints() > 0); - } - - return; - } - - // The contact point will be in a new contact manifold, we now have too much - // manifolds condidates. We need to remove one. We choose to keep the manifolds - // with the largest contact depth among their points - int smallestDepthIndex = -1; - decimal minDepth = contact->getPenetrationDepth(); - assert(mNbManifolds == mNbMaxManifolds); - for (int i=0; igetLargestContactDepth(); - if (depth < minDepth) { - minDepth = depth; - smallestDepthIndex = i; - } - } - - // If we do not want to keep to new manifold (not created yet) with the - // new contact point - if (smallestDepthIndex == -1) { - - // Delete the new contact - contact->~ContactPoint(); - mMemoryAllocator.release(contact, sizeof(ContactPoint)); - - return; - } - - assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds); - - // Here we need to replace an existing manifold with a new one (that contains - // the new contact point) - removeManifold(smallestDepthIndex); - createManifold(normalDirectionId); - mManifolds[mNbManifolds-1]->addContactPoint(contact); - assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0); - for (int i=0; igetNbContactPoints() > 0); - } - - return; -} - -// Return the index of the contact manifold with a similar average normal. -// If no manifold has close enough average normal, it returns -1 -int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const { - - // Return the Id of the manifold with the same normal direction id (if exists) - for (int i=0; igetNormalDirectionId()) { - return i; - } - } - - return -1; -} - -// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) -// Each face of the cube is divided into 4x4 buckets. This method maps the -// normal vector into of the of the bucket and returns a unique Id for the bucket -short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const { - - assert(normal.lengthSquare() > MACHINE_EPSILON); - - int faceNo; - decimal u, v; - decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z)); - Vector3 normalScaled = normal / max; - - if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { - faceNo = normalScaled.x > 0 ? 0 : 1; - u = normalScaled.y; - v = normalScaled.z; - } - else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) { - faceNo = normalScaled.y > 0 ? 2 : 3; - u = normalScaled.x; - v = normalScaled.z; + // Update the old manifold with the new one + updateManifoldWithNewOne(similarManifold, contactManifoldInfo); } else { - faceNo = normalScaled.z > 0 ? 4 : 5; - u = normalScaled.x; - v = normalScaled.y; + + // Create a new contact manifold + createManifold(contactManifoldInfo); } - - int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); - int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); - if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; - if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; - - const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; - return faceNo * 200 + indexU * nbSubDivInFace + indexV; } -// Update the contact manifolds -void ContactManifoldSet::update() { +// Update a previous similar manifold with a new one +void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) { - for (int i=mNbManifolds-1; i>=0; i--) { + assert(oldManifold != nullptr); + assert(newManifold != nullptr); - // Update the contact manifold - mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), - mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform()); + // For each contact point of the new manifold + ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo(); + assert(contactPointInfo != nullptr); + while (contactPointInfo != nullptr) { - // Remove the contact manifold if has no contact points anymore - if (mManifolds[i]->getNbContactPoints() == 0) { - removeManifold(i); + // For each contact point in the old manifold + bool isSimilarPointFound = false; + ContactPoint* oldContactPoint = oldManifold->getContactPoints(); + while (oldContactPoint != nullptr) { + + assert(oldContactPoint != nullptr); + + // If the new contact point is similar (very close) to the old contact point + if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) { + + // Replace (update) the old contact point with the new one + oldContactPoint->update(contactPointInfo); + isSimilarPointFound = true; + break; + } + + oldContactPoint = oldContactPoint->getNext(); + } + + // If we have not found a similar contact point + if (!isSimilarPointFound) { + + // Add the contact point to the manifold + oldManifold->addContactPoint(contactPointInfo); + } + + contactPointInfo = contactPointInfo->next; + } + + // The old manifold is no longer obsolete + oldManifold->setIsObsolete(false, false); +} + +// Remove a contact manifold that is the least optimal (smaller penetration depth) +void ContactManifoldSet::removeNonOptimalManifold() { + + assert(mNbManifolds > mNbMaxManifolds); + assert(mManifolds != nullptr); + + // Look for a manifold that is not new and with the smallest contact penetration depth. + // At the same time, we also look for a new manifold with the smallest contact penetration depth + // in case no old manifold exists. + ContactManifold* minDepthManifold = nullptr; + decimal minDepth = DECIMAL_LARGEST; + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + + // Get the largest contact point penetration depth of the manifold + const decimal depth = manifold->getLargestContactDepth(); + + if (depth < minDepth) { + minDepth = depth; + minDepthManifold = manifold; } + + manifold = manifold->getNext(); } + + // Remove the non optimal manifold + assert(minDepthManifold != nullptr); + removeManifold(minDepthManifold); +} + +// Return the contact manifold with a similar contact normal. +// If no manifold has close enough contact normal, it returns nullptr +ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const { + + // Get the contact normal of the first point of the manifold + const ContactPointInfo* contactPoint = contactManifold->getFirstContactPointInfo(); + assert(contactPoint != nullptr); + + ContactManifold* manifold = mManifolds; + + // Return the Id of the manifold with the same normal direction id (if exists) + while (manifold != nullptr) { + + // Get the first contact point of the current manifold + const ContactPoint* point = manifold->getContactPoints(); + assert(point != nullptr); + + // If the contact normal of the two manifolds are close enough + if (contactPoint->normal.dot(point->getNormal()) >= COS_ANGLE_SIMILAR_CONTACT_MANIFOLD) { + return manifold; + } + + manifold = manifold->getNext(); + } + + return nullptr; } // Clear the contact manifold set void ContactManifoldSet::clear() { - // Destroy all the contact manifolds - for (int i=mNbManifolds-1; i>=0; i--) { - removeManifold(i); + ContactManifold* manifold = mManifolds; + while(manifold != nullptr) { + + ContactManifold* nextManifold = manifold->getNext(); + + // Delete the contact manifold + manifold->~ContactManifold(); + mMemoryAllocator.release(manifold, sizeof(ContactManifold)); + + manifold = nextManifold; + + mNbManifolds--; } assert(mNbManifolds == 0); } // Create a new contact manifold and add it to the set -void ContactManifoldSet::createManifold(short int normalDirectionId) { - assert(mNbManifolds < mNbMaxManifolds); +void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) { + + ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) + ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator); + manifold->setPrevious(nullptr); + manifold->setNext(mManifolds); + if (mManifolds != nullptr) { + mManifolds->setPrevious(manifold); + } + mManifolds = manifold; - mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) - ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId); mNbManifolds++; } // Remove a contact manifold from the set -void ContactManifoldSet::removeManifold(int index) { +void ContactManifoldSet::removeManifold(ContactManifold* manifold) { assert(mNbManifolds > 0); - assert(index >= 0 && index < mNbManifolds); + assert(manifold != nullptr); - // Delete the new contact - mManifolds[index]->~ContactManifold(); - mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); + ContactManifold* previous = manifold->getPrevious(); + ContactManifold* next = manifold->getNext(); - for (int i=index; (i+1) < mNbManifolds; i++) { - mManifolds[i] = mManifolds[i+1]; + if (previous != nullptr) { + previous->setNext(next); + } + else { + mManifolds = next; + } + + if (next != nullptr) { + next->setPrevious(previous); } + // Delete the contact manifold + manifold->~ContactManifold(); + mMemoryAllocator.release(manifold, sizeof(ContactManifold)); mNbManifolds--; } + +// Make all the contact manifolds and contact points obsolete +void ContactManifoldSet::makeContactsObsolete() { + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + + manifold->setIsObsolete(true, true); + + manifold = manifold->getNext(); + } +} + +// Clear the obsolete contact manifolds and contact points +void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() { + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + + // Get the next manifold in the linked-list + ContactManifold* nextManifold = manifold->getNext(); + + // If the manifold is obsolete + if (manifold->getIsObsolete()) { + + // Delete the contact manifold + removeManifold(manifold); + } + else { + + // Clear the obsolete contact points of the manifold + manifold->clearObsoleteContactPoints(); + } + + manifold = nextManifold; + } +} + + +// Remove some contact manifolds and contact points if there are too many of them +void ContactManifoldSet::reduce() { + + // Remove non optimal contact manifold while there are too many manifolds in the set + while (mNbManifolds > mNbMaxManifolds) { + removeNonOptimalManifold(); + } + + // Reduce all the contact manifolds in case they have too many contact points + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + manifold->reduce(); + manifold = manifold->getNext(); + } +} diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 21944e36..56b09b04 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -60,27 +60,34 @@ class ContactManifoldSet { /// Pointer to the second proxy shape of the contact ProxyShape* mShape2; - /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; + /// Reference to the memory allocator for the contact manifolds + MemoryAllocator& mMemoryAllocator; /// Contact manifolds of the set - ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; + ContactManifold* mManifolds; // -------------------- Methods -------------------- // /// Create a new contact manifold and add it to the set - void createManifold(short normalDirectionId); + void createManifold(const ContactManifoldInfo* manifoldInfo); - /// Remove a contact manifold from the set - void removeManifold(int index); + // Return the contact manifold with a similar contact normal. + ContactManifold* selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const; - // Return the index of the contact manifold with a similar average normal. - int selectManifoldWithSimilarNormal(short int normalDirectionId) const; + /// Remove a contact manifold that is the least optimal (smaller penetration depth) + void removeNonOptimalManifold(); - // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) - // Each face of the cube is divided into 4x4 buckets. This method maps the - // normal vector into of the of the bucket and returns a unique Id for the bucket - short int computeCubemapNormalId(const Vector3& normal) const; + /// Update a previous similar manifold with a new one + void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold); + + /// Return the maximum number of contact manifolds allowed between to collision shapes + int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2); + + /// Clear the contact manifold set + void clear(); + + /// Delete a contact manifold + void removeManifold(ContactManifold* manifold); public: @@ -88,34 +95,37 @@ class ContactManifoldSet { /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - PoolAllocator& memoryAllocator, int nbMaxManifolds); + MemoryAllocator& memoryAllocator); /// Destructor ~ContactManifoldSet(); + /// Add a contact manifold in the set + void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); + /// Return the first proxy shape ProxyShape* getShape1() const; /// Return the second proxy shape ProxyShape* getShape2() const; - /// Add a contact point to the manifold set - void addContactPoint(ContactPoint* contact); - - /// Update the contact manifolds - void update(); - - /// Clear the contact manifold set - void clear(); - /// Return the number of manifolds in the set int getNbContactManifolds() const; - /// Return a given contact manifold - ContactManifold* getContactManifold(int index) const; + /// Return a pointer to the first element of the linked-list of contact manifolds + ContactManifold* getContactManifolds() const; + + /// Make all the contact manifolds and contact points obsolete + void makeContactsObsolete(); /// Return the total number of contact points in the set of manifolds int getTotalNbContactPoints() const; + + /// Clear the obsolete contact manifolds and contact points + void clearObsoleteManifoldsAndContactPoints(); + + // Remove some contact manifolds and contact points if there are too many of them + void reduce(); }; // Return the first proxy shape @@ -133,21 +143,38 @@ inline int ContactManifoldSet::getNbContactManifolds() const { return mNbManifolds; } -// Return a given contact manifold -inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const { - assert(index >= 0 && index < mNbManifolds); - return mManifolds[index]; +// Return a pointer to the first element of the linked-list of contact manifolds +inline ContactManifold* ContactManifoldSet::getContactManifolds() const { + return mManifolds; } // Return the total number of contact points in the set of manifolds inline int ContactManifoldSet::getTotalNbContactPoints() const { int nbPoints = 0; - for (int i=0; igetNbContactPoints(); + + ContactManifold* manifold = mManifolds; + while (manifold != nullptr) { + nbPoints += manifold->getNbContactPoints(); + + manifold = manifold->getNext(); } + return nbPoints; } +// Return the maximum number of contact manifolds allowed between to collision shapes +inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) { + + // If both shapes are convex + if (shape1->isConvex() && shape2->isConvex()) { + return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; + + } // If there is at least one concave shape + else { + return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; + } +} + } #endif diff --git a/src/collision/narrowphase/EPA/EdgeEPA.h b/src/collision/ContactPointInfo.h similarity index 52% rename from src/collision/narrowphase/EPA/EdgeEPA.h rename to src/collision/ContactPointInfo.h index ba3dcfee..6fbf9834 100644 --- a/src/collision/narrowphase/EPA/EdgeEPA.h +++ b/src/collision/ContactPointInfo.h @@ -23,100 +23,68 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_EDGE_EPA_H -#define REACTPHYSICS3D_EDGE_EPA_H - +#ifndef REACTPHYSICS3D_CONTACT_POINT_INFO_H +#define REACTPHYSICS3D_CONTACT_POINT_INFO_H // Libraries +#include "body/CollisionBody.h" #include "mathematics/mathematics.h" +#include "configuration.h" /// ReactPhysics3D namespace namespace reactphysics3d { -// Class declarations -class TriangleEPA; -class TrianglesStore; - -// Class EdgeEPA +// Structure ContactPointInfo /** - * This class represents an edge of the current polytope in the EPA algorithm. + * This structure contains informations about a collision contact + * computed during the narrow-phase collision detection. Those + * informations are used to compute the contact set for a contact + * between two bodies. */ -class EdgeEPA { +struct ContactPointInfo { private: - // -------------------- Attributes -------------------- // - - /// Pointer to the triangle that contains this edge - TriangleEPA* mOwnerTriangle; - - /// Index of the edge in the triangle (between 0 and 2). - /// The edge with index i connect triangle vertices i and (i+1 % 3) - int mIndex; + // -------------------- Methods -------------------- // public: + // -------------------- Attributes -------------------- // + + /// Normalized normal vector of the collision contact in world space + Vector3 normal; + + /// Penetration depth of the contact + decimal penetrationDepth; + + /// Contact point of body 1 in local space of body 1 + Vector3 localPoint1; + + /// Contact point of body 2 in local space of body 2 + Vector3 localPoint2; + + /// Pointer to the next contact point info + ContactPointInfo* next; + + /// True if the contact point has already been inserted into a manifold + bool isUsed; + // -------------------- Methods -------------------- // /// Constructor - EdgeEPA() = default; + ContactPointInfo(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) + : normal(contactNormal), penetrationDepth(penDepth), + localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) { - /// Constructor - EdgeEPA(TriangleEPA* ownerTriangle, int index); - - /// Copy-constructor - EdgeEPA(const EdgeEPA& edge); + assert(contactNormal.lengthSquare() > decimal(0.8)); + assert(penDepth > decimal(0.0)); + } /// Destructor - ~EdgeEPA() = default; - - /// Return the pointer to the owner triangle - TriangleEPA* getOwnerTriangle() const; - - /// Return the index of the edge in the triangle - int getIndex() const; - - /// Return index of the source vertex of the edge - uint getSourceVertexIndex() const; - - /// Return the index of the target vertex of the edge - uint getTargetVertexIndex() const; - - /// Execute the recursive silhouette algorithm from this edge - bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); - - /// Assignment operator - EdgeEPA& operator=(const EdgeEPA& edge); + ~ContactPointInfo() = default; }; -// Return the pointer to the owner triangle -inline TriangleEPA* EdgeEPA::getOwnerTriangle() const { - return mOwnerTriangle; -} - -// Return the edge index -inline int EdgeEPA::getIndex() const { - return mIndex; -} - -// Assignment operator -inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) { - mOwnerTriangle = edge.mOwnerTriangle; - mIndex = edge.mIndex; - return *this; -} - -// Return the index of the next counter-clockwise edge of the ownver triangle -inline int indexOfNextCounterClockwiseEdge(int i) { - return (i + 1) % 3; -} - -// Return the index of the previous counter-clockwise edge of the ownver triangle -inline int indexOfPreviousCounterClockwiseEdge(int i) { - return (i + 2) % 3; -} - } #endif - diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp new file mode 100644 index 00000000..70a731d5 --- /dev/null +++ b/src/collision/HalfEdgeStructure.cpp @@ -0,0 +1,116 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "HalfEdgeStructure.h" +#include + +using namespace reactphysics3d; + +// Initialize the structure (when all vertices and faces have been added) +void HalfEdgeStructure::init() { + + using edgeKey = std::pair; + + std::map edges; + std::map nextEdges; + std::map mapEdgeToStartVertex; + std::map mapEdgeToIndex; + std::map mapEdgeIndexToKey; + std::map mapFaceIndexToEdgeKey; + + // For each face + for (uint f=0; f currentFaceEdges; + + edgeKey firstEdgeKey; + + // For each vertex of the face + for (uint v=0; v < face.faceVertices.size(); v++) { + uint v1Index = face.faceVertices[v]; + uint v2Index = face.faceVertices[v == (face.faceVertices.size() - 1) ? 0 : v + 1]; + + const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index); + + // Create a new half-edge + Edge edge; + edge.faceIndex = f; + edge.vertexIndex = v1Index; + if (v == 0) { + firstEdgeKey = pairV1V2; + } + else if (v >= 1) { + nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2)); + } + if (v == (face.faceVertices.size() - 1)) { + nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey)); + } + edges.insert(std::make_pair(pairV1V2, edge)); + + const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index); + + mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index)); + mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index)); + + mapFaceIndexToEdgeKey.insert(std::make_pair(f, pairV1V2)); + + auto itEdge = edges.find(pairV2V1); + if (itEdge != edges.end()) { + + const uint edgeIndex = mEdges.size(); + + itEdge->second.twinEdgeIndex = edgeIndex + 1; + edge.twinEdgeIndex = edgeIndex; + + mapEdgeIndexToKey[edgeIndex] = pairV2V1; + mapEdgeIndexToKey[edgeIndex + 1] = pairV1V2; + + mVertices[v1Index].edgeIndex = edgeIndex + 1; + mVertices[v2Index].edgeIndex = edgeIndex; + + mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1)); + mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex)); + + mEdges.add(itEdge->second); + mEdges.add(edge); + } + + currentFaceEdges.push_back(pairV1V2); + } + } + + // Set next edges + for (uint i=0; i < mEdges.size(); i++) { + mEdges[i].nextEdgeIndex = mapEdgeToIndex[nextEdges[mapEdgeIndexToKey[i]]]; + } + + // Set face edge + for (uint f=0; f < mFaces.size(); f++) { + mFaces[f].edgeIndex = mapEdgeToIndex[mapFaceIndexToEdgeKey[f]]; + } +} diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h new file mode 100644 index 00000000..15658a93 --- /dev/null +++ b/src/collision/HalfEdgeStructure.h @@ -0,0 +1,175 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H +#define REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H + +// Libraries +#include "mathematics/mathematics.h" +#include + +namespace reactphysics3d { + +// Class HalfEdgeStructure +/** + * This class describes a polyhedron mesh made of faces and vertices. + * The faces do not have to be triangle. Note that the half-edge structure + * is only valid if the mesh is closed (each edge has two adjacent faces). + */ +class HalfEdgeStructure { + + public: + + struct Edge { + uint vertexIndex; // Index of the vertex at the beginning of the edge + uint twinEdgeIndex; // Index of the twin edge + uint faceIndex; // Adjacent face index of the edge + uint nextEdgeIndex; // Index of the next edge + }; + + struct Face { + uint edgeIndex; // Index of an half-edge of the face + List faceVertices; // Index of the vertices of the face + + /// Constructor + Face(MemoryAllocator& allocator) : faceVertices(allocator) {} + + /// Constructor + Face(List vertices) : faceVertices(vertices) {} + }; + + struct Vertex { + uint vertexPointIndex; // Index of the vertex point in the origin vertex array + uint edgeIndex; // Index of one edge emanting from this vertex + + /// Constructor + Vertex(uint vertexCoordsIndex) : vertexPointIndex(vertexCoordsIndex) { } + }; + + private: + + /// Reference to a memory allocator + MemoryAllocator& mAllocator; + + /// All the faces + List mFaces; + + /// All the vertices + List mVertices; + + /// All the half-edges + List mEdges; + + public: + + /// Constructor + HalfEdgeStructure(MemoryAllocator& allocator, uint facesCapacity, uint verticesCapacity, + uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity), + mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {} + + /// Destructor + ~HalfEdgeStructure() = default; + + /// Initialize the structure (when all vertices and faces have been added) + void init(); + + /// Add a vertex + uint addVertex(uint vertexPointIndex); + + /// Add a face + void addFace(List faceVertices); + + /// Return the number of faces + uint getNbFaces() const; + + /// Return the number of half-edges + uint getNbHalfEdges() const; + + /// Return the number of vertices + uint getNbVertices() const; + + /// Return a given face + const Face& getFace(uint index) const; + + /// Return a given edge + const Edge& getHalfEdge(uint index) const; + + /// Return a given vertex + const Vertex& getVertex(uint index) const; + +}; + +// Add a vertex +inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) { + Vertex vertex(vertexPointIndex); + mVertices.add(vertex); + return mVertices.size() - 1; +} + +// Add a face +inline void HalfEdgeStructure::addFace(List faceVertices) { + + // Create a new face + Face face(faceVertices); + mFaces.add(face); +} + +// Return the number of faces +inline uint HalfEdgeStructure::getNbFaces() const { + return static_cast(mFaces.size()); +} + +// Return the number of edges +inline uint HalfEdgeStructure::getNbHalfEdges() const { + return static_cast(mEdges.size()); +} + +// Return the number of vertices +inline uint HalfEdgeStructure::getNbVertices() const { + return static_cast(mVertices.size()); +} + +// Return a given face +inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const { + assert(index < mFaces.size()); + return mFaces[index]; +} + +// Return a given edge +inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const { + assert(index < mEdges.size()); + return mEdges[index]; +} + +// Return a given vertex +inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const { + assert(index < mVertices.size()); + return mVertices[index]; +} + +} + +#endif + diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp new file mode 100644 index 00000000..a53271a4 --- /dev/null +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -0,0 +1,60 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "collision/MiddlePhaseTriangleCallback.h" + +using namespace reactphysics3d; + +// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) +void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { + + // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the + // destructor of the corresponding NarrowPhaseInfo. + TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) + TriangleShape(trianglePoints, verticesNormals, shapeId, mAllocator); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler to the triangle shape + triangleShape->setProfiler(mProfiler); + +#endif + + bool isShape1Convex = mOverlappingPair->getShape1()->getCollisionShape()->isConvex(); + ProxyShape* shape1 = isShape1Convex ? mConvexProxyShape : mConcaveProxyShape; + ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape; + + // Create a narrow phase info for the narrow-phase collision detection + NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; + narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo))) + NarrowPhaseInfo(mOverlappingPair, + isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape, + isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(), + shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), + mAllocator); + narrowPhaseInfoList->next = firstNarrowPhaseInfo; +} diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h new file mode 100644 index 00000000..b4798a2c --- /dev/null +++ b/src/collision/MiddlePhaseTriangleCallback.h @@ -0,0 +1,100 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H +#define REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H + +// Libraries +#include "collision/shapes/ConcaveShape.h" +#include "collision/narrowphase/NarrowPhaseAlgorithm.h" + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class ConvexVsTriangleCallback +/** + * This class is used to report a collision between the triangle + * of a concave mesh shape and a convex shape during the + * middle-phase algorithm. + */ +class MiddlePhaseTriangleCallback : public TriangleCallback { + + protected: + + /// Broadphase overlapping pair + OverlappingPair* mOverlappingPair; + + /// Pointer to the concave proxy shape + ProxyShape* mConcaveProxyShape; + + /// Pointer to the convex proxy shape + ProxyShape* mConvexProxyShape; + + /// Pointer to the concave collision shape + const ConcaveShape* mConcaveShape; + + /// Reference to the single-frame memory allocator + MemoryAllocator& mAllocator; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + + public: + + /// Pointer to the first element of the linked-list of narrow-phase info + NarrowPhaseInfo* narrowPhaseInfoList; + + /// Constructor + MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, + ProxyShape* concaveProxyShape, + ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, + MemoryAllocator& allocator) + :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), + mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), + mAllocator(allocator), narrowPhaseInfoList(nullptr) { + + } + + /// Test collision between a triangle and the convex mesh shape + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) { + mProfiler = profiler; + } + +#endif + +}; + +} + +#endif diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp new file mode 100644 index 00000000..2c7a226c --- /dev/null +++ b/src/collision/NarrowPhaseInfo.cpp @@ -0,0 +1,110 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 +#include "NarrowPhaseInfo.h" +#include "ContactPointInfo.h" +#include "collision/shapes/TriangleShape.h" +#include "engine/OverlappingPair.h" + +using namespace reactphysics3d; + +// Constructor +NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, MemoryAllocator& shapeAllocator) + : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), + shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), + contactPoints(nullptr), next(nullptr), collisionShapeAllocator(shapeAllocator) { + + // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) + overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); +} + +// Destructor +NarrowPhaseInfo::~NarrowPhaseInfo() { + + assert(contactPoints == nullptr); + + // Release the memory of the TriangleShape (this memory was allocated in the + // MiddlePhaseTriangleCallback::testTriangle() method) + if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) { + collisionShape1->~CollisionShape(); + collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape)); + } + if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) { + collisionShape2->~CollisionShape(); + collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape)); + } +} + +// Add a new contact point +void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2) { + + assert(penDepth > decimal(0.0)); + + // Get the memory allocator + MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator(); + + // Create the contact point info + ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo))) + ContactPointInfo(contactNormal, penDepth, localPt1, localPt2); + + // Add it into the linked list of contact points + contactPointInfo->next = contactPoints; + contactPoints = contactPointInfo; +} + +/// Take all the generated contact points and create a new potential +/// contact manifold into the overlapping pair +void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() { + overlappingPair->addPotentialContactPoints(this); +} + +// Reset the remaining contact points +void NarrowPhaseInfo::resetContactPoints() { + + // Get the memory allocator + MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator(); + + // For each remaining contact point info + ContactPointInfo* element = contactPoints; + while(element != nullptr) { + + ContactPointInfo* elementToDelete = element; + + element = element->next; + + // Call the destructor + elementToDelete->~ContactPointInfo(); + + // Delete the current element + allocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + contactPoints = nullptr; +} diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index b97a71be..f9ea1ac9 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -28,11 +28,13 @@ // Libraries #include "shapes/CollisionShape.h" +#include "collision/ContactManifoldInfo.h" +#include "engine/OverlappingPair.h" /// Namespace ReactPhysics3D namespace reactphysics3d { -class OverlappingPair; +struct LastFrameCollisionInfo; // Class NarrowPhaseInfo /** @@ -47,10 +49,10 @@ struct NarrowPhaseInfo { OverlappingPair* overlappingPair; /// Pointer to the first collision shape to test collision with - const CollisionShape* collisionShape1; + CollisionShape* collisionShape1; /// Pointer to the second collision shape to test collision with - const CollisionShape* collisionShape2; + CollisionShape* collisionShape2; /// Transform that maps from collision shape 1 local-space to world-space Transform shape1ToWorldTransform; @@ -58,28 +60,42 @@ struct NarrowPhaseInfo { /// Transform that maps from collision shape 2 local-space to world-space Transform shape2ToWorldTransform; - /// Cached collision data of the proxy shape - // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 - void** cachedCollisionData1; - - /// Cached collision data of the proxy shape - // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 - void** cachedCollisionData2; + /// Linked-list of contact points created during the narrow-phase + ContactPointInfo* contactPoints; /// Pointer to the next element in the linked list NarrowPhaseInfo* next; - /// Constructor - NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1, - const CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2) - : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), - shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), - cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) { + /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) + MemoryAllocator& collisionShapeAllocator; - } + /// Constructor + NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, + CollisionShape* shape2, const Transform& shape1Transform, + const Transform& shape2Transform, MemoryAllocator& shapeAllocator); + + /// Destructor + ~NarrowPhaseInfo(); + + /// Add a new contact point + void addContactPoint(const Vector3& contactNormal, decimal penDepth, + const Vector3& localPt1, const Vector3& localPt2); + + /// Create a new potential contact manifold into the overlapping pair using current contact points + void addContactPointsAsPotentialContactManifold(); + + /// Reset the remaining contact points + void resetContactPoints(); + + /// Get the last collision frame info for temporal coherence + LastFrameCollisionInfo* getLastFrameCollisionInfo() const; }; +// Get the last collision frame info for temporal coherence +inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const { + return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId()); +} + } #endif diff --git a/src/collision/OverlapCallback.h b/src/collision/OverlapCallback.h new file mode 100644 index 00000000..bbba73d1 --- /dev/null +++ b/src/collision/OverlapCallback.h @@ -0,0 +1,57 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H +#define REACTPHYSICS3D_OVERLAP_CALLBACK_H + +// Libraries +#include "body/CollisionBody.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class OverlapCallback +/** + * This class can be used to register a callback for collision overlap queries. + * You should implement your own class inherited from this one and implement + * the notifyOverlap() method. This method will called each time a contact + * point is reported. + */ +class OverlapCallback { + + public: + + /// Destructor + virtual ~OverlapCallback() { + + } + + /// This method will be called for each reported overlapping bodies + virtual void notifyOverlap(CollisionBody* collisionBody)=0; +}; + +} + +#endif diff --git a/src/collision/PolygonVertexArray.cpp b/src/collision/PolygonVertexArray.cpp new file mode 100644 index 00000000..309a0d62 --- /dev/null +++ b/src/collision/PolygonVertexArray.cpp @@ -0,0 +1,75 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "PolygonVertexArray.h" + +using namespace reactphysics3d; + +// Constructor +/// Note that your data will not be copied into the PolygonVertexArray and +/// therefore, you need to make sure that those data are always valid during +/// the lifetime of the PolygonVertexArray. +/** + */ +PolygonVertexArray::PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride, + void* indexesStart, int indexesStride, + uint nbFaces, PolygonFace* facesStart, + VertexDataType vertexDataType, IndexDataType indexDataType) { + mNbVertices = nbVertices; + mVerticesStart = reinterpret_cast(verticesStart); + mVerticesStride = verticesStride; + mIndicesStart = reinterpret_cast(indexesStart); + mIndicesStride = indexesStride; + mNbFaces = nbFaces; + mPolygonFacesStart = facesStart; + mVertexDataType = vertexDataType; + mIndexDataType = indexDataType; +} + +// Return the vertex index of a given vertex in a face +uint PolygonVertexArray::getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const { + + assert(faceIndex < mNbFaces); + + // Get the face + PolygonFace* face = getPolygonFace(faceIndex); + + assert(noVertexInFace < face->nbVertices); + + void* vertexIndexPointer = mIndicesStart + (face->indexBase + noVertexInFace) * mIndicesStride; + + if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { + return *((uint*)vertexIndexPointer); + } + else if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_SHORT_TYPE) { + return *((unsigned short*)vertexIndexPointer); + } + else { + assert(false); + } + + return 0; +} diff --git a/src/collision/PolygonVertexArray.h b/src/collision/PolygonVertexArray.h new file mode 100644 index 00000000..5ae02e0e --- /dev/null +++ b/src/collision/PolygonVertexArray.h @@ -0,0 +1,188 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H +#define REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H + +// Libraries +#include "configuration.h" +#include + +namespace reactphysics3d { + +// Class PolygonVertexArray +/** + * This class is used to describe the vertices and faces of a polyhedron mesh. + * A PolygonVertexArray represents an array of vertices and polygon faces + * of a polyhedron mesh. When you create a PolygonVertexArray, no data is copied + * into the array. It only stores pointer to the data. The purpose is to allow + * the user to share vertices data between the physics engine and the rendering + * part. Therefore, make sure that the data pointed by a PolygonVertexArray + * remains valid during the PolygonVertexArray life. + */ +class PolygonVertexArray { + + public: + + /// Data type for the vertices in the array + enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; + + /// Data type for the indices in the array + enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; + + /// Represent a polygon face of the polyhedron + struct PolygonFace { + + /// Number of vertices in the polygon face + uint nbVertices; + + /// Index of the first vertex of the polygon face + /// inside the array with all vertex indices + uint indexBase; + }; + + protected: + + /// Number of vertices in the array + uint mNbVertices; + + /// Pointer to the first vertex value in the array + unsigned char* mVerticesStart; + + /// Stride (number of bytes) between the beginning of two vertices + /// values in the array + int mVerticesStride; + + /// Pointer to the first vertex index of the array + unsigned char* mIndicesStart; + + /// Stride (number of bytes) between the beginning of two indices in + /// the array + int mIndicesStride; + + /// Number of polygon faces in the array + uint mNbFaces; + + /// Pointer to the first polygon face in the polyhedron + PolygonFace* mPolygonFacesStart; + + /// Data type of the vertices in the array + VertexDataType mVertexDataType; + + /// Data type of the indices in the array + IndexDataType mIndexDataType; + + public: + + /// Constructor + PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride, + void* indexesStart, int indexesStride, + uint nbFaces, PolygonFace* facesStart, + VertexDataType vertexDataType, IndexDataType indexDataType); + + /// Destructor + ~PolygonVertexArray() = default; + + /// Return the vertex data type + VertexDataType getVertexDataType() const; + + /// Return the index data type + IndexDataType getIndexDataType() const; + + /// Return the number of vertices + uint getNbVertices() const; + + /// Return the number of faces + uint getNbFaces() const; + + /// Return the vertices stride (number of bytes) + int getVerticesStride() const; + + /// Return the indices stride (number of bytes) + int getIndicesStride() const; + + /// Return the vertex index of a given vertex in a face + uint getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const; + + /// Return a polygon face of the polyhedron + PolygonFace* getPolygonFace(uint faceIndex) const; + + /// Return the pointer to the start of the vertices array + unsigned char* getVerticesStart() const; + + /// Return the pointer to the start of the indices array + unsigned char* getIndicesStart() const; +}; + +// Return the vertex data type +inline PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType() const { + return mVertexDataType; +} + +// Return the index data type +inline PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() const { + return mIndexDataType; +} + +// Return the number of vertices +inline uint PolygonVertexArray::getNbVertices() const { + return mNbVertices; +} + +// Return the number of faces +inline uint PolygonVertexArray::getNbFaces() const { + return mNbFaces; +} + +// Return the vertices stride (number of bytes) +inline int PolygonVertexArray::getVerticesStride() const { + return mVerticesStride; +} + +// Return the indices stride (number of bytes) +inline int PolygonVertexArray::getIndicesStride() const { + return mIndicesStride; +} + +// Return a polygon face of the polyhedron +inline PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint faceIndex) const { + assert(faceIndex < mNbFaces); + return &mPolygonFacesStart[faceIndex]; +} + +// Return the pointer to the start of the vertices array +inline unsigned char* PolygonVertexArray::getVerticesStart() const { + return mVerticesStart; +} + +// Return the pointer to the start of the indices array +inline unsigned char* PolygonVertexArray::getIndicesStart() const { + return mIndicesStart; +} + +} + +#endif + diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp new file mode 100644 index 00000000..64db474e --- /dev/null +++ b/src/collision/PolyhedronMesh.cpp @@ -0,0 +1,152 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "PolyhedronMesh.h" +#include "memory/MemoryManager.h" + +using namespace reactphysics3d; + + +// Constructor +/* + * Create a polyhedron mesh given an array of polygons. + * @param polygonVertexArray Pointer to the array of polygons and their vertices + */ +PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) + : mHalfEdgeStructure(MemoryManager::getBaseAllocator(), + polygonVertexArray->getNbFaces(), + polygonVertexArray->getNbVertices(), + (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) { + + mPolygonVertexArray = polygonVertexArray; + + // Create the half-edge structure of the mesh + createHalfEdgeStructure(); + + // Create the face normals array + mFacesNormals = new Vector3[mHalfEdgeStructure.getNbFaces()]; + + // Compute the faces normals + computeFacesNormals(); + + // Compute the centroid + computeCentroid(); +} + +// Destructor +PolyhedronMesh::~PolyhedronMesh() { + delete[] mFacesNormals; +} + +// Create the half-edge structure of the mesh +void PolyhedronMesh::createHalfEdgeStructure() { + + // For each vertex of the mesh + for (uint v=0; v < mPolygonVertexArray->getNbVertices(); v++) { + mHalfEdgeStructure.addVertex(v); + } + + // For each polygon face of the mesh + for (uint f=0; f < mPolygonVertexArray->getNbFaces(); f++) { + + // Get the polygon face + PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f); + + List faceVertices(MemoryManager::getBaseAllocator(), face->nbVertices); + + // For each vertex of the face + for (uint v=0; v < face->nbVertices; v++) { + faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v)); + } + + assert(faceVertices.size() >= 3); + + // Addd the face into the half-edge structure + mHalfEdgeStructure.addFace(faceVertices); + } + + // Initialize the half-edge structure + mHalfEdgeStructure.init(); +} + +/// Return a vertex +Vector3 PolyhedronMesh::getVertex(uint index) const { + assert(index < getNbVertices()); + + // Get the vertex index in the array with all vertices + uint vertexIndex = mHalfEdgeStructure.getVertex(index).vertexPointIndex; + + PolygonVertexArray::VertexDataType vertexType = mPolygonVertexArray->getVertexDataType(); + unsigned char* verticesStart = mPolygonVertexArray->getVerticesStart(); + int vertexStride = mPolygonVertexArray->getVerticesStride(); + + Vector3 vertex; + if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { + const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); + vertex.x = decimal(vertices[0]); + vertex.y = decimal(vertices[1]); + vertex.z = decimal(vertices[2]); + } + else if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { + const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); + vertex.x = decimal(vertices[0]); + vertex.y = decimal(vertices[1]); + vertex.z = decimal(vertices[2]); + } + else { + assert(false); + } + + return vertex; +} + +// Compute the faces normals +void PolyhedronMesh::computeFacesNormals() { + + // For each face + for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) { + const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f); + + assert(face.faceVertices.size() >= 3); + + const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]); + const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]); + mFacesNormals[f] = vec1.cross(vec2); + mFacesNormals[f].normalize(); + } +} + +// Compute the centroid of the polyhedron +void PolyhedronMesh::computeCentroid() { + + mCentroid.setToZero(); + + for (uint v=0; v < getNbVertices(); v++) { + mCentroid += getVertex(v); + } + + mCentroid /= getNbVertices(); +} diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h new file mode 100644 index 00000000..b459723a --- /dev/null +++ b/src/collision/PolyhedronMesh.h @@ -0,0 +1,123 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_POLYHEDRON_MESH_H +#define REACTPHYSICS3D_POLYHEDRON_MESH_H + +// Libraries +#include "mathematics/mathematics.h" +#include "HalfEdgeStructure.h" +#include "collision/PolygonVertexArray.h" +#include "memory/DefaultAllocator.h" +#include + +namespace reactphysics3d { + +// Class PolyhedronMesh +/** + * This class describes a polyhedron mesh made of faces and vertices. + * The faces do not have to be triangles. + */ +class PolyhedronMesh { + + private: + + // -------------------- Attributes -------------------- // + + /// Pointer the the polygon vertex array with vertices and faces + /// of the mesh + PolygonVertexArray* mPolygonVertexArray; + + /// Half-edge structure of the mesh + HalfEdgeStructure mHalfEdgeStructure; + + /// Array with the face normals + Vector3* mFacesNormals; + + /// Centroid of the polyhedron + Vector3 mCentroid; + + // -------------------- Methods -------------------- // + + /// Create the half-edge structure of the mesh + void createHalfEdgeStructure(); + + /// Compute the faces normals + void computeFacesNormals(); + + /// Compute the centroid of the polyhedron + void computeCentroid() ; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + PolyhedronMesh(PolygonVertexArray* polygonVertexArray); + + /// Destructor + ~PolyhedronMesh(); + + /// Return the number of vertices + uint getNbVertices() const; + + /// Return a vertex + Vector3 getVertex(uint index) const; + + /// Return a face normal + Vector3 getFaceNormal(uint faceIndex) const; + + /// Return the half-edge structure of the mesh + const HalfEdgeStructure& getHalfEdgeStructure() const; + + /// Return the centroid of the polyhedron + Vector3 getCentroid() const; +}; + +// Return the number of vertices +inline uint PolyhedronMesh::getNbVertices() const { + return mHalfEdgeStructure.getNbVertices(); +} + +// Return a face normal +inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const { + assert(faceIndex < mHalfEdgeStructure.getNbFaces()); + return mFacesNormals[faceIndex]; +} + +// Return the half-edge structure of the mesh +inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { + return mHalfEdgeStructure; +} + +// Return the centroid of the polyhedron +inline Vector3 PolyhedronMesh::getCentroid() const { + return mCentroid; +} + +} + +#endif + diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 50418011..8847d094 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -35,20 +35,15 @@ using namespace reactphysics3d; * @param transform Transformation from collision shape local-space to body local-space * @param mass Mass of the collision shape (in kilograms) */ -ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass) - :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), - mNext(nullptr), mBroadPhaseID(-1), mCachedCollisionData(nullptr), mUserData(nullptr), - mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { +ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, MemoryManager& memoryManager) + :mMemoryManager(memoryManager), mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), + mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { } // Destructor ProxyShape::~ProxyShape() { - // Release the cached collision data memory - if (mCachedCollisionData != nullptr) { - free(mCachedCollisionData); - } } // Return true if a point is inside the collision shape @@ -81,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { worldToLocalTransform * ray.point2, ray.maxFraction); - bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this); + bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator()); // Convert the raycast info into world-space raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 45c0add2..0da79e28 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -29,6 +29,7 @@ // Libraries #include "body/CollisionBody.h" #include "shapes/CollisionShape.h" +#include "memory/MemoryManager.h" namespace reactphysics3d { @@ -48,6 +49,9 @@ class ProxyShape { // -------------------- Attributes -------------------- // + /// Reference to the memory manager + MemoryManager& mMemoryManager; + /// Pointer to the parent body CollisionBody* mBody; @@ -66,9 +70,6 @@ class ProxyShape { /// Broad-phase ID (node ID in the dynamic AABB tree) int mBroadPhaseID; - /// Cached collision data - void* mCachedCollisionData; - /// Pointer to user data void* mUserData; @@ -85,13 +86,25 @@ class ProxyShape { /// proxy shape will collide with every collision categories by default. unsigned short mCollideWithMaskBits; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + + // -------------------- Methods -------------------- // + + /// Return the collision shape + CollisionShape* getCollisionShape(); + public: // -------------------- Methods -------------------- // /// Constructor ProxyShape(CollisionBody* body, CollisionShape* shape, - const Transform& transform, decimal mass); + const Transform& transform, decimal mass, MemoryManager& memoryManager); /// Destructor virtual ~ProxyShape(); @@ -156,15 +169,19 @@ class ProxyShape { /// Return the next proxy shape in the linked list of proxy shapes const ProxyShape* getNext() const; - /// Return the pointer to the cached collision data - void** getCachedCollisionData(); - /// Return the local scaling vector of the collision shape Vector3 getLocalScaling() const; /// Set the local scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + // -------------------- Friendship -------------------- // friend class OverlappingPair; @@ -175,17 +192,13 @@ class ProxyShape { friend class CollisionDetection; friend class CollisionWorld; friend class DynamicsWorld; - friend class EPAAlgorithm; friend class GJKAlgorithm; friend class ConvexMeshShape; + friend class ContactManifoldSet; + friend class MiddlePhaseTriangleCallback; }; -// Return the pointer to the cached collision data -inline void** ProxyShape::getCachedCollisionData() { - return &mCachedCollisionData; -} - // Return the collision shape /** * @return Pointer to the internal collision shape @@ -194,6 +207,14 @@ inline const CollisionShape* ProxyShape::getCollisionShape() const { return mCollisionShape; } +// Return the collision shape +/** +* @return Pointer to the internal collision shape +*/ +inline CollisionShape* ProxyShape::getCollisionShape() { + return mCollisionShape; +} + // Return the parent body /** * @return Pointer to the parent body @@ -318,7 +339,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit * @return The local scaling vector */ inline Vector3 ProxyShape::getLocalScaling() const { - return mCollisionShape->getScaling(); + return mCollisionShape->getLocalScaling(); } // Set the local scaling vector of the collision shape @@ -345,6 +366,18 @@ inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const { return worldAABB.testCollision(getWorldAABB()); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ProxyShape::setProfiler(Profiler* profiler) { + + mProfiler = profiler; + + mCollisionShape->setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/TriangleMesh.h b/src/collision/TriangleMesh.h index 936824e9..e33fad67 100644 --- a/src/collision/TriangleMesh.h +++ b/src/collision/TriangleMesh.h @@ -38,7 +38,7 @@ namespace reactphysics3d { * This class represents a mesh made of triangles. A TriangleMesh contains * one or several parts. Each part is a set of triangles represented in a * TriangleVertexArray object describing all the triangles vertices of the part. - * A TriangleMesh object is used to create a ConcaveMeshShape from a triangle + * A TriangleMesh object can be used to create a ConcaveMeshShape from a triangle * mesh for instance. */ class TriangleMesh { diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp index 883b0ab7..7cfb2076 100644 --- a/src/collision/TriangleVertexArray.cpp +++ b/src/collision/TriangleVertexArray.cpp @@ -25,32 +25,258 @@ // Libraries #include "TriangleVertexArray.h" +#include "mathematics/Vector3.h" +#include using namespace reactphysics3d; -// Constructor +// Constructor without vertices normals /// Note that your data will not be copied into the TriangleVertexArray and /// therefore, you need to make sure that those data are always valid during -/// the lifetime of the TriangleVertexArray. +/// the lifetime of the TriangleVertexArray. With this constructor, you do not +/// need to provide vertices normals for smooth mesh collision. Therefore, the +/// vertices normals will be computed automatically. The vertices normals are +/// computed with weighted average of the associated triangle face normal. The +/// weights are the angle between the associated edges of neighbor triangle face. /** * @param nbVertices Number of vertices in the array * @param verticesStart Pointer to the first vertices of the array * @param verticesStride Number of bytes between the beginning of two consecutive vertices * @param nbTriangles Number of triangles in the array * @param indexesStart Pointer to the first triangle index + * @param indexesStride Number of bytes between the beginning of the three indices of two triangles + * @param vertexDataType Type of data for the vertices (float, double) + * @param indexDataType Type of data for the indices (short, int) + */ +TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, + VertexDataType vertexDataType, IndexDataType indexDataType) { + mNbVertices = nbVertices; + mVerticesStart = static_cast(verticesStart); + mVerticesStride = verticesStride; + mVerticesNormalsStart = nullptr; + mVerticesNormalsStride = 3 * sizeof(float); + mNbTriangles = nbTriangles; + mIndicesStart = static_cast(indexesStart); + mIndicesStride = indexesStride; + mVertexDataType = vertexDataType; + mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE; + mIndexDataType = indexDataType; + mAreVerticesNormalsProvidedByUser = false; + + // Compute the vertices normals because they are not provided by the user + computeVerticesNormals(); +} + +// Constructor with vertices normals +/// Note that your data will not be copied into the TriangleVertexArray and +/// therefore, you need to make sure that those data are always valid during +/// the lifetime of the TriangleVertexArray. With this constructor, you need +/// to provide the vertices normals that will be used for smooth mesh collision. +/** + * @param nbVertices Number of vertices in the array + * @param verticesStart Pointer to the first vertices of the array + * @param verticesStride Number of bytes between the beginning of two consecutive vertices + * @param verticesNormalsStart Pointer to the first vertex normal of the array + * @param verticesNormalsStride Number of bytes between the beginning of two consecutive vertex normals + * @param nbTriangles Number of triangles in the array + * @param indexesStart Pointer to the first triangle index * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices * @param vertexDataType Type of data for the vertices (float, double) * @param indexDataType Type of data for the indices (short, int) */ -TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - uint nbTriangles, void* indexesStart, int indexesStride, - VertexDataType vertexDataType, IndexDataType indexDataType) { +TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + const void* verticesNormalsStart, uint verticesNormalsStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, + VertexDataType vertexDataType, NormalDataType normalDataType, + IndexDataType indexDataType) { + mNbVertices = nbVertices; - mVerticesStart = reinterpret_cast(verticesStart); + mVerticesStart = static_cast(verticesStart); mVerticesStride = verticesStride; + mVerticesNormalsStart = static_cast(verticesNormalsStart); + mVerticesNormalsStride = verticesNormalsStride; mNbTriangles = nbTriangles; - mIndicesStart = reinterpret_cast(indexesStart); + mIndicesStart = static_cast(indexesStart); mIndicesStride = indexesStride; mVertexDataType = vertexDataType; + mVertexNormaldDataType = normalDataType; mIndexDataType = indexDataType; + mAreVerticesNormalsProvidedByUser = true; + + assert(mVerticesNormalsStart != nullptr); +} + +// Destructor +TriangleVertexArray::~TriangleVertexArray() { + + // If the vertices normals have not been provided by the user + if (!mAreVerticesNormalsProvidedByUser) { + + // Release the allocated memory + const void* verticesNormalPointer = static_cast(mVerticesNormalsStart); + const float* verticesNormals = static_cast(verticesNormalPointer); + delete[] verticesNormals; + } +} + +// Compute the vertices normals when they are not provided by the user +/// The vertices normals are computed with weighted average of the associated +/// triangle face normal. The weights are the angle between the associated edges +/// of neighbor triangle face. +void TriangleVertexArray::computeVerticesNormals() { + + // Allocate memory for the vertices normals + float* verticesNormals = new float[mNbVertices * 3]; + + // Init vertices normals to zero + for (uint i=0; i= decimal(0.0)); + Vector3 normalComponent = arcSinA * crossProduct; + + // Add the normal component of this vertex into the normals array + verticesNormals[verticesIndices[v] * 3] += normalComponent.x; + verticesNormals[verticesIndices[v] * 3 + 1] += normalComponent.y; + verticesNormals[verticesIndices[v] * 3 + 2] += normalComponent.z; + } + } + + // Normalize the computed vertices normals + for (uint v=0; v(verticesNormals); + mVerticesNormalsStart = static_cast(verticesNormalsPointer); +} + +// Return the indices of the three vertices of a given triangle in the array +void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const { + + assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); + + const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * mIndicesStride; + const void* startTriangleIndices = static_cast(triangleIndicesPointer); + + // For each vertex of the triangle + for (int i=0; i < 3; i++) { + + // Get the index of the current vertex in the triangle + if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { + outVerticesIndices[i] = static_cast(startTriangleIndices)[i]; + } + else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { + outVerticesIndices[i] = static_cast(startTriangleIndices)[i]; + } + else { + assert(false); + } + } +} + +// Return the vertices coordinates of a triangle +void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const { + + assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); + + // Get the three vertex index of the three vertices of the triangle + uint verticesIndices[3]; + getTriangleVerticesIndices(triangleIndex, verticesIndices); + + // For each vertex of the triangle + for (int k=0; k < 3; k++) { + + const uchar* vertexPointerChar = mVerticesStart + verticesIndices[k] * mVerticesStride; + const void* vertexPointer = static_cast(vertexPointerChar); + + // Get the vertices components of the triangle + if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { + const float* vertices = static_cast(vertexPointer); + outTriangleVertices[k][0] = decimal(vertices[0]); + outTriangleVertices[k][1] = decimal(vertices[1]); + outTriangleVertices[k][2] = decimal(vertices[2]); + } + else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { + const double* vertices = static_cast(vertexPointer); + outTriangleVertices[k][0] = decimal(vertices[0]); + outTriangleVertices[k][1] = decimal(vertices[1]); + outTriangleVertices[k][2] = decimal(vertices[2]); + } + else { + assert(false); + } + } +} + +// Return the three vertices normals of a triangle +void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const { + + assert(triangleIndex >= 0 && triangleIndex < mNbTriangles); + + // Get the three vertex index of the three vertices of the triangle + uint verticesIndices[3]; + getTriangleVerticesIndices(triangleIndex, verticesIndices); + + // For each vertex of the triangle + for (int k=0; k < 3; k++) { + + const uchar* vertexNormalPointerChar = mVerticesNormalsStart + verticesIndices[k] * mVerticesNormalsStride; + const void* vertexNormalPointer = static_cast(vertexNormalPointerChar); + + // Get the normals from the array + if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) { + const float* normal = static_cast(vertexNormalPointer); + outTriangleVerticesNormals[k][0] = decimal(normal[0]); + outTriangleVerticesNormals[k][1] = decimal(normal[1]); + outTriangleVerticesNormals[k][2] = decimal(normal[2]); + } + else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) { + const double* normal = static_cast(vertexNormalPointer); + outTriangleVerticesNormals[k][0] = decimal(normal[0]); + outTriangleVerticesNormals[k][1] = decimal(normal[1]); + outTriangleVerticesNormals[k][2] = decimal(normal[2]); + } + else { + assert(false); + } + } } diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h index 8e434919..3a48c2e3 100644 --- a/src/collision/TriangleVertexArray.h +++ b/src/collision/TriangleVertexArray.h @@ -31,6 +31,8 @@ namespace reactphysics3d { +struct Vector3; + // Class TriangleVertexArray /** * This class is used to describe the vertices and faces of a triangular mesh. @@ -48,50 +50,90 @@ class TriangleVertexArray { /// Data type for the vertices in the array enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; + /// Data type for the vertex normals in the array + enum class NormalDataType {NORMAL_FLOAT_TYPE, NORMAL_DOUBLE_TYPE}; + /// Data type for the indices in the array enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; protected: + // -------------------- Attributes -------------------- // + /// Number of vertices in the array uint mNbVertices; /// Pointer to the first vertex value in the array - unsigned char* mVerticesStart; + const uchar* mVerticesStart; /// Stride (number of bytes) between the beginning of two vertices /// values in the array - int mVerticesStride; + uint mVerticesStride; + + /// Pointer to the first vertex normal value in the array + const uchar* mVerticesNormalsStart; + + /// Stride (number of bytes) between the beginning of two vertex normals + /// values in the array + uint mVerticesNormalsStride; /// Number of triangles in the array uint mNbTriangles; /// Pointer to the first vertex index of the array - unsigned char* mIndicesStart; + const uchar* mIndicesStart; - /// Stride (number of bytes) between the beginning of two indices in - /// the array - int mIndicesStride; + /// Stride (number of bytes) between the beginning of the three indices of two triangles + uint mIndicesStride; /// Data type of the vertices in the array VertexDataType mVertexDataType; + /// Data type of the vertex normals in the array + NormalDataType mVertexNormaldDataType; + /// Data type of the indices in the array IndexDataType mIndexDataType; + /// True if the vertices normals are provided by the user + bool mAreVerticesNormalsProvidedByUser; + + // -------------------- Methods -------------------- // + + /// Compute the vertices normals when they are not provided by the user + void computeVerticesNormals(); + public: - /// Constructor - TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride, - uint nbTriangles, void* indexesStart, int indexesStride, + // -------------------- Methods -------------------- // + + /// Constructor without vertices normals + TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, VertexDataType vertexDataType, IndexDataType indexDataType); + /// Constructor with vertices normals + TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride, + const void* verticesNormalsStart, uint uverticesNormalsStride, + uint nbTriangles, const void* indexesStart, uint indexesStride, + VertexDataType vertexDataType, NormalDataType normalDataType, + IndexDataType indexDataType); + /// Destructor - ~TriangleVertexArray() = default; + ~TriangleVertexArray(); + + /// Deleted assignment operator + TriangleVertexArray& operator=(const TriangleVertexArray& triangleVertexArray) = delete; + + /// Deleted copy-constructor + TriangleVertexArray(const TriangleVertexArray& triangleVertexArray) = delete; /// Return the vertex data type VertexDataType getVertexDataType() const; + /// Return the vertex normal data type + NormalDataType getVertexNormalDataType() const; + /// Return the index data type IndexDataType getIndexDataType() const; @@ -102,16 +144,31 @@ class TriangleVertexArray { uint getNbTriangles() const; /// Return the vertices stride (number of bytes) - int getVerticesStride() const; + uint getVerticesStride() const; + + /// Return the vertex normals stride (number of bytes) + uint getVerticesNormalsStride() const; /// Return the indices stride (number of bytes) - int getIndicesStride() const; + uint getIndicesStride() const; /// Return the pointer to the start of the vertices array - unsigned char* getVerticesStart() const; + const void* getVerticesStart() const; + + /// Return the pointer to the start of the vertex normals array + const void* getVerticesNormalsStart() const; /// Return the pointer to the start of the indices array - unsigned char* getIndicesStart() const; + const void* getIndicesStart() const; + + /// Return the vertices coordinates of a triangle + void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const; + + /// Return the three vertices normals of a triangle + void getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const; + + /// Return the indices of the three vertices of a given triangle in the array + void getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const; }; // Return the vertex data type @@ -119,6 +176,11 @@ inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataTyp return mVertexDataType; } +// Return the vertex normal data type +inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const { + return mVertexNormaldDataType; +} + // Return the index data type inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const { return mIndexDataType; @@ -135,22 +197,32 @@ inline uint TriangleVertexArray::getNbTriangles() const { } // Return the vertices stride (number of bytes) -inline int TriangleVertexArray::getVerticesStride() const { +inline uint TriangleVertexArray::getVerticesStride() const { return mVerticesStride; } +// Return the vertex normals stride (number of bytes) +inline uint TriangleVertexArray::getVerticesNormalsStride() const { + return mVerticesNormalsStride; +} + // Return the indices stride (number of bytes) -inline int TriangleVertexArray::getIndicesStride() const { +inline uint TriangleVertexArray::getIndicesStride() const { return mIndicesStride; } // Return the pointer to the start of the vertices array -inline unsigned char* TriangleVertexArray::getVerticesStart() const { +inline const void* TriangleVertexArray::getVerticesStart() const { return mVerticesStart; } +// Return the pointer to the start of the vertex normals array +inline const void* TriangleVertexArray::getVerticesNormalsStart() const { + return mVerticesNormalsStart; +} + // Return the pointer to the start of the indices array -inline unsigned char* TriangleVertexArray::getIndicesStart() const { +inline const void* TriangleVertexArray::getIndicesStart() const { return mIndicesStart; } diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index bc03f44d..8998859e 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -44,6 +44,13 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) // Allocate memory for the array of potential overlapping pairs mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); assert(mPotentialPairs != nullptr); + +#ifdef IS_PROFILING_ACTIVE + + mProfiler = nullptr; + +#endif + } // Destructor @@ -117,6 +124,8 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { // Add a proxy collision shape into the broad-phase collision detection void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { + assert(proxyShape->mBroadPhaseID == -1); + // Add the collision shape into the dynamic AABB tree and get its broad-phase ID int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape); @@ -131,8 +140,12 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const A // Remove a proxy collision shape from the broad-phase collision detection void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { + assert(proxyShape->mBroadPhaseID != -1); + int broadPhaseID = proxyShape->mBroadPhaseID; + proxyShape->mBroadPhaseID = -1; + // Remove the collision shape from the dynamic AABB tree mDynamicAABBTree.removeObject(broadPhaseID); @@ -172,14 +185,14 @@ void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb, } // Compute all the overlapping pairs of collision shapes -void BroadPhaseAlgorithm::computeOverlappingPairs(Allocator& allocator) { +void BroadPhaseAlgorithm::computeOverlappingPairs(MemoryManager& memoryManager) { // TODO : Try to see if we can allocate potential pairs in single frame allocator // Reset the potential overlapping pairs mNbPotentialPairs = 0; - LinkedList overlappingNodes(allocator); + LinkedList overlappingNodes(memoryManager.getPoolAllocator()); // For all collision shapes that have moved (or have been created) during the // last simulation step diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 8d2cda35..30a67920 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -162,6 +162,13 @@ class BroadPhaseAlgorithm { /// Reference to the collision detection object CollisionDetection& mCollisionDetection; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public : // -------------------- Methods -------------------- // @@ -203,7 +210,7 @@ class BroadPhaseAlgorithm { void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList& overlappingNodes) const; /// Compute all the overlapping pairs of collision shapes - void computeOverlappingPairs(Allocator& allocator); + void computeOverlappingPairs(MemoryManager& memoryManager); /// Return the proxy shape corresponding to the broad-phase node id in parameter ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const; @@ -215,8 +222,15 @@ class BroadPhaseAlgorithm { const AABB& getFatAABB(int broadPhaseId) const; /// Ray casting method - void raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits) const; + void raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; // Method used to compare two pairs for sorting algorithm @@ -232,6 +246,9 @@ inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const Broad // Return true if the two broad-phase collision shapes are overlapping inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const { + + if (shape1->mBroadPhaseID == -1 || shape2->mBroadPhaseID == -1) return false; + // Get the two AABBs of the collision shapes const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID); const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID); @@ -249,7 +266,7 @@ inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const { inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { - PROFILE("BroadPhaseAlgorithm::raycast()"); + PROFILE("BroadPhaseAlgorithm::raycast()", mProfiler); BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); @@ -261,6 +278,16 @@ inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPh return static_cast(mDynamicAABBTree.getNodeDataPointer(broadPhaseId)); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void BroadPhaseAlgorithm::setProfiler(Profiler* profiler) { + mProfiler = profiler; + mDynamicAABBTree.setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index f829c000..5fa33d9d 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -171,7 +171,7 @@ void DynamicAABBTree::removeObject(int nodeID) { /// (this can be useful if the shape AABB has become much smaller than the previous one for instance). bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) { - PROFILE("DynamicAABBTree::updateObject()"); + PROFILE("DynamicAABBTree::updateObject()", mProfiler); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); @@ -633,7 +633,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, // Ray casting method void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const { - PROFILE("DynamicAABBTree::raycast()"); + PROFILE("DynamicAABBTree::raycast()", mProfiler); decimal maxFraction = ray.maxFraction; diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 8025e4f9..54751138 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -155,6 +155,13 @@ class DynamicAABBTree { /// without triggering a large modification of the tree which can be costly decimal mExtraAABBGap; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Allocate and return a node to use in the tree @@ -237,6 +244,14 @@ class DynamicAABBTree { /// Clear all the nodes and reset the tree void reset(); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; // Return true if the node is a leaf of the tree @@ -292,6 +307,15 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) { return nodeId; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void DynamicAABBTree::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp new file mode 100755 index 00000000..dce9b04b --- /dev/null +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -0,0 +1,227 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "CapsuleVsCapsuleAlgorithm.h" +#include "collision/shapes/CapsuleShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Compute the narrow-phase collision detection between two capsules +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. +bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + MemoryAllocator& memoryAllocator) { + + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + // Get the capsule collision shapes + const CapsuleShape* capsuleShape1 = static_cast(narrowPhaseInfo->collisionShape1); + const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfo->collisionShape2); + + // Get the transform from capsule 1 local-space to capsule 2 local-space + const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; + + // Compute the end-points of the inner segment of the first capsule + Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0); + Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0); + capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA; + capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB; + + // Compute the end-points of the inner segment of the second capsule + const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0); + const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0); + + // The two inner capsule segments + const Vector3 seg1 = capsule1SegB - capsule1SegA; + const Vector3 seg2 = capsule2SegB - capsule2SegA; + + // Compute the sum of the radius of the two capsules (virtual spheres) + decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius(); + + // If the two capsules are parallel (we create two contact points) + bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2); + if (areCapsuleInnerSegmentsParralel) { + + // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) + const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); + if (segmentsPerpendicularDistance >= sumRadius) { + + // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. + // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision. + return false; + } + + // Compute the planes that goes through the extreme points of the inner segment of capsule 1 + decimal d1 = seg1.dot(capsule1SegA); + decimal d2 = -seg1.dot(capsule1SegB); + + // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner + // segment of capsule 1 + decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); + decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); + + // If the segments were overlapping (the clip segment is valid) + if (t1 > decimal(0.0) && t2 > decimal(0.0)) { + + if (reportContacts) { + + // Clip the inner segment of capsule 2 + if (t1 > decimal(1.0)) t1 = decimal(1.0); + const Vector3 clipPointA = capsule2SegB - t1 * seg2; + if (t2 > decimal(1.0)) t2 = decimal(1.0); + const Vector3 clipPointB = capsule2SegA + t2 * seg2; + + // Project point capsule2SegA onto line of innner segment of capsule 1 + const Vector3 seg1Normalized = seg1.getUnit(); + Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; + + Vector3 normalCapsule2SpaceNormalized; + Vector3 segment1ToSegment2; + + // If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping) + if (segmentsPerpendicularDistance > MACHINE_EPSILON) { + + // Compute a perpendicular vector from segment 1 to segment 2 + segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); + normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit(); + } + else { // If the capsule inner segments are overlapping (degenerate case) + + // We cannot use the vector between segments as a contact normal. To generate a contact normal, we take + // any vector that is orthogonal to the inner capsule segments. + + Vector3 vec1(1, 0, 0); + Vector3 vec2(0, 1, 0); + + Vector3 seg2Normalized = seg2.getUnit(); + + // Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product) + decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2)) + decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2)) + + segment1ToSegment2.setToZero(); + + // We choose as a contact normal, any direction that is perpendicular to the inner capsules segments + normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2); + } + + Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); + const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius()); + const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius()); + const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius(); + const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius(); + + decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance; + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + } + + return true; + } + } + + // Compute the closest points between the two inner capsule segments + Vector3 closestPointCapsule1Seg; + Vector3 closestPointCapsule2Seg; + computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB, + closestPointCapsule1Seg, closestPointCapsule2Seg); + + // Compute the distance between the sphere center and the closest point on the segment + Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg); + const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare(); + + // If the collision shapes overlap + if (closestPointsDistanceSquare < sumRadius * sumRadius) { + + if (reportContacts) { + + // If the distance between the inner segments is not zero + if (closestPointsDistanceSquare > MACHINE_EPSILON) { + + decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); + closestPointsSeg1ToSeg2 /= closestPointsDistance; + + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; + + decimal penetrationDepth = sumRadius - closestPointsDistance; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + } + else { // The segment are overlapping (degenerate case) + + // If the capsule segments are parralel + if (areCapsuleInnerSegmentsParralel) { + + // The segment are parallel, not overlapping and their distance is zero. + // Therefore, the capsules are just touching at the top of their inner segments + decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare(); + + Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB; + Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint); + normalCapsuleSpace2.normalize(); + + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); + } + else { // If the capsules inner segments are not parallel + + // We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross + // product between the two segments. + Vector3 normalCapsuleSpace2 = seg1.cross(seg2); + normalCapsuleSpace2.normalize(); + + // Compute the contact points on both shapes + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius(); + + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2; + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local); + } + } + } + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h new file mode 100644 index 00000000..b122ca14 --- /dev/null +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -0,0 +1,70 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H +#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class CapsuleVsCapsuleAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two capsules collision shapes. + */ +class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + CapsuleVsCapsuleAlgorithm() = default; + + /// Destructor + virtual ~CapsuleVsCapsuleAlgorithm() override = default; + + /// Deleted copy-constructor + CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; + + /// Compute the narrow-phase collision detection between two capsules + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp new file mode 100644 index 00000000..33a381f0 --- /dev/null +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -0,0 +1,164 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "CapsuleVsConvexPolyhedronAlgorithm.h" +#include "SAT/SATAlgorithm.h" +#include "GJK/GJKAlgorithm.h" +#include "collision/shapes/CapsuleShape.h" +#include "collision/shapes/ConvexPolyhedronShape.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Compute the narrow-phase collision detection between a capsule and a polyhedron +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. +bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + MemoryAllocator& memoryAllocator) { + + // First, we run the GJK algorithm + GJKAlgorithm gjkAlgorithm; + SATAlgorithm satAlgorithm(memoryAllocator); + +#ifdef IS_PROFILING_ACTIVE + + gjkAlgorithm.setProfiler(mProfiler); + satAlgorithm.setProfiler(mProfiler); + +#endif + + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); + + lastFrameCollisionInfo->wasUsingGJK = true; + lastFrameCollisionInfo->wasUsingSAT = false; + + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + // If we have found a contact point inside the margins (shallow penetration) + if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + + if (reportContacts) { + + // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the + // capsule inner segment and parallel to the contact point normal, we would like to create + // two contact points instead of a single one (as in the deep contact case with SAT algorithm) + + // Get the contact point created by GJK + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + assert(contactPoint != nullptr); + + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; + + // Get the collision shapes + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + // For each face of the polyhedron + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(f); + Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; + + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); + capsuleInnerSegmentDirection.normalize(); + + bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); + bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); + + // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal + // is in direction of the contact normal (from the polyhedron point of view). + if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection) + && areParallelVectors(faceNormalWorld, contactPoint->normal)) { + + // Remove the previous contact point computed by GJK + narrowPhaseInfo->resetContactPoints(); + + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + + // Convert the inner capsule segment points into the polyhedron local-space + const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); + const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; + const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + + const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + + if (isCapsuleShape1) { + faceNormalWorld = -faceNormalWorld; + } + + // Compute and create two contact points + bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, + polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + narrowPhaseInfo, isCapsuleShape1); + if (!contactsFound) { + return false; + } + + break; + } + } + } + + lastFrameCollisionInfo->wasUsingSAT = false; + lastFrameCollisionInfo->wasUsingGJK = false; + + // Return true + return true; + } + + // If we have overlap even without the margins (deep penetration) + if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { + + // Run the SAT algorithm to find the separating axis and compute contact point + bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts); + + lastFrameCollisionInfo->wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; + + return isColliding; + } + + return false; +} diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h new file mode 100644 index 00000000..58bb14fb --- /dev/null +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -0,0 +1,70 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H +#define REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class CapsuleVsConvexPolyhedronAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between a capsule and a convex polyhedron. + */ +class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + CapsuleVsConvexPolyhedronAlgorithm() = default; + + /// Destructor + virtual ~CapsuleVsConvexPolyhedronAlgorithm() override = default; + + /// Deleted copy-constructor + CapsuleVsConvexPolyhedronAlgorithm(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Compute the narrow-phase collision detection between a capsule and a polyhedron + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 230ebbb3..d8351d6a 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -41,6 +41,13 @@ class CollisionDispatch { protected: +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public: /// Constructor @@ -49,13 +56,29 @@ class CollisionDispatch { /// Destructor virtual ~CollisionDispatch() = default; - /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, - int shape2Type)=0; + virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type)=0; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler); + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionDispatch::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp deleted file mode 100644 index c0da7611..00000000 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ /dev/null @@ -1,299 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "collision/shapes/ConcaveShape.h" -#include "collision/shapes/TriangleShape.h" -#include "ConcaveVsConvexAlgorithm.h" -#include "collision/CollisionDetection.h" -#include "engine/CollisionWorld.h" -#include - -using namespace reactphysics3d; - -// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) -void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints) { - - // Create a triangle collision shape - decimal margin = mConcaveShape->getTriangleMargin(); - TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) - TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - - // Create a narrow phase info for the narrow-phase collision detection - NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList; - narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo))) - NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(), - triangleShape, mConvexProxyShape->getLocalToWorldTransform(), - mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(), - mConcaveProxyShape->getCachedCollisionData()); - narrowPhaseInfoList->next = firstNarrowPhaseInfo; -} - -// Return true and compute a contact info if the two bounding volumes collide -void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - NarrowPhaseCallback* narrowPhaseCallback) { - -// ProxyShape* convexProxyShape; -// ProxyShape* concaveProxyShape; -// const ConvexShape* convexShape; -// const ConcaveShape* concaveShape; - -// // Collision shape 1 is convex, collision shape 2 is concave -// if (shape1Info.collisionShape->isConvex()) { -// convexProxyShape = shape1Info.proxyShape; -// convexShape = static_cast(shape1Info.collisionShape); -// concaveProxyShape = shape2Info.proxyShape; -// concaveShape = static_cast(shape2Info.collisionShape); -// } -// else { // Collision shape 2 is convex, collision shape 1 is concave -// convexProxyShape = shape2Info.proxyShape; -// convexShape = static_cast(shape2Info.collisionShape); -// concaveProxyShape = shape1Info.proxyShape; -// concaveShape = static_cast(shape1Info.collisionShape); -// } - -// // Set the parameters of the callback object -// ConvexVsTriangleCallback convexVsTriangleCallback; -// convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); -// convexVsTriangleCallback.setConvexShape(convexShape); -// convexVsTriangleCallback.setConcaveShape(concaveShape); -// convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); -// convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); - -// // Compute the convex shape AABB in the local-space of the convex shape -// AABB aabb; -// convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); - -// // If smooth mesh collision is enabled for the concave mesh -// if (concaveShape->getIsSmoothMeshCollisionEnabled()) { - -// std::vector contactPoints; - -// SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); - -// convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); - -// // Call the convex vs triangle callback for each triangle of the concave shape -// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); - -// // Run the smooth mesh collision algorithm -// processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); -// } -// else { - -// convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); - -// // Call the convex vs triangle callback for each triangle of the concave shape -// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); -// } -} - -//// Test collision between a triangle and the convex mesh shape -//void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) { - -// // Create a triangle collision shape -// decimal margin = mConcaveShape->getTriangleMargin(); -// TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - -// // Select the collision algorithm to use between the triangle and the convex shape -// NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), -// mConvexShape->getType()); - -// // If there is no collision algorithm between those two kinds of shapes -// if (algo == nullptr) return; - -// // Notify the narrow-phase algorithm about the overlapping pair we are going to test -// algo->setCurrentOverlappingPair(mOverlappingPair); - -// // Create the CollisionShapeInfo objects -// CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(), -// mOverlappingPair, mConvexProxyShape->getCachedCollisionData()); -// CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape, -// mConcaveProxyShape->getLocalToWorldTransform(), -// mOverlappingPair, mConcaveProxyShape->getCachedCollisionData()); - -// // Use the collision algorithm to test collision between the triangle and the other convex shape -// algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback); -//} - -// Process the concave triangle mesh collision using the smooth mesh collision algorithm described -// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision -// issue with some internal edges. -void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, - std::vector contactPoints, - NarrowPhaseCallback* narrowPhaseCallback) { - - // Set with the triangle vertices already processed to void further contacts with same triangle - std::unordered_multimap processTriangleVertices; - - // Sort the list of narrow-phase contacts according to their penetration depth - std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); - - // For each contact point (from smaller penetration depth to larger) - std::vector::const_iterator it; - for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { - - const SmoothMeshContactInfo info = *it; - const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - - // Compute the barycentric coordinates of the point in the triangle - decimal u, v, w; - computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], - info.triangleVertices[1], - info.triangleVertices[2], - contactPoint, u, v, w); - int nbZeros = 0; - bool isUZero = approxEqual(u, 0, 0.0001); - bool isVZero = approxEqual(v, 0, 0.0001); - bool isWZero = approxEqual(w, 0, 0.0001); - if (isUZero) nbZeros++; - if (isVZero) nbZeros++; - if (isWZero) nbZeros++; - - // If it is a vertex contact - if (nbZeros == 2) { - - Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); - - // Check that this triangle vertex has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { - - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } - } - else if (nbZeros == 1) { // If it is an edge contact - - Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); - Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); - - // Check that this triangle edge has not been processed yet - if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && - !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { - - // Keep the contact as it is and report it - narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); - } - - } - else { // If it is a face contact - - ContactPointInfo newContactInfo(info.contactInfo); - - ProxyShape* firstShape; - ProxyShape* secondShape; - if (info.isFirstShapeTriangle) { - firstShape = overlappingPair->getShape1(); - secondShape = overlappingPair->getShape2(); - } - else { - firstShape = overlappingPair->getShape2(); - secondShape = overlappingPair->getShape1(); - } - - // We use the triangle normal as the contact normal - Vector3 a = info.triangleVertices[1] - info.triangleVertices[0]; - Vector3 b = info.triangleVertices[2] - info.triangleVertices[0]; - Vector3 localNormal = a.cross(b); - newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; - Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; - newContactInfo.normal.normalize(); - if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { - newContactInfo.normal = -newContactInfo.normal; - } - - // We recompute the contact point on the second body with the new normal as described in - // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and - // Dirk Gregorius) to avoid adding torque - Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); - if (info.isFirstShapeTriangle) { - Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; - newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; - } - else { - Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; - newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; - } - - // Report the contact - narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); - } - - // Add the three vertices of the triangle to the set of processed - // triangle vertices - addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); - addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); - } -} - -// Return true if the vertex is in the set of already processed vertices -bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, const Vector3& vertex) const { - - int key = int(vertex.x * vertex.y * vertex.z); - - auto range = processTriangleVertices.equal_range(key); - for (auto it = range.first; it != range.second; ++it) { - if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true; - } - - return false; -} - - -//// Called by a narrow-phase collision algorithm when a new contact has been found -//void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, -// const ContactPointInfo& contactInfo) { -// Vector3 triangleVertices[3]; -// bool isFirstShapeTriangle; - -// // If the collision shape 1 is the triangle -// if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) { -// assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE); - -// const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape1); -// triangleVertices[0] = triangleShape->getVertex(0); -// triangleVertices[1] = triangleShape->getVertex(1); -// triangleVertices[2] = triangleShape->getVertex(2); - -// isFirstShapeTriangle = true; -// } -// else { // If the collision shape 2 is the triangle -// assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE); - -// const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape2); -// triangleVertices[0] = triangleShape->getVertex(0); -// triangleVertices[1] = triangleShape->getVertex(1); -// triangleVertices[2] = triangleShape->getVertex(2); - -// isFirstShapeTriangle = false; -// } -// SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); - -// // Add the narrow-phase contact into the list of contact to process for -// // smooth mesh collision -// mContactPoints.push_back(smoothContactInfo); -//} diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h deleted file mode 100644 index 531615da..00000000 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ /dev/null @@ -1,204 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H -#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H - -// Libraries -#include "NarrowPhaseAlgorithm.h" -#include "collision/shapes/ConvexShape.h" -#include "collision/shapes/ConcaveShape.h" -#include "memory/SingleFrameAllocator.h" -#include - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Class ConvexVsTriangleCallback -/** - * This class is used to report a collision between the triangle - * of a concave mesh shape and a convex shape during the - * middle-phase algorithm - */ -class MiddlePhaseTriangleCallback : public TriangleCallback { - - protected: - - /// Broadphase overlapping pair - OverlappingPair* mOverlappingPair; - - /// Pointer to the concave proxy shape - ProxyShape* mConcaveProxyShape; - - /// Pointer to the convex proxy shape - ProxyShape* mConvexProxyShape; - - /// Pointer to the concave collision shape - const ConcaveShape* mConcaveShape; - - /// Reference to the single-frame memory allocator - Allocator& mAllocator; - - public: - - /// Pointer to the first element of the linked-list of narrow-phase info - NarrowPhaseInfo* narrowPhaseInfoList; - - /// Constructor - MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair, - ProxyShape* concaveProxyShape, - ProxyShape* convexProxyShape, const ConcaveShape* concaveShape, - Allocator& allocator) - :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape), - mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape), - mAllocator(allocator), narrowPhaseInfoList(nullptr) { - - } - - /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle(const Vector3* trianglePoints) override; -}; - -// Class SmoothMeshContactInfo -/** - * This class is used to store data about a contact with a triangle for the smooth - * mesh algorithm. - */ -class SmoothMeshContactInfo { - - public: - - ContactPointInfo contactInfo; - bool isFirstShapeTriangle; - Vector3 triangleVertices[3]; - - /// Constructor - SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1, - const Vector3& trianglePoint2, const Vector3& trianglePoint3) - : contactInfo(contact) { - isFirstShapeTriangle = firstShapeTriangle; - triangleVertices[0] = trianglePoint1; - triangleVertices[1] = trianglePoint2; - triangleVertices[2] = trianglePoint3; - } - -}; - -struct ContactsDepthCompare { - bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2) - { - return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; - } -}; - -/// Method used to compare two smooth mesh contact info to sort them -//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1, -// const SmoothMeshContactInfo& contact2) { -// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth; -//} - -// TODO : Delete this -// Class SmoothCollisionNarrowPhaseCallback -/** - * This class is used as a narrow-phase callback to get narrow-phase contacts - * of the concave triangle mesh to temporary store them in order to be used in - * the smooth mesh collision algorithm if this one is enabled. - */ -class SmoothCollisionNarrowPhaseCallback { - - private: - - std::vector& mContactPoints; - - - public: - - // Constructor - SmoothCollisionNarrowPhaseCallback(std::vector& contactPoints) - : mContactPoints(contactPoints) { - - } - -}; - -// TODO : Delete this -// Class ConcaveVsConvexAlgorithm -/** - * This class is used to compute the narrow-phase collision detection - * between a concave collision shape and a convex collision shape. The idea is - * to use the GJK collision detection algorithm to compute the collision between - * the convex shape and each of the triangles in the concave shape. - */ -class ConcaveVsConvexAlgorithm { - - protected : - - // -------------------- Attributes -------------------- // - - // -------------------- Methods -------------------- // - - /// Process the concave triangle mesh collision using the smooth mesh collision algorithm - void processSmoothMeshCollision(OverlappingPair* overlappingPair, - std::vector contactPoints, - NarrowPhaseCallback* narrowPhaseCallback); - - /// Add a triangle vertex into the set of processed triangles - void addProcessedVertex(std::unordered_multimap& processTriangleVertices, - const Vector3& vertex); - - /// Return true if the vertex is in the set of already processed vertices - bool hasVertexBeenProcessed(const std::unordered_multimap& processTriangleVertices, - const Vector3& vertex) const; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - ConcaveVsConvexAlgorithm() = default; - - /// Destructor - ~ConcaveVsConvexAlgorithm() = default; - - /// Private copy-constructor - ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm) = delete; - - /// Private assignment operator - ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm) = delete; - - /// Compute a contact info if the two bounding volume collide - void testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - NarrowPhaseCallback* narrowPhaseCallback); -}; - -// Add a triangle vertex into the set of processed triangles -inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap& processTriangleVertices, const Vector3& vertex) { - processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex)); -} - -} - -#endif - diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp new file mode 100644 index 00000000..375814cd --- /dev/null +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -0,0 +1,58 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" +#include "GJK/GJKAlgorithm.h" +#include "SAT/SATAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Compute the narrow-phase collision detection between two convex polyhedra +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + MemoryAllocator& memoryAllocator) { + + // Run the SAT algorithm to find the separating axis and compute contact point + SATAlgorithm satAlgorithm(memoryAllocator); + +#ifdef IS_PROFILING_ACTIVE + + satAlgorithm.setProfiler(mProfiler); + +#endif + + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts); + + lastFrameCollisionInfo->wasUsingSAT = true; + lastFrameCollisionInfo->wasUsingGJK = false; + + return isColliding; +} diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h new file mode 100644 index 00000000..ddd10585 --- /dev/null +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -0,0 +1,70 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H +#define REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two convex polyhedra. + */ +class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default; + + /// Destructor + virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default; + + /// Deleted copy-constructor + ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Compute the narrow-phase collision detection between two convex polyhedra + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 35b1e50b..05616b27 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -36,15 +36,34 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t CollisionShapeType shape1Type = static_cast(type1); CollisionShapeType shape2Type = static_cast(type2); - // Convex vs Convex algorithm (GJK algorithm) - if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { - return &mGJKAlgorithm; - } - // Sphere vs Sphere algorithm - else if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { - return &mSphereVsSphereAlgorithm; - } - else { + if (type1 > type2) { return nullptr; } + // Sphere vs Sphere algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) { + return &mSphereVsSphereAlgorithm; + } + // Sphere vs Capsule algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) { + return &mSphereVsCapsuleAlgorithm; + } + // Capsule vs Capsule algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) { + return &mCapsuleVsCapsuleAlgorithm; + } + // Sphere vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mSphereVsConvexPolyhedronAlgorithm; + } + // Capsule vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mCapsuleVsConvexPolyhedronAlgorithm; + } + // Convex Polyhedron vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON && + shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mConvexPolyhedronVsConvexPolyhedronAlgorithm; + } + + return nullptr; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index f62a6564..fe22b4a9 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -28,8 +28,12 @@ // Libraries #include "CollisionDispatch.h" -#include "ConcaveVsConvexAlgorithm.h" #include "SphereVsSphereAlgorithm.h" +#include "SphereVsConvexPolyhedronAlgorithm.h" +#include "SphereVsCapsuleAlgorithm.h" +#include "CapsuleVsCapsuleAlgorithm.h" +#include "CapsuleVsConvexPolyhedronAlgorithm.h" +#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" #include "GJK/GJKAlgorithm.h" namespace reactphysics3d { @@ -47,8 +51,20 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Sphere vs Sphere collision algorithm SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; - /// GJK Algorithm - GJKAlgorithm mGJKAlgorithm; + /// Capsule vs Capsule collision algorithm + CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm; + + /// Sphere vs Capsule collision algorithm + SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm; + + /// Sphere vs Convex Polyhedron collision algorithm + SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm; + + /// Capsule vs Convex Polyhedron collision algorithm + CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm; + + /// Convex Polyhedron vs Convex Polyhedron collision algorithm + ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm; public: @@ -61,8 +77,33 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler) override; + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void DefaultCollisionDispatch::setProfiler(Profiler* profiler) { + + CollisionDispatch::setProfiler(profiler); + + mSphereVsSphereAlgorithm.setProfiler(profiler); + mCapsuleVsCapsuleAlgorithm.setProfiler(profiler); + mSphereVsCapsuleAlgorithm.setProfiler(profiler); + mSphereVsConvexPolyhedronAlgorithm.setProfiler(profiler); + mCapsuleVsConvexPolyhedronAlgorithm.setProfiler(profiler); + mConvexPolyhedronVsConvexPolyhedronAlgorithm.setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp deleted file mode 100644 index 6afa7a10..00000000 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ /dev/null @@ -1,436 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "EPAAlgorithm.h" -#include "engine/Profiler.h" -#include "collision/narrowphase//GJK/GJKAlgorithm.h" -#include "TrianglesStore.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Decide if the origin is in the tetrahedron. -/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of -/// the vertex that is wrong if the origin is not in the tetrahedron -int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const { - - // Check vertex 1 - Vector3 normal1 = (p2-p1).cross(p3-p1); - if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { - return 4; - } - - // Check vertex 2 - Vector3 normal2 = (p4-p2).cross(p3-p2); - if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { - return 1; - } - - // Check vertex 3 - Vector3 normal3 = (p4-p3).cross(p1-p3); - if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { - return 2; - } - - // Check vertex 4 - Vector3 normal4 = (p2-p4).cross(p1-p4); - if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { - return 3; - } - - // The origin is in the tetrahedron, we return 0 - return 0; -} - -// Compute the penetration depth with the EPA algorithm. -/// This method computes the penetration depth and contact points between two -/// enlarged objects (with margin) where the original objects (without margin) -/// intersect. An initial simplex that contains origin has been computed with -/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find -/// the correct penetration depth. This method returns true if the EPA penetration -/// depth computation has succeeded and false it has failed. -bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, - const NarrowPhaseInfo* narrowPhaseInfo, - Vector3& v, - ContactPointInfo& contactPointInfo) { - - PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); - - decimal gjkPenDepthSquare = v.lengthSquare(); - - assert(narrowPhaseInfo->collisionShape1->isConvex()); - assert(narrowPhaseInfo->collisionShape2->isConvex()); - - const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); - const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - - void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; - void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; - - Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates - Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates - Vector3 points[MAX_SUPPORT_POINTS]; // Current points - TrianglesStore triangleStore; // Store the triangles - TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face - // candidate of the EPA algorithm - - // Transform a point from local space of body 2 to local - // space of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2Tobody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform; - - // Matrix that transform a direction from local - // space of body 1 into local space of body 2 - Quaternion rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getInverse() * - narrowPhaseInfo->shape1ToWorldTransform.getOrientation(); - - // Get the simplex computed previously by the GJK algorithm - int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); - - // Compute the tolerance - decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); - - // Number of triangles in the polytope - unsigned int nbTriangles = 0; - - // Clear the storing of triangles - triangleStore.clear(); - - // Select an action according to the number of points in the simplex - // computed with GJK algorithm in order to obtain an initial polytope for - // The EPA algorithm. - switch(nbVertices) { - case 1: - // Only one point in the simplex (which should be the origin). - // We have a touching contact with zero penetration depth. - // We drop that kind of contact. Therefore, we return false - return false; - - case 2: { - // The simplex returned by GJK is a line segment d containing the origin. - // We add two additional support points to construct a hexahedron (two tetrahedron - // glued together with triangle faces. The idea is to compute three different vectors - // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively - // rotated of 120 degree around the d segment. The the three new points to - // construct the polytope are the three support points in those three directions - // v1, v2 and v3. - - // Direction of the segment - Vector3 d = (points[1] - points[0]).getUnit(); - - // Choose the coordinate axis from the minimal absolute component of the vector d - int minAxis = d.getAbsoluteVector().getMinAxis(); - - // Compute sin(60) - const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5); - - // Create a rotation quaternion to rotate the vector v1 to get the vectors - // v2 and v3 - Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); - - // Compute the vector v1, v2, v3 - Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); - Vector3 v2 = rotationQuat * v1; - Vector3 v3 = rotationQuat * v2; - - // Compute the support point in the direction of v1 - suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); - suppPointsB[2] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); - points[2] = suppPointsA[2] - suppPointsB[2]; - - // Compute the support point in the direction of v2 - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); - suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); - points[3] = suppPointsA[3] - suppPointsB[3]; - - // Compute the support point in the direction of v3 - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); - suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); - points[4] = suppPointsA[4] - suppPointsB[4]; - - // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the - // tetrahedron that contains the origin in order that the initial polytope of the - // EPA algorithm is a tetrahedron, which is simpler to deal with. - - // If the origin is in the tetrahedron of points 0, 2, 3, 4 - if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { - // We use the point 4 instead of point 1 for the initial tetrahedron - suppPointsA[1] = suppPointsA[4]; - suppPointsB[1] = suppPointsB[4]; - points[1] = points[4]; - } - // If the origin is in the tetrahedron of points 1, 2, 3, 4 - else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { - // We use the point 4 instead of point 0 for the initial tetrahedron - suppPointsA[0] = suppPointsA[4]; - suppPointsB[0] = suppPointsB[4]; - points[0] = points[4]; - } - else { - // The origin is not in the initial polytope - return false; - } - - // The polytope contains now 4 vertices - nbVertices = 4; - } - case 4: { - // The simplex computed by the GJK algorithm is a tetrahedron. Here we check - // if this tetrahedron contains the origin. If it is the case, we keep it and - // otherwise we remove the wrong vertex of the tetrahedron and go in the case - // where the GJK algorithm compute a simplex of three vertices. - - // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) - int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]); - - // If the origin is in the tetrahedron - if (badVertex == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. - - // Comstruct the 4 triangle faces of the tetrahedron - TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2); - TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1); - TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); - TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); - - // If the constructed tetrahedron is not correct - if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr) - && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 - && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { - return false; - } - - // Associate the edges of neighbouring triangle faces - link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); - link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); - link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); - link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); - link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); - link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - - break; - } - - // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) - // Remove the wrong vertex and continue to the next case with the - // three remaining vertices - if (badVertex < 4) { - - suppPointsA[badVertex-1] = suppPointsA[3]; - suppPointsB[badVertex-1] = suppPointsB[3]; - points[badVertex-1] = points[3]; - } - - // We have removed the wrong vertex - nbVertices = 3; - } - case 3: { - // The GJK algorithm returned a triangle that contains the origin. - // We need two new vertices to create two tetrahedron. The two new - // vertices are the support points in the "n" and "-n" direction - // where "n" is the normal of the triangle. Then, we use only the - // tetrahedron that contains the origin. - - // Compute the normal of the triangle - Vector3 v1 = points[1] - points[0]; - Vector3 v2 = points[2] - points[0]; - Vector3 n = v1.cross(v2); - - // Compute the two new vertices to obtain a hexahedron - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); - suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); - points[3] = suppPointsA[3] - suppPointsB[3]; - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); - suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); - points[4] = suppPointsA[4] - suppPointsB[4]; - - TriangleEPA* face0 = nullptr; - TriangleEPA* face1 = nullptr; - TriangleEPA* face2 = nullptr; - TriangleEPA* face3 = nullptr; - - // If the origin is in the first tetrahedron - if (isOriginInTetrahedron(points[0], points[1], - points[2], points[3]) == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. - - // Comstruct the 4 triangle faces of the tetrahedron - face0 = triangleStore.newTriangle(points, 0, 1, 2); - face1 = triangleStore.newTriangle(points, 0, 3, 1); - face2 = triangleStore.newTriangle(points, 0, 2, 3); - face3 = triangleStore.newTriangle(points, 1, 3, 2); - } - else if (isOriginInTetrahedron(points[0], points[1], - points[2], points[4]) == 0) { - - // The tetrahedron is a correct initial polytope for the EPA algorithm. - // Therefore, we construct the tetrahedron. - - // Comstruct the 4 triangle faces of the tetrahedron - face0 = triangleStore.newTriangle(points, 0, 1, 2); - face1 = triangleStore.newTriangle(points, 0, 4, 1); - face2 = triangleStore.newTriangle(points, 0, 2, 4); - face3 = triangleStore.newTriangle(points, 1, 4, 2); - } - else { - return false; - } - - // If the constructed tetrahedron is not correct - if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr) - && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 - && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { - return false; - } - - // Associate the edges of neighbouring triangle faces - link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); - link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); - link(EdgeEPA(face0, 2), EdgeEPA(face2, 0)); - link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); - link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); - link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - - nbVertices = 4; - - } - break; - } - - // At this point, we have a polytope that contains the origin. Therefore, we - // can run the EPA algorithm. - - if (nbTriangles == 0) { - return false; - } - - TriangleEPA* triangle = 0; - decimal upperBoundSquarePenDepth = DECIMAL_LARGEST; - - do { - triangle = triangleHeap[0]; - - // Get the next candidate face (the face closest to the origin) - std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison); - nbTriangles--; - - // If the candidate face in the heap is not obsolete - if (!triangle->getIsObsolete()) { - - // If we have reached the maximum number of support points - if (nbVertices == MAX_SUPPORT_POINTS) { - assert(false); - break; - } - - // Compute the support point of the Minkowski - // difference (A-B) in the closest point direction - suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( - triangle->getClosestPoint(), shape1CachedCollisionData); - suppPointsB[nbVertices] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * - (-triangle->getClosestPoint()), shape2CachedCollisionData); - points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; - - int indexNewVertex = nbVertices; - nbVertices++; - - // Update the upper bound of the penetration depth - decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); - - decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); - if (wDotVSquare < upperBoundSquarePenDepth) { - upperBoundSquarePenDepth = wDotVSquare; - } - - // Compute the error - decimal error = wDotv - triangle->getDistSquare(); - if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || - points[indexNewVertex] == points[(*triangle)[0]] || - points[indexNewVertex] == points[(*triangle)[1]] || - points[indexNewVertex] == points[(*triangle)[2]]) { - break; - } - - // Now, we compute the silhouette cast by the new vertex. The current triangle - // face will not be in the convex hull. We start the local recursive silhouette - // algorithm from the current triangle face. - int i = triangleStore.getNbTriangles(); - if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { - break; - } - - // Add all the new triangle faces computed with the silhouette algorithm - // to the candidates list of faces of the current polytope - while(i != triangleStore.getNbTriangles()) { - TriangleEPA* newTriangle = &triangleStore[i]; - addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth); - i++; - } - } - } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); - - // Compute the contact info - v = narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * triangle->getClosestPoint(); - Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); - Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); - Vector3 normal = v.getUnit(); - decimal penetrationDepth = v.length(); - - // If the length of the normal vector is too small, skip this contact point - if (normal.lengthSquare() < MACHINE_EPSILON) { - return false; - } - - if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) { - - // Create the contact info object - contactPointInfo.init(normal, penetrationDepth, pALocal, pBLocal); - - return true; - } - - return false; -} diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h deleted file mode 100644 index 1d241540..00000000 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_EPA_ALGORITHM_H -#define REACTPHYSICS3D_EPA_ALGORITHM_H - -// Libraries -#include "collision/narrowphase/GJK/VoronoiSimplex.h" -#include "collision/shapes/CollisionShape.h" -#include "collision/NarrowPhaseInfo.h" -#include "constraint/ContactPoint.h" -#include "collision/narrowphase/NarrowPhaseAlgorithm.h" -#include "mathematics/mathematics.h" -#include "TriangleEPA.h" -#include "memory/PoolAllocator.h" -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// ---------- Constants ---------- // - -/// Maximum number of support points of the polytope -constexpr unsigned int MAX_SUPPORT_POINTS = 100; - -/// Maximum number of facets of the polytope -constexpr unsigned int MAX_FACETS = 200; - - -// Class TriangleComparison -/** - * This class allows the comparison of two triangles in the heap - * The comparison between two triangles is made using their square distance to the closest - * point to the origin. The goal is that in the heap, the first triangle is the one with the - * smallest square distance. - */ -class TriangleComparison { - - public: - - /// Comparison operator - bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { - return (face1->getDistSquare() > face2->getDistSquare()); - } -}; - - -// Class EPAAlgorithm -/** - * This class is the implementation of the Expanding Polytope Algorithm (EPA). - * The EPA algorithm computes the penetration depth and contact points between - * two enlarged objects (with margin) where the original objects (without margin) - * intersect. The penetration depth of a pair of intersecting objects A and B is - * the length of a point on the boundary of the Minkowski sum (A-B) closest to the - * origin. The goal of the EPA algorithm is to start with an initial simplex polytope - * that contains the origin and expend it in order to find the point on the boundary - * of (A-B) that is closest to the origin. An initial simplex that contains origin - * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex - * polytope to find the correct penetration depth. The implementation of the EPA - * algorithm is based on the book "Collision Detection in 3D Environments". - */ -class EPAAlgorithm { - - private: - - // -------------------- Attributes -------------------- // - - /// Triangle comparison operator - TriangleComparison mTriangleComparison; - - // -------------------- Methods -------------------- // - - /// Add a triangle face in the candidate triangle heap - void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles, - decimal upperBoundSquarePenDepth); - - /// Decide if the origin is in the tetrahedron. - int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, - const Vector3& p3, const Vector3& p4) const; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - EPAAlgorithm() = default; - - /// Destructor - ~EPAAlgorithm() = default; - - /// Deleted copy-constructor - EPAAlgorithm(const EPAAlgorithm& algorithm) = delete; - - /// Deleted assignment operator - EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete; - - /// Compute the penetration depth with EPA algorithm. - bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, - const NarrowPhaseInfo* narrowPhaseInfo, - Vector3& v, - ContactPointInfo &contactPointInfo); -}; - -// Add a triangle face in the candidate triangle heap in the EPA algorithm -inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, - uint& nbTriangles, decimal upperBoundSquarePenDepth) { - - // If the closest point of the affine hull of triangle - // points is internal to the triangle and if the distance - // of the closest point from the origin is at most the - // penetration depth upper bound - if (triangle->isClosestPointInternalToTriangle() && - triangle->getDistSquare() <= upperBoundSquarePenDepth) { - - // Add the triangle face to the list of candidates - heap[nbTriangles] = triangle; - nbTriangles++; - std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison); - } -} - -} - -#endif - diff --git a/src/collision/narrowphase/EPA/EdgeEPA.cpp b/src/collision/narrowphase/EPA/EdgeEPA.cpp deleted file mode 100644 index dcbf61fd..00000000 --- a/src/collision/narrowphase/EPA/EdgeEPA.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "EdgeEPA.h" -#include "TriangleEPA.h" -#include "TrianglesStore.h" -#include - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Constructor -EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index) - : mOwnerTriangle(ownerTriangle), mIndex(index) { - assert(index >= 0 && index < 3); -} - -// Copy-constructor -EdgeEPA::EdgeEPA(const EdgeEPA& edge) { - mOwnerTriangle = edge.mOwnerTriangle; - mIndex = edge.mIndex; -} - -// Return the index of the source vertex of the edge (vertex starting the edge) -uint EdgeEPA::getSourceVertexIndex() const { - return (*mOwnerTriangle)[mIndex]; -} - -// Return the index of the target vertex of the edge (vertex ending the edge) -uint EdgeEPA::getTargetVertexIndex() const { - return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)]; -} - -// Execute the recursive silhouette algorithm from this edge -bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, - TrianglesStore& triangleStore) { - // If the edge has not already been visited - if (!mOwnerTriangle->getIsObsolete()) { - - // If the triangle of this edge is not visible from the given point - if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); - - // If the triangle has been created - if (triangle != nullptr) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } - - return false; - } - else { - - // The current triangle is visible and therefore obsolete - mOwnerTriangle->setIsObsolete(true); - - int backup = triangleStore.getNbTriangles(); - - if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { - mOwnerTriangle->setIsObsolete(false); - - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); - - // If the triangle has been created - if (triangle != nullptr) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } - - return false; - } - else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { - mOwnerTriangle->setIsObsolete(false); - - triangleStore.setNbTriangles(backup); - - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, - getTargetVertexIndex(), - getSourceVertexIndex()); - - if (triangle != nullptr) { - halfLink(EdgeEPA(triangle, 1), *this); - return true; - } - - return false; - } - } - } - - return true; -} diff --git a/src/collision/narrowphase/EPA/TriangleEPA.cpp b/src/collision/narrowphase/EPA/TriangleEPA.cpp deleted file mode 100644 index c322059f..00000000 --- a/src/collision/narrowphase/EPA/TriangleEPA.cpp +++ /dev/null @@ -1,145 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "TriangleEPA.h" -#include "EdgeEPA.h" -#include "TrianglesStore.h" - -// We use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Constructor -TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3) - : mIsObsolete(false) { - mIndicesVertices[0] = indexVertex1; - mIndicesVertices[1] = indexVertex2; - mIndicesVertices[2] = indexVertex3; -} - -// Compute the point v closest to the origin of this triangle -bool TriangleEPA::computeClosestPoint(const Vector3* vertices) { - const Vector3& p0 = vertices[mIndicesVertices[0]]; - - Vector3 v1 = vertices[mIndicesVertices[1]] - p0; - Vector3 v2 = vertices[mIndicesVertices[2]] - p0; - decimal v1Dotv1 = v1.dot(v1); - decimal v1Dotv2 = v1.dot(v2); - decimal v2Dotv2 = v2.dot(v2); - decimal p0Dotv1 = p0.dot(v1); - decimal p0Dotv2 = p0.dot(v2); - - // Compute determinant - mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; - - // Compute lambda values - mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; - mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; - - // If the determinant is positive - if (mDet > 0.0) { - // Compute the closest point v - mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2); - - // Compute the square distance of closest point to the origin - mDistSquare = mClosestPoint.dot(mClosestPoint); - - return true; - } - - return false; -} - -/// Link an edge with another one. It means that the current edge of a triangle will -/// be associated with the edge of another triangle in order that both triangles -/// are neighbour along both edges). -bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { - bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && - edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - - if (isPossible) { - edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; - edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0; - } - - return isPossible; -} - -/// Make an half link of an edge with another one from another triangle. An half-link -/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an -/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will -/// be made later. -void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) { - assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && - edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - - // Link - edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; -} - -// Execute the recursive silhouette algorithm from this triangle face. -/// The parameter "vertices" is an array that contains the vertices of the current polytope and the -/// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the -/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore, -/// the triangle faces that are visible from the new vertex must be removed from the polytope and we -/// need to add triangle faces where each face contains the new vertex and an edge of the silhouette. -/// The silhouette is the connected set of edges that are part of the border between faces that -/// are seen and faces that are not seen from the new vertex. This method starts from the nearest -/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in -/// order that we always have a convex polytope. The faces visible from the new vertex are set -/// obselete and will not be considered as being a candidate face in the future. -bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, - TrianglesStore& triangleStore) { - - uint first = triangleStore.getNbTriangles(); - - // Mark the current triangle as obsolete because it - setIsObsolete(true); - - // Execute recursively the silhouette algorithm for the adjacent edges of neighboring - // triangles of the current triangle - bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); - - if (result) { - int i,j; - - // For each triangle face that contains the new vertex and an edge of the silhouette - for (i=first, j=triangleStore.getNbTriangles()-1; - i != triangleStore.getNbTriangles(); j = i++) { - TriangleEPA* triangle = &triangleStore[i]; - halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); - - if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) { - return false; - } - } - - } - - return result; -} diff --git a/src/collision/narrowphase/EPA/TriangleEPA.h b/src/collision/narrowphase/EPA/TriangleEPA.h deleted file mode 100644 index 865c69d4..00000000 --- a/src/collision/narrowphase/EPA/TriangleEPA.h +++ /dev/null @@ -1,197 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_TRIANGLE_EPA_H -#define REACTPHYSICS3D_TRIANGLE_EPA_H - -// Libraries -#include "mathematics/mathematics.h" -#include "configuration.h" -#include "EdgeEPA.h" -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Prototypes -bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); -void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); - - -// Class TriangleEPA -/** - * This class represents a triangle face of the current polytope in the EPA algorithm. - */ -class TriangleEPA { - - private: - - // -------------------- Attributes -------------------- // - - /// Indices of the vertices y_i of the triangle - uint mIndicesVertices[3]; - - /// Three adjacent edges of the triangle (edges of other triangles) - EdgeEPA mAdjacentEdges[3]; - - /// True if the triangle face is visible from the new support point - bool mIsObsolete; - - /// Determinant - decimal mDet; - - /// Point v closest to the origin on the affine hull of the triangle - Vector3 mClosestPoint; - - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - decimal mLambda1; - - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - decimal mLambda2; - - /// Square distance of the point closest point v to the origin - decimal mDistSquare; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - TriangleEPA() = default; - - /// Constructor - TriangleEPA(uint v1, uint v2, uint v3); - - /// Destructor - ~TriangleEPA() = default; - - /// Deleted copy-constructor - TriangleEPA(const TriangleEPA& triangle) = delete; - - /// Deleted assignment operator - TriangleEPA& operator=(const TriangleEPA& triangle) = delete; - - /// Return an adjacent edge of the triangle - EdgeEPA& getAdjacentEdge(int index); - - /// Set an adjacent edge of the triangle - void setAdjacentEdge(int index, EdgeEPA& edge); - - /// Return the square distance of the closest point to origin - decimal getDistSquare() const; - - /// Set the isObsolete value - void setIsObsolete(bool isObsolete); - - /// Return true if the triangle face is obsolete - bool getIsObsolete() const; - - /// Return the point closest to the origin - const Vector3& getClosestPoint() const; - - // Return true if the closest point on affine hull is inside the triangle - bool isClosestPointInternalToTriangle() const; - - /// Return true if the triangle is visible from a given vertex - bool isVisibleFromVertex(const Vector3* vertices, uint index) const; - - /// Compute the point v closest to the origin of this triangle - bool computeClosestPoint(const Vector3* vertices); - - /// Compute the point of an object closest to the origin - Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; - - /// Execute the recursive silhouette algorithm from this triangle face. - bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore); - - /// Access operator - uint operator[](int i) const; - - /// Associate two edges - friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); - - /// Make a half-link between two edges - friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); -}; - -// Return an edge of the triangle -inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) { - assert(index >= 0 && index < 3); - return mAdjacentEdges[index]; -} - -// Set an adjacent edge of the triangle -inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) { - assert(index >=0 && index < 3); - mAdjacentEdges[index] = edge; -} - -// Return the square distance of the closest point to origin -inline decimal TriangleEPA::getDistSquare() const { - return mDistSquare; -} - -// Set the isObsolete value -inline void TriangleEPA::setIsObsolete(bool isObsolete) { - mIsObsolete = isObsolete; -} - -// Return true if the triangle face is obsolete -inline bool TriangleEPA::getIsObsolete() const { - return mIsObsolete; -} - -// Return the point closest to the origin -inline const Vector3& TriangleEPA::getClosestPoint() const { - return mClosestPoint; -} - -// Return true if the closest point on affine hull is inside the triangle -inline bool TriangleEPA::isClosestPointInternalToTriangle() const { - return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet); -} - -// Return true if the triangle is visible from a given vertex -inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const { - Vector3 closestToVert = vertices[index] - mClosestPoint; - return (mClosestPoint.dot(closestToVert) > 0.0); -} - -// Compute the point of an object closest to the origin -inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{ - const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]]; - return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) + - mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0)); -} - -// Access operator -inline uint TriangleEPA::operator[](int i) const { - assert(i >= 0 && i <3); - return mIndicesVertices[i]; -} - -} - -#endif diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index e1649af0..62685867 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -26,6 +26,8 @@ // Libraries #include "GJKAlgorithm.h" #include "constraint/ContactPoint.h" +#include "engine/OverlappingPair.h" +#include "collision/shapes/TriangleShape.h" #include "configuration.h" #include "engine/Profiler.h" #include @@ -45,10 +47,9 @@ using namespace reactphysics3d; /// algorithm on the enlarged object to obtain a simplex polytope that contains the /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. -bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { +GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) { - PROFILE("GJKAlgorithm::testCollision()"); + PROFILE("GJKAlgorithm::testCollision()", mProfiler); Vector3 suppA; // Support point of object A Vector3 suppB; // Support point of object B @@ -65,23 +66,18 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; - void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; - - bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); - // Get the local-space to world-space transforms const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; // Transform a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2Tobody1 = transform1.getInverse() * transform2; + Transform transform1Inverse = transform1.getInverse(); + Transform body2Tobody1 = transform1Inverse * transform2; - // Matrix that transform a direction from local + // Quaternion that transform a direction from local // space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * - transform1.getOrientation().getMatrix(); + Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation(); // Initialize the margin (sum of margins of both objects) decimal margin = shape1->getMargin() + shape2->getMargin(); @@ -91,8 +87,18 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, // Create a simplex set VoronoiSimplex simplex; + // Get the last collision frame info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + // Get the previous point V (last cached separating axis) - Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis(); + Vector3 v; + if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) { + v = lastFrameCollisionInfo->gjkSeparatingAxis; + assert(v.lengthSquare() > decimal(0.000001)); + } + else { + v.setAllValues(0, 1, 0); + } // Initialize the upper bound for the square distance decimal distSquare = DECIMAL_LARGEST; @@ -100,9 +106,8 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, do { // Compute the support points for original objects (without margins) A and B - suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); - suppB = body2Tobody1 * - shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); + suppA = shape1->getLocalSupportPointWithoutMargin(-v); + suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -113,10 +118,10 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v); + lastFrameCollisionInfo->gjkSeparatingAxis = v; // No intersection, we return - return false; + return GJKResult::SEPARATED; } // If the objects intersect only in the margins @@ -147,12 +152,6 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, break; } - // Closest point is almost the origin, go to EPA algorithm - // Vector v to small to continue computing support points - if (v.lengthSquare() < MACHINE_EPSILON) { - break; - } - // Store and update the squared distance of the closest point prevDistSquare = distSquare; distSquare = v.lengthSquare(); @@ -170,22 +169,7 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, break; } - } while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() && - distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint())); - - // If no contact has been found (penetration case) - if (!contactFound) { - - // The objects (without margins) intersect. Therefore, we run the GJK algorithm - // again but on the enlarged objects to compute a simplex polytope that contains - // the origin. Then, we give that simplex polytope to the EPA algorithm to compute - // the correct penetration depth and contact points between the enlarged objects. - if(computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v)) { - - // A contact has been found with EPA algorithm, we return true - return true; - } - } + } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()); if (contactFound && distSquare > MACHINE_EPSILON) { @@ -205,104 +189,30 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, // If the penetration depth is negative (due too numerical errors), there is no contact if (penetrationDepth <= decimal(0.0)) { - return false; + return GJKResult::SEPARATED; } // Do not generate a contact point with zero normal length if (normal.lengthSquare() < MACHINE_EPSILON) { - return false; + return GJKResult::SEPARATED; } - // Create the contact info object - contactPointInfo.init(normal, penetrationDepth, pA, pB); + if (reportContacts) { - return true; + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2, + penetrationDepth, normal); + + // Add a new contact point + narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB); + } + + return GJKResult::COLLIDE_IN_MARGIN; } - return false; + return GJKResult::INTERPENETRATE; } -/// This method runs the GJK algorithm on the two enlarged objects (with margin) -/// to compute a simplex polytope that contains the origin. The two objects are -/// assumed to intersect in the original objects (without margin). Therefore such -/// a polytope must exist. Then, we give that polytope to the EPA algorithm to -/// compute the correct penetration depth and contact points of the enlarged objects. -bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo, - Vector3& v) { - PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); - - VoronoiSimplex simplex; - Vector3 suppA; - Vector3 suppB; - Vector3 w; - decimal vDotw; - decimal distSquare = DECIMAL_LARGEST; - decimal prevDistSquare; - - assert(narrowPhaseInfo->collisionShape1->isConvex()); - assert(narrowPhaseInfo->collisionShape2->isConvex()); - - const ConvexShape* shape1 = static_cast(narrowPhaseInfo->collisionShape1); - const ConvexShape* shape2 = static_cast(narrowPhaseInfo->collisionShape2); - - bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); - - void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1; - void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2; - - // Transform a point from local space of body 2 to local space - // of body 1 (the GJK algorithm is done in local space of body 1) - Transform body2ToBody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform; - - // Matrix that transform a direction from local space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getMatrix().getTranspose() * - narrowPhaseInfo->shape1ToWorldTransform.getOrientation().getMatrix(); - - do { - // Compute the support points for the enlarged object A and B - suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); - suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); - - // Compute the support point for the Minkowski difference A-B - w = suppA - suppB; - - vDotw = v.dot(w); - - // If the enlarge objects do not intersect - if (vDotw > decimal(0.0)) { - - // No intersection, we return - return false; - } - - // Add the new support point to the simplex - simplex.addPoint(w, suppA, suppB); - - if (simplex.isAffinelyDependent()) { - return false; - } - - if (!simplex.computeClosestPoint(v)) { - return false; - } - - // Store and update the square distance - prevDistSquare = distSquare; - distSquare = v.lengthSquare(); - - if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { - return false; - } - - } while((!simplex.isFull() && isPolytopeShape) || (!isPolytopeShape && !simplex.isFull() && - distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint())); - - // Give the simplex computed with GJK algorithm to the EPA algorithm - // which will compute the correct penetration depth and contact points - // between the two enlarged objects - return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, narrowPhaseInfo, v, contactPointInfo); -} // Use the GJK Algorithm to find if a point is inside a convex collision shape bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) { @@ -315,8 +225,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - // Support point of object B (object B is a single point) const Vector3 suppB(localPoint); @@ -332,7 +240,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS do { // Compute the support points for original objects (without margins) A and B - suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); + suppA = shape->getLocalSupportPointWithoutMargin(-v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -378,8 +286,6 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin) Vector3 suppB; // Support point on the collision shape const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; @@ -399,7 +305,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); decimal lambda = decimal(0.0); suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) - suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); + suppB = shape->getLocalSupportPointWithoutMargin(rayDirection); Vector3 v = suppA - suppB; decimal vDotW, vDotR; decimal distSquare = v.lengthSquare(); @@ -409,7 +315,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { // Compute the support points - suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); + suppB = shape->getLocalSupportPointWithoutMargin(v); w = suppA - suppB; vDotW = v.dot(w); diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index e0dad891..a9daa897 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -27,11 +27,10 @@ #define REACTPHYSICS3D_GJK_ALGORITHM_H // Libraries -#include "collision/narrowphase/NarrowPhaseAlgorithm.h" -#include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" +#include "collision/NarrowPhaseInfo.h" #include "collision/shapes/ConvexShape.h" -#include "collision/narrowphase/EPA/EPAAlgorithm.h" - +#include "VoronoiSimplex.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -57,24 +56,29 @@ constexpr int MAX_ITERATIONS_GJK_RAYCAST = 32; * Polytope Algorithm) to compute the correct penetration depth between the * enlarged objects. */ -class GJKAlgorithm : public NarrowPhaseAlgorithm { +class GJKAlgorithm { private : // -------------------- Attributes -------------------- // - /// EPA Algorithm - EPAAlgorithm mAlgoEPA; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif // -------------------- Methods -------------------- // - /// Compute the penetration depth for enlarged objects. - bool computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo, - Vector3& v); - public : + enum class GJKResult { + SEPARATED, // The two shapes are separated outside the margin + COLLIDE_IN_MARGIN, // The two shapes overlap only in the margin (shallow penetration) + INTERPENETRATE // The two shapes overlap event without the margin (deep penetration) + }; + // -------------------- Methods -------------------- // /// Constructor @@ -90,16 +94,32 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete; /// Compute a contact info if the two bounding volumes collide. - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; + GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts); /// Use the GJK Algorithm to find if a point is inside a convex collision shape bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape); /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void GJKAlgorithm::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index f7f68907..3088d874 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include "body/Body.h" -#include "constraint/ContactPoint.h" +#include "collision/ContactManifoldInfo.h" #include "memory/PoolAllocator.h" #include "engine/OverlappingPair.h" #include "collision/NarrowPhaseInfo.h" @@ -67,6 +67,13 @@ class NarrowPhaseAlgorithm { // -------------------- Attributes -------------------- // +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + public : // -------------------- Methods -------------------- // @@ -83,10 +90,28 @@ class NarrowPhaseAlgorithm { /// Deleted assignment operator NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo)=0; + /// Compute a contact info if the two bounding volumes collide + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + MemoryAllocator& memoryAllocator)=0; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void NarrowPhaseAlgorithm::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp new file mode 100644 index 00000000..66be7d06 --- /dev/null +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -0,0 +1,1050 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "SATAlgorithm.h" +#include "constraint/ContactPoint.h" +#include "collision/PolyhedronMesh.h" +#include "collision/shapes/CapsuleShape.h" +#include "collision/shapes/SphereShape.h" +#include "engine/OverlappingPair.h" +#include "collision/shapes/TriangleShape.h" +#include "configuration.h" +#include "engine/Profiler.h" +#include +#include +#include +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Static variables initialization +const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001); + +// Constructor +SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) { + +} + +// Test collision between a sphere and a convex mesh +bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { + + PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()", mProfiler); + + bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; + + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + + // Get the capsule collision shapes + const SphereShape* sphere = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + + // Get the transform from sphere local-space to polyhedron local-space + const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse(); + const Transform sphereToPolyhedronSpaceTransform = worldToPolyhedronTransform * sphereToWorldTransform; + + // Transform the center of the sphere into the local-space of the convex polyhedron + const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition(); + + // Minimum penetration depth + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + // Compute the penetration depth of the shapes along the face normal direction + decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + } + } + + if (reportContacts) { + + const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); + Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal; + Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius()); + Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + + Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld; + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, + isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, + isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal); + } + + return true; +} + +// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction +decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron, + const SphereShape* sphere, const Vector3& sphereCenter) const { + + PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler); + + // Get the face + const HalfEdgeStructure::Face& face = polyhedron->getFace(faceIndex); + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex); + + Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter; + decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius(); + + return penetrationDepth; +} + +// Test collision between a capsule and a convex mesh +bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { + + PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler); + + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; + + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + // Get the collision shapes + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + + const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA; + + // Minimum penetration depth + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + uint minEdgeIndex = 0; + bool isMinPenetrationFaceNormal = false; + Vector3 separatingAxisCapsuleSpace; + Vector3 separatingPolyhedronEdgeVertex1; + Vector3 separatingPolyhedronEdgeVertex2; + + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + Vector3 outFaceNormalCapsuleSpace; + + // Compute the penetration depth + const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape, + polyhedronToCapsuleTransform, + outFaceNormalCapsuleSpace); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace; + } + } + + // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron + for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { + + // Get an edge from the polyhedron (convert it into the capsule local-space) + const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(e); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + + const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); + const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); + + // Check using the Gauss Map if this edge cross product can be as separating axis + if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) { + + Vector3 outAxis; + + // Compute the penetration depth + const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape, + capsuleSegmentAxis, edgeVertex1, + edgeDirectionCapsuleSpace, + polyhedronToCapsuleTransform, + outAxis); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minEdgeIndex = e; + isMinPenetrationFaceNormal = false; + separatingAxisCapsuleSpace = outAxis; + separatingPolyhedronEdgeVertex1 = edgeVertex1; + separatingPolyhedronEdgeVertex2 = edgeVertex2; + } + } + } + + // Convert the inner capsule segment points into the polyhedron local-space + const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); + const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; + const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + + Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; + if (isCapsuleShape1) { + normalWorld = -normalWorld; + } + const decimal capsuleRadius = capsuleShape->getRadius(); + + // If the separating axis is a face normal + // We need to clip the inner capsule segment with the adjacent faces of the separating face + if (isMinPenetrationFaceNormal) { + + if (reportContacts) { + + return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, + polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + narrowPhaseInfo, isCapsuleShape1); + } + } + else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment + + if (reportContacts) { + + // Compute the closest points between the inner capsule segment and the + // edge of the polyhedron in polyhedron local-space + Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment; + computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2, + closestPointCapsuleInnerSegment, closestPointPolyhedronEdge); + + // Project closest capsule inner segment point into the capsule bounds + Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, + isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); + + // Create the contact point + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, + isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); + } + } + + return true; +} + +// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment +decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule, + const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1, + const Vector3& edgeDirectionCapsuleSpace, + const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const { + + PROFILE("SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth)", mProfiler); + + decimal penetrationDepth = DECIMAL_LARGEST; + + // Compute the axis to test (cross product between capsule inner segment and polyhedron edge) + outAxis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace); + + // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment + if (outAxis.lengthSquare() >= decimal(0.00001)) { + + const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid(); + const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1; + + // Swap axis direction if necessary such that it points out of the polyhedron + if (outAxis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) { + outAxis = -outAxis; + } + + outAxis.normalize(); + + // Compute the penetration depth + const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis); + const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint; + penetrationDepth = capsuleSupportPointToEdgePoint.dot(outAxis); + } + + return penetrationDepth; +} + +// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction +decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron, + const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform, + Vector3& outFaceNormalCapsuleSpace) const { + + PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler); + + // Get the face + const HalfEdgeStructure::Face& face = polyhedron->getFace(polyhedronFaceIndex); + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex); + + // Compute the penetration depth (using the capsule support in the direction opposite to the face normal) + outFaceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace); + const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); + const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; + const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(outFaceNormalCapsuleSpace); + + return penetrationDepth; +} + +// Compute the two contact points between a polyhedron and a capsule when the separating +// axis is a face normal of the polyhedron +bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, + decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, + Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, + NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { + + PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler); + + const HalfEdgeStructure::Face& face = polyhedron->getFace(referenceFaceIndex); + + // Get the face normal + Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); + + uint firstEdgeIndex = face.edgeIndex; + uint edgeIndex = firstEdgeIndex; + + List planesPoints(mMemoryAllocator, 2); + List planesNormals(mMemoryAllocator, 2); + + // For each adjacent edge of the separating face of the polyhedron + do { + + const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(edgeIndex); + const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + + // Compute the edge vertices and edge direction + Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex); + Vector3 edgeV2 = polyhedron->getVertexPosition(twinEdge.vertexIndex); + Vector3 edgeDirection = edgeV2 - edgeV1; + + // Compute the normal of the clipping plane for this edge + // The clipping plane is perpendicular to the edge direction and the reference face normal + Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection); + + // Construct a clipping plane for each adjacent edge of the separating face of the polyhedron + planesPoints.add(polyhedron->getVertexPosition(edge.vertexIndex)); + planesNormals.add(clipPlaneNormal); + + edgeIndex = edge.nextEdgeIndex; + + } while(edgeIndex != firstEdgeIndex); + + // First we clip the inner segment of the capsule with the four planes of the adjacent faces + List clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator); + + // Project the two clipped points into the polyhedron face + const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); + + bool contactFound = false; + + // For each of the two clipped points + for (uint i = 0; i penetrationDepth - capsuleRadius - decimal(0.001)) { + + contactFound = true; + + Vector3 contactPointPolyhedron = clipSegment[i] + delta; + + // Project the clipped point into the capsule bounds + Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius; + + + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, + isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + penetrationDepth, normalWorld); + + + // Create the contact point + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, + isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron, + isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); + } + } + + return contactFound; +} + +// This method returns true if an edge of a polyhedron and a capsule forms a +// face of the Minkowski Difference. This test is used to know if two edges +// (one edge of the polyhedron vs the inner segment of the capsule in this case) +// have to be test as a possible separating axis +bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, + const Vector3& edgeAdjacentFace2Normal) const { + + // Return true if the arc on the Gauss Map corresponding to the polyhedron edge + // intersect the unit circle plane corresponding to capsule Gauss Map + return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0); +} + +// Test collision between two convex polyhedrons +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const { + + PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler); + + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + + const ConvexPolyhedronShape* polyhedron1 = static_cast(narrowPhaseInfo->collisionShape1); + const ConvexPolyhedronShape* polyhedron2 = static_cast(narrowPhaseInfo->collisionShape2); + + const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform; + const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse(); + + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + bool isMinPenetrationFaceNormal = false; + bool isMinPenetrationFaceNormalPolyhedron1 = false; + uint minSeparatingEdge1Index, minSeparatingEdge2Index; + Vector3 separatingEdge1A, separatingEdge1B; + Vector3 separatingEdge2A, separatingEdge2B; + Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // True if the shapes were overlapping in the previous frame and are + // still overlapping on the same axis in this frame + bool isTemporalCoherenceValid = false; + + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) { + + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. + + // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 1 + if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) { + + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, + lastFrameCollisionInfo->satMinAxisFaceIndex); + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = true; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + + return true; + } + else { // Contact points have not been found (the set of clipped points was empty) + + // Therefore, we need to run the whole SAT algorithm again + isTemporalCoherenceValid = false; + } + } + } + else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 2 + + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, + lastFrameCollisionInfo->satMinAxisFaceIndex); + + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = false; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + + return true; + } + else { // Contact points have not been found (the set of clipped points was empty) + + // Therefore, we need to run the whole SAT algorithm again + isTemporalCoherenceValid = false; + } + } + } + else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges + + const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index); + const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); + + Vector3 separatingAxisPolyhedron2Space; + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding; + + // Temporal coherence is valid only if the two edges build a minkowski + // face (and the cross product is therefore a candidate for separating axis + if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + isTemporalCoherenceValid = false; + } + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormal = false; + isMinPenetrationFaceNormalPolyhedron1 = false; + minSeparatingEdge1Index = lastFrameCollisionInfo->satMinEdge1Index; + minSeparatingEdge2Index = lastFrameCollisionInfo->satMinEdge2Index; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + } + } + + // We the shapes are still overlapping in the same axis as in + // the previous frame, we skip the whole SAT algorithm + if (!isTemporalCoherenceValid) { + + // Test all the face normals of the polyhedron 1 for separating axis + uint faceIndex; + decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = true; + } + + // Test all the face normals of the polyhedron 2 for separating axis + penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true; + lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = false; + } + + // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis + for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { + + // Get an edge of polyhedron 1 + const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i); + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + + for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { + + // Get an edge of polyhedron 2 + const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j); + + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // If the two edges build a minkowski face (and the cross product is + // therefore a candidate for separating axis + if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + + Vector3 separatingAxisPolyhedron2Space; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + if (penetrationDepth <= decimal(0.0)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinEdge1Index = i; + lastFrameCollisionInfo->satMinEdge2Index = j; + + // We have found a separating axis + return false; + } + + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormalPolyhedron1 = false; + isMinPenetrationFaceNormal = false; + minSeparatingEdge1Index = i; + minSeparatingEdge2Index = j; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + + } + } + } + } + + // Here we know the shapes are overlapping on a given minimum separating axis. + // Now, we will clip the shapes along this axis to find the contact points + + assert(minPenetrationDepth > decimal(0.0)); + assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal); + + // If the minimum separating axis is a face normal + if (isMinPenetrationFaceNormal) { + + if (reportContacts) { + + // Compute the contact points between two faces of two convex polyhedra. + bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, + polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, + minFaceIndex, narrowPhaseInfo, minPenetrationDepth); + + // There should be clipping points here. If it is not the case, it might be + // because of a numerical issue + if (!contactsFound) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + + // Return no collision + return false; + } + } + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + } + else { // If we have an edge vs edge contact + + if (reportContacts) { + + // Compute the closest points between the two edges (in the local-space of poylhedron 2) + Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge; + computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B, + closestPointPolyhedron1Edge, closestPointPolyhedron2Edge); + + // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1 + Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; + + // Compute the world normal + Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + minPenetrationDepth, normalWorld); + + // Create the contact point + narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + } + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index; + lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index; + } + + return true; +} + +// Compute the contact points between two faces of two convex polyhedra. +/// The method returns true if contact points have been found +bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, + const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1, + uint minFaceIndex, NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const { + + PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler); + + const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; + const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1; + const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; + const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + + assert(minPenetrationDepth > decimal(0.0)); + + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); + const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; + + // Compute the world normal + Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace : + -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace); + + // Get the reference face + const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex); + + // Find the incident face on the other polyhedron (most anti-parallel face) + uint incidentFaceIndex = incidentPolyhedron->findMostAntiParallelFace(axisIncidentSpace); + + // Get the incident face + const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); + + uint nbIncidentFaceVertices = static_cast(incidentFace.faceVertices.size()); + List polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face + List planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes + List planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes + + // Get all the vertices of the incident face (in the reference local-space) + for (uint i=0; i < incidentFace.faceVertices.size(); i++) { + const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]); + polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace); + } + + // Get the reference face clipping planes + uint currentEdgeIndex = referenceFace.edgeIndex; + uint firstEdgeIndex = currentEdgeIndex; + do { + + // Get the adjacent edge + const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); + + // Get the twin edge + const HalfEdgeStructure::Edge& twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + + // Compute the edge vertices and edge direction + Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex); + Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex); + Vector3 edgeDirection = edgeV2 - edgeV1; + + // Compute the normal of the clipping plane for this edge + // The clipping plane is perpendicular to the edge direction and the reference face normal + Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); + + planesNormals.add(clipPlaneNormal); + planesPoints.add(edgeV1); + + // Go to the next adjacent edge of the reference face + currentEdgeIndex = edge.nextEdgeIndex; + + } while (currentEdgeIndex != firstEdgeIndex); + + assert(planesNormals.size() > 0); + assert(planesNormals.size() == planesPoints.size()); + + // Clip the reference faces with the adjacent planes of the reference face + List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator); + + // We only keep the clipped points that are below the reference face + const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); + bool contactPointsFound = false; + for (uint i=0; i decimal(0.0)) { + + contactPointsFound = true; + + Vector3 outWorldNormal = normalWorld; + + // Convert the clip incident polyhedron vertex into the incident polyhedron local-space + Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clipPolygonVertices[i]; + + // Project the contact point onto the reference face + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); + + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle + TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, + narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, + penetrationDepth, outWorldNormal); + + // Create a new contact point + narrowPhaseInfo->addContactPoint(outWorldNormal, penetrationDepth, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, + isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); + } + } + + return contactPointsFound; +} + + +// Compute and return the distance between the two edges in the direction of the candidate separating axis +decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, + const Vector3& edge1Direction, const Vector3& edge2Direction, + Vector3& outSeparatingAxisPolyhedron2Space) const { + + PROFILE("SATAlgorithm::computeDistanceBetweenEdges", mProfiler); + + // If the two edges are parallel + if (areParallelVectors(edge1Direction, edge2Direction)) { + + // Return a large penetration depth to skip those edges + return DECIMAL_LARGEST; + } + + // Compute the candidate separating axis (cross product between two polyhedrons edges) + Vector3 axis = edge1Direction.cross(edge2Direction).getUnit(); + + // Make sure the axis direction is going from first to second polyhedron + if (axis.dot(edge2A - polyhedron2Centroid) > decimal(0.0)) { + axis = -axis; + } + + outSeparatingAxisPolyhedron2Space = axis; + + // Compute and return the distance between the edges + return -axis.dot(edge2A - edge1A); +} + +// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron +decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint faceIndex) const { + + PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler); + + const HalfEdgeStructure::Face& face = polyhedron1->getFace(faceIndex); + + // Get the face normal + const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex); + + // Convert the face normal into the local-space of polyhedron 2 + const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; + + // Get the support point of polyhedron 2 in the inverse direction of face normal + const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space); + + // Compute the penetration depth + const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); + decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space); + + return penetrationDepth; +} + +// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case +decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint& minFaceIndex) const { + + PROFILE("SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron", mProfiler); + + decimal minPenetrationDepth = DECIMAL_LARGEST; + + // For each face of the first polyhedron + for (uint f = 0; f < polyhedron1->getNbFaces(); f++) { + + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, f); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + minFaceIndex = f; + return penetrationDepth; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + } + } + + return minPenetrationDepth; +} + + +// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) +bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1, + const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, + const Transform& polyhedron1ToPolyhedron2) const { + + PROFILE("SATAlgorithm::testEdgesBuildMinkowskiFace", mProfiler); + + const Vector3 a = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(edge1.faceIndex); + const Vector3 b = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex); + + const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex); + const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex); + + // Compute b.cross(a) using the edge direction + const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex); + const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex1 - edge1Vertex2); + + // Compute d.cross(c) using the edge direction + const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex); + const Vector3 dCrossC = edge2Vertex1 - edge2Vertex2; + + // Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face) + // Note that we negate the normals of the second polyhedron because we are looking at the + // Gauss map of the minkowski difference of the polyhedrons + return testGaussMapArcsIntersect(a, b, -c, -d, bCrossA, dCrossC); +} + + +// Return true if the arcs AB and CD on the Gauss Map (unit sphere) intersect +/// This is used to know if the edge between faces with normal A and B on first polyhedron +/// and edge between faces with normal C and D on second polygon create a face on the Minkowski +/// sum of both polygons. If this is the case, it means that the cross product of both edges +/// might be a separating axis. +bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, + const Vector3& bCrossA, const Vector3& dCrossC) const { + + PROFILE("SATAlgorithm::testGaussMapArcsIntersect", mProfiler); + + const decimal cba = c.dot(bCrossA); + const decimal dba = d.dot(bCrossA); + const decimal adc = a.dot(dCrossC); + const decimal bdc = b.dot(dCrossC); + + return cba * dba < decimal(0.0) && adc * bdc < decimal(0.0) && cba * bdc > decimal(0.0); +} diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h new file mode 100644 index 00000000..0402067f --- /dev/null +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -0,0 +1,170 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SAT_ALGORITHM_H +#define REACTPHYSICS3D_SAT_ALGORITHM_H + +// Libraries +#include "collision/ContactManifoldInfo.h" +#include "collision/NarrowPhaseInfo.h" +#include "collision/shapes/ConvexPolyhedronShape.h" + + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +class CapsuleShape; +class SphereShape; + +// Class SATAlgorithm +class SATAlgorithm { + + private : + + // -------------------- Attributes -------------------- // + + /// Bias used to make sure the SAT algorithm returns the same penetration axis between frames + /// when there are multiple separating axis with the same penetration depth. The goal is to + /// make sure the contact manifold does not change too much between frames. + static const decimal SAME_SEPARATING_AXIS_BIAS; + + /// Memory allocator + MemoryAllocator& mMemoryAllocator; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + + // -------------------- Methods -------------------- // + + /// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis) + bool testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1, + const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2, + const Transform& polyhedron1ToPolyhedron2) const; + + /// Return true if the arcs AB and CD on the Gauss Map intersect + bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, + const Vector3& c, const Vector3& d, + const Vector3& bCrossA, const Vector3& dCrossC) const; + + /// Compute and return the distance between the two edges in the direction of the candidate separating axis + decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, + const Vector3& edge1Direction, const Vector3& edge2Direction, + Vector3& outSeparatingAxis) const; + + /// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron + decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint faceIndex) const; + + + /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case + decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; + + /// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction + decimal computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron, + const SphereShape* sphere, const Vector3& sphereCenter) const; + + /// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction + decimal computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron, + const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform, + Vector3& outFaceNormalCapsuleSpace) const; + + /// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment + decimal computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule, + const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1, + const Vector3& edgeDirectionCapsuleSpace, + const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const; + + /// Compute the contact points between two faces of two convex polyhedra. + bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2, + const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex, + NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const; + + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SATAlgorithm(MemoryAllocator& memoryAllocator); + + /// Destructor + ~SATAlgorithm() = default; + + /// Deleted copy-constructor + SATAlgorithm(const SATAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete; + + /// Test collision between a sphere and a convex mesh + bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; + + /// Test collision between a capsule and a convex mesh + bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; + + /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron + bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, + decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, + Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, + NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const; + + // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference + bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, + const Vector3& edgeAdjacentFace2Normal) const; + + /// Test collision between two convex meshes + bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + +}; + +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void SATAlgorithm::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + +} + +#endif diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp new file mode 100755 index 00000000..2e86dfe3 --- /dev/null +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -0,0 +1,134 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "SphereVsCapsuleAlgorithm.h" +#include "collision/shapes/SphereShape.h" +#include "collision/shapes/CapsuleShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Compute the narrow-phase collision detection between a sphere and a capsule +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. +bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + MemoryAllocator& memoryAllocator) { + + bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; + + assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); + + // Get the collision shapes + const SphereShape* sphereShape = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const CapsuleShape* capsuleShape = static_cast(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + // Get the transform from sphere local-space to capsule local-space + const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse(); + const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform; + + // Transform the center of the sphere into the local-space of the capsule shape + const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition(); + + // Compute the end-points of the inner segment of the capsule + const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5); + const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0); + const Vector3 capsuleSegB(0, capsuleHalfHeight, 0); + + // Compute the point on the inner capsule segment that is the closes to center of sphere + const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter); + + // Compute the distance between the sphere center and the closest point on the segment + Vector3 sphereCenterToSegment = (closestPointOnSegment - sphereCenter); + const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare(); + + // Compute the sum of the radius of the sphere and the capsule (virtual sphere) + decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius(); + + // If the collision shapes overlap + if (sphereSegmentDistanceSquare < sumRadius * sumRadius) { + + decimal penetrationDepth; + Vector3 normalWorld; + Vector3 contactPointSphereLocal; + Vector3 contactPointCapsuleLocal; + + if (reportContacts) { + + // If the sphere center is not on the capsule inner segment + if (sphereSegmentDistanceSquare > MACHINE_EPSILON) { + + decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); + sphereCenterToSegment /= sphereSegmentDistance; + + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); + contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); + + normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; + + penetrationDepth = sumRadius - sphereSegmentDistance; + + if (!isSphereShape1) { + normalWorld = -normalWorld; + } + } + else { // If the sphere center is on the capsule inner segment (degenerate case) + + // We take any direction that is orthogonal to the inner capsule segment as a contact normal + + // Capsule inner segment + Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit(); + + Vector3 vec1(1, 0, 0); + Vector3 vec2(0, 1, 0); + + // Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product) + decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2)) + decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2)) + + penetrationDepth = sumRadius; + + // We choose as a contact normal, any direction that is perpendicular to the inner capsule segment + Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2); + normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace; + + // Compute the two local contact points + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius()); + contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius(); + } + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, + isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, + isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); + } + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h new file mode 100644 index 00000000..c7c4b39e --- /dev/null +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -0,0 +1,70 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H +#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class SphereVsCapsuleAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between a sphere collision shape and a capsule collision shape. + */ +class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SphereVsCapsuleAlgorithm() = default; + + /// Destructor + virtual ~SphereVsCapsuleAlgorithm() override = default; + + /// Deleted copy-constructor + SphereVsCapsuleAlgorithm(const SphereVsCapsuleAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; + + /// Compute the narrow-phase collision detection between a sphere and a capsule + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp new file mode 100644 index 00000000..8d4f562c --- /dev/null +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -0,0 +1,90 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "SphereVsConvexPolyhedronAlgorithm.h" +#include "GJK/GJKAlgorithm.h" +#include "SAT/SATAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Compute the narrow-phase collision detection between a sphere and a convex polyhedron +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. +bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + MemoryAllocator& memoryAllocator) { + + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + + // First, we run the GJK algorithm + GJKAlgorithm gjkAlgorithm; + +#ifdef IS_PROFILING_ACTIVE + + gjkAlgorithm.setProfiler(mProfiler); + +#endif + + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); + + lastFrameCollisionInfo->wasUsingGJK = true; + lastFrameCollisionInfo->wasUsingSAT = false; + + // If we have found a contact point inside the margins (shallow penetration) + if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + + // Return true + return true; + } + + // If we have overlap even without the margins (deep penetration) + if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { + + // Run the SAT algorithm to find the separating axis and compute contact point + SATAlgorithm satAlgorithm(memoryAllocator); + +#ifdef IS_PROFILING_ACTIVE + + satAlgorithm.setProfiler(mProfiler); + +#endif + + bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts); + + lastFrameCollisionInfo->wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; + + return isColliding; + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h new file mode 100644 index 00000000..1929f43c --- /dev/null +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -0,0 +1,70 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H +#define REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class SphereVsConvexPolyhedronAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between a sphere and a convex polyhedron. + */ +class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SphereVsConvexPolyhedronAlgorithm() = default; + + /// Destructor + virtual ~SphereVsConvexPolyhedronAlgorithm() override = default; + + /// Deleted copy-constructor + SphereVsConvexPolyhedronAlgorithm(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp old mode 100644 new mode 100755 index 6b28372d..8103380e --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -1,69 +1,90 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "SphereVsSphereAlgorithm.h" -#include "collision/shapes/SphereShape.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { - - // Get the sphere collision shapes - const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); - const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); - - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; - const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; - - // Compute the distance between the centers - Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); - decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); - - // Compute the sum of the radius - decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); - - // If the sphere collision shapes intersect - if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { - Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); - Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - Vector3 intersectionOnBody1 = sphereShape1->getRadius() * - centerSphere2InBody1LocalSpace.getUnit(); - Vector3 intersectionOnBody2 = sphereShape2->getRadius() * - centerSphere1InBody2LocalSpace.getUnit(); - decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); - - // Create the contact info object - contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); - - return true; - } - - return false; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "SphereVsSphereAlgorithm.h" +#include "collision/shapes/SphereShape.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, + MemoryAllocator& memoryAllocator) { + + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE); + assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + + // Get the sphere collision shapes + const SphereShape* sphereShape1 = static_cast(narrowPhaseInfo->collisionShape1); + const SphereShape* sphereShape2 = static_cast(narrowPhaseInfo->collisionShape2); + + // Get the local-space to world-space transforms + const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; + const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; + + // Compute the distance between the centers + Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); + decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare(); + + // Compute the sum of the radius + decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); + + // If the sphere collision shapes intersect + if (squaredDistanceBetweenCenters < sumRadius * sumRadius) { + + if (reportContacts) { + + Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); + Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); + decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + Vector3 intersectionOnBody1; + Vector3 intersectionOnBody2; + Vector3 normal; + + // If the two sphere centers are not at the same position + if (squaredDistanceBetweenCenters > MACHINE_EPSILON) { + + intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit(); + intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit(); + normal = vectorBetweenCenters.getUnit(); + } + else { // If the sphere centers are at the same position (degenerate case) + + // Take any contact normal direction + normal.setAllValues(0, 1, 0); + + intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal); + intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal); + } + + // Create the contact info object + narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); + } + + return true; + } + + return false; +} diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index 24886f7f..142053c4 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -1,71 +1,70 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H -#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H - -// Libraries -#include "body/Body.h" -#include "constraint/ContactPoint.h" -#include "NarrowPhaseAlgorithm.h" - - -/// Namespace ReactPhysics3D -namespace reactphysics3d { - -// Class SphereVsSphereAlgorithm -/** - * This class is used to compute the narrow-phase collision detection - * between two sphere collision shapes. - */ -class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { - - protected : - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - SphereVsSphereAlgorithm() = default; - - /// Destructor - virtual ~SphereVsSphereAlgorithm() override = default; - - /// Deleted copy-constructor - SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; - - /// Deleted assignment operator - SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; - - /// Compute a contact info if the two bounding volume collide - virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) override; -}; - -} - -#endif - +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H +#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class SphereVsSphereAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two sphere collision shapes. + */ +class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SphereVsSphereAlgorithm() = default; + + /// Destructor + virtual ~SphereVsSphereAlgorithm() override = default; + + /// Deleted copy-constructor + SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; + + /// Compute a contact info if the two bounding volume collide + virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override; +}; + +} + +#endif + diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index ce3197e3..c13e32f4 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -27,6 +27,7 @@ #include "BoxShape.h" #include "collision/ProxyShape.h" #include "configuration.h" +#include "memory/MemoryManager.h" #include #include @@ -35,13 +36,49 @@ using namespace reactphysics3d; // Constructor /** * @param extent The vector with the three extents of the box (in meters) - * @param margin The collision margin (in meters) around the collision shape */ -BoxShape::BoxShape(const Vector3& extent, decimal margin) - : ConvexShape(CollisionShapeType::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { - assert(extent.x > decimal(0.0) && extent.x > margin); - assert(extent.y > decimal(0.0) && extent.y > margin); - assert(extent.z > decimal(0.0) && extent.z > margin); +BoxShape::BoxShape(const Vector3& extent) + : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent), + mHalfEdgeStructure(MemoryManager::getBaseAllocator(), 6, 8, 24) { + + assert(extent.x > decimal(0.0)); + assert(extent.y > decimal(0.0)); + assert(extent.z > decimal(0.0)); + + // Vertices + mHalfEdgeStructure.addVertex(0); + mHalfEdgeStructure.addVertex(1); + mHalfEdgeStructure.addVertex(2); + mHalfEdgeStructure.addVertex(3); + mHalfEdgeStructure.addVertex(4); + mHalfEdgeStructure.addVertex(5); + mHalfEdgeStructure.addVertex(6); + mHalfEdgeStructure.addVertex(7); + + MemoryAllocator& allocator = MemoryManager::getBaseAllocator(); + + // Faces + List face0(allocator, 4); + face0.add(0); face0.add(1); face0.add(2); face0.add(3); + List face1(allocator, 4); + face1.add(1); face1.add(5); face1.add(6); face1.add(2); + List face2(allocator, 4); + face2.add(4); face2.add(7); face2.add(6); face2.add(5); + List face3(allocator, 4); + face3.add(4); face3.add(0); face3.add(3); face3.add(7); + List face4(allocator, 4); + face4.add(4); face4.add(5); face4.add(1); face4.add(0); + List face5(allocator, 4); + face5.add(2); face5.add(6); face5.add(7); face5.add(3); + + mHalfEdgeStructure.addFace(face0); + mHalfEdgeStructure.addFace(face1); + mHalfEdgeStructure.addFace(face2); + mHalfEdgeStructure.addFace(face3); + mHalfEdgeStructure.addFace(face4); + mHalfEdgeStructure.addFace(face5); + + mHalfEdgeStructure.init(); } // Return the local inertia tensor of the collision shape @@ -52,17 +89,16 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) */ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { decimal factor = (decimal(1.0) / decimal(3.0)) * mass; - Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); - decimal xSquare = realExtent.x * realExtent.x; - decimal ySquare = realExtent.y * realExtent.y; - decimal zSquare = realExtent.z * realExtent.z; + decimal xSquare = mExtent.x * mExtent.x; + decimal ySquare = mExtent.y * mExtent.y; + decimal zSquare = mExtent.z * mExtent.z; tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + zSquare), 0.0, 0.0, 0.0, factor * (xSquare + ySquare)); } // Raycast method with feedback information -bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { Vector3 rayDirection = ray.point2 - ray.point1; decimal tMin = DECIMAL_SMALLEST; diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 776356ba..2ebcdce1 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -28,10 +28,10 @@ // Libraries #include -#include "ConvexShape.h" +#include "ConvexPolyhedronShape.h" #include "body/CollisionBody.h" #include "mathematics/mathematics.h" - +#include "memory/DefaultAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -41,16 +41,9 @@ namespace reactphysics3d { * This class represents a 3D box shape. Those axis are unit length. * The three extents are half-widths of the box along the three * axis x, y, z local axis. The "transform" of the corresponding - * rigid body will give an orientation and a position to the box. This - * collision shape uses an extra margin distance around it for collision - * detection purpose. The default margin is 4cm (if your units are meters, - * which is recommended). In case, you want to simulate small objects - * (smaller than the margin distance), you might want to reduce the margin by - * specifying your own margin distance using the "margin" parameter in the - * constructor of the box shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the constructor. + * body will give an orientation and a position to the box. */ -class BoxShape : public ConvexShape { +class BoxShape : public ConvexPolyhedronShape { protected : @@ -59,17 +52,19 @@ class BoxShape : public ConvexShape { /// Extent sizes of the box in the x, y and z direction Vector3 mExtent; + /// Half-edge structure of the polyhedron + HalfEdgeStructure mHalfEdgeStructure; + // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -79,7 +74,7 @@ class BoxShape : public ConvexShape { // -------------------- Methods -------------------- // /// Constructor - BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN); + BoxShape(const Vector3& extent); /// Destructor virtual ~BoxShape() override = default; @@ -99,11 +94,35 @@ class BoxShape : public ConvexShape { /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; - /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; + + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; + + /// Return a given face of the polyhedron + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; }; // Return the extents of the box @@ -111,7 +130,7 @@ class BoxShape : public ConvexShape { * @return The vector with the three extents of the box shape (in meters) */ inline Vector3 BoxShape::getExtent() const { - return mExtent + Vector3(mMargin, mMargin, mMargin); + return mExtent; } // Set the scaling vector of the collision shape @@ -131,29 +150,23 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) { inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { // Maximum bounds - max = mExtent + Vector3(mMargin, mMargin, mMargin); + max = mExtent; // Minimum bounds min = -max; } -// Return true if the collision shape is a polyhedron -inline bool BoxShape::isPolyhedron() const { - return true; -} - // Return the number of bytes used by the collision shape inline size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); } -// Return a local support point in a given direction without the objec margin -inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +// Return a local support point in a given direction without the object margin +inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { - return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, - direction.y < 0.0 ? -mExtent.y : mExtent.y, - direction.z < 0.0 ? -mExtent.z : mExtent.z); + return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x, + direction.y < decimal(0.0) ? -mExtent.y : mExtent.y, + direction.z < decimal(0.0) ? -mExtent.z : mExtent.z); } // Return true if a point is inside the collision shape @@ -163,6 +176,78 @@ inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* pro localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]); } +// Return the number of faces of the polyhedron +inline uint BoxShape::getNbFaces() const { + return 6; +} + +// Return a given face of the polyhedron +inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const { + assert(faceIndex < mHalfEdgeStructure.getNbFaces()); + return mHalfEdgeStructure.getFace(faceIndex); +} + +// Return the number of vertices of the polyhedron +inline uint BoxShape::getNbVertices() const { + return 8; +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mHalfEdgeStructure.getVertex(vertexIndex); +} + +// Return the position of a given vertex +inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + + Vector3 extent = getExtent(); + + switch(vertexIndex) { + case 0: return Vector3(-extent.x, -extent.y, extent.z); + case 1: return Vector3(extent.x, -extent.y, extent.z); + case 2: return Vector3(extent.x, extent.y, extent.z); + case 3: return Vector3(-extent.x, extent.y, extent.z); + case 4: return Vector3(-extent.x, -extent.y, -extent.z); + case 5: return Vector3(extent.x, -extent.y, -extent.z); + case 6: return Vector3(extent.x, extent.y, -extent.z); + case 7: return Vector3(-extent.x, extent.y, -extent.z); + } +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + + switch(faceIndex) { + case 0: return Vector3(0, 0, 1); + case 1: return Vector3(1, 0, 0); + case 2: return Vector3(0, 0, -1); + case 3: return Vector3(-1, 0, 0); + case 4: return Vector3(0, -1, 0); + case 5: return Vector3(0, 1, 0); + } + + assert(false); +} + +// Return the centroid of the box +inline Vector3 BoxShape::getCentroid() const { + return Vector3::zero(); +} + +// Return the number of half-edges of the polyhedron +inline uint BoxShape::getNbHalfEdges() const { + return 24; +} + +// Return a given half-edge of the polyhedron +inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mHalfEdgeStructure.getHalfEdge(edgeIndex); +} + } #endif diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index f9f442ce..3f89d090 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/src/collision/shapes/CapsuleShape.cpp @@ -37,7 +37,7 @@ using namespace reactphysics3d; * @param height The height of the capsule (in meters) */ CapsuleShape::CapsuleShape(decimal radius, decimal height) - : ConvexShape(CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { + : ConvexShape(CollisionShapeName::CAPSULE, CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) { assert(radius > decimal(0.0)); assert(height > decimal(0.0)); } @@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS } // Raycast method with feedback information -bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { const Vector3 n = ray.point2 - ray.point1; diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 9c0a4545..7843f139 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -56,14 +56,13 @@ class CapsuleShape : public ConvexShape { // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Raycasting method between a ray one of the two spheres end cap of the capsule bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2, @@ -169,8 +168,7 @@ inline bool CapsuleShape::isPolyhedron() const { /// Therefore, in this method, we compute the support points of both top and bottom spheres of /// the capsule and return the point with the maximum dot product with the direction vector. Note /// that the object margin is implicitly the radius and height of the capsule. -inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // Support point top sphere decimal dotProductTop = mHalfHeight * direction.y; diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index 301c97d2..0b64190e 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -32,39 +32,57 @@ using namespace reactphysics3d; // Constructor -CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) { +CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) + : mType(type), mName(name), mScaling(1.0, 1.0, 1.0), mId(0) { } -// Compute the world-space AABB of the collision shape given a transform +// Compute the world-space AABB of the collision shape given a transform from shape +// local-space to world-space. The technique is described in the book Real-Time Collision +// Detection by Christer Ericson. /** * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape * computed in world-space coordinates - * @param transform Transform used to compute the AABB of the collision shape + * @param transform Transform from shape local-space to world-space used to compute + * the AABB of the collision shape */ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const { - PROFILE("CollisionShape::computeAABB()"); + PROFILE("CollisionShape::computeAABB()", mProfiler); // Get the local bounds in x,y and z direction Vector3 minBounds; Vector3 maxBounds; getLocalBounds(minBounds, maxBounds); - // Rotate the local bounds according to the orientation of the body - Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix(); - Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), - worldAxis.getColumn(1).dot(minBounds), - worldAxis.getColumn(2).dot(minBounds)); - Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), - worldAxis.getColumn(1).dot(maxBounds), - worldAxis.getColumn(2).dot(maxBounds)); + const Vector3 translation = transform.getPosition(); + Matrix3x3 matrix = transform.getOrientation().getMatrix(); + Vector3 resultMin; + Vector3 resultMax; - // Compute the minimum and maximum coordinates of the rotated extents - Vector3 minCoordinates = transform.getPosition() + worldMinBounds; - Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds; + // For each of the three axis + for (int i=0; i<3; i++) { + + // Add translation component + resultMin[i] = translation[i]; + resultMax[i] = translation[i]; + + for (int j=0; j<3; j++) { + decimal e = matrix[i][j] * minBounds[j]; + decimal f = matrix[i][j] * maxBounds[j]; + + if (e < f) { + resultMin[i] += e; + resultMax[i] += f; + } + else { + resultMin[i] += f; + resultMax[i] += e; + } + } + } // Update the AABB with the new minimum and maximum coordinates - aabb.setMin(minCoordinates); - aabb.setMax(maxCoordinates); + aabb.setMin(resultMin); + aabb.setMax(resultMax); } diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index b33ed4f1..8d84ab55 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -35,14 +35,17 @@ #include "AABB.h" #include "collision/RaycastInfo.h" #include "memory/PoolAllocator.h" +#include "engine/Profiler.h" /// ReactPhysics3D namespace namespace reactphysics3d { -/// Type of the collision shape -enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER, - CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD}; -const int NB_COLLISION_SHAPE_TYPES = 9; +/// Type of collision shapes +enum class CollisionShapeType {SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_SHAPE}; +const int NB_COLLISION_SHAPE_TYPES = 4; + +/// Names of collision shapes +enum class CollisionShapeName { TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, TRIANGLE_MESH, HEIGHTFIELD }; // Declarations class ProxyShape; @@ -62,8 +65,21 @@ class CollisionShape { /// Type of the collision shape CollisionShapeType mType; + /// Name of the colision shape + CollisionShapeName mName; + /// Scaling vector of the collision shape Vector3 mScaling; + + /// Unique identifier of the shape inside an overlapping pair + uint mId; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif // -------------------- Methods -------------------- // @@ -71,7 +87,7 @@ class CollisionShape { virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const=0; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const = 0; @@ -81,7 +97,7 @@ class CollisionShape { // -------------------- Methods -------------------- // /// Constructor - CollisionShape(CollisionShapeType type); + CollisionShape(CollisionShapeName name, CollisionShapeType type); /// Destructor virtual ~CollisionShape() = default; @@ -92,7 +108,10 @@ class CollisionShape { /// Deleted assignment operator CollisionShape& operator=(const CollisionShape& shape) = delete; - /// Return the type of the collision shapes + /// Return the name of the collision shape + CollisionShapeName getName() const; + + /// Return the type of the collision shape CollisionShapeType getType() const; /// Return true if the collision shape is convex, false if it is concave @@ -105,23 +124,26 @@ class CollisionShape { virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; /// Return the scaling vector of the collision shape - Vector3 getScaling() const; + Vector3 getLocalScaling() const; /// Set the local scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); + /// Return the id of the shape + uint getId() const; + /// Return the local inertia tensor of the collision shapes virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; /// Compute the world-space AABB of the collision shape given a transform virtual void computeAABB(AABB& aabb, const Transform& transform) const; - /// Return true if the collision shape type is a convex shape - static bool isConvex(CollisionShapeType shapeType); +#ifdef IS_PROFILING_ACTIVE - /// Return the maximum number of contact manifolds in an overlapping pair given two shape types - static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2); + /// Set the profiler + virtual void setProfiler(Profiler* profiler); + +#endif // -------------------- Friendship -------------------- // @@ -129,21 +151,24 @@ class CollisionShape { friend class CollisionWorld; }; +// Return the name of the collision shape +/** +* @return The name of the collision shape (box, sphere, triangle, ...) +*/ +inline CollisionShapeName CollisionShape::getName() const { + return mName; +} + // Return the type of the collision shape /** - * @return The type of the collision shape (box, sphere, cylinder, ...) + * @return The type of the collision shape (sphere, capsule, convex polyhedron, concave mesh) */ inline CollisionShapeType CollisionShape::getType() const { return mType; } -// Return true if the collision shape type is a convex shape -inline bool CollisionShape::isConvex(CollisionShapeType shapeType) { - return shapeType != CollisionShapeType::CONCAVE_MESH && shapeType != CollisionShapeType::HEIGHTFIELD; -} - // Return the scaling vector of the collision shape -inline Vector3 CollisionShape::getScaling() const { +inline Vector3 CollisionShape::getLocalScaling() const { return mScaling; } @@ -152,19 +177,21 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) { mScaling = scaling; } -// Return the maximum number of contact manifolds allowed in an overlapping -// pair wit the given two collision shape types -inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2) { - // If both shapes are convex - if (isConvex(shapeType1) && isConvex(shapeType2)) { - return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; - } // If there is at least one concave shape - else { - return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; - } +// Return the id of the shape +inline uint CollisionShape::getId() const { + return mId; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void CollisionShape::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index a0bd1094..8f215fc2 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) - : ConcaveShape(CollisionShapeType::CONCAVE_MESH) { + : ConcaveShape(CollisionShapeName::TRIANGLE_MESH) { mTriangleMesh = triangleMesh; mRaycastTestType = TriangleRaycastSide::FRONT; @@ -49,55 +49,21 @@ void ConcaveMeshShape::initBVHTree() { // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); - TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); - // For each triangle of the concave mesh for (uint triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { - void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); Vector3 trianglePoints[3]; - // For each vertex of the triangle - for (int k=0; k < 3; k++) { + // Get the triangle vertices + triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints); - // Get the index of the current vertex in the triangle - int vertexIndex = 0; - if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; - } - else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } - - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; - trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; - trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; - } - else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x; - trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y; - trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z; - } - else { - assert(false); - } - } + // Apply the scaling factor to the vertices + trianglePoints[0] *= mScaling.x; + trianglePoints[1] *= mScaling.y; + trianglePoints[2] *= mScaling.z; // Create the AABB for the triangle AABB aabb = AABB::createAABBForTriangle(trianglePoints); - aabb.inflate(mTriangleMargin, mTriangleMargin, mTriangleMargin); // Add the AABB with the index of the triangle into the dynamic AABB tree mDynamicAABBTree.addObject(aabb, subPart, triangleIndex); @@ -106,56 +72,32 @@ void ConcaveMeshShape::initBVHTree() { } // Return the three vertices coordinates (in the array outTriangleVertices) of a triangle -// given the start vertex index pointer of the triangle -void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, - Vector3* outTriangleVertices) const { +void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex, + Vector3* outTriangleVertices) const { // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); - TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); + // Get the vertices coordinates of the triangle + triangleVertexArray->getTriangleVertices(triangleIndex, outTriangleVertices); - void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); - - // For each vertex of the triangle - for (int k=0; k < 3; k++) { - - // Get the index of the current vertex in the triangle - int vertexIndex = 0; - if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex = ((uint*)vertexIndexPointer)[k]; - } - else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } - - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; - } - else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x; - outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y; - outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z; - } - else { - assert(false); - } - } + // Apply the scaling factor to the vertices + outTriangleVertices[0] *= mScaling.x; + outTriangleVertices[1] *= mScaling.y; + outTriangleVertices[2] *= mScaling.z; } +// Return the three vertex normals (in the array outVerticesNormals) of a triangle +void ConcaveMeshShape::getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const { + + // Get the triangle vertex array of the current sub-part + TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); + + // Get the vertices normals of the triangle + triangleVertexArray->getTriangleVerticesNormals(triangleIndex, outVerticesNormals); +} + + // Use a callback method on all triangles of the concave shape inside a given AABB void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { @@ -169,12 +111,19 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { - PROFILE("ConcaveMeshShape::raycast()"); + PROFILE("ConcaveMeshShape::raycast()", mProfiler); // Create the callback object that will compute ray casting against triangles - ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray); + ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray, allocator); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + raycastCallback.setProfiler(mProfiler); + +#endif // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray. // The raycastCallback object will then compute ray casting against the triangles @@ -186,6 +135,22 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh return raycastCallback.getIsHit(); } +// Compute the shape Id for a given triangle of the mesh +uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const { + + uint shapeId = 0; + + uint i=0; + while (i < subPart) { + + shapeId += mTriangleMesh->getSubpart(i)->getNbTriangles(); + + i++; + } + + return shapeId + triangleIndex; +} + // Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { @@ -208,16 +173,26 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { // Get the triangle vertices for this node from the concave mesh shape Vector3 trianglePoints[3]; - mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints); + + // Get the vertices normals of the triangle + Vector3 verticesNormals[3]; + mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Create a triangle collision shape - decimal margin = mConcaveMeshShape.getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler to the triangle shape + triangleShape.setProfiler(mProfiler); + +#endif // Ray casting test against the collision shape RaycastInfo raycastInfo; - bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); + bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { @@ -235,5 +210,6 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { smallestHitFraction = raycastInfo.hitFraction; mIsHit = true; } + } } diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 83e5103a..c8c91972 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -77,14 +77,22 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { RaycastInfo& mRaycastInfo; const Ray& mRay; bool mIsHit; + MemoryAllocator& mAllocator; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif public: // Constructor ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape, - ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray) + ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, MemoryAllocator& allocator) : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape), - mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) { + mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) { } @@ -98,12 +106,21 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { bool getIsHit() const { return mIsHit; } + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) { + mProfiler = profiler; + } + +#endif }; // Class ConcaveMeshShape /** * This class represents a static concave mesh shape. Note that collision detection - * with a concave mesh shape can be very expensive. You should use only use + * with a concave mesh shape can be very expensive. You should only use * this shape for a static mesh. */ class ConcaveMeshShape : public ConcaveShape { @@ -118,10 +135,14 @@ class ConcaveMeshShape : public ConcaveShape { /// Dynamic AABB tree to accelerate collision with the triangles DynamicAABBTree mDynamicAABBTree; + /// Array with computed vertices normals for each TriangleVertexArray of the triangle mesh (only + /// if the user did not provide its own vertices normals) + Vector3** mComputedVerticesNormals; + // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -130,9 +151,13 @@ class ConcaveMeshShape : public ConcaveShape { void initBVHTree(); /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle - /// given the start vertex index pointer of the triangle. - void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, - Vector3* outTriangleVertices) const; + void getTriangleVertices(uint subPart, uint triangleIndex, Vector3* outTriangleVertices) const; + + /// Return the three vertex normals (in the array outVerticesNormals) of a triangle + void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const; + + /// Compute the shape Id for a given triangle of the mesh + uint computeTriangleShapeId(uint subPart, uint triangleIndex) const; public: @@ -160,6 +185,13 @@ class ConcaveMeshShape : public ConcaveShape { /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const override; +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + virtual void setProfiler(Profiler* profiler) override; + +#endif + // ---------- Friendship ----------- // friend class ConvexTriangleAABBOverlapCallback; @@ -224,12 +256,28 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) // Get the triangle vertices for this node from the concave mesh shape Vector3 trianglePoints[3]; - mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints); + + // Get the vertices normals of the triangle + Vector3 verticesNormals[3]; + mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Call the callback to test narrow-phase collision with this triangle - mTriangleTestCallback.testTriangle(trianglePoints); + mTriangleTestCallback.testTriangle(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1])); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ConcaveMeshShape::setProfiler(Profiler* profiler) { + + CollisionShape::setProfiler(profiler); + + mDynamicAABBTree.setProfiler(profiler); +} + +#endif + } #endif diff --git a/src/collision/shapes/ConcaveShape.cpp b/src/collision/shapes/ConcaveShape.cpp index 3f691468..cac82e45 100644 --- a/src/collision/shapes/ConcaveShape.cpp +++ b/src/collision/shapes/ConcaveShape.cpp @@ -31,8 +31,7 @@ using namespace reactphysics3d; // Constructor -ConcaveShape::ConcaveShape(CollisionShapeType type) - : CollisionShape(type), mIsSmoothMeshCollisionEnabled(false), - mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) { +ConcaveShape::ConcaveShape(CollisionShapeName name) + : CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mRaycastTestType(TriangleRaycastSide::FRONT) { } diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index c5657b2b..eb00d8f0 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -46,7 +46,7 @@ class TriangleCallback { virtual ~TriangleCallback() = default; /// Report a triangle - virtual void testTriangle(const Vector3* trianglePoints)=0; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId)=0; }; @@ -62,12 +62,6 @@ class ConcaveShape : public CollisionShape { // -------------------- Attributes -------------------- // - /// True if the smooth mesh collision algorithm is enabled - bool mIsSmoothMeshCollisionEnabled; - - // Margin use for collision detection for each triangle - decimal mTriangleMargin; - /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; @@ -81,7 +75,7 @@ class ConcaveShape : public CollisionShape { // -------------------- Methods -------------------- // /// Constructor - ConcaveShape(CollisionShapeType type); + ConcaveShape(CollisionShapeName name); /// Destructor virtual ~ConcaveShape() override = default; @@ -92,9 +86,6 @@ class ConcaveShape : public CollisionShape { /// Deleted assignment operator ConcaveShape& operator=(const ConcaveShape& shape) = delete; - /// Return the triangle margin - decimal getTriangleMargin() const; - /// Return the raycast test type (front, back, front-back) TriangleRaycastSide getRaycastTestType() const; @@ -109,19 +100,8 @@ class ConcaveShape : public CollisionShape { /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0; - - /// Return true if the smooth mesh collision is enabled - bool getIsSmoothMeshCollisionEnabled() const; - - /// Enable/disable the smooth mesh collision algorithm - void setIsSmoothMeshCollisionEnabled(bool isEnabled); }; -// Return the triangle margin -inline decimal ConcaveShape::getTriangleMargin() const { - return mTriangleMargin; -} - // Return true if the collision shape is convex, false if it is concave inline bool ConcaveShape::isConvex() const { return false; @@ -137,19 +117,6 @@ inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* return false; } -// Return true if the smooth mesh collision is enabled -inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const { - return mIsSmoothMeshCollisionEnabled; -} - -// Enable/disable the smooth mesh collision algorithm -/// Smooth mesh collision is used to avoid collisions against some internal edges -/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother -/// but collisions computation is a bit more expensive. -inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) { - mIsSmoothMeshCollisionEnabled = isEnabled; -} - // Return the raycast test type (front, back, front-back) inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const { return mRaycastTestType; diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp deleted file mode 100644 index 4a1ece33..00000000 --- a/src/collision/shapes/ConeShape.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 -#include "configuration.h" -#include "ConeShape.h" -#include "collision/ProxyShape.h" - -using namespace reactphysics3d; - -// Constructor -/** - * @param radius Radius of the cone (in meters) - * @param height Height of the cone (in meters) - * @param margin Collision margin (in meters) around the collision shape - */ -ConeShape::ConeShape(decimal radius, decimal height, decimal margin) - : ConvexShape(CollisionShapeType::CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) { - assert(mRadius > decimal(0.0)); - assert(mHalfHeight > decimal(0.0)); - - // Compute the sine of the semi-angle at the apex point - mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); -} - -// Return a local support point in a given direction without the object margin -Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - - const Vector3& v = direction; - decimal sinThetaTimesLengthV = mSinTheta * v.length(); - Vector3 supportPoint; - - if (v.y > sinThetaTimesLengthV) { - supportPoint = Vector3(0.0, mHalfHeight, 0.0); - } - else { - decimal projectedLength = sqrt(v.x * v.x + v.z * v.z); - if (projectedLength > MACHINE_EPSILON) { - decimal d = mRadius / projectedLength; - supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d); - } - else { - supportPoint = Vector3(0.0, -mHalfHeight, 0.0); - } - } - - return supportPoint; -} - -// Raycast method with feedback information -// This implementation is based on the technique described by David Eberly in the article -// "Intersection of a Line and a Cone" that can be found at -// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf -bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - - const Vector3 r = ray.point2 - ray.point1; - - const decimal epsilon = decimal(0.00001); - Vector3 V(0, mHalfHeight, 0); - Vector3 centerBase(0, -mHalfHeight, 0); - Vector3 axis(0, decimal(-1.0), 0); - decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight; - decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius); - decimal factor = decimal(1.0) - cosThetaSquare; - Vector3 delta = ray.point1 - V; - decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - - cosThetaSquare * delta.z * delta.z; - decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; - decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; - decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)}; - Vector3 localHitPoint[3]; - Vector3 localNormal[3]; - - // If c2 is different from zero - if (std::abs(c2) > MACHINE_EPSILON) { - decimal gamma = c1 * c1 - c0 * c2; - - // If there is no real roots in the quadratic equation - if (gamma < decimal(0.0)) { - return false; - } - else if (gamma > decimal(0.0)) { // The equation has two real roots - - // Compute two intersections - decimal sqrRoot = std::sqrt(gamma); - tHit[0] = (-c1 - sqrRoot) / c2; - tHit[1] = (-c1 + sqrRoot) / c2; - } - else { // If the equation has a single real root - - // Compute the intersection - tHit[0] = -c1 / c2; - } - } - else { // If c2 == 0 - - // If c2 = 0 and c1 != 0 - if (std::abs(c1) > MACHINE_EPSILON) { - tHit[0] = -c0 / (decimal(2.0) * c1); - } - else { // If c2 = c1 = 0 - - // If c0 is different from zero, no solution and if c0 = 0, we have a - // degenerate case, the whole ray is contained in the cone side - // but we return no hit in this case - return false; - } - } - - // If the origin of the ray is inside the cone, we return no hit - if (testPointInside(ray.point1, nullptr)) return false; - - localHitPoint[0] = ray.point1 + tHit[0] * r; - localHitPoint[1] = ray.point1 + tHit[1] * r; - - // Only keep hit points in one side of the double cone (the cone we are interested in) - if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) { - tHit[0] = decimal(-1.0); - } - if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) { - tHit[1] = decimal(-1.0); - } - - // Only keep hit points that are within the correct height of the cone - if (localHitPoint[0].y < decimal(-mHalfHeight)) { - tHit[0] = decimal(-1.0); - } - if (localHitPoint[1].y < decimal(-mHalfHeight)) { - tHit[1] = decimal(-1.0); - } - - // If the ray is in direction of the base plane of the cone - if (r.y > epsilon) { - - // Compute the intersection with the base plane of the cone - tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y); - - // Only keep this intersection if it is inside the cone radius - localHitPoint[2] = ray.point1 + tHit[2] * r; - - if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) { - tHit[2] = decimal(-1.0); - } - - // Compute the normal direction - localNormal[2] = axis; - } - - // Find the smallest positive t value - int hitIndex = -1; - decimal t = DECIMAL_LARGEST; - for (int i=0; i<3; i++) { - if (tHit[i] < decimal(0.0)) continue; - if (tHit[i] < t) { - hitIndex = i; - t = tHit[hitIndex]; - } - } - - if (hitIndex < 0) return false; - if (t > ray.maxFraction) return false; - - // Compute the normal direction for hit against side of the cone - if (hitIndex != 2) { - decimal h = decimal(2.0) * mHalfHeight; - decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + - localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); - decimal rOverH = mRadius / h; - decimal value2 = decimal(1.0) + rOverH * rOverH; - decimal factor = decimal(1.0) / std::sqrt(value1 * value2); - decimal x = localHitPoint[hitIndex].x * factor; - decimal z = localHitPoint[hitIndex].z * factor; - localNormal[hitIndex].x = x; - localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH; - localNormal[hitIndex].z = z; - } - - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint[hitIndex]; - raycastInfo.worldNormal = localNormal[hitIndex]; - - return true; -} diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h deleted file mode 100644 index 16102163..00000000 --- a/src/collision/shapes/ConeShape.h +++ /dev/null @@ -1,194 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_CONE_SHAPE_H -#define REACTPHYSICS3D_CONE_SHAPE_H - -// Libraries -#include "ConvexShape.h" -#include "body/CollisionBody.h" -#include "mathematics/mathematics.h" - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class ConeShape -/** - * This class represents a cone collision shape centered at the - * origin and alligned with the Y axis. The cone is defined - * by its height and by the radius of its base. The center of the - * cone is at the half of the height. The "transform" of the - * corresponding rigid body gives an orientation and a position - * to the cone. This collision shape uses an extra margin distance around - * it for collision detection purpose. The default margin is 4cm (if your - * units are meters, which is recommended). In case, you want to simulate small - * objects (smaller than the margin distance), you might want to reduce the margin - * by specifying your own margin distance using the "margin" parameter in the - * constructor of the cone shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the constructor. - */ -class ConeShape : public ConvexShape { - - protected : - - // -------------------- Attributes -------------------- // - - /// Radius of the base - decimal mRadius; - - /// Half height of the cone - decimal mHalfHeight; - - /// sine of the semi angle at the apex point - decimal mSinTheta; - - // -------------------- Methods -------------------- // - - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; - - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; - - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const override; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN); - - /// Destructor - virtual ~ConeShape() override = default; - - /// Deleted copy-constructor - ConeShape(const ConeShape& shape) = delete; - - /// Deleted assignment operator - ConeShape& operator=(const ConeShape& shape) = delete; - - /// Return the radius - decimal getRadius() const; - - /// Return the height - decimal getHeight() const; - - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling) override; - - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const override; - - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; - - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; -}; - -// Return the radius -/** - * @return Radius of the cone (in meters) - */ -inline decimal ConeShape::getRadius() const { - return mRadius; -} - -// Return the height -/** - * @return Height of the cone (in meters) - */ -inline decimal ConeShape::getHeight() const { - return decimal(2.0) * mHalfHeight; -} - -// Set the scaling vector of the collision shape -inline void ConeShape::setLocalScaling(const Vector3& scaling) { - - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; - - CollisionShape::setLocalScaling(scaling); -} - -// Return the number of bytes used by the collision shape -inline size_t ConeShape::getSizeInBytes() const { - return sizeof(ConeShape); -} - -// Return the local bounds of the shape in x, y and z directions -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const { - - // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; - - // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; -} - -// Return true if the collision shape is a polyhedron -inline bool ConeShape::isPolyhedron() const { - return false; -} - -// Return the local inertia tensor of the collision shape -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal rSquare = mRadius * mRadius; - decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight); - tensor.setAllValues(diagXZ, 0.0, 0.0, - 0.0, decimal(0.3) * mass * rSquare, - 0.0, 0.0, 0.0, diagXZ); -} - -// Return true if a point is inside the collision shape -inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { - const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) / - (mHalfHeight * decimal(2.0)); - return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) && - (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight); -} - -} - -#endif diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index cc880a25..5b6c70d6 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -30,6 +30,9 @@ using namespace reactphysics3d; +// TODO : Check in every collision shape that localScalling is used correctly and even with SAT +// algorithm (not only in getLocalSupportPoint***() methods) + // Constructor to initialize with an array of 3D vertices. /// This method creates an internal copy of the input vertices. /** @@ -38,108 +41,13 @@ using namespace reactphysics3d; * @param stride Stride between the beginning of two elements in the vertices array * @param margin Collision margin (in meters) around the collision shape */ -ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin) - : ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0), - mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) { - assert(nbVertices > 0); - assert(stride > 0); - - const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; - - // Copy all the vertices into the internal array - for (uint i=0; igetVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); - - // For each vertex of the mesh - for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) { - - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + v * vertexStride); - - Vector3 vertex(vertices[0], vertices[1], vertices[2] ); - vertex = vertex * mScaling; - mVertices.push_back(vertex); - } - else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + v * vertexStride); - - Vector3 vertex(vertices[0], vertices[1], vertices[2] ); - vertex = vertex * mScaling; - mVertices.push_back(vertex); - } - } - - // If we need to use the edges information of the mesh - if (mIsEdgesInformationUsed) { - - // For each triangle of the mesh - for (uint triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { - - void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); - - uint vertexIndex[3] = {0, 0, 0}; - - // For each vertex of the triangle - for (int k=0; k < 3; k++) { - - // Get the index of the current vertex in the triangle - if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) { - vertexIndex[k] = ((uint*)vertexIndexPointer)[k]; - } - else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) { - vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k]; - } - else { - assert(false); - } - } - - // Add information about the edges - addEdge(vertexIndex[0], vertexIndex[1]); - addEdge(vertexIndex[0], vertexIndex[2]); - addEdge(vertexIndex[1], vertexIndex[2]); - } - } - - mNbVertices = mVertices.size(); - recalculateBounds(); -} - -// Constructor. -/// If you use this constructor, you will need to set the vertices manually one by one using -/// the addVertex() method. -ConvexMeshShape::ConvexMeshShape(decimal margin) - : ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0), - mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) { - -} - // Return a local support point in a given direction without the object margin. /// If the edges information is not used for collision detection, this method will go through /// the whole vertices list and pick up the vertex with the largest dot product in the support @@ -148,81 +56,28 @@ ConvexMeshShape::ConvexMeshShape(decimal margin) /// it as a start in a hill-climbing (local search) process to find the new support vertex which /// will be in most of the cases very close to the previous one. Using hill-climbing, this method /// runs in almost constant time. -Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { - assert(mNbVertices == mVertices.size()); - assert(cachedCollisionData != nullptr); + double maxDotProduct = DECIMAL_SMALLEST; + uint indexMaxDotProduct = 0; - // Allocate memory for the cached collision data if not allocated yet - if ((*cachedCollisionData) == nullptr) { - *cachedCollisionData = (int*) malloc(sizeof(int)); - *((int*)(*cachedCollisionData)) = 0; - } + // For each vertex of the mesh + for (uint i=0; igetNbVertices(); i++) { - // If the edges information is used to speed up the collision detection - if (mIsEdgesInformationUsed) { + // Compute the dot product of the current vertex + double dotProduct = direction.dot(mPolyhedronMesh->getVertex(i)); - assert(mEdgesAdjacencyList.size() == mNbVertices); - - uint maxVertex = *((int*)(*cachedCollisionData)); - decimal maxDotProduct = direction.dot(mVertices[maxVertex]); - bool isOptimal; - - // Perform hill-climbing (local search) - do { - isOptimal = true; - - assert(mEdgesAdjacencyList.at(maxVertex).size() > 0); - - // For all neighbors of the current vertex - std::set::const_iterator it; - std::set::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin(); - std::set::const_iterator itEnd = mEdgesAdjacencyList.at(maxVertex).end(); - for (it = itBegin; it != itEnd; ++it) { - - // Compute the dot product - decimal dotProduct = direction.dot(mVertices[*it]); - - // If the current vertex is a better vertex (larger dot product) - if (dotProduct > maxDotProduct) { - maxVertex = *it; - maxDotProduct = dotProduct; - isOptimal = false; - } - } - - } while(!isOptimal); - - // Cache the support vertex - *((int*)(*cachedCollisionData)) = maxVertex; - - // Return the support vertex - return mVertices[maxVertex] * mScaling; - } - else { // If the edges information is not used - - double maxDotProduct = DECIMAL_SMALLEST; - uint indexMaxDotProduct = 0; - - // For each vertex of the mesh - for (uint i=0; i maxDotProduct) { - indexMaxDotProduct = i; - maxDotProduct = dotProduct; - } + // If the current dot product is larger than the maximum one + if (dotProduct > maxDotProduct) { + indexMaxDotProduct = i; + maxDotProduct = dotProduct; } - - assert(maxDotProduct >= decimal(0.0)); - - // Return the vertex with the largest dot product in the support direction - return mVertices[indexMaxDotProduct] * mScaling; } + + assert(maxDotProduct >= decimal(0.0)); + + // Return the vertex with the largest dot product in the support direction + return mPolyhedronMesh->getVertex(indexMaxDotProduct) * mScaling; } // Recompute the bounds of the mesh @@ -235,16 +90,16 @@ void ConvexMeshShape::recalculateBounds() { mMaxBounds.setToZero(); // For each vertex of the mesh - for (uint i=0; igetNbVertices(); i++) { - if (mVertices[i].x > mMaxBounds.x) mMaxBounds.x = mVertices[i].x; - if (mVertices[i].x < mMinBounds.x) mMinBounds.x = mVertices[i].x; + if (mPolyhedronMesh->getVertex(i).x > mMaxBounds.x) mMaxBounds.x = mPolyhedronMesh->getVertex(i).x; + if (mPolyhedronMesh->getVertex(i).x < mMinBounds.x) mMinBounds.x = mPolyhedronMesh->getVertex(i).x; - if (mVertices[i].y > mMaxBounds.y) mMaxBounds.y = mVertices[i].y; - if (mVertices[i].y < mMinBounds.y) mMinBounds.y = mVertices[i].y; + if (mPolyhedronMesh->getVertex(i).y > mMaxBounds.y) mMaxBounds.y = mPolyhedronMesh->getVertex(i).y; + if (mPolyhedronMesh->getVertex(i).y < mMinBounds.y) mMinBounds.y = mPolyhedronMesh->getVertex(i).y; - if (mVertices[i].z > mMaxBounds.z) mMaxBounds.z = mVertices[i].z; - if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z; + if (mPolyhedronMesh->getVertex(i).z > mMaxBounds.z) mMaxBounds.z = mPolyhedronMesh->getVertex(i).z; + if (mPolyhedronMesh->getVertex(i).z < mMinBounds.z) mMinBounds.z = mPolyhedronMesh->getVertex(i).z; } // Apply the local scaling factor @@ -257,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() { } // Raycast method with feedback information -bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( ray, proxyShape, raycastInfo); } diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 66b624aa..44d01df4 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -1,4 +1,4 @@ -/******************************************************************************** + /******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * * Copyright (c) 2010-2016 Daniel Chappuis * ********************************************************************************* @@ -27,10 +27,11 @@ #define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H // Libraries -#include "ConvexShape.h" +#include "ConvexPolyhedronShape.h" #include "engine/CollisionWorld.h" #include "mathematics/mathematics.h" #include "collision/TriangleMesh.h" +#include "collision/PolyhedronMesh.h" #include "collision/narrowphase/GJK/GJKAlgorithm.h" #include #include @@ -45,30 +46,18 @@ class CollisionWorld; // Class ConvexMeshShape /** * This class represents a convex mesh shape. In order to create a convex mesh shape, you - * need to indicate the local-space position of the mesh vertices. You do it either by - * passing a vertices array to the constructor or using the addVertex() method. Make sure - * that the set of vertices that you use to create the shape are indeed part of a convex - * mesh. The center of mass of the shape will be at the origin of the local-space geometry - * that you use to create the mesh. The method used for collision detection with a convex - * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh. - * Therefore, you should try not to use too many vertices. However, it is possible to speed - * up the collision detection by using the edges information of your mesh. The running time - * of the collision detection that uses the edges is almost O(1) constant time at the cost - * of additional memory used to store the vertices. You can indicate edges information - * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method - * in order to use the edges information for collision detection. + * need to indicate the local-space position of the mesh vertices. The center of mass + * of the shape will be at the origin of the local-space geometry that you use to create + * the mesh. */ -class ConvexMeshShape : public ConvexShape { +class ConvexMeshShape : public ConvexPolyhedronShape { protected : // -------------------- Attributes -------------------- // - /// Array with the vertices of the mesh - std::vector mVertices; - - /// Number of vertices in the mesh - uint mNbVertices; + /// Polyhedron structure of the mesh + PolyhedronMesh* mPolyhedronMesh; /// Mesh minimum bounds in the three local x, y and z directions Vector3 mMinBounds; @@ -76,30 +65,19 @@ class ConvexMeshShape : public ConvexShape { /// Mesh maximum bounds in the three local x, y and z directions Vector3 mMaxBounds; - /// True if the shape contains the edges of the convex mesh in order to - /// make the collision detection faster - bool mIsEdgesInformationUsed; - - /// Adjacency list representing the edges of the mesh - std::map > mEdgesAdjacencyList; - // -------------------- Methods -------------------- // /// Recompute the bounds of the mesh void recalculateBounds(); - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling) override; - /// Return a local support point in a given direction without the object margin. - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -108,16 +86,8 @@ class ConvexMeshShape : public ConvexShape { // -------------------- Methods -------------------- // - /// Constructor to initialize with an array of 3D vertices. - ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, - decimal margin = OBJECT_MARGIN); - - /// Constructor to initialize with a triangle vertex array - ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, - decimal margin = OBJECT_MARGIN); - - /// Constructor. - ConvexMeshShape(decimal margin = OBJECT_MARGIN); + /// Constructor + ConvexMeshShape(PolyhedronMesh* polyhedronMesh); /// Destructor virtual ~ConvexMeshShape() override = default; @@ -128,27 +98,41 @@ class ConvexMeshShape : public ConvexShape { /// Deleted assignment operator ConvexMeshShape& operator=(const ConvexMeshShape& shape) = delete; + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const Vector3& scaling) override; + /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const override; /// Return the local inertia tensor of the collision shape. virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; - /// Add a vertex into the convex mesh - void addVertex(const Vector3& vertex); + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; - /// Add an edge into the convex mesh by specifying the two vertex indices of the edge. - void addEdge(uint v1, uint v2); + /// Return a given face of the polyhedron + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; - /// Return true if the edges information is used to speed up the collision detection - bool isEdgesInformationUsed() const; + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; - /// Set the variable to know if the edges information is used to speed up the - /// collision detection - void setIsEdgesInformationUsed(bool isEdgesUsed); + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; }; /// Set the scaling vector of the collision shape @@ -162,11 +146,6 @@ inline size_t ConvexMeshShape::getSizeInBytes() const { return sizeof(ConvexMeshShape); } -// Return true if the collision shape is a polyhedron -inline bool ConvexMeshShape::isPolyhedron() const { - return true; -} - // Return the local bounds of the shape in x, y and z directions /** * @param min The minimum bounds of the shape in local-space coordinates @@ -197,68 +176,6 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decima 0.0, 0.0, factor * (xSquare + ySquare)); } -// Add a vertex into the convex mesh -/** - * @param vertex Vertex to be added - */ -inline void ConvexMeshShape::addVertex(const Vector3& vertex) { - - // Add the vertex in to vertices array - mVertices.push_back(vertex); - mNbVertices++; - - // Update the bounds of the mesh - if (vertex.x * mScaling.x > mMaxBounds.x) mMaxBounds.x = vertex.x * mScaling.x; - if (vertex.x * mScaling.x < mMinBounds.x) mMinBounds.x = vertex.x * mScaling.x; - if (vertex.y * mScaling.y > mMaxBounds.y) mMaxBounds.y = vertex.y * mScaling.y; - if (vertex.y * mScaling.y < mMinBounds.y) mMinBounds.y = vertex.y * mScaling.y; - if (vertex.z * mScaling.z > mMaxBounds.z) mMaxBounds.z = vertex.z * mScaling.z; - if (vertex.z * mScaling.z < mMinBounds.z) mMinBounds.z = vertex.z * mScaling.z; -} - -// Add an edge into the convex mesh by specifying the two vertex indices of the edge. -/// Note that the vertex indices start at zero and need to correspond to the order of -/// the vertices in the vertices array in the constructor or the order of the calls -/// of the addVertex() methods that you use to add vertices into the convex mesh. -/** -* @param v1 Index of the first vertex of the edge to add -* @param v2 Index of the second vertex of the edge to add -*/ -inline void ConvexMeshShape::addEdge(uint v1, uint v2) { - - // If the entry for vertex v1 does not exist in the adjacency list - if (mEdgesAdjacencyList.count(v1) == 0) { - mEdgesAdjacencyList.insert(std::make_pair(v1, std::set())); - } - - // If the entry for vertex v2 does not exist in the adjacency list - if (mEdgesAdjacencyList.count(v2) == 0) { - mEdgesAdjacencyList.insert(std::make_pair(v2, std::set())); - } - - // Add the edge in the adjacency list - mEdgesAdjacencyList[v1].insert(v2); - mEdgesAdjacencyList[v2].insert(v1); -} - -// Return true if the edges information is used to speed up the collision detection -/** - * @return True if the edges information is used and false otherwise - */ -inline bool ConvexMeshShape::isEdgesInformationUsed() const { - return mIsEdgesInformationUsed; -} - -// Set the variable to know if the edges information is used to speed up the -// collision detection -/** - * @param isEdgesUsed True if you want to use the edges information to speed up - * the collision detection with the convex mesh shape - */ -inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { - mIsEdgesInformationUsed = isEdgesUsed; -} - // Return true if a point is inside the collision shape inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { @@ -268,6 +185,56 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); } +// Return the number of faces of the polyhedron +inline uint ConvexMeshShape::getNbFaces() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces(); +} + +// Return a given face of the polyhedron +inline const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex); +} + +// Return the number of vertices of the polyhedron +inline uint ConvexMeshShape::getNbVertices() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbVertices(); +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex ConvexMeshShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mPolyhedronMesh->getHalfEdgeStructure().getVertex(vertexIndex); +} + +// Return the number of half-edges of the polyhedron +inline uint ConvexMeshShape::getNbHalfEdges() const { + return mPolyhedronMesh->getHalfEdgeStructure().getNbHalfEdges(); +} + +// Return a given half-edge of the polyhedron +inline const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex); +} + +// Return the position of a given vertex +inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < getNbVertices()); + return mPolyhedronMesh->getVertex(vertexIndex) * mScaling; +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < getNbFaces()); + return mPolyhedronMesh->getFaceNormal(faceIndex); +} + +// Return the centroid of the polyhedron +inline Vector3 ConvexMeshShape::getCentroid() const { + return mPolyhedronMesh->getCentroid() * mScaling; +} + } #endif diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp new file mode 100644 index 00000000..24f80de6 --- /dev/null +++ b/src/collision/shapes/ConvexPolyhedronShape.cpp @@ -0,0 +1,59 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "ConvexPolyhedronShape.h" + + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name) + : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) { + +} + +// Find and return the index of the polyhedron face with the most anti-parallel face +// normal given a direction vector. This is used to find the incident face on +// a polyhedron of a given reference face of another polyhedron +uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const { + + decimal minDotProduct = DECIMAL_LARGEST; + uint mostAntiParallelFace = 0; + + // For each face of the polyhedron + for (uint i=0; i < getNbFaces(); i++) { + + // Get the face normal + decimal dotProduct = getFaceNormal(i).dot(direction); + if (dotProduct < minDotProduct) { + minDotProduct = dotProduct; + mostAntiParallelFace = i; + } + } + + return mostAntiParallelFace; +} diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h new file mode 100644 index 00000000..50c6df89 --- /dev/null +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -0,0 +1,105 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_H +#define REACTPHYSICS3D_CONVEX_POLYHEDRON_H + +// Libraries +#include "ConvexShape.h" +#include "collision/HalfEdgeStructure.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class ConvexPolyhedronShape +/** + * This abstract class represents a convex polyhedron collision shape associated with a + * body that is used during the narrow-phase collision detection. + */ +class ConvexPolyhedronShape : public ConvexShape { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + ConvexPolyhedronShape(CollisionShapeName name); + + /// Destructor + virtual ~ConvexPolyhedronShape() override = default; + + /// Deleted copy-constructor + ConvexPolyhedronShape(const ConvexPolyhedronShape& shape) = delete; + + /// Deleted assignment operator + ConvexPolyhedronShape& operator=(const ConvexPolyhedronShape& shape) = delete; + + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const=0; + + /// Return a given face of the polyhedron + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const=0; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const=0; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const=0; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const=0; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const=0; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const=0; + + /// Return a given half-edge of the polyhedron + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const=0; + + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const=0; + + /// Find and return the index of the polyhedron face with the most anti-parallel face + /// normal given a direction vector + uint findMostAntiParallelFace(const Vector3& direction) const; +}; + +// Return true if the collision shape is a polyhedron +inline bool ConvexPolyhedronShape::isPolyhedron() const { + return true; +} + + +} + +#endif + diff --git a/src/collision/shapes/ConvexShape.cpp b/src/collision/shapes/ConvexShape.cpp index f077cdcb..90c5356f 100644 --- a/src/collision/shapes/ConvexShape.cpp +++ b/src/collision/shapes/ConvexShape.cpp @@ -31,17 +31,16 @@ using namespace reactphysics3d; // Constructor -ConvexShape::ConvexShape(CollisionShapeType type, decimal margin) - : CollisionShape(type), mMargin(margin) { +ConvexShape::ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin) + : CollisionShape(name, type), mMargin(margin) { } // Return a local support point in a given direction with the object margin -Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const { +Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction) const { // Get the support point without margin - Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); + Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); if (mMargin != decimal(0.0)) { diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 4b12cae4..04f8aad6 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -49,19 +49,17 @@ class ConvexShape : public CollisionShape { // -------------------- Methods -------------------- // // Return a local support point in a given direction with the object margin - Vector3 getLocalSupportPointWithMargin(const Vector3& direction, - void** cachedCollisionData) const; + Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const=0; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const=0; public : // -------------------- Methods -------------------- // /// Constructor - ConvexShape(CollisionShapeType type, decimal margin); + ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin = decimal(0.0)); /// Destructor virtual ~ConvexShape() override = default; @@ -81,10 +79,10 @@ class ConvexShape : public CollisionShape { // -------------------- Friendship -------------------- // friend class GJKAlgorithm; - friend class EPAAlgorithm; + friend class SATAlgorithm; }; -/// Return true if the collision shape is convex, false if it is concave +// Return true if the collision shape is convex, false if it is concave inline bool ConvexShape::isConvex() const { return true; } diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp deleted file mode 100644 index 8cb405f1..00000000 --- a/src/collision/shapes/CylinderShape.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "CylinderShape.h" -#include "collision/ProxyShape.h" -#include "configuration.h" - -using namespace reactphysics3d; - -// Constructor -/** - * @param radius Radius of the cylinder (in meters) - * @param height Height of the cylinder (in meters) - * @param margin Collision margin (in meters) around the collision shape - */ -CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin) - : ConvexShape(CollisionShapeType::CYLINDER, margin), mRadius(radius), - mHalfHeight(height/decimal(2.0)) { - assert(radius > decimal(0.0)); - assert(height > decimal(0.0)); -} - -// Return a local support point in a given direction without the object margin -Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { - - Vector3 supportPoint(0.0, 0.0, 0.0); - decimal uDotv = direction.y; - Vector3 w(direction.x, 0.0, direction.z); - decimal lengthW = std::sqrt(direction.x * direction.x + direction.z * direction.z); - - if (lengthW > MACHINE_EPSILON) { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; - supportPoint += (mRadius / lengthW) * w; - } - else { - if (uDotv < 0.0) supportPoint.y = -mHalfHeight; - else supportPoint.y = mHalfHeight; - } - - return supportPoint; -} - -// Raycast method with feedback information -/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by -/// Morgan Kaufmann. -bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { - - const Vector3 n = ray.point2 - ray.point1; - - const decimal epsilon = decimal(0.01); - Vector3 p(decimal(0), -mHalfHeight, decimal(0)); - Vector3 q(decimal(0), mHalfHeight, decimal(0)); - Vector3 d = q - p; - Vector3 m = ray.point1 - p; - decimal t; - - decimal mDotD = m.dot(d); - decimal nDotD = n.dot(d); - decimal dDotD = d.dot(d); - - // Test if the segment is outside the cylinder - if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false; - if (mDotD > dDotD && mDotD + nDotD > dDotD) return false; - - decimal nDotN = n.dot(n); - decimal mDotN = m.dot(n); - - decimal a = dDotD * nDotN - nDotD * nDotD; - decimal k = m.dot(m) - mRadius * mRadius; - decimal c = dDotD * k - mDotD * mDotD; - - // If the ray is parallel to the cylinder axis - if (std::abs(a) < epsilon) { - - // If the origin is outside the surface of the cylinder, we return no hit - if (c > decimal(0.0)) return false; - - // Here we know that the segment intersect an endcap of the cylinder - - // If the ray intersects with the "p" endcap of the cylinder - if (mDotD < decimal(0.0)) { - - t = -mDotN / nDotN; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(-1), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder - - t = (nDotD - mDotN) / nDotN; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else { // If the origin is inside the cylinder, we return no hit - return false; - } - } - decimal b = dDotD * mDotN - nDotD * mDotD; - decimal discriminant = b * b - a * c; - - // If the discriminant is negative, no real roots and therfore, no hit - if (discriminant < decimal(0.0)) return false; - - // Compute the smallest root (first intersection along the ray) - decimal t0 = t = (-b - std::sqrt(discriminant)) / a; - - // If the intersection is outside the cylinder on "p" endcap side - decimal value = mDotD + t * nDotD; - if (value < decimal(0.0)) { - - // If the ray is pointing away from the "p" endcap, we return no hit - if (nDotD <= decimal(0.0)) return false; - - // Compute the intersection against the "p" endcap (intersection agains whole plane) - t = -mDotD / nDotD; - - // Keep the intersection if the it is inside the cylinder radius - if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(-1.0), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side - - // If the ray is pointing away from the "q" endcap, we return no hit - if (nDotD >= decimal(0.0)) return false; - - // Compute the intersection against the "q" endcap (intersection against whole plane) - t = (dDotD - mDotD) / nDotD; - - // Keep the intersection if it is inside the cylinder radius - if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) > - decimal(0.0)) return false; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 normalDirection(0, decimal(1.0), 0); - raycastInfo.worldNormal = normalDirection; - - return true; - } - - t = t0; - - // If the intersection is behind the origin of the ray or beyond the maximum - // raycasting distance, we return no hit - if (t < decimal(0.0) || t > ray.maxFraction) return false; - - // Compute the hit information - Vector3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); - raycastInfo.proxyShape = proxyShape; - raycastInfo.hitFraction = t; - raycastInfo.worldPoint = localHitPoint; - Vector3 v = localHitPoint - p; - Vector3 w = (v.dot(d) / d.lengthSquare()) * d; - Vector3 normalDirection = (localHitPoint - (p + w)); - raycastInfo.worldNormal = normalDirection; - - return true; -} diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h deleted file mode 100644 index e0744a80..00000000 --- a/src/collision/shapes/CylinderShape.h +++ /dev/null @@ -1,190 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_CYLINDER_SHAPE_H -#define REACTPHYSICS3D_CYLINDER_SHAPE_H - -// Libraries -#include "ConvexShape.h" -#include "body/CollisionBody.h" -#include "mathematics/mathematics.h" - - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Class CylinderShape -/** - * This class represents a cylinder collision shape around the Y axis - * and centered at the origin. The cylinder is defined by its height - * and the radius of its base. The "transform" of the corresponding - * rigid body gives an orientation and a position to the cylinder. - * This collision shape uses an extra margin distance around it for collision - * detection purpose. The default margin is 4cm (if your units are meters, - * which is recommended). In case, you want to simulate small objects - * (smaller than the margin distance), you might want to reduce the margin by - * specifying your own margin distance using the "margin" parameter in the - * constructor of the cylinder shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the constructor. - */ -class CylinderShape : public ConvexShape { - - protected : - - // -------------------- Attributes -------------------- // - - /// Radius of the base - decimal mRadius; - - /// Half height of the cylinder - decimal mHalfHeight; - - // -------------------- Methods -------------------- // - - /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; - - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override ; - - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const override; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN); - - /// Destructor - virtual ~CylinderShape() override = default; - - /// Deleted copy-constructor - CylinderShape(const CylinderShape& shape) = delete; - - /// Deleted assignment operator - CylinderShape& operator=(const CylinderShape& shape) = delete; - - /// Return the radius - decimal getRadius() const; - - /// Return the height - decimal getHeight() const; - - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; - - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const Vector3& scaling) override; - - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(Vector3& min, Vector3& max) const override; - - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override; -}; - -// Return the radius -/** - * @return Radius of the cylinder (in meters) - */ -inline decimal CylinderShape::getRadius() const { - return mRadius; -} - -// Return the height -/** - * @return Height of the cylinder (in meters) - */ -inline decimal CylinderShape::getHeight() const { - return mHalfHeight + mHalfHeight; -} - -// Return true if the collision shape is a polyhedron -inline bool CylinderShape::isPolyhedron() const { - return false; -} - -// Set the scaling vector of the collision shape -inline void CylinderShape::setLocalScaling(const Vector3& scaling) { - - mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y; - mRadius = (mRadius / mScaling.x) * scaling.x; - - CollisionShape::setLocalScaling(scaling); -} - -// Return the number of bytes used by the collision shape -inline size_t CylinderShape::getSizeInBytes() const { - return sizeof(CylinderShape); -} - -// Return the local bounds of the shape in x, y and z directions -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const { - - // Maximum bounds - max.x = mRadius + mMargin; - max.y = mHalfHeight + mMargin; - max.z = max.x; - - // Minimum bounds - min.x = -max.x; - min.y = -max.y; - min.z = min.x; -} - -// Return the local inertia tensor of the cylinder -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal height = decimal(2.0) * mHalfHeight; - decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height); - tensor.setAllValues(diag, 0.0, 0.0, 0.0, - decimal(0.5) * mass * mRadius * mRadius, 0.0, - 0.0, 0.0, diag); -} - -// Return true if a point is inside the collision shape -inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{ - return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius && - localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight); -} - -} - -#endif - diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 02ef852f..badfb68d 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -42,7 +42,7 @@ using namespace reactphysics3d; HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight, const void* heightFieldData, HeightDataType dataType, int upAxis, decimal integerHeightScale) - : ConcaveShape(CollisionShapeType::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), + : ConcaveShape(CollisionShapeName::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows), mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight), mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale), mHeightDataType(dataType) { @@ -133,24 +133,48 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& for (int j = jMin; j < jMax; j++) { // Compute the four point of the current quad - Vector3 p1 = getVertexAt(i, j); - Vector3 p2 = getVertexAt(i, j + 1); - Vector3 p3 = getVertexAt(i + 1, j); - Vector3 p4 = getVertexAt(i + 1, j + 1); + const Vector3 p1 = getVertexAt(i, j); + const Vector3 p2 = getVertexAt(i, j + 1); + const Vector3 p3 = getVertexAt(i + 1, j); + const Vector3 p4 = getVertexAt(i + 1, j + 1); // Generate the first triangle for the current grid rectangle Vector3 trianglePoints[3] = {p1, p2, p3}; + // Compute the triangle normal + Vector3 triangle1Normal = (p2 - p1).cross(p3 - p1).getUnit(); + + // Use the triangle face normal as vertices normals (this is an aproximation. The correct + // solution would be to compute all the normals of the neighbor triangles and use their + // weighted average (with incident angle as weight) at the vertices. However, this solution + // seems too expensive (it requires to compute the normal of all neighbor triangles instead + // and compute the angle of incident edges with asin(). Maybe we could also precompute the + // vertices normal at the HeightFieldShape constructor but it will require extra memory to + // store them. + Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal}; + // Test collision against the first triangle - callback.testTriangle(trianglePoints); + callback.testTriangle(trianglePoints, verticesNormals1, computeTriangleShapeId(i, j, 0)); // Generate the second triangle for the current grid rectangle trianglePoints[0] = p3; trianglePoints[1] = p2; trianglePoints[2] = p4; + // Compute the triangle normal + Vector3 triangle2Normal = (p2 - p3).cross(p4 - p3).getUnit(); + + // Use the triangle face normal as vertices normals (this is an aproximation. The correct + // solution would be to compute all the normals of the neighbor triangles and use their + // weighted average (with incident angle as weight) at the vertices. However, this solution + // seems too expensive (it requires to compute the normal of all neighbor triangles instead + // and compute the angle of incident edges with asin(). Maybe we could also precompute the + // vertices normal at the HeightFieldShape constructor but it will require extra memory to + // store them. + Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal}; + // Test collision against the second triangle - callback.testTriangle(trianglePoints); + callback.testTriangle(trianglePoints, verticesNormals2, computeTriangleShapeId(i, j, 1)); } } } @@ -188,14 +212,21 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor // Raycast method with feedback information /// Note that only the first triangle hit by the ray in the mesh will be returned, even if /// the ray hits many triangles. -bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { // TODO : Implement raycasting without using an AABB for the ray // but using a dynamic AABB tree or octree instead - PROFILE("HeightFieldShape::raycast()"); + PROFILE("HeightFieldShape::raycast()", mProfiler); - TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); + TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this, allocator); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + triangleCallback.setProfiler(mProfiler); + +#endif // Compute the AABB for the ray const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); @@ -232,16 +263,22 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const { } // Raycast test between a ray and a triangle of the heightfield -void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) { +void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { // Create a triangle collision shape - decimal margin = mHeightFieldShape.getTriangleMargin(); - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId, mAllocator); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler to the triangle shape + triangleShape.setProfiler(mProfiler); + +#endif + // Ray casting test against the collision shape RaycastInfo raycastInfo; - bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape); + bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator); // If the ray hit the collision shape if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) { diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index 6f2b8459..fc587d4c 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -49,14 +49,22 @@ class TriangleOverlapCallback : public TriangleCallback { bool mIsHit; decimal mSmallestHitFraction; const HeightFieldShape& mHeightFieldShape; + MemoryAllocator& mAllocator; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif public: // Constructor TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo, - const HeightFieldShape& heightFieldShape) + const HeightFieldShape& heightFieldShape, MemoryAllocator& allocator) : mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo), - mHeightFieldShape (heightFieldShape) { + mHeightFieldShape (heightFieldShape), mAllocator(allocator) { mIsHit = false; mSmallestHitFraction = mRay.maxFraction; } @@ -64,7 +72,17 @@ class TriangleOverlapCallback : public TriangleCallback { bool getIsHit() const {return mIsHit;} /// Raycast test between a ray and a triangle of the heightfield - virtual void testTriangle(const Vector3* trianglePoints) override; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler) { + mProfiler = profiler; + } + +#endif + }; @@ -126,7 +144,7 @@ class HeightFieldShape : public ConcaveShape { // -------------------- Methods -------------------- // /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -151,6 +169,9 @@ class HeightFieldShape : public ConcaveShape { /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const; + /// Compute the shape Id for a given triangle + uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const; + public: /// Constructor @@ -252,6 +273,12 @@ inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim 0, 0, mass); } +// Compute the shape Id for a given triangle +inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const { + + return (jIndex * (mNbColumns - 1) + iIndex) * 2 + secondTriangleIncrement; +} + } #endif diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 73fe49aa..3ad7afe7 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -35,12 +35,13 @@ using namespace reactphysics3d; /** * @param radius Radius of the sphere (in meters) */ -SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeType::SPHERE, radius) { +SphereShape::SphereShape(decimal radius) + : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) { assert(radius > decimal(0.0)); } // Raycast method with feedback information -bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { const Vector3 m = ray.point1; decimal c = m.dot(m) - mMargin * mMargin; diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index fe55a7a8..90b20d2d 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -49,14 +49,13 @@ class SphereShape : public ConvexShape { // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; @@ -123,8 +122,7 @@ inline size_t SphereShape::getSizeInBytes() const { } // Return a local support point in a given direction without the object margin -inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { // Return the center of the sphere (the radius is taken into account in the object margin) return Vector3(0.0, 0.0, 0.0); diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 4767435b..2257c0e6 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -26,33 +26,194 @@ // Libraries #include "TriangleShape.h" #include "collision/ProxyShape.h" +#include "mathematics/mathematics_functions.h" #include "engine/Profiler.h" #include "configuration.h" #include using namespace reactphysics3d; + // Constructor /** + * Do not use this constructor. It is supposed to be used internally only. + * Use a ConcaveMeshShape instead. * @param point1 First point of the triangle * @param point2 Second point of the triangle * @param point3 Third point of the triangle + * @param verticesNormals The three vertices normals for smooth mesh collision * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin) - : ConvexShape(CollisionShapeType::TRIANGLE, margin) { - mPoints[0] = point1; - mPoints[1] = point2; - mPoints[2] = point3; +TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId, + MemoryAllocator& allocator) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} { + + mPoints[0] = vertices[0]; + mPoints[1] = vertices[1]; + mPoints[2] = vertices[2]; + + // Compute the triangle normal + mNormal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]); + mNormal.normalize(); + + mVerticesNormals[0] = verticesNormals[0]; + mVerticesNormals[1] = verticesNormals[1]; + mVerticesNormals[2] = verticesNormals[2]; + + // Faces + for (uint i=0; i<2; i++) { + mFaces[i].faceVertices.reserve(3); + mFaces[i].faceVertices.add(0); + mFaces[i].faceVertices.add(1); + mFaces[i].faceVertices.add(2); + mFaces[i].edgeIndex = i; + } + + // Edges + for (uint i=0; i<6; i++) { + switch(i) { + case 0: + mEdges[0].vertexIndex = 0; + mEdges[0].twinEdgeIndex = 1; + mEdges[0].faceIndex = 0; + mEdges[0].nextEdgeIndex = 2; + break; + case 1: + mEdges[1].vertexIndex = 1; + mEdges[1].twinEdgeIndex = 0; + mEdges[1].faceIndex = 1; + mEdges[1].nextEdgeIndex = 5; + break; + case 2: + mEdges[2].vertexIndex = 1; + mEdges[2].twinEdgeIndex = 3; + mEdges[2].faceIndex = 0; + mEdges[2].nextEdgeIndex = 4; + break; + case 3: + mEdges[3].vertexIndex = 2; + mEdges[3].twinEdgeIndex = 2; + mEdges[3].faceIndex = 1; + mEdges[3].nextEdgeIndex = 1; + break; + case 4: + mEdges[4].vertexIndex = 2; + mEdges[4].twinEdgeIndex = 5; + mEdges[4].faceIndex = 0; + mEdges[4].nextEdgeIndex = 0; + break; + case 5: + mEdges[5].vertexIndex = 0; + mEdges[5].twinEdgeIndex = 4; + mEdges[5].faceIndex = 1; + mEdges[5].nextEdgeIndex = 3; + break; + } + } + + mRaycastTestType = TriangleRaycastSide::FRONT; + + mId = shapeId; +} + +// This method compute the smooth mesh contact with a triangle in case one of the two collision +// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh +// at the contact point instead of the triangle normal to avoid the internal edge collision issue. +// This method will return the new smooth world contact +// normal of the triangle and the the local contact point on the other shape. +void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, + Vector3& localContactPointShape1, Vector3& localContactPointShape2, + const Transform& shape1ToWorld, const Transform& shape2ToWorld, + decimal penetrationDepth, Vector3& outSmoothVertexNormal) { + + assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE); + + // If one the shape is a triangle + bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE; + if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) { + + const TriangleShape* triangleShape = isShape1Triangle ? static_cast(shape1): + static_cast(shape2); + + // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape + triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, + isShape1Triangle ? shape1ToWorld : shape2ToWorld, + isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), + penetrationDepth, isShape1Triangle, + isShape1Triangle ? localContactPointShape2 : localContactPointShape1, + outSmoothVertexNormal); + } +} + + +// This method implements the technique described in Game Physics Pearl book +// by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is +// to replace the contact normal of the triangle shape with the precomputed normal of the triangle +// mesh at this point. Then, we need to recompute the contact point on the other shape in order to +// stay aligned with the new contact normal. This method will return the new smooth world contact +// normal of the triangle and the the local contact point on the other shape. +void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1, + Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const { + + // Get the smooth contact normal of the mesh at the contact point on the triangle + Vector3 triangleLocalNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle); + + // Convert the local contact normal into world-space + Vector3 triangleWorldNormal = triangleShapeToWorldTransform.getOrientation() * triangleLocalNormal; + + // Penetration axis with direction from triangle to other shape + Vector3 triangleToOtherShapePenAxis = isTriangleShape1 ? outSmoothWorldContactTriangleNormal : -outSmoothWorldContactTriangleNormal; + + // The triangle normal should be the one in the direction out of the current colliding face of the triangle + if (triangleWorldNormal.dot(triangleToOtherShapePenAxis) < decimal(0.0)) { + triangleWorldNormal = -triangleWorldNormal; + triangleLocalNormal = -triangleLocalNormal; + } + + // Compute the final contact normal from shape 1 to shape 2 + outSmoothWorldContactTriangleNormal = isTriangleShape1 ? triangleWorldNormal : -triangleWorldNormal; + + // Re-align the local contact point on the other shape such that it is aligned along the new contact normal + Vector3 otherShapePointTriangleSpace = localContactPointTriangle - triangleLocalNormal * penetrationDepth; + Vector3 otherShapePoint = worldToOtherShapeTransform * triangleShapeToWorldTransform * otherShapePointTriangleSpace; + outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z); +} + +// Get a smooth contact normal for collision for a triangle of the mesh +/// This is used to avoid the internal edges issue that occurs when a shape is colliding with +/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance, +/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface +/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface +/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5 +/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the +/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only +/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the +/// middle of the triangle, we return the true triangle normal. +Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const { + + // Compute the barycentric coordinates of the point in the triangle + decimal u, v, w; + computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w); + + // If the contact is in the middle of the triangle face (not on the edges) + if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) { + + // We return the true triangle face normal (not the interpolated one) + return mNormal; + } + + // We compute the contact normal as the barycentric interpolation of the three vertices normals + return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit(); } // Raycast method with feedback information /// This method use the line vs triangle raycasting technique described in /// Real-time Collision Detection by Christer Ericson. -bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { +bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const { - PROFILE("TriangleShape::raycast()"); + PROFILE("TriangleShape::raycast()", mProfiler); const Vector3 pq = ray.point2 - ray.point1; const Vector3 pa = mPoints[0] - ray.point1; diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 29732f8f..23f26adc 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -28,7 +28,7 @@ // Libraries #include "mathematics/mathematics.h" -#include "ConvexShape.h" +#include "ConvexPolyhedronShape.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -49,42 +49,71 @@ enum class TriangleRaycastSide { // Class TriangleShape /** * This class represents a triangle collision shape that is centered - * at the origin and defined three points. + * at the origin and defined three points. A user cannot instanciate + * an object of this class. This class is for internal use only. Instances + * of this class are created when the user creates an HeightFieldShape and + * a ConcaveMeshShape */ -class TriangleShape : public ConvexShape { +class TriangleShape : public ConvexPolyhedronShape { protected: // -------------------- Attribute -------------------- // + /// Three points of the triangle Vector3 mPoints[3]; + /// Normal of the triangle + Vector3 mNormal; + + /// Three vertices normals for smooth collision with triangle mesh + Vector3 mVerticesNormals[3]; + /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; + /// Faces information for the two faces of the triangle + HalfEdgeStructure::Face mFaces[2]; + + /// Edges information for the six edges of the triangle + HalfEdgeStructure::Edge mEdges[6]; + // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin - virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const override; + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override; + + /// Get a smooth contact normal for collision for a triangle of the mesh + Vector3 computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const; /// Return true if a point is inside the collision shape virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override; /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override; + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, + MemoryAllocator& allocator) const override; /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; + /// Generate the id of the shape (used for temporal coherence) + void generateId(); + + // -------------------- Methods -------------------- // + + /// This method implements the technique described in Game Physics Pearl book + void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1, + Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const; + public: // -------------------- Methods -------------------- // /// Constructor - TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - decimal margin = OBJECT_MARGIN); + TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, + uint shapeId, MemoryAllocator& allocator); /// Destructor virtual ~TriangleShape() override = default; @@ -113,16 +142,44 @@ class TriangleShape : public ConvexShape { // Set the raycast test type (front, back, front-back) void setRaycastTestType(TriangleRaycastSide testType); - /// Return the coordinates of a given vertex of the triangle - Vector3 getVertex(int index) const; + /// Return the number of faces of the polyhedron + virtual uint getNbFaces() const override; - /// Return true if the collision shape is a polyhedron - virtual bool isPolyhedron() const override; + /// Return a given face of the polyhedron + virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override; + + /// Return the number of vertices of the polyhedron + virtual uint getNbVertices() const override; + + /// Return a given vertex of the polyhedron + virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override; + + /// Return the position of a given vertex + virtual Vector3 getVertexPosition(uint vertexIndex) const override; + + /// Return the normal vector of a given face of the polyhedron + virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the number of half-edges of the polyhedron + virtual uint getNbHalfEdges() const override; + + /// Return a given half-edge of the polyhedron + virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; + + /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh + static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, + Vector3& localContactPointShape1, Vector3& localContactPointShape2, + const Transform& shape1ToWorld, const Transform& shape2ToWorld, + decimal penetrationDepth, Vector3& outSmoothVertexNormal); // ---------- Friendship ---------- // friend class ConcaveMeshRaycastCallback; friend class TriangleOverlapCallback; + friend class MiddlePhaseTriangleCallback; }; // Return the number of bytes used by the collision shape @@ -131,8 +188,7 @@ inline size_t TriangleShape::getSizeInBytes() const { } // Return a local support point in a given direction without the object margin -inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, - void** cachedCollisionData) const { +inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2])); return mPoints[dotProducts.getMaxAxis()]; } @@ -199,6 +255,63 @@ inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape return false; } +// Return the number of faces of the polyhedron +inline uint TriangleShape::getNbFaces() const { + return 2; +} + +// Return a given face of the polyhedron +inline const HalfEdgeStructure::Face& TriangleShape::getFace(uint faceIndex) const { + assert(faceIndex < 2); + return mFaces[faceIndex]; +} + +// Return the number of vertices of the polyhedron +inline uint TriangleShape::getNbVertices() const { + return 3; +} + +// Return a given vertex of the polyhedron +inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const { + assert(vertexIndex < 3); + + HalfEdgeStructure::Vertex vertex(vertexIndex); + switch (vertexIndex) { + case 0: vertex.edgeIndex = 0; break; + case 1: vertex.edgeIndex = 2; break; + case 2: vertex.edgeIndex = 4; break; + } + return vertex; +} + +// Return a given half-edge of the polyhedron +inline const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const { + assert(edgeIndex < getNbHalfEdges()); + return mEdges[edgeIndex]; +} + +// Return the position of a given vertex +inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const { + assert(vertexIndex < 3); + return mPoints[vertexIndex]; +} + +// Return the normal vector of a given face of the polyhedron +inline Vector3 TriangleShape::getFaceNormal(uint faceIndex) const { + assert(faceIndex < 2); + return faceIndex == 0 ? mNormal : -mNormal; +} + +// Return the centroid of the box +inline Vector3 TriangleShape::getCentroid() const { + return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0); +} + +// Return the number of half-edges of the polyhedron +inline uint TriangleShape::getNbHalfEdges() const { + return 6; +} + // Return the raycast test type (front, back, front-back) inline TriangleRaycastSide TriangleShape::getRaycastTestType() const { return mRaycastTestType; @@ -212,20 +325,6 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { mRaycastTestType = testType; } -// Return the coordinates of a given vertex of the triangle -/** - * @param index Index (0 to 2) of a vertex of the triangle - */ -inline Vector3 TriangleShape::getVertex(int index) const { - assert(index >= 0 && index < 3); - return mPoints[index]; -} - -// Return true if the collision shape is a polyhedron -inline bool TriangleShape::isPolyhedron() const { - return true; -} - } #endif diff --git a/src/configuration.h b/src/configuration.h index 04cb0956..96391e76 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -48,13 +48,18 @@ namespace reactphysics3d { // ------------------- Type definitions ------------------- // using uint = unsigned int; +using uchar = unsigned char; +using ushort = unsigned short; using luint = long unsigned int; using bodyindex = luint; using bodyindexpair = std::pair; -using int8 = int8_t; -using int16 = int16_t; -using int32 = int32_t; +using int8 = std::int8_t; +using uint8 = std::uint8_t; +using int16 = std::int16_t; +using uint16 = std::uint16_t; +using int32 = std::int32_t; +using uint32 = std::uint32_t; // ------------------- Enumerations ------------------- // @@ -97,11 +102,8 @@ constexpr decimal DEFAULT_BOUNCINESS = decimal(0.5); /// Default rolling resistance constexpr decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0); -/// True if the spleeping technique is enabled -constexpr bool SPLEEPING_ENABLED = true; - -/// Object margin for collision detection in meters (for the GJK-EPA Algorithm) -constexpr decimal OBJECT_MARGIN = decimal(0.04); +/// True if the sleeping technique is enabled +constexpr bool SLEEPING_ENABLED = true; /// Distance threshold for two contact points for a valid persistent contact (in meters) constexpr decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); @@ -144,8 +146,13 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; /// least one concave collision shape. constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; +/// This is used to test if two contact manifold are similar (same contact normal) in order to +/// merge them. If the cosine of the angle between the normals of the two manifold are larger +/// than the value bellow, the manifold are considered to be similar. +constexpr decimal COS_ANGLE_SIMILAR_CONTACT_MANIFOLD = decimal(0.95); + /// Size (in bytes) of the single frame allocator -constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb +constexpr size_t INIT_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576; // 1Mb } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 7a5fce1b..272d922e 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -45,8 +45,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies center of mass and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 00ed60d7..37dc34a3 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -31,13 +31,30 @@ using namespace reactphysics3d; using namespace std; // Constructor -ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) - : mNormal(contactInfo.normal), - mPenetrationDepth(contactInfo.penetrationDepth), - mLocalPointOnBody1(contactInfo.localPoint1), - mLocalPointOnBody2(contactInfo.localPoint2), - mIsRestingContact(false) { +ContactPoint::ContactPoint(const ContactPointInfo* contactInfo) + : mNormal(contactInfo->normal), + mPenetrationDepth(contactInfo->penetrationDepth), + mLocalPointOnShape1(contactInfo->localPoint1), + mLocalPointOnShape2(contactInfo->localPoint2), + mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) { assert(mPenetrationDepth > decimal(0.0)); + assert(mNormal.lengthSquare() > decimal(0.8)); + mIsObsolete = false; +} + +// Update the contact point with a new one that is similar (very close) +/// The idea is to keep the cache impulse (for warm starting the contact solver) +void ContactPoint::update(const ContactPointInfo* contactInfo) { + + assert(isSimilarWithContactPoint(contactInfo)); + assert(contactInfo->penetrationDepth > decimal(0.0)); + + mNormal = contactInfo->normal; + mPenetrationDepth = contactInfo->penetrationDepth; + mLocalPointOnShape1 = contactInfo->localPoint1; + mLocalPointOnShape2 = contactInfo->localPoint2; + + mIsObsolete = false; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 4028ed8f..d78659f8 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -28,60 +28,14 @@ // Libraries #include "body/CollisionBody.h" -#include "collision/NarrowPhaseInfo.h" +#include "collision/ContactPointInfo.h" #include "configuration.h" #include "mathematics/mathematics.h" -#include "configuration.h" /// ReactPhysics3D namespace namespace reactphysics3d { -// Structure ContactPointInfo -/** - * This structure contains informations about a collision contact - * computed during the narrow-phase collision detection. Those - * informations are used to compute the contact set for a contact - * between two bodies. - */ -struct ContactPointInfo { - - private: - - // -------------------- Methods -------------------- // - - public: - - // -------------------- Attributes -------------------- // - - /// Normalized normal vector of the collision contact in world space - Vector3 normal; - - /// Penetration depth of the contact - decimal penetrationDepth; - - /// Contact point of body 1 in local space of body 1 - Vector3 localPoint1; - - /// Contact point of body 2 in local space of body 2 - Vector3 localPoint2; - - // -------------------- Methods -------------------- // - - /// Constructor - ContactPointInfo() = default; - - /// Destructor - ~ContactPointInfo() = default; - - /// Initialize the contact point info - void init(const Vector3& contactNormal, decimal penDepth, - const Vector3& localPt1, const Vector3& localPt2) { - normal = contactNormal; - penetrationDepth = penDepth; - localPoint1 = localPt1; - localPoint2 = localPt2; - } -}; +struct NarrowPhaseInfo; // Class ContactPoint /** @@ -95,22 +49,16 @@ class ContactPoint { // -------------------- Attributes -------------------- // /// Normalized normal vector of the contact (from body1 toward body2) in world space - const Vector3 mNormal; + Vector3 mNormal; /// Penetration depth decimal mPenetrationDepth; - /// Contact point on body 1 in local space of body 1 - const Vector3 mLocalPointOnBody1; + /// Contact point on proxy shape 1 in local-space of proxy shape 1 + Vector3 mLocalPointOnShape1; - /// Contact point on body 2 in local space of body 2 - const Vector3 mLocalPointOnBody2; - - /// Contact point on body 1 in world space - Vector3 mWorldPointOnBody1; - - /// Contact point on body 2 in world space - Vector3 mWorldPointOnBody2; + /// Contact point on proxy shape 2 in local-space of proxy shape 2 + Vector3 mLocalPointOnShape2; /// True if the contact is a resting contact (exists for more than one time step) bool mIsRestingContact; @@ -118,12 +66,48 @@ class ContactPoint { /// Cached penetration impulse decimal mPenetrationImpulse; + /// True if the contact point is obsolete + bool mIsObsolete; + + /// Pointer to the next contact point in the double linked-list + ContactPoint* mNext; + + /// Pointer to the previous contact point in the double linked-list + ContactPoint* mPrevious; + + // -------------------- Methods -------------------- // + + /// Update the contact point with a new one that is similar (very close) + void update(const ContactPointInfo* contactInfo); + + /// Return true if the contact point is similar (close enougth) to another given contact point + bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const; + + /// Set the cached penetration impulse + void setPenetrationImpulse(decimal impulse); + + + /// Set the mIsRestingContact variable + void setIsRestingContact(bool isRestingContact); + + /// Set to true to make the contact point obsolete + void setIsObsolete(bool isObselete); + + /// Set the next contact point in the linked list + void setNext(ContactPoint* next); + + /// Set the previous contact point in the linked list + void setPrevious(ContactPoint* previous); + + /// Return true if the contact point is obsolete + bool getIsObsolete() const; + public : // -------------------- Methods -------------------- // /// Constructor - ContactPoint(const ContactPointInfo& contactInfo); + ContactPoint(const ContactPointInfo* contactInfo); /// Destructor ~ContactPoint() = default; @@ -134,80 +118,52 @@ class ContactPoint { /// Deleted assignment operator ContactPoint& operator=(const ContactPoint& contact) = delete; - /// Update the world contact points - void updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform); - - /// Update the penetration depth - void updatePenetrationDepth(); - /// Return the normal vector of the contact Vector3 getNormal() const; - /// Return the contact local point on body 1 - Vector3 getLocalPointOnBody1() const; + /// Return the contact point on the first proxy shape in the local-space of the proxy shape + const Vector3& getLocalPointOnShape1() const; - /// Return the contact local point on body 2 - Vector3 getLocalPointOnBody2() const; - - /// Return the contact world point on body 1 - Vector3 getWorldPointOnBody1() const; - - /// Return the contact world point on body 2 - Vector3 getWorldPointOnBody2() const; + /// Return the contact point on the second proxy shape in the local-space of the proxy shape + const Vector3& getLocalPointOnShape2() const; /// Return the cached penetration impulse decimal getPenetrationImpulse() const; - /// Set the cached penetration impulse - void setPenetrationImpulse(decimal impulse); - /// Return true if the contact is a resting contact bool getIsRestingContact() const; - /// Set the mIsRestingContact variable - void setIsRestingContact(bool isRestingContact); + /// Return the previous contact point in the linked list + inline ContactPoint* getPrevious() const; + + /// Return the next contact point in the linked list + ContactPoint* getNext() const; /// Return the penetration depth decimal getPenetrationDepth() const; /// Return the number of bytes used by the contact point size_t getSizeInBytes() const; + + // Friendship + friend class ContactManifold; + friend class ContactManifoldSet; + friend class ContactSolver; }; -// Update the world contact points -inline void ContactPoint::updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform) { - mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1; - mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2; -} - -// Update the penetration depth -inline void ContactPoint::updatePenetrationDepth() { - mPenetrationDepth = (mWorldPointOnBody1 - mWorldPointOnBody2).dot(mNormal); -} - // Return the normal vector of the contact inline Vector3 ContactPoint::getNormal() const { return mNormal; } -// Return the contact point on body 1 -inline Vector3 ContactPoint::getLocalPointOnBody1() const { - return mLocalPointOnBody1; +// Return the contact point on the first proxy shape in the local-space of the proxy shape +inline const Vector3& ContactPoint::getLocalPointOnShape1() const { + return mLocalPointOnShape1; } -// Return the contact point on body 2 -inline Vector3 ContactPoint::getLocalPointOnBody2() const { - return mLocalPointOnBody2; -} - -// Return the contact world point on body 1 -inline Vector3 ContactPoint::getWorldPointOnBody1() const { - return mWorldPointOnBody1; -} - -// Return the contact world point on body 2 -inline Vector3 ContactPoint::getWorldPointOnBody2() const { - return mWorldPointOnBody2; +// Return the contact point on the second proxy shape in the local-space of the proxy shape +inline const Vector3& ContactPoint::getLocalPointOnShape2() const { + return mLocalPointOnShape2; } // Return the cached penetration impulse @@ -215,6 +171,12 @@ inline decimal ContactPoint::getPenetrationImpulse() const { return mPenetrationImpulse; } +// Return true if the contact point is similar (close enougth) to another given contact point +inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const { + return (localContactPointBody1->localPoint1 - mLocalPointOnShape1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD * + PERSISTENT_CONTACT_DIST_THRESHOLD); +} + // Set the cached penetration impulse inline void ContactPoint::setPenetrationImpulse(decimal impulse) { mPenetrationImpulse = impulse; @@ -230,6 +192,36 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } +// Return true if the contact point is obsolete +inline bool ContactPoint::getIsObsolete() const { + return mIsObsolete; +} + +// Set to true to make the contact point obsolete +inline void ContactPoint::setIsObsolete(bool isObsolete) { + mIsObsolete = isObsolete; +} + +// Return the next contact point in the linked list +inline ContactPoint* ContactPoint::getNext() const { + return mNext; +} + +// Set the next contact point in the linked list +inline void ContactPoint::setNext(ContactPoint* next) { + mNext = next; +} + +// Return the previous contact point in the linked list +inline ContactPoint* ContactPoint::getPrevious() const { + return mPrevious; +} + +// Set the previous contact point in the linked list +inline void ContactPoint::setPrevious(ContactPoint* previous) { + mPrevious = previous; +} + // Return the penetration depth of the contact inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 8f0b105c..01fe3957 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -53,8 +53,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 1634bf27..7b400a2a 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -68,8 +68,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 919d91c0..7697013d 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -67,8 +67,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the veloc ity array - mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; - mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + mIndexBody1 = mBody1->mArrayIndex; + mIndexBody2 = mBody2->mArrayIndex; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; diff --git a/src/containers/LinkedList.h b/src/containers/LinkedList.h index 281c134c..997035b7 100644 --- a/src/containers/LinkedList.h +++ b/src/containers/LinkedList.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_LINKED_LIST_H // Libraries -#include "memory/Allocator.h" +#include "memory/MemoryAllocator.h" namespace reactphysics3d { @@ -64,14 +64,14 @@ class LinkedList { ListElement* mListHead; /// Memory allocator used to allocate the list elements - Allocator& mAllocator; + MemoryAllocator& mAllocator; public: // -------------------- Methods -------------------- // /// Constructor - LinkedList(Allocator& allocator) : mListHead(nullptr), mAllocator(allocator) { + LinkedList(MemoryAllocator& allocator) : mListHead(nullptr), mAllocator(allocator) { } diff --git a/src/containers/List.h b/src/containers/List.h new file mode 100644 index 00000000..6391d52b --- /dev/null +++ b/src/containers/List.h @@ -0,0 +1,341 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_LIST_H +#define REACTPHYSICS3D_LIST_H + +// Libraries +#include "configuration.h" +#include "memory/MemoryAllocator.h" +#include +#include + +namespace reactphysics3d { + +// Class List +/** + * This class represents a simple generic list with custom memory allocator. + */ +template +class List { + + private: + + // -------------------- Attributes -------------------- // + + /// Buffer for the list elements + void* mBuffer; + + /// Number of elements in the list + size_t mSize; + + /// Number of allocated elements in the list + size_t mCapacity; + + /// Memory allocator + MemoryAllocator& mAllocator; + + // -------------------- Methods -------------------- // + + + public: + + /// Class Iterator + /** + * This class represents an iterator for the List + */ + class Iterator { + + private: + + size_t mCurrentIndex; + T* mBuffer; + size_t mSize; + + public: + + // Iterator traits + using value_type = T; + using difference_type = std::ptrdiff_t; + using pointer = T*; + using reference = T&; + using iterator_category = std::bidirectional_iterator_tag; + + /// Constructor + Iterator() = default; + + /// Constructor + Iterator(T* buffer, size_t index, size_t size) + :mCurrentIndex(index), mBuffer(buffer), mSize(size) { + + } + + /// Copy constructor + Iterator(const Iterator& it) + :mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.size) { + + } + + /// Deferencable + reference operator*() const { + assert(mCurrentIndex >= 0 && mCurrentIndex < mSize); + return mBuffer[mCurrentIndex]; + } + + /// Deferencable + pointer operator->() const { + assert(mCurrentIndex >= 0 && mCurrentIndex < mSize); + return &(mBuffer[mCurrentIndex]); + } + + /// Post increment (it++) + Iterator& operator++() { + assert(mCurrentIndex < mSize - 1); + mCurrentIndex++; + return *this; + } + + /// Pre increment (++it) + Iterator operator++(int number) { + assert(mCurrentIndex < mSize - 1); + Iterator tmp = *this; + mCurrentIndex++; + return tmp; + } + + /// Post decrement (it--) + Iterator& operator--() { + assert(mCurrentIndex > 0); + mCurrentIndex--; + return *this; + } + + /// Pre decrement (--it) + Iterator operator--(int number) { + assert(mCurrentIndex > 0); + Iterator tmp = *this; + mCurrentIndex--; + return tmp; + } + + /// Equality operator (it == end()) + bool operator==(const Iterator& iterator) const { + assert(mCurrentIndex >= 0 && mCurrentIndex <= mSize); + + // If both iterators points to the end of the list + if (mCurrentIndex == mSize && iterator.mCurrentIndex == iterator.mSize) { + return true; + } + + return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[mCurrentIndex]); + } + + /// Inequality operator (it != end()) + bool operator!=(const Iterator& iterator) const { + return !(*this == iterator); + } + }; + + // -------------------- Methods -------------------- // + + /// Constructor + List(MemoryAllocator& allocator, size_t capacity = 0) + : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) { + + if (capacity > 0) { + + // Allocate memory + reserve(capacity); + } + } + + /// Copy constructor + List(const List& list) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) { + + // All all the elements of the list to the current one + addRange(list); + } + + /// Destructor + ~List() { + + // If elements have been allocated + if (mCapacity > 0) { + + // Clear the list + clear(); + + // Release the memory allocated on the heap + mAllocator.release(mBuffer, mCapacity * sizeof(T)); + } + } + + /// Allocate memory for a given number of elements + void reserve(size_t capacity) { + + if (capacity <= mCapacity) return; + + // Allocate memory for the new array + void* newMemory = mAllocator.allocate(capacity * sizeof(T)); + + if (mBuffer != nullptr) { + + // Copy the elements to the new allocated memory location + std::memcpy(newMemory, mBuffer, mSize * sizeof(T)); + + // Release the previously allocated memory + mAllocator.release(mBuffer, mCapacity * sizeof(T)); + } + + mBuffer = newMemory; + assert(mBuffer != nullptr); + + mCapacity = capacity; + } + + /// Add an element into the list + void add(const T& element) { + + // If we need to allocate more memory + if (mSize == mCapacity) { + reserve(mCapacity == 0 ? 1 : mCapacity * 2); + } + + // Use the copy-constructor to construct the element + new (static_cast(mBuffer) + mSize * sizeof(T)) T(element); + + mSize++; + } + + /// Remove an element from the list at a given index + void remove(uint index) { + + assert(index >= 0 && index < mSize); + + // Call the destructor + (static_cast(mBuffer)[index]).~T(); + + mSize--; + + if (index != mSize) { + + // Move the elements to fill in the empty slot + char* dest = static_cast(mBuffer) + index * sizeof(T); + char* src = dest + sizeof(T); + std::memcpy(static_cast(dest), static_cast(src), (mSize - index) * sizeof(T)); + } + } + + /// Append another list at the end of the current one + void addRange(const List& list) { + + // If we need to allocate more memory + if (mSize + list.size() > mCapacity) { + + // Allocate memory + reserve(mSize + list.size()); + } + + // Add the elements of the list to the current one + for(uint i=0; i(mBuffer) + mSize * sizeof(T)) T(list[i]); + mSize++; + } + } + + /// Clear the list + void clear() { + + // Call the destructor of each element + for (uint i=0; i < mSize; i++) { + (static_cast(mBuffer)[i]).~T(); + } + + mSize = 0; + } + + /// Return the number of elements in the list + size_t size() const { + return mSize; + } + + /// Return the capacity of the list + size_t capacity() const { + return mCapacity; + } + + /// Overloaded index operator + T& operator[](const uint index) { + assert(index >= 0 && index < mSize); + return (static_cast(mBuffer)[index]); + } + + /// Overloaded const index operator + const T& operator[](const uint index) const { + assert(index >= 0 && index < mSize); + return (static_cast(mBuffer)[index]); + } + + /// Overloaded equality operator + bool operator==(const List& list) const { + + if (mSize != list.mSize) return false; + + T* items = static_cast(mBuffer); + for (int i=0; i < mSize; i++) { + if (items[i] != list[i]) { + return false; + } + } + + return true; + } + + /// Overloaded not equal operator + bool operator!=(const List& list) const { + + return !((*this) == list); + } + + /// Overloaded assignment operator + List& operator=(const List& list) { + + if (this != &list) { + + // Clear all the elements + clear(); + + // Add all the elements of the list to the current one + addRange(list); + } + + return *this; + } +}; + +} + +#endif diff --git a/src/containers/Map.h b/src/containers/Map.h new file mode 100644 index 00000000..5510b98d --- /dev/null +++ b/src/containers/Map.h @@ -0,0 +1,532 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MAP_H +#define REACTPHYSICS3D_MAP_H + +// Libraries +#include "memory/MemoryAllocator.h" +#include "mathematics/mathematics_functions.h" +#include +#include + +namespace reactphysics3d { + +// Class Map +/** + * This class represents a simple generic associative map + */ +template +class Map { + + private: + + /// An entry of the map + struct Entry { + + size_t hashCode; // Hash code of the entry + int next; // Index of the next entry + std::pair* keyValue; // Pointer to the pair with key and value + + /// Constructor + Entry() { + next = -1; + keyValue = nullptr; + } + + /// Destructor + ~Entry() { + + assert(keyValue == nullptr); + } + + }; + + // -------------------- Constants -------------------- // + + /// Number of prime numbers in array + static constexpr int NB_PRIMES = 70; + + /// Array of prime numbers for the size of the map + static const int PRIMES[NB_PRIMES]; + + /// Largest prime number + static int LARGEST_PRIME; + + // -------------------- Attributes -------------------- // + + /// Current number of used entries in the map + int mNbUsedEntries; + + /// Number of free entries among the used ones + int mNbFreeEntries; + + /// Current capacity of the map + int mCapacity; + + /// Array with all the buckets + int* mBuckets; + + /// Array with all the entries + Entry* mEntries; + + /// Memory allocator + MemoryAllocator& mAllocator; + + /// Index to the fist free entry + int mFreeIndex; + + // -------------------- Methods -------------------- // + + /// Initialize the map + void initialize(int capacity) { + + // Compute the next larger prime size + mCapacity = getPrimeSize(capacity); + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Initialize the buckets and entries + for (int i=0; i mCapacity); + assert(isPrimeNumber(newCapacity)); + + // Allocate memory for the buckets + int* newBuckets = static_cast(mAllocator.allocate(newCapacity * sizeof(int))); + + // Allocate memory for the entries + Entry* newEntries = static_cast(mAllocator.allocate(newCapacity * sizeof(Entry))); + + // Initialize the new buckets + for (int i=0; i(&newEntries[i])) Entry(); + } + + // For each used entry + for (int i=0; i 0) { + + size_t hashCode = std::hash()(key); + int bucket = hashCode % mCapacity; + + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == key) { + return i; + } + } + } + + return -1; + } + + /// Return the prime number that is larger or equal to the number in parameter + /// for the size of the map + static int getPrimeSize(int number) { + + // Check if the next larger prime number is in the precomputed array of primes + for (int i = 0; i < NB_PRIMES; i++) { + if (PRIMES[i] >= number) return PRIMES[i]; + } + + // Manually compute the next larger prime number + for (int i = (number | 1); i < std::numeric_limits::max(); i+=2) { + + if (isPrimeNumber(i)) { + return i; + } + } + + return number; + } + + /// Clear and reset the map + void reset() { + + // If elements have been allocated + if (mCapacity > 0) { + + // Clear the list + clear(); + + // Destroy the entries + for (int i=0; i < mCapacity; i++) { + mEntries[i].~Entry(); + } + + mAllocator.release(mBuckets, mCapacity * sizeof(int)); + mAllocator.release(mEntries, mCapacity * sizeof(Entry)); + + mNbUsedEntries = 0; + mNbFreeEntries = 0; + mCapacity = 0; + mBuckets = nullptr; + mEntries = nullptr; + mFreeIndex = -1; + } + } + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + Map(MemoryAllocator& allocator, size_t capacity = 0) + : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr), + mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) { + + // If the largest prime has not been computed yet + if (LARGEST_PRIME == -1) { + + // Compute the largest prime number (largest map capacity) + LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2); + } + + if (capacity > 0) { + + initialize(capacity); + } + } + + /// Copy constructor + Map(const Map& map) + :mNbUsedEntries(map.mNbUsedEntries), mNbFreeEntries(map.mNbFreeEntries), mCapacity(map.mCapacity), + mAllocator(map.mAllocator), mFreeIndex(map.mFreeIndex) { + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Copy the buckets + std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int)); + + // Copy the entries + std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry)); + } + + /// Destructor + ~Map() { + + reset(); + } + + /// Allocate memory for a given number of elements + void reserve(size_t capacity) { + + if (capacity <= mCapacity) return; + + if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) { + capacity = LARGEST_PRIME; + } + else { + capacity = getPrimeSize(capacity); + } + + expand(capacity); + } + + /// Return true if the map contains an item with the given key + bool containsKey(const K& key) const { + return findEntry(key) != -1; + } + + /// Add an element into the map + void add(const std::pair& keyValue) { + + if (mCapacity == 0) { + initialize(0); + } + + // Compute the hash code of the key + size_t hashCode = std::hash()(keyValue.first); + + // Compute the corresponding bucket index + int bucket = hashCode % mCapacity; + + // Check if the item is already in the map + for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { + + // If there is already an item with the same key in the map + if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == keyValue.first) { + + throw std::runtime_error("The key and value pair already exists in the map"); + } + } + + size_t entryIndex; + + // If there are free entries to use + if (mNbFreeEntries > 0) { + assert(mFreeIndex >= 0); + entryIndex = mFreeIndex; + mFreeIndex = mEntries[entryIndex].next; + mNbFreeEntries--; + } + else { + + // If we need to allocator more entries + if (mNbUsedEntries == mCapacity) { + + // Allocate more memory + reserve(mCapacity * 2); + + // Recompute the bucket index + bucket = hashCode % mCapacity; + } + + entryIndex = mNbUsedEntries; + mNbUsedEntries++; + } + + assert(mEntries[entryIndex].keyValue == nullptr); + mEntries[entryIndex].hashCode = hashCode; + mEntries[entryIndex].next = mBuckets[bucket]; + mEntries[entryIndex].keyValue = static_cast*>(mAllocator.allocate(sizeof(std::pair))); + assert(mEntries[entryIndex].keyValue != nullptr); + new (mEntries[entryIndex].keyValue) std::pair(keyValue); + mBuckets[bucket] = entryIndex; + } + + /// Remove the element from the map with a given key + bool remove(const K& key) { + + if (mCapacity > 0) { + + size_t hashcode = std::hash()(key); + int bucket = hashcode % mCapacity; + int last = -1; + for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) { + + if (mEntries[i].hashCode == hashcode && mEntries[i].keyValue->first == key) { + + if (last < 0 ) { + mBuckets[bucket] = mEntries[i].next; + } + else { + mEntries[last].next = mEntries[i].next; + } + + // Release memory for the key/value pair if any + if (mEntries[i].keyValue != nullptr) { + mEntries[i].keyValue->~pair(); + mAllocator.release(mEntries[i].keyValue, sizeof(std::pair)); + mEntries[i].keyValue = nullptr; + } + mEntries[i].hashCode = -1; + mEntries[i].next = mFreeIndex; + mFreeIndex = i; + mNbFreeEntries++; + + return true; + } + } + } + + return false; + } + + /// Clear the list + void clear() { + + if (mNbUsedEntries > 0) { + + for (int i=0; i < mCapacity; i++) { + mBuckets[i] = -1; + mEntries[i].next = -1; + if (mEntries[i].keyValue != nullptr) { + mEntries[i].keyValue->~pair(); + mAllocator.release(mEntries[i].keyValue, sizeof(std::pair)); + mEntries[i].keyValue = nullptr; + } + } + + mFreeIndex = -1; + mNbUsedEntries = 0; + mNbFreeEntries = 0; + } + + assert(size() == 0); + } + + /// Return the number of elements in the map + int size() const { + return mNbUsedEntries - mNbFreeEntries; + } + + /// Return the capacity of the map + int capacity() const { + return mCapacity; + } + + /// Overloaded index operator + V& operator[](const K& key) { + + int entry = -1; + + if (mCapacity > 0) { + entry = findEntry(key); + } + + if (entry == -1) { + throw std::runtime_error("No item with given key has been found in the map"); + } + + assert(mEntries[entry].keyValue != nullptr); + + return mEntries[entry].keyValue->second; + } + + /// Overloaded index operator + const V& operator[](const K& key) const { + + int entry = -1; + + if (mCapacity > 0) { + entry = findEntry(key); + } + + if (entry == -1) { + throw std::runtime_error("No item with given key has been found in the map"); + } + + return mEntries[entry]; + } + + /// Overloaded equality operator + bool operator==(const Map& map) const { + + // TODO : Implement this + return false; + } + + /// Overloaded not equal operator + bool operator!=(const Map& map) const { + + return !((*this) == map); + } + + /// Overloaded assignment operator + Map& operator=(const Map& map) { + + // Check for self assignment + if (this != &map) { + + // Reset the map + reset(); + + if (map.mCapacity > 0) { + + // Compute the next larger prime size + mCapacity = getPrimeSize(map.mCapacity); + + // Allocate memory for the buckets + mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); + + // Allocate memory for the entries + mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); + + // Copy the buckets + std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int)); + + // Copy the entries + std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry)); + + mNbUsedEntries = map.mNbUsedEntries; + mNbFreeEntries = map.mNbFreeEntries; + mFreeIndex = map.mFreeIndex; + } + } + + return *this; + } +}; + +template +const int Map::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919, + 1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591, + 17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437, + 187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263, + 1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559}; + +template +int Map::LARGEST_PRIME = -1; + +} + +#endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index d160021c..da5bf609 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -33,10 +33,16 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES), - mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0), + : mCollisionDetection(this, mMemoryManager), mCurrentBodyID(0), mEventListener(nullptr) { +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + mCollisionDetection.setProfiler(&mProfiler); + +#endif + } // Destructor @@ -67,7 +73,8 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the collision body - CollisionBody* collisionBody = new (mPoolAllocator.allocate(sizeof(CollisionBody))) + CollisionBody* collisionBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(CollisionBody))) CollisionBody(transform, *this, bodyID); assert(collisionBody != nullptr); @@ -75,6 +82,12 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { // Add the collision body to the world mBodies.insert(collisionBody); +#ifdef IS_PROFILING_ACTIVE + + collisionBody->setProfiler(&mProfiler); + +#endif + // Return the pointer to the rigid body return collisionBody; } @@ -98,7 +111,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { mBodies.erase(collisionBody); // Free the object from the memory allocator - mPoolAllocator.release(collisionBody, sizeof(CollisionBody)); + mMemoryManager.release(MemoryManager::AllocationType::Pool, collisionBody, sizeof(CollisionBody)); } // Return the next available body ID @@ -164,3 +177,13 @@ bool CollisionWorld::testOverlap(CollisionBody* body1, CollisionBody* body2) { return mCollisionDetection.testOverlap(body1, body2); } + +// Return the current world-space AABB of given proxy shape +AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const { + + if (proxyShape->mBroadPhaseID == -1) { + return AABB(); + } + + return mCollisionDetection.getWorldAABB(proxyShape); +} diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index dc162716..9f1fea58 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -39,7 +39,7 @@ #include "collision/CollisionDetection.h" #include "constraint/Joint.h" #include "constraint/ContactPoint.h" -#include "memory/PoolAllocator.h" +#include "memory/MemoryManager.h" #include "EventListener.h" /// Namespace reactphysics3d @@ -61,11 +61,8 @@ class CollisionWorld { // -------------------- Attributes -------------------- // - /// Pool Memory allocator - PoolAllocator mPoolAllocator; - - /// Single frame Memory allocator - SingleFrameAllocator mSingleFrameAllocator; + /// Memory manager + MemoryManager mMemoryManager; /// Reference to the collision detection CollisionDetection mCollisionDetection; @@ -82,6 +79,12 @@ class CollisionWorld { /// Pointer to an event listener object EventListener* mEventListener; +#ifdef IS_PROFILING_ACTIVE + + /// Real-time hierarchical profiler + Profiler mProfiler; +#endif + // -------------------- Methods -------------------- // /// Return the next available body ID @@ -147,6 +150,15 @@ class CollisionWorld { /// Test and report collisions between all shapes of the world void testCollision(CollisionCallback* callback); +#ifdef IS_PROFILING_ACTIVE + + /// Set the name of the profiler + void setProfilerName(std::string name); +#endif + + /// Return the current world-space AABB of given proxy shape + AABB getWorldAABB(const ProxyShape* proxyShape) const; + // -------------------- Friendship -------------------- // friend class CollisionDetection; @@ -233,64 +245,15 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits); } -// Class CollisionCallback -/** - * This class can be used to register a callback for collision test queries. - * You should implement your own class inherited from this one and implement - * the notifyContact() method. This method will called each time a contact - * point is reported. - */ -class CollisionCallback { +#ifdef IS_PROFILING_ACTIVE - public: +// Set the name of the profiler +inline void CollisionWorld::setProfilerName(std::string name) { + mProfiler.setName(name); +} - struct CollisionCallbackInfo { - - public: - const ContactPointInfo& contactPoint; - CollisionBody* body1; - CollisionBody* body2; - const ProxyShape* proxyShape1; - const ProxyShape* proxyShape2; - - // Constructor - CollisionCallbackInfo(const ContactPointInfo& point, CollisionBody* b1, CollisionBody* b2, - const ProxyShape* proxShape1, const ProxyShape* proxShape2) : - contactPoint(point), body1(b1), body2(b2), - proxyShape1(proxShape1), proxyShape2(proxShape2) { - - } - }; - - /// Destructor - virtual ~CollisionCallback() { - - } - - /// This method will be called for each reported contact point - virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0; -}; - -// Class OverlapCallback -/** - * This class can be used to register a callback for collision overlap queries. - * You should implement your own class inherited from this one and implement - * the notifyOverlap() method. This method will called each time a contact - * point is reported. - */ -class OverlapCallback { - - public: - - /// Destructor - virtual ~OverlapCallback() { - - } - - /// This method will be called for each reported overlapping bodies - virtual void notifyOverlap(CollisionBody* collisionBody)=0; -}; +#endif } - #endif +#endif diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 547aea6e..5345cbaf 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -30,16 +30,20 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVelocityIndex) - : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) { +ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) { + +#ifdef IS_PROFILING_ACTIVE + + mProfiler = nullptr; + +#endif } // Initialize the constraint solver for a given island void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { - PROFILE("ConstraintSolver::initializeForIsland()"); + PROFILE("ConstraintSolver::initializeForIsland()", mProfiler); assert(island != nullptr); assert(island->getNbBodies() > 0); @@ -69,7 +73,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { // Solve the velocity constraints void ConstraintSolver::solveVelocityConstraints(Island* island) { - PROFILE("ConstraintSolver::solveVelocityConstraints()"); + PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler); assert(island != nullptr); assert(island->getNbJoints() > 0); @@ -86,7 +90,7 @@ void ConstraintSolver::solveVelocityConstraints(Island* island) { // Solve the position constraints void ConstraintSolver::solvePositionConstraints(Island* island) { - PROFILE("ConstraintSolver::solvePositionConstraints()"); + PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler); assert(island != nullptr); assert(island->getNbJoints() > 0); diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index b40cc983..105045e0 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -60,18 +60,12 @@ struct ConstraintSolverData { /// Reference to the bodies orientations Quaternion* orientations; - /// Reference to the map that associates rigid body to their index - /// in the constrained velocities array - const std::map& mapBodyToConstrainedVelocityIndex; - /// True if warm starting of the solver is active bool isWarmStartingActive; /// Constructor - ConstraintSolverData(const std::map& refMapBodyToConstrainedVelocityIndex) - :linearVelocities(nullptr), angularVelocities(nullptr), - positions(nullptr), orientations(nullptr), - mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ + ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr), + positions(nullptr), orientations(nullptr) { } @@ -152,10 +146,6 @@ class ConstraintSolver { // -------------------- Attributes -------------------- // - /// Reference to the map that associates rigid body to their index in - /// the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; - /// Current time step decimal mTimeStep; @@ -165,12 +155,18 @@ class ConstraintSolver { /// Constraint solver data used to initialize and solve the constraints ConstraintSolverData mConstraintSolverData; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; +#endif + public : // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(const std::map& mapBodyToVelocityIndex); + ConstraintSolver(); /// Destructor ~ConstraintSolver() = default; @@ -197,6 +193,14 @@ class ConstraintSolver { /// Set the constrained positions/orientations arrays void setConstrainedPositionsArrays(Vector3* constrainedPositions, Quaternion* constrainedOrientations); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + }; // Set the constrained velocities arrays @@ -221,6 +225,15 @@ inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrained mConstraintSolverData.orientations = constrainedOrientations; } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ConstraintSolver::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d5825d5c..f4762c49 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -39,12 +39,10 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, - SingleFrameAllocator& allocator) - :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), - mContactConstraints(nullptr), mSingleFrameAllocator(allocator), +ContactSolver::ContactSolver(MemoryManager& memoryManager) + :mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr), + mSplitAngularVelocities(nullptr), mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), - mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsSplitImpulseActive(true) { } @@ -52,7 +50,7 @@ ContactSolver::ContactSolver(const std::map& mapBodyToVelocity // Initialize the contact constraints void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { - PROFILE("ContactSolver::init()"); + PROFILE("ContactSolver::init()", mProfiler); mTimeStep = timeStep; @@ -77,10 +75,12 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { if (nbContactManifolds == 0 || nbContactPoints == 0) return; // TODO : Count exactly the number of constraints to allocate here - mContactPoints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints)); + mContactPoints = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(ContactPointSolver) * nbContactPoints)); assert(mContactPoints != nullptr); - mContactConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds)); + mContactConstraints = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(ContactManifoldSolver) * nbContactManifolds)); assert(mContactConstraints != nullptr); // For each island of the world @@ -98,7 +98,7 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { // Initialize the constraint solver for a given island void ContactSolver::initializeForIsland(Island* island) { - PROFILE("ContactSolver::initializeForIsland()"); + PROFILE("ContactSolver::initializeForIsland()", mProfiler); assert(island != nullptr); assert(island->getNbBodies() > 0); @@ -120,6 +120,10 @@ void ContactSolver::initializeForIsland(Island* island) { assert(body1 != nullptr); assert(body2 != nullptr); + // Get the two contact shapes + const ProxyShape* shape1 = externalManifold->getShape1(); + const ProxyShape* shape2 = externalManifold->getShape2(); + // Get the position of the two bodies const Vector3& x1 = body1->mCenterOfMassWorld; const Vector3& x2 = body2->mCenterOfMassWorld; @@ -127,8 +131,8 @@ void ContactSolver::initializeForIsland(Island* island) { // Initialize the internal contact manifold structure using the external // contact manifold new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); - mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex; + mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex; mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; @@ -148,34 +152,59 @@ void ContactSolver::initializeForIsland(Island* island) { const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; // For each contact point of the contact manifold - for (uint c=0; cgetNbContactPoints(); c++) { - - // Get a contact point - ContactPoint* externalContact = externalManifold->getContactPoint(c); + ContactPoint* externalContact = externalManifold->getContactPoints(); + assert(externalContact != nullptr); + while (externalContact != nullptr) { // Get the contact point on the two bodies - Vector3 p1 = externalContact->getWorldPointOnBody1(); - Vector3 p2 = externalContact->getWorldPointOnBody2(); + Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnShape1(); + Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnShape2(); new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = externalContact; mContactPoints[mNbContactPoints].normal = externalContact->getNormal(); - mContactPoints[mNbContactPoints].r1 = p1 - x1; - mContactPoints[mNbContactPoints].r2 = p2 - x2; + mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x; + mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y; + mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z; + mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x; + mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y; + mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z; mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth(); mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse(); mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0; - mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1; - mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2; + mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x; + mContactConstraints[mNbContactManifolds].frictionPointBody1.y += p1.y; + mContactConstraints[mNbContactManifolds].frictionPointBody1.z += p1.z; + mContactConstraints[mNbContactManifolds].frictionPointBody2.x += p2.x; + mContactConstraints[mNbContactManifolds].frictionPointBody2.y += p2.y; + mContactConstraints[mNbContactManifolds].frictionPointBody2.z += p2.z; // Compute the velocity difference - Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); + //deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); + Vector3 deltaV(v2.x + w2.y * mContactPoints[mNbContactPoints].r2.z - w2.z * mContactPoints[mNbContactPoints].r2.y + - v1.x - w1.y * mContactPoints[mNbContactPoints].r1.z - w1.z * mContactPoints[mNbContactPoints].r1.y, + v2.y + w2.z * mContactPoints[mNbContactPoints].r2.x - w2.x * mContactPoints[mNbContactPoints].r2.z + - v1.y - w1.z * mContactPoints[mNbContactPoints].r1.x - w1.x * mContactPoints[mNbContactPoints].r1.z, + v2.z + w2.x * mContactPoints[mNbContactPoints].r2.y - w2.y * mContactPoints[mNbContactPoints].r2.x + - v1.z - w1.x * mContactPoints[mNbContactPoints].r1.y - w1.y * mContactPoints[mNbContactPoints].r1.x); - Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); - Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + // r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r1CrossN(mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.z - + mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.y, + mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.x - + mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.z, + mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.y - + mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.x); + // r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r2CrossN(mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.z - + mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.y, + mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.x - + mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.z, + mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.y - + mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.x); mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN; mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN; @@ -191,21 +220,32 @@ void ContactSolver::initializeForIsland(Island* island) { // at the beginning of the contact. Note that if it is a resting contact (normal // velocity bellow a given threshold), we do not add a restitution velocity bias mContactPoints[mNbContactPoints].restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + // deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x + + deltaV.y * mContactPoints[mNbContactPoints].normal.y + + deltaV.z * mContactPoints[mNbContactPoints].normal.z; const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN; } - mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; + mContactConstraints[mNbContactManifolds].normal.x += mContactPoints[mNbContactPoints].normal.x; + mContactConstraints[mNbContactManifolds].normal.y += mContactPoints[mNbContactPoints].normal.y; + mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z; mNbContactPoints++; + + externalContact = externalContact->getNext(); } mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); - mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1; - mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2; + mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x; + mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y; + mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z; + mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x; + mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y; + mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z; mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1(); mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2(); @@ -225,8 +265,20 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - - v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); + // deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - + // v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); + Vector3 deltaVFrictionPoint(v2.x + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.z - + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.y - + v1.x - w1.y * mContactConstraints[mNbContactManifolds].r1Friction.z - + w1.z * mContactConstraints[mNbContactManifolds].r1Friction.y, + v2.y + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.x - + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.z - + v1.y - w1.z * mContactConstraints[mNbContactManifolds].r1Friction.x - + w1.x * mContactConstraints[mNbContactManifolds].r1Friction.z, + v2.z + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.y - + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.x - + v1.z - w1.x * mContactConstraints[mNbContactManifolds].r1Friction.y - + w1.y * mContactConstraints[mNbContactManifolds].r1Friction.x); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]); @@ -265,7 +317,7 @@ void ContactSolver::initializeForIsland(Island* island) { /// the solution of the linear system void ContactSolver::warmStart() { - PROFILE("ContactSolver::warmStart()"); + PROFILE("ContactSolver::warmStart()", mProfiler); uint contactPointIndex = 0; @@ -284,13 +336,25 @@ void ContactSolver::warmStart() { // --------- Penetration --------- // // Update the velocities of the body 1 by applying the impulse P - Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse; - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration; - mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse; + Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse, + mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse, + mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse); + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; + + mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; + + mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; } else { // If it is a new contact point @@ -307,20 +371,27 @@ void ContactSolver::warmStart() { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 + - mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2; + Vector3 oldFrictionImpulse(mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.x + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.x, + mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.y + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.y, + mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.z + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.z); mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1); mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2); // ------ First friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * - mContactConstraints[c].friction1Impulse; - Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * - mContactConstraints[c].friction1Impulse; - Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * - mContactConstraints[c].friction1Impulse; + Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * mContactConstraints[c].friction1Impulse, + -mContactConstraints[c].r1CrossT1.y * mContactConstraints[c].friction1Impulse, + -mContactConstraints[c].r1CrossT1.z * mContactConstraints[c].friction1Impulse); + Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].frictionVector1.y * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].frictionVector1.z * mContactConstraints[c].friction1Impulse); + Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse, + mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse); // Update the velocities of the body 1 by applying the impulse P mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; @@ -333,23 +404,40 @@ void ContactSolver::warmStart() { // ------ Second friction constraint at the center of the contact manifold ----- // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse; - linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse; - angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * mContactConstraints[c].friction2Impulse; + angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * mContactConstraints[c].friction2Impulse; + linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * mContactConstraints[c].friction2Impulse; + angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; - angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.x = -mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.y = -mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody1.z = -mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; + + angularImpulseBody2.x = mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody2.y = mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; @@ -382,7 +470,7 @@ void ContactSolver::warmStart() { // Solve the contacts void ContactSolver::solve() { - PROFILE("ContactSolver::solve()"); + PROFILE("ContactSolver::solve()", mProfiler); decimal deltaLambda; decimal lambdaTemp; @@ -404,8 +492,15 @@ void ContactSolver::solve() { // --------- Penetration --------- // // Compute J*v - Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1); - decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal); + //Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1); + Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x - + w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y, + v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y - + w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z, + v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z - + w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x); + decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y + + deltaV.z * mContactPoints[contactPointIndex].normal.z; decimal Jv = deltaVDotN; // Compute the bias "b" of the constraint @@ -428,15 +523,27 @@ void ContactSolver::solve() { deltaLambda, decimal(0.0)); deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp; - Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda; + Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda, + mContactPoints[contactPointIndex].normal.y * deltaLambda, + mContactPoints[contactPointIndex].normal.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; - mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + + mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + + mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; + mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; @@ -448,9 +555,17 @@ void ContactSolver::solve() { const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1]; const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2]; const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); - decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal); + + //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); + Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x - + w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y, + v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y - + w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z, + v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z - + w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x); + decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x + + deltaVSplit.y * mContactPoints[contactPointIndex].normal.y + + deltaVSplit.z * mContactPoints[contactPointIndex].normal.z; decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * mContactPoints[contactPointIndex].inversePenetrationMass; decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse; @@ -459,15 +574,27 @@ void ContactSolver::solve() { deltaLambdaSplit, decimal(0.0)); deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit; - Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit; + Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit, + mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit, + mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; - mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; + mSplitLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + + mSplitAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; + mSplitLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + + mSplitAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; } contactPointIndex++; @@ -476,9 +603,16 @@ void ContactSolver::solve() { // ------ First friction constraint at the center of the contact manifol ------ // // Compute J*v - Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - - v1 - w1.cross(mContactConstraints[c].r1Friction); - decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1); + // deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + Vector3 deltaV(v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x - + w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y, + v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y - + w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z, + v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z - + w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x); + decimal Jv = deltaV.x * mContactConstraints[c].frictionVector1.x + + deltaV.y * mContactConstraints[c].frictionVector1.y + + deltaV.z * mContactConstraints[c].frictionVector1.z; // Compute the Lagrange multiplier lambda decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass; @@ -490,23 +624,42 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda; + Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda, + -mContactConstraints[c].r1CrossT1.y * deltaLambda, + -mContactConstraints[c].r1CrossT1.z * deltaLambda); + Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda, + mContactConstraints[c].frictionVector1.y * deltaLambda, + mContactConstraints[c].frictionVector1.z * deltaLambda); + Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda, + mContactConstraints[c].r2CrossT1.y * deltaLambda, + mContactConstraints[c].r2CrossT1.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); - Jv = deltaV.dot(mContactConstraints[c].frictionVector2); + //deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + deltaV.x = v2.x + w2.y * mContactConstraints[c].r2Friction.z - v2.z * mContactConstraints[c].r2Friction.y - v1.x - + w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y; + deltaV.y = v2.y + w2.z * mContactConstraints[c].r2Friction.x - v2.x * mContactConstraints[c].r2Friction.z - v1.y - + w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z; + deltaV.z = v2.z + w2.x * mContactConstraints[c].r2Friction.y - v2.y * mContactConstraints[c].r2Friction.x - v1.z - + w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x; + Jv = deltaV.x * mContactConstraints[c].frictionVector2.x + deltaV.y * mContactConstraints[c].frictionVector2.y + + deltaV.z * mContactConstraints[c].frictionVector2.z; // Compute the Lagrange multiplier lambda deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass; @@ -518,23 +671,36 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda; - linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda; - angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda; + angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda; + angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda; + angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda; + + linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda; + linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda; + linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda; + + angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda; + angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda; + angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; + mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; + mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifol ------ // // Compute J*v deltaV = w2 - w1; - Jv = deltaV.dot(mContactConstraints[c].normal); + Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y + + deltaV.z * mContactConstraints[c].normal.z; deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass); frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; @@ -545,7 +711,9 @@ void ContactSolver::solve() { deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda; + angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda; + angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda; + angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; @@ -581,7 +749,7 @@ void ContactSolver::solve() { // warm start the solver at the next iteration void ContactSolver::storeImpulses() { - PROFILE("ContactSolver::storeImpulses()"); + PROFILE("ContactSolver::storeImpulses()", mProfiler); uint contactPointIndex = 0; @@ -609,13 +777,16 @@ void ContactSolver::storeImpulses() { void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { - PROFILE("ContactSolver::computeFrictionVectors()"); + PROFILE("ContactSolver::computeFrictionVectors()", mProfiler); assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; + Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, + deltaVelocity.y * contact.normal.y * contact.normal.y, + deltaVelocity.z * contact.normal.z * contact.normal.z); + Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, + deltaVelocity.z - normalVelocity.z); // If the velocty difference in the tangential plane is not zero decimal lengthTangenVelocity = tangentVelocity.length(); diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 40e0721d..8799a1ad 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -272,6 +272,10 @@ class ContactSolver { // -------------------- Attributes -------------------- // + /// Memory manager + MemoryManager& mMemoryManager; + + /// Split linear velocities for the position contact solver (split impulse) Vector3* mSplitLinearVelocities; @@ -293,21 +297,22 @@ class ContactSolver { /// Number of contact constraints uint mNbContactManifolds; - /// Single frame memory allocator - SingleFrameAllocator& mSingleFrameAllocator; - /// Array of linear velocities Vector3* mLinearVelocities; /// Array of angular velocities Vector3* mAngularVelocities; - /// Reference to the map of rigid body to their index in the constrained velocities array - const std::map& mMapBodyToConstrainedVelocityIndex; - /// True if the split impulse position correction is active bool mIsSplitImpulseActive; +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; + +#endif + // -------------------- Methods -------------------- // /// Compute the collision restitution factor from the restitution factor of each body @@ -335,8 +340,7 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(const std::map& mapBodyToVelocityIndex, - SingleFrameAllocator& allocator); + ContactSolver(MemoryManager& memoryManager); /// Destructor ~ContactSolver() = default; @@ -367,6 +371,13 @@ class ContactSolver { /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif }; // Set the split velocities arrays @@ -425,6 +436,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void ContactSolver::setProfiler(Profiler* profiler) { + + mProfiler = profiler; +} + +#endif + } #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 4f7bd15a..35b1ba04 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -29,6 +29,7 @@ #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" #include "constraint/FixedJoint.h" +#include // Namespaces using namespace reactphysics3d; @@ -40,11 +41,10 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator), - mConstraintSolver(mMapBodyToConstrainedVelocityIndex), + mContactSolver(mMemoryManager), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), - mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity), + mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), @@ -53,6 +53,14 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + mConstraintSolver.setProfiler(&mProfiler); + mContactSolver.setProfiler(&mProfiler); + +#endif + } // Destructor @@ -80,10 +88,10 @@ DynamicsWorld::~DynamicsWorld() { #ifdef IS_PROFILING_ACTIVE // Print the profiling report - Profiler::printReport(std::cout); - - // Destroy the profiler (release the allocated memory) - Profiler::destroy(); + ofstream myfile; + myfile.open(mProfiler.getName() + ".txt"); + mProfiler.printReport(myfile); + myfile.close(); #endif } @@ -96,10 +104,10 @@ void DynamicsWorld::update(decimal timeStep) { #ifdef IS_PROFILING_ACTIVE // Increment the frame counter of the profiler - Profiler::incrementFrameCounter(); + mProfiler.incrementFrameCounter(); #endif - PROFILE("DynamicsWorld::update()"); + PROFILE("DynamicsWorld::update()", &mProfiler); mTimeStep = timeStep; @@ -139,7 +147,7 @@ void DynamicsWorld::update(decimal timeStep) { resetBodiesForceAndTorque(); // Reset the single frame memory allocator - mSingleFrameAllocator.reset(); + mMemoryManager.resetFrameAllocator(); } // Integrate position and orientation of the rigid bodies. @@ -147,7 +155,7 @@ void DynamicsWorld::update(decimal timeStep) { /// the sympletic Euler time stepping scheme. void DynamicsWorld::integrateRigidBodiesPositions() { - PROFILE("DynamicsWorld::integrateRigidBodiesPositions()"); + PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", &mProfiler); // For each island of the world for (uint i=0; i < mNbIslands; i++) { @@ -158,7 +166,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // Get the constrained velocity - uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint indexArray = bodies[b]->mArrayIndex; Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; @@ -186,7 +194,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() { // Update the postion/orientation of the bodies void DynamicsWorld::updateBodiesState() { - PROFILE("DynamicsWorld::updateBodiesState()"); + PROFILE("DynamicsWorld::updateBodiesState()", &mProfiler); // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { @@ -196,7 +204,7 @@ void DynamicsWorld::updateBodiesState() { for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { - uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint index = bodies[b]->mArrayIndex; // Update the linear and angular velocity of the body bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; @@ -223,17 +231,23 @@ void DynamicsWorld::updateBodiesState() { // Initialize the bodies velocities arrays for the next simulation step. void DynamicsWorld::initVelocityArrays() { - PROFILE("DynamicsWorld::initVelocityArrays()"); + PROFILE("DynamicsWorld::initVelocityArrays()", &mProfiler); // Allocate memory for the bodies velocity arrays uint nbBodies = mRigidBodies.size(); - mSplitLinearVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mSplitAngularVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedLinearVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedAngularVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedPositions = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); - mConstrainedOrientations = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Quaternion))); + mSplitLinearVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mSplitAngularVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedLinearVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedAngularVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedPositions = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Vector3))); + mConstrainedOrientations = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBodies * sizeof(Quaternion))); assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); assert(mConstrainedLinearVelocities != nullptr); @@ -241,21 +255,14 @@ void DynamicsWorld::initVelocityArrays() { assert(mConstrainedPositions != nullptr); assert(mConstrainedOrientations != nullptr); - // Reset the velocities arrays - for (uint i=0; i::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + mSplitLinearVelocities[i].setToZero(); mSplitAngularVelocities[i].setToZero(); - } - // Initialize the map of body indexes in the velocity arrays - mMapBodyToConstrainedVelocityIndex.clear(); - std::set::const_iterator it; - uint indexBody = 0; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - - // Add the body into the map - mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); - indexBody++; + (*it)->mArrayIndex = i++; } } @@ -266,7 +273,7 @@ void DynamicsWorld::initVelocityArrays() { /// contact solver. void DynamicsWorld::integrateRigidBodiesVelocities() { - PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()"); + PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", &mProfiler); // Initialize the bodies velocity arrays initVelocityArrays(); @@ -280,7 +287,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { // Insert the body into the map of constrained velocities - uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + uint indexBody = bodies[b]->mArrayIndex; assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); @@ -328,7 +335,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { // Solve the contacts and constraints void DynamicsWorld::solveContactsAndConstraints() { - PROFILE("DynamicsWorld::solveContactsAndConstraints()"); + PROFILE("DynamicsWorld::solveContactsAndConstraints()", &mProfiler); // Set the velocities arrays mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); @@ -349,20 +356,6 @@ void DynamicsWorld::solveContactsAndConstraints() { // Check if there are contacts and constraints to solve bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - //if (!isConstraintsToSolve && !isContactsToSolve) continue; - - // If there are contacts in the current island -// if (isContactsToSolve) { - -// // Initialize the solver -// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - -// // Warm start the contact solver -// if (mContactSolver.IsWarmStartingActive()) { -// mContactSolver.warmStart(); -// } -// } // If there are constraints if (isConstraintsToSolve) { @@ -384,15 +377,6 @@ void DynamicsWorld::solveContactsAndConstraints() { } mContactSolver.solve(); - - // Solve the contacts -// if (isContactsToSolve) { - -// mContactSolver.resetTotalPenetrationImpulse(); - -// mContactSolver.solvePenetrationConstraints(); -// mContactSolver.solveFrictionConstraints(); -// } } mContactSolver.storeImpulses(); @@ -401,7 +385,7 @@ void DynamicsWorld::solveContactsAndConstraints() { // Solve the position error correction of the constraints void DynamicsWorld::solvePositionCorrection() { - PROFILE("DynamicsWorld::solvePositionCorrection()"); + PROFILE("DynamicsWorld::solvePositionCorrection()", &mProfiler); // Do not continue if there is no constraints if (mJoints.empty()) return; @@ -434,14 +418,20 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the rigid body - RigidBody* rigidBody = new (mPoolAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, - *this, bodyID); + RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(RigidBody))) RigidBody(transform, *this, bodyID); assert(rigidBody != nullptr); // Add the rigid body to the physics world mBodies.insert(rigidBody); mRigidBodies.insert(rigidBody); +#ifdef IS_PROFILING_ACTIVE + + rigidBody->setProfiler(&mProfiler); + +#endif + // Return the pointer to the rigid body return rigidBody; } @@ -475,7 +465,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mRigidBodies.erase(rigidBody); // Free the object from the memory allocator - mPoolAllocator.release(rigidBody, sizeof(RigidBody)); + 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 @@ -493,7 +483,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Ball-and-Socket joint case JointType::BALLSOCKETJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(BallAndSocketJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); newJoint = new (allocatedMemory) BallAndSocketJoint(info); @@ -503,7 +494,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Slider joint case JointType::SLIDERJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(SliderJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) SliderJoint(info); break; @@ -512,7 +504,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Hinge joint case JointType::HINGEJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(HingeJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) HingeJoint(info); break; @@ -521,7 +514,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Fixed joint case JointType::FIXEDJOINT: { - void* allocatedMemory = mPoolAllocator.allocate(sizeof(FixedJoint)); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) FixedJoint(info); break; @@ -574,8 +568,8 @@ void DynamicsWorld::destroyJoint(Joint* joint) { mJoints.erase(joint); // Remove the joint from the joint list of the bodies involved in the joint - joint->mBody1->removeJointFromJointsList(mPoolAllocator, joint); - joint->mBody2->removeJointFromJointsList(mPoolAllocator, joint); + joint->mBody1->removeJointFromJointsList(mMemoryManager, joint); + joint->mBody2->removeJointFromJointsList(mMemoryManager, joint); size_t nbBytes = joint->getSizeInBytes(); @@ -583,7 +577,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { joint->~Joint(); // Release the allocated memory - mPoolAllocator.release(joint, nbBytes); + mMemoryManager.release(MemoryManager::AllocationType::Pool, joint, nbBytes); } // Add the joint to the list of joints of the two bodies involved in the joint @@ -592,13 +586,15 @@ 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 = mPoolAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory1 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(JointListElement)); JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, joint->mBody1->mJointsList); joint->mBody1->mJointsList = jointListElement1; // Add the joint at the beginning of the linked list of joints of the second body - void* allocatedMemory2 = mPoolAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory2 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(JointListElement)); JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, joint->mBody2->mJointsList); joint->mBody2->mJointsList = jointListElement2; @@ -613,13 +609,14 @@ void DynamicsWorld::addJointToBody(Joint* joint) { /// it). Then, we create an island with this group of connected bodies. void DynamicsWorld::computeIslands() { - PROFILE("DynamicsWorld::computeIslands()"); + PROFILE("DynamicsWorld::computeIslands()", &mProfiler); uint nbBodies = mRigidBodies.size(); // Allocate and create the array of islands pointer. This memory is allocated // in the single frame allocator - mIslands = static_cast(mSingleFrameAllocator.allocate(sizeof(Island*) * nbBodies)); + mIslands = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(Island*) * nbBodies)); mNbIslands = 0; int nbContactManifolds = 0; @@ -635,7 +632,8 @@ void DynamicsWorld::computeIslands() { // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; - RigidBody** stackBodiesToVisit = static_cast(mSingleFrameAllocator.allocate(nbBytesStack)); + RigidBody** stackBodiesToVisit = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + nbBytesStack)); // For each rigid body of the world for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { @@ -658,9 +656,10 @@ void DynamicsWorld::computeIslands() { body->mIsAlreadyInIsland = true; // Create the new island - void* allocatedMemoryIsland = mSingleFrameAllocator.allocate(sizeof(Island)); + void* allocatedMemoryIsland = mMemoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(Island)); mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(), - mSingleFrameAllocator); + mMemoryManager); // While there are still some bodies to visit in the stack while (stackIndex > 0) { @@ -683,9 +682,9 @@ void DynamicsWorld::computeIslands() { // For each contact manifold in which the current body is involded ContactManifoldListElement* contactElement; for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr; - contactElement = contactElement->next) { + contactElement = contactElement->getNext()) { - ContactManifold* contactManifold = contactElement->contactManifold; + ContactManifold* contactManifold = contactElement->getContactManifold(); assert(contactManifold->getNbContactPoints() > 0); @@ -757,7 +756,7 @@ void DynamicsWorld::computeIslands() { /// time, we put all the bodies of the island to sleep. void DynamicsWorld::updateSleepingBodies() { - PROFILE("DynamicsWorld::updateSleepingBodies()"); + PROFILE("DynamicsWorld::updateSleepingBodies()", &mProfiler); const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; @@ -842,12 +841,13 @@ std::vector DynamicsWorld::getContactsList() const { // For each contact manifold of the pair const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - for (int i=0; igetNext(); } } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index b632dcf2..0c614b65 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -100,9 +100,6 @@ class DynamicsWorld : public CollisionWorld { /// Array of constrained rigid bodies orientation (for position error correction) Quaternion* mConstrainedOrientations; - /// Map body to their index in the constrained velocities array - std::map mMapBodyToConstrainedVelocityIndex; - /// Number of islands in the world uint mNbIslands; @@ -124,19 +121,9 @@ class DynamicsWorld : public CollisionWorld { /// Integrate the positions and orientations of rigid bodies. void integrateRigidBodiesPositions(); - /// Update the AABBs of the bodies - void updateRigidBodiesAABB(); - /// Reset the external force and torque applied to the bodies void resetBodiesForceAndTorque(); - /// Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, - Vector3 newAngVelocity); - - /// Compute and set the interpolation factor to all bodies - void setInterpolationFactorToAllBodies(); - /// Initialize the bodies velocities arrays for the next simulation step. void initVelocityArrays(); @@ -149,9 +136,6 @@ class DynamicsWorld : public CollisionWorld { /// Solve the position error correction of the constraints void solvePositionCorrection(); - /// Cleanup the constrained velocities array at each step - void cleanupConstrainedVelocitiesArray(); - /// Compute the islands of awake bodies. void computeIslands(); @@ -478,7 +462,6 @@ inline void DynamicsWorld::setEventListener(EventListener* eventListener) { mEventListener = eventListener; } - } #endif diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h index 562e8ac7..061724a0 100644 --- a/src/engine/EventListener.h +++ b/src/engine/EventListener.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_EVENT_LISTENER_H // Libraries -#include "constraint/ContactPoint.h" +#include "collision/CollisionCallback.h" namespace reactphysics3d { @@ -49,17 +49,11 @@ class EventListener { /// Destructor virtual ~EventListener() = default; - /// Called when a new contact point is found between two bodies that were separated before - /** - * @param contact Information about the contact - */ - virtual void beginContact(const ContactPointInfo& contact) {} - /// Called when a new contact point is found between two bodies /** * @param contact Information about the contact */ - virtual void newContact(const ContactPointInfo& contact) {} + virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {} /// Called at the beginning of an internal tick of the simulation step. /// Each time the DynamicsWorld::update() method is called, the physics diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index 19eef304..34b65540 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -29,14 +29,17 @@ using namespace reactphysics3d; // Constructor -Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, SingleFrameAllocator& allocator) +Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, MemoryManager& memoryManager) : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0), mNbContactManifolds(0), mNbJoints(0) { // Allocate memory for the arrays on the single frame allocator - mBodies = static_cast(allocator.allocate(sizeof(RigidBody*) * nbMaxBodies)); - mContactManifolds = static_cast(allocator.allocate(sizeof(ContactManifold*) * nbMaxContactManifolds)); - mJoints = static_cast(allocator.allocate(sizeof(Joint*) * nbMaxJoints)); + mBodies = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(RigidBody*) * nbMaxBodies)); + mContactManifolds = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(ContactManifold*) * nbMaxContactManifolds)); + mJoints = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, + sizeof(Joint*) * nbMaxJoints)); } // Destructor diff --git a/src/engine/Island.h b/src/engine/Island.h index 5f370004..ca1e6fdd 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -69,7 +69,7 @@ class Island { /// Constructor Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - SingleFrameAllocator& allocator); + MemoryManager& memoryManager); /// Destructor ~Island(); diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 48658f5c..7a95be95 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -24,15 +24,179 @@ ********************************************************************************/ // Libraries +#include #include "OverlappingPair.h" +#include "collision/ContactManifoldInfo.h" +#include "collision/NarrowPhaseInfo.h" using namespace reactphysics3d; - // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, PoolAllocator& memoryAllocator) - : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), - mCachedSeparatingAxis(0.0, 1.0, 0.0) { + MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator) + : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr), + mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) { -} +} + +// Destructor +OverlappingPair::~OverlappingPair() { + assert(mPotentialContactManifolds == nullptr); + + // Remove all the remaining last frame collision info + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { + + // Call the constructor + it->second->~LastFrameCollisionInfo(); + + // Release memory + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + } +} + +// Create a new potential contact manifold using contact-points from narrow-phase +void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { + + assert(narrowPhaseInfo->contactPoints != nullptr); + + // For each potential contact point to add + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + while (contactPoint != nullptr) { + + ContactPointInfo* nextContactPoint = contactPoint->next; + + // Look if the contact point correspond to an existing potential manifold + // (if the contact point normal is similar to the normal of an existing manifold) + ContactManifoldInfo* manifold = mPotentialContactManifolds; + bool similarManifoldFound = false; + while(manifold != nullptr) { + + // Get the first contact point + const ContactPointInfo* point = manifold->getFirstContactPointInfo(); + assert(point != nullptr); + + // If we have found a corresponding manifold for the new contact point + // (a manifold with a similar contact normal direction) + if (point->normal.dot(contactPoint->normal) >= COS_ANGLE_SIMILAR_CONTACT_MANIFOLD) { + + // Add the contact point to the manifold + manifold->addContactPoint(contactPoint); + + similarManifoldFound = true; + + break; + } + + manifold = manifold->getNext(); + } + + // If we have not found an existing manifold with a similar contact normal + if (!similarManifoldFound) { + + // Create a new potential contact manifold + ContactManifoldInfo* manifoldInfo = new (mTempMemoryAllocator.allocate(sizeof(ContactManifoldInfo))) + ContactManifoldInfo(mTempMemoryAllocator); + + // Add the manifold into the linked-list of potential contact manifolds + manifoldInfo->mNext = mPotentialContactManifolds; + mPotentialContactManifolds = manifoldInfo; + + // Add the contact point to the manifold + manifoldInfo->addContactPoint(contactPoint); + } + + contactPoint = nextContactPoint; + } + + // All the contact point info of the narrow-phase info have been moved + // into the potential contacts of the overlapping pair + narrowPhaseInfo->contactPoints = nullptr; +} + +// Clear all the potential contact manifolds +void OverlappingPair::clearPotentialContactManifolds() { + + ContactManifoldInfo* element = mPotentialContactManifolds; + while(element != nullptr) { + + // Remove the proxy collision shape + ContactManifoldInfo* elementToRemove = element; + element = element->getNext(); + + // Delete the element + elementToRemove->~ContactManifoldInfo(); + mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); + } + + mPotentialContactManifolds = nullptr; +} + +// Reduce the number of contact points of all the potential contact manifolds +void OverlappingPair::reducePotentialContactManifolds() { + + // For each potential contact manifold + ContactManifoldInfo* manifold = mPotentialContactManifolds; + while (manifold != nullptr) { + + // Reduce the number of contact points of the manifold + manifold->reduce(mContactManifoldSet.getShape1()->getLocalToWorldTransform()); + + manifold = manifold->getNext(); + } +} + + +// Add a new last frame collision info if it does not exist for the given shapes already +void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { + + // Try to get the corresponding last frame collision info + auto it = mLastFrameCollisionInfos.find(std::make_pair(shapeId1, shapeId2)); + + // If there is no collision info for those two shapes already + if (it == mLastFrameCollisionInfos.end()) { + + // Create a new collision info + LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo))) + LastFrameCollisionInfo(); + + // Add it into the map of collision infos + std::map, LastFrameCollisionInfo*>::iterator it; + auto ret = mLastFrameCollisionInfos.insert(std::make_pair(std::make_pair(shapeId1, shapeId2), collisionInfo)); + assert(ret.second); + } + else { + + // The existing collision info is not obsolete + it->second->isObsolete = false; + } +} + + +// Delete all the obsolete last frame collision info +void OverlappingPair::clearObsoleteLastFrameCollisionInfos() { + + // For each collision info + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ) { + + // If the collision info is obsolete + if (it->second->isObsolete) { + + // Delete it + it->second->~LastFrameCollisionInfo(); + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + + mLastFrameCollisionInfos.erase(it++); + } + else { + ++it; + } + } +} + +// Make all the last frame collision infos obsolete +void OverlappingPair::makeLastFrameCollisionInfosObsolete() { + + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { + it->second->isObsolete = true; + } +} diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 70ee1c3f..f3631775 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -30,6 +30,7 @@ #include "collision/ContactManifoldSet.h" #include "collision/ProxyShape.h" #include "collision/shapes/CollisionShape.h" +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -37,6 +38,53 @@ namespace reactphysics3d { // Type for the overlapping pair ID using overlappingpairid = std::pair; +// Structure LastFrameCollisionInfo +/** + * This structure contains collision info about the last frame. + * This is used for temporal coherence between frames. + */ +struct LastFrameCollisionInfo { + + /// True if we have information about the previous frame + bool isValid; + + /// True if the frame info is obsolete (the collision shape are not overlapping in middle phase) + bool isObsolete; + + /// True if the two shapes were colliding in the previous frame + bool wasColliding; + + /// True if we were using GJK algorithm to check for collision in the previous frame + bool wasUsingGJK; + + /// True if we were using SAT algorithm to check for collision in the previous frame + bool wasUsingSAT; + + // ----- GJK Algorithm ----- + + /// Previous separating axis + Vector3 gjkSeparatingAxis; + + // SAT Algorithm + bool satIsAxisFacePolyhedron1; + bool satIsAxisFacePolyhedron2; + uint satMinAxisFaceIndex; + uint satMinEdge1Index; + uint satMinEdge2Index; + + /// Constructor + LastFrameCollisionInfo() { + + isValid = false; + isObsolete = false; + wasColliding = false; + wasUsingSAT = false; + wasUsingGJK = false; + + gjkSeparatingAxis = Vector3(0, 1, 0); + } +}; + // Class OverlappingPair /** * This class represents a pair of two proxy collision shapes that are overlapping @@ -54,19 +102,32 @@ class OverlappingPair { /// Set of persistent contact manifolds ContactManifoldSet mContactManifoldSet; - /// Cached previous separating axis - Vector3 mCachedSeparatingAxis; + /// Temporal coherence collision data for each overlapping collision shapes of this pair. + /// Temporal coherence data store collision information about the last frame. + /// If two convex shapes overlap, we have a single collision data but if one shape is concave, + /// we might have collision data for several overlapping triangles. The key in the map is the + /// shape Ids of the two collision shapes. + std::map, LastFrameCollisionInfo*> mLastFrameCollisionInfos; + + /// Linked-list of potential contact manifold + ContactManifoldInfo* mPotentialContactManifolds; + + /// Persistent memory allocator + MemoryAllocator& mPersistentAllocator; + + /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo + MemoryAllocator& mTempMemoryAllocator; public: // -------------------- Methods -------------------- // /// Constructor - OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, PoolAllocator& memoryAllocator); + OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& persistentMemoryAllocator, + MemoryAllocator& temporaryMemoryAllocator); /// Destructor - ~OverlappingPair() = default; + ~OverlappingPair(); /// Deleted copy-constructor OverlappingPair(const OverlappingPair& pair) = delete; @@ -80,26 +141,56 @@ class OverlappingPair { /// Return the pointer to second body ProxyShape* getShape2() const; - /// Add a contact to the contact cache - void addContact(ContactPoint* contact); - - /// Update the contact cache - void update(); - - /// Return the cached separating axis - Vector3 getCachedSeparatingAxis() const; - - /// Set the cached separating axis - void setCachedSeparatingAxis(const Vector3& axis); - - /// Return the number of contacts in the cache - uint getNbContactPoints() const; + /// Return the last frame collision info + LastFrameCollisionInfo* getLastFrameCollisionInfo(std::pair shapeIds); /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); - /// Clear the contact points of the contact manifold - void clearContactPoints(); + /// Clear all the potential contact manifolds + void clearPotentialContactManifolds(); + + /// Add potential contact-points from narrow-phase into potential contact manifolds + void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo); + + /// Add a contact to the contact manifold + void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); + + /// Return a reference to the temporary memory allocator + MemoryAllocator& getTemporaryAllocator(); + + /// Return true if one of the shapes of the pair is a concave shape + bool hasConcaveShape() const; + + /// Return true if the overlapping pair has contact manifolds with contacts + bool hasContacts() const; + + /// Return a pointer to the first potential contact manifold in the linked-list + ContactManifoldInfo* getPotentialContactManifolds(); + + /// Reduce the number of contact points of all the potential contact manifolds + void reducePotentialContactManifolds(); + + /// Make the contact manifolds and contact points obsolete + void makeContactsObsolete(); + + /// Clear the obsolete contact manifold and contact points + void clearObsoleteManifoldsAndContactPoints(); + + /// Reduce the contact manifolds that have too many contact points + void reduceContactManifolds(); + + /// Add a new last frame collision info if it does not exist for the given shapes already + void addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2); + + /// Return the last frame collision info for a given pair of shape ids + LastFrameCollisionInfo* getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const; + + /// Delete all the obsolete last frame collision info + void clearObsoleteLastFrameCollisionInfos(); + + /// Make all the last frame collision infos obsolete + void makeLastFrameCollisionInfosObsolete(); /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -123,29 +214,18 @@ inline ProxyShape* OverlappingPair::getShape2() const { } // Add a contact to the contact manifold -inline void OverlappingPair::addContact(ContactPoint* contact) { - mContactManifoldSet.addContactPoint(contact); +inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { + mContactManifoldSet.addContactManifold(contactManifoldInfo); } -// Update the contact manifold -inline void OverlappingPair::update() { - mContactManifoldSet.update(); -} +// Return the last frame collision info for a given shape id or nullptr if none is found +inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(std::pair shapeIds) { + std::map, LastFrameCollisionInfo*>::iterator it = mLastFrameCollisionInfos.find(shapeIds); + if (it != mLastFrameCollisionInfos.end()) { + return it->second; + } -// Return the cached separating axis -inline Vector3 OverlappingPair::getCachedSeparatingAxis() const { - return mCachedSeparatingAxis; -} - -// Set the cached separating axis -inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { - mCachedSeparatingAxis = axis; -} - - -// Return the number of contact points in the contact manifold -inline uint OverlappingPair::getNbContactPoints() const { - return mContactManifoldSet.getTotalNbContactPoints(); + return nullptr; } // Return the contact manifold @@ -153,6 +233,12 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() { return mContactManifoldSet; } +// Make the contact manifolds and contact points obsolete +inline void OverlappingPair::makeContactsObsolete() { + + mContactManifoldSet.makeContactsObsolete(); +} + // Return the pair of bodies index inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0); @@ -177,9 +263,40 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body return indexPair; } -// Clear the contact points of the contact manifold -inline void OverlappingPair::clearContactPoints() { - mContactManifoldSet.clear(); +// Return a reference to the temporary memory allocator +inline MemoryAllocator& OverlappingPair::getTemporaryAllocator() { + return mTempMemoryAllocator; +} + +// Return true if one of the shapes of the pair is a concave shape +inline bool OverlappingPair::hasConcaveShape() const { + return !getShape1()->getCollisionShape()->isConvex() || + !getShape2()->getCollisionShape()->isConvex(); +} + +// Return true if the overlapping pair has contact manifolds with contacts +inline bool OverlappingPair::hasContacts() const { + return mContactManifoldSet.getContactManifolds() != nullptr; +} + +// Return a pointer to the first potential contact manifold in the linked-list +inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() { + return mPotentialContactManifolds; +} + +// Clear the obsolete contact manifold and contact points +inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() { + mContactManifoldSet.clearObsoleteManifoldsAndContactPoints(); +} + +// Reduce the contact manifolds that have too many contact points +inline void OverlappingPair::reduceContactManifolds() { + mContactManifoldSet.reduce(); +} + +// Return the last frame collision info for a given pair of shape ids +inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const { + return mLastFrameCollisionInfos.at(std::make_pair(shapeId1, shapeId2)); } } diff --git a/src/engine/Profiler.cpp b/src/engine/Profiler.cpp index 9437dca0..3212e4a1 100644 --- a/src/engine/Profiler.cpp +++ b/src/engine/Profiler.cpp @@ -27,14 +27,12 @@ // Libraries #include "Profiler.h" +#include using namespace reactphysics3d; // Initialization of static variables -ProfileNode Profiler::mRootNode("Root", nullptr); -ProfileNode* Profiler::mCurrentNode = &Profiler::mRootNode; -long double Profiler::mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0; -uint Profiler::mFrameCounter = 0; +int Profiler::mNbProfilers = 0; // Constructor ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode) @@ -157,6 +155,35 @@ void ProfileNodeIterator::enterParent() { mCurrentChildNode = mCurrentParentNode->getChildNode(); } +// Constructor +Profiler::Profiler(std::string name) :mRootNode("Root", nullptr) { + + // Set the name of the profiler + if (name == "") { + + if (mNbProfilers == 0) { + mName = "profiler"; + } + else { + mName = std::string("profiler") + std::to_string(mNbProfilers); + } + } + else { + mName = name; + } + + mCurrentNode = &mRootNode; + mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0; + mFrameCounter = 0; + + mNbProfilers++; +} + +// Destructor +Profiler::~Profiler() { + destroy(); +} + // Method called when we want to start profiling a block of code. void Profiler::startProfilingBlock(const char* name) { diff --git a/src/engine/Profiler.h b/src/engine/Profiler.h index 818d6cc8..45b9b2e9 100644 --- a/src/engine/Profiler.h +++ b/src/engine/Profiler.h @@ -184,57 +184,73 @@ class Profiler { // -------------------- Attributes -------------------- // + /// Profiler name + std::string mName; + + /// Total number of profilers + static int mNbProfilers; + /// Root node of the profiler tree - static ProfileNode mRootNode; + ProfileNode mRootNode; /// Current node in the current execution - static ProfileNode* mCurrentNode; + ProfileNode* mCurrentNode; /// Frame counter - static uint mFrameCounter; + uint mFrameCounter; /// Starting profiling time - static long double mProfilingStartTime; + long double mProfilingStartTime; /// Recursively print the report of a given node of the profiler tree - static void printRecursiveNodeReport(ProfileNodeIterator* iterator, - int spacing, - std::ostream& outputStream); + void printRecursiveNodeReport(ProfileNodeIterator* iterator, int spacing, std::ostream& outputStream); + + /// Destroy a previously allocated iterator + void destroyIterator(ProfileNodeIterator* iterator); + + /// Destroy the profiler (release the memory) + void destroy(); public : // -------------------- Methods -------------------- // + /// Constructor + Profiler(std::string name = ""); + + /// Destructor + ~Profiler(); + /// Method called when we want to start profiling a block of code. - static void startProfilingBlock(const char *name); + void startProfilingBlock(const char *name); /// Method called at the end of the scope where the /// startProfilingBlock() method has been called. - static void stopProfilingBlock(); + void stopProfilingBlock(); /// Reset the timing data of the profiler (but not the profiler tree structure) - static void reset(); + void reset(); /// Return the number of frames - static uint getNbFrames(); + uint getNbFrames(); + + /// Get the name of the profiler + std::string getName() const; + + /// Set the name of the profiler + void setName(std::string name); /// Return the elasped time since the start/reset of the profiling - static long double getElapsedTimeSinceStart(); + long double getElapsedTimeSinceStart(); /// Increment the frame counter - static void incrementFrameCounter(); + void incrementFrameCounter(); /// Return an iterator over the profiler tree starting at the root - static ProfileNodeIterator* getIterator(); + ProfileNodeIterator* getIterator(); /// Print the report of the profiler in a given output stream - static void printReport(std::ostream& outputStream); - - /// Destroy a previously allocated iterator - static void destroyIterator(ProfileNodeIterator* iterator); - - /// Destroy the profiler (release the memory) - static void destroy(); + void printReport(std::ostream& outputStream); }; // Class ProfileSample @@ -245,27 +261,33 @@ class Profiler { */ class ProfileSample { + private: + + Profiler* mProfiler; + public : // -------------------- Methods -------------------- // /// Constructor - ProfileSample(const char* name) { + ProfileSample(const char* name, Profiler* profiler) :mProfiler(profiler) { + + assert(profiler != nullptr); // Ask the profiler to start profiling a block of code - Profiler::startProfilingBlock(name); + mProfiler->startProfilingBlock(name); } /// Destructor ~ProfileSample() { // Tell the profiler to stop profiling a block of code - Profiler::stopProfilingBlock(); + mProfiler->stopProfilingBlock(); } }; // Use this macro to start profile a block of code -#define PROFILE(name) ProfileSample profileSample(name) +#define PROFILE(name, profiler) ProfileSample profileSample(name, profiler) // Return true if we are at the root of the profiler tree inline bool ProfileNodeIterator::isRoot() { @@ -352,6 +374,16 @@ inline uint Profiler::getNbFrames() { return mFrameCounter; } +// Get the name of the profiler +inline std::string Profiler::getName() const { + return mName; +} + +// Set the name of the profiler +inline void Profiler::setName(std::string name) { + mName = name; +} + // Return the elasped time since the start/reset of the profiling inline long double Profiler::getElapsedTimeSinceStart() { long double currentTime = Timer::getCurrentSystemTime() * 1000.0; diff --git a/src/mathematics/Matrix2x2.cpp b/src/mathematics/Matrix2x2.cpp index c501c1db..bf4adbe5 100644 --- a/src/mathematics/Matrix2x2.cpp +++ b/src/mathematics/Matrix2x2.cpp @@ -28,31 +28,6 @@ using namespace reactphysics3d; -// Constructor of the class Matrix2x2 -Matrix2x2::Matrix2x2() { - - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix2x2::Matrix2x2(decimal value) { - setAllValues(value, value, value, value); -} - -// Constructor with arguments -Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { - - // Initialize the matrix with the values - setAllValues(a1, a2, b1, b2); -} - -// Copy-constructor -Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], - matrix.mRows[1][0], matrix.mRows[1][1]); -} - // Assignment operator Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) { diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h index ee31b07a..8315eb99 100644 --- a/src/mathematics/Matrix2x2.h +++ b/src/mathematics/Matrix2x2.h @@ -147,6 +147,31 @@ class Matrix2x2 { Vector2& operator[](int row); }; +// Constructor of the class Matrix2x2 +inline Matrix2x2::Matrix2x2() { + + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0); +} + +// Constructor +inline Matrix2x2::Matrix2x2(decimal value) { + setAllValues(value, value, value, value); +} + +// Constructor with arguments +inline Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) { + + // Initialize the matrix with the values + setAllValues(a1, a2, b1, b2); +} + +// Copy-constructor +inline Matrix2x2::Matrix2x2(const Matrix2x2& matrix) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], + matrix.mRows[1][0], matrix.mRows[1][1]); +} + // Method to set all the values in the matrix inline void Matrix2x2::setAllValues(decimal a1, decimal a2, decimal b1, decimal b2) { diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index a5491ed4..e86e63a1 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -30,32 +30,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor of the class Matrix3x3 -Matrix3x3::Matrix3x3() { - // Initialize all values in the matrix to zero - setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); -} - -// Constructor -Matrix3x3::Matrix3x3(decimal value) { - setAllValues(value, value, value, value, value, value, value, value, value); -} - -// Constructor with arguments -Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, - decimal b1, decimal b2, decimal b3, - decimal c1, decimal c2, decimal c3) { - // Initialize the matrix with the values - setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); -} - -// Copy-constructor -Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { - setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], - matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], - matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); -} - // Assignment operator Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) { diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index dfa23085..6897a2cf 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -155,6 +155,32 @@ class Matrix3x3 { Vector3& operator[](int row); }; +// Constructor of the class Matrix3x3 +inline Matrix3x3::Matrix3x3() { + // Initialize all values in the matrix to zero + setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); +} + +// Constructor +inline Matrix3x3::Matrix3x3(decimal value) { + setAllValues(value, value, value, value, value, value, value, value, value); +} + +// Constructor with arguments +inline Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, + decimal b1, decimal b2, decimal b3, + decimal c1, decimal c2, decimal c3) { + // Initialize the matrix with the values + setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3); +} + +// Copy-constructor +inline Matrix3x3::Matrix3x3(const Matrix3x3& matrix) { + setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2], + matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2], + matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]); +} + // Method to set all the values in the matrix inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, @@ -233,9 +259,9 @@ inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vect // Return the matrix with absolute values inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const { - return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]), - fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]), - fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2])); + return Matrix3x3(std::fabs(mRows[0][0]), std::fabs(mRows[0][1]), std::fabs(mRows[0][2]), + std::fabs(mRows[1][0]), std::fabs(mRows[1][1]), std::fabs(mRows[1][2]), + std::fabs(mRows[2][0]), std::fabs(mRows[2][1]), std::fabs(mRows[2][2])); } // Overloaded operator for addition diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index 0b2587f5..0e877612 100644 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -31,30 +31,23 @@ // Namespace using namespace reactphysics3d; -// Constructor of the class -Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { +// Return a quaternion constructed from Euler angles (in radians) +Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) { + Quaternion quaternion; + quaternion.initWithEulerAngles(angleX, angleY, angleZ); + + return quaternion; } -// Constructor with arguments -Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) - :x(newX), y(newY), z(newZ), w(newW) { -} +// Return a quaternion constructed from Euler angles (in radians) +Quaternion Quaternion::fromEulerAngles(const Vector3& eulerAngles) { -// Constructor with the component w and the vector v=(x y z) -Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { + Quaternion quaternion; + quaternion.initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); -} - -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(decimal angleX, decimal angleY, decimal angleZ) { - initWithEulerAngles(angleX, angleY, angleZ); -} - -// Constructor which convert Euler angles (in radians) to a quaternion -Quaternion::Quaternion(const Vector3& eulerAngles) { - initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z); + return quaternion; } // Copy-constructor @@ -72,10 +65,10 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { decimal r; decimal s; - if (trace < 0.0) { + if (trace < decimal(0.0)) { if (matrix[1][1] > matrix[0][0]) { if(matrix[2][2] > matrix[1][1]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); + r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -85,7 +78,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { w = (matrix[1][0] - matrix[0][1]) * s; } else { - r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0)); + r = std::sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -96,7 +89,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { } } else if (matrix[2][2] > matrix[0][0]) { - r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); + r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -106,7 +99,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { w = (matrix[1][0] - matrix[0][1]) * s; } else { - r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0)); + r = std::sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -117,7 +110,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { } } else { - r = sqrt(trace + decimal(1.0)); + r = std::sqrt(trace + decimal(1.0)); s = decimal(0.5) / r; // Compute the quaternion @@ -132,22 +125,12 @@ Quaternion::Quaternion(const Matrix3x3& matrix) { /// This method is used to get the rotation angle (in radian) and the unit /// rotation axis of an orientation quaternion. void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const { - Quaternion quaternion; - - // If the quaternion is unit - if (length() == 1.0) { - quaternion = *this; - } - else { - // We compute the unit quaternion - quaternion = getUnit(); - } // Compute the roation angle - angle = acos(quaternion.w) * decimal(2.0); + angle = std::acos(w) * decimal(2.0); // Compute the 3D rotation axis - Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z); + Vector3 rotationAxis(x, y, z); // Normalize the rotation axis rotationAxis = rotationAxis.getUnit(); @@ -162,7 +145,7 @@ Matrix3x3 Quaternion::getMatrix() const { decimal nQ = x*x + y*y + z*z + w*w; decimal s = 0.0; - if (nQ > 0.0) { + if (nQ > decimal(0.0)) { s = decimal(2.0) / nQ; } @@ -190,7 +173,7 @@ Matrix3x3 Quaternion::getMatrix() const { /// The t argument has to be such that 0 <= t <= 1. This method is static. Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, decimal t) { - assert(t >= 0.0 && t <= 1.0); + assert(t >= decimal(0.0) && t <= decimal(1.0)); decimal invert = 1.0; @@ -198,7 +181,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, decimal cosineTheta = quaternion1.dot(quaternion2); // Take care of the sign of cosineTheta - if (cosineTheta < 0.0) { + if (cosineTheta < decimal(0.0)) { cosineTheta = -cosineTheta; invert = -1.0; } @@ -212,14 +195,14 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, } // Compute the theta angle - decimal theta = acos(cosineTheta); + decimal theta = std::acos(cosineTheta); // Compute sin(theta) - decimal sineTheta = sin(theta); + decimal sineTheta = std::sin(theta); // Compute the two coefficients that are in the spherical linear interpolation formula - decimal coeff1 = sin((decimal(1.0)-t)*theta) / sineTheta; - decimal coeff2 = sin(t*theta) / sineTheta * invert; + decimal coeff1 = std::sin((decimal(1.0)-t)*theta) / sineTheta; + decimal coeff2 = std::sin(t*theta) / sineTheta * invert; // Compute and return the interpolated quaternion return quaternion1 * coeff1 + quaternion2 * coeff2; diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 360cf229..dd0726dc 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -69,11 +69,8 @@ struct Quaternion { /// Constructor with the component w and the vector v=(x y z) Quaternion(decimal newW, const Vector3& v); - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(decimal angleX, decimal angleY, decimal angleZ); - - /// Constructor which convert Euler angles (in radians) to a quaternion - Quaternion(const Vector3& eulerAngles); + /// Constructor with the component w and the vector v=(x y z) + Quaternion(const Vector3& v, decimal newW); /// Copy-constructor Quaternion(const Quaternion& quaternion); @@ -123,6 +120,12 @@ struct Quaternion { /// Return the identity quaternion static Quaternion identity(); + /// Return a quaternion constructed from Euler angles (in radians) + static Quaternion fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ); + + /// Return a quaternion constructed from Euler angles (in radians) + static Quaternion fromEulerAngles(const Vector3& eulerAngles); + /// Dot product between two quaternions decimal dot(const Quaternion& quaternion) const; @@ -166,7 +169,28 @@ struct Quaternion { void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ); }; -/// Set all the values +// Constructor of the class +inline Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) { + +} + +// Constructor with arguments +inline Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW) + :x(newX), y(newY), z(newZ), w(newW) { + +} + +// Constructor with the component w and the vector v=(x y z) +inline Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) { + +} + +// Constructor with the component w and the vector v=(x y z) +inline Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) { + +} + +// Set all the values inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) { x = newX; y = newY; @@ -174,7 +198,7 @@ inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, d w = newW; } -/// Set the quaternion to zero +// Set the quaternion to zero inline void Quaternion::setToZero() { x = 0; y = 0; @@ -199,7 +223,7 @@ inline Vector3 Quaternion::getVectorV() const { // Return the length of the quaternion (inline) inline decimal Quaternion::length() const { - return sqrt(x*x + y*y + z*z + w*w); + return std::sqrt(x*x + y*y + z*z + w*w); } // Return the square of the length of the quaternion @@ -224,16 +248,10 @@ inline void Quaternion::normalize() { // Inverse the quaternion inline void Quaternion::inverse() { - // Get the square length of the quaternion - decimal lengthSquareQuaternion = lengthSquare(); - - assert (lengthSquareQuaternion > MACHINE_EPSILON); - - // Compute and return the inverse quaternion - x /= -lengthSquareQuaternion; - y /= -lengthSquareQuaternion; - z /= -lengthSquareQuaternion; - w /= lengthSquareQuaternion; + // Use the conjugate of the current quaternion + x = -x; + y = -y; + z = -z; } // Return the unit quaternion @@ -261,13 +279,8 @@ inline Quaternion Quaternion::getConjugate() const { // Return the inverse of the quaternion (inline) inline Quaternion Quaternion::getInverse() const { - decimal lengthSquareQuaternion = lengthSquare(); - - assert (lengthSquareQuaternion > MACHINE_EPSILON); - - // Compute and return the inverse quaternion - return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion, - -z / lengthSquareQuaternion, w / lengthSquareQuaternion); + // Return the conjugate quaternion + return Quaternion(-x, -y, -z, w); } // Scalar product between two quaternions @@ -314,16 +327,35 @@ inline Quaternion Quaternion::operator*(decimal nb) const { // Overloaded operator for the multiplication of two quaternions inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const { + + /* The followin code is equivalent to this return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()), - w * quaternion.getVectorV() + quaternion.w * getVectorV() + - getVectorV().cross(quaternion.getVectorV())); + w * quaternion.getVectorV() + quaternion.w * getVectorV() + + getVectorV().cross(quaternion.getVectorV())); + */ + + return Quaternion(w * quaternion.x + quaternion.w * x + y * quaternion.z - z * quaternion.y, + w * quaternion.y + quaternion.w * y + z * quaternion.x - x * quaternion.z, + w * quaternion.z + quaternion.w * z + x * quaternion.y - y * quaternion.x, + w * quaternion.w - x * quaternion.x - y * quaternion.y - z * quaternion.z); } // Overloaded operator for the multiplication with a vector. /// This methods rotates a point given the rotation of a quaternion. inline Vector3 Quaternion::operator*(const Vector3& point) const { - Quaternion p(point.x, point.y, point.z, 0.0); - return (((*this) * p) * getConjugate()).getVectorV(); + + /* The following code is equivalent to this + * Quaternion p(point.x, point.y, point.z, 0.0); + * return (((*this) * p) * getConjugate()).getVectorV(); + */ + + const decimal prodX = w * point.x + y * point.z - z * point.y; + const decimal prodY = w * point.y + z * point.x - x * point.z; + const decimal prodZ = w * point.z + x * point.y - y * point.x; + const decimal prodW = -x * point.x - y * point.y - z * point.z; + return Vector3(w * prodX - prodY * z + prodZ * y - prodW * x, + w * prodY - prodZ * x + prodX * z - prodW * y, + w * prodZ - prodX * y + prodY * x - prodW * z); } // Overloaded operator for the assignment diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp index 4bc91253..d5cf0399 100644 --- a/src/mathematics/Transform.cpp +++ b/src/mathematics/Transform.cpp @@ -29,25 +29,3 @@ // Namespaces using namespace reactphysics3d; -// Constructor -Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Matrix3x3& orientation) - : mPosition(position), mOrientation(Quaternion(orientation)) { - -} - -// Constructor -Transform::Transform(const Vector3& position, const Quaternion& orientation) - : mPosition(position), mOrientation(orientation) { - -} - -// Copy-constructor -Transform::Transform(const Transform& transform) - : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { - -} diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index a93734cd..aeaf4234 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -118,6 +118,29 @@ class Transform { Transform& operator=(const Transform& transform); }; +// Constructor +inline Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) { + +} + +// Constructor +inline Transform::Transform(const Vector3& position, const Matrix3x3& orientation) + : mPosition(position), mOrientation(Quaternion(orientation)) { + +} + +// Constructor +inline Transform::Transform(const Vector3& position, const Quaternion& orientation) + : mPosition(position), mOrientation(orientation) { + +} + +// Copy-constructor +inline Transform::Transform(const Transform& transform) + : mPosition(transform.mPosition), mOrientation(transform.mOrientation) { + +} + // Return the position of the transform inline const Vector3& Transform::getPosition() const { return mPosition; @@ -169,8 +192,7 @@ inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const { // Return the inverse of the transform inline Transform Transform::getInverse() const { const Quaternion& invQuaternion = mOrientation.getInverse(); - Matrix3x3 invMatrix = invQuaternion.getMatrix(); - return Transform(invMatrix * (-mPosition), invQuaternion); + return Transform(invQuaternion * (-mPosition), invQuaternion); } // Return an interpolated transform @@ -195,13 +217,36 @@ inline Transform Transform::identity() { // Return the transformed vector inline Vector3 Transform::operator*(const Vector3& vector) const { - return (mOrientation.getMatrix() * vector) + mPosition; + return (mOrientation * vector) + mPosition; } // Operator of multiplication of a transform with another one inline Transform Transform::operator*(const Transform& transform2) const { - return Transform(mPosition + mOrientation.getMatrix() * transform2.mPosition, - mOrientation * transform2.mOrientation); + + // The following code is equivalent to this + //return Transform(mPosition + mOrientation * transform2.mPosition, + // mOrientation * transform2.mOrientation); + + const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.y * transform2.mPosition.z + - mOrientation.z * transform2.mPosition.y; + const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x + - mOrientation.x * transform2.mPosition.z; + const decimal prodZ = mOrientation.w * transform2.mPosition.z + mOrientation.x * transform2.mPosition.y + - mOrientation.y * transform2.mPosition.x; + const decimal prodW = -mOrientation.x * transform2.mPosition.x - mOrientation.y * transform2.mPosition.y + - mOrientation.z * transform2.mPosition.z; + + return Transform(Vector3(mPosition.x + mOrientation.w * prodX - prodY * mOrientation.z + prodZ * mOrientation.y - prodW * mOrientation.x, + mPosition.y + mOrientation.w * prodY - prodZ * mOrientation.x + prodX * mOrientation.z - prodW * mOrientation.y, + mPosition.z + mOrientation.w * prodZ - prodX * mOrientation.y + prodY * mOrientation.x - prodW * mOrientation.z), + Quaternion(mOrientation.w * transform2.mOrientation.x + transform2.mOrientation.w * mOrientation.x + + mOrientation.y * transform2.mOrientation.z - mOrientation.z * transform2.mOrientation.y, + mOrientation.w * transform2.mOrientation.y + transform2.mOrientation.w * mOrientation.y + + mOrientation.z * transform2.mOrientation.x - mOrientation.x * transform2.mOrientation.z, + mOrientation.w * transform2.mOrientation.z + transform2.mOrientation.w * mOrientation.z + + mOrientation.x * transform2.mOrientation.y - mOrientation.y * transform2.mOrientation.x, + mOrientation.w * transform2.mOrientation.w - mOrientation.x * transform2.mOrientation.x + - mOrientation.y * transform2.mOrientation.y - mOrientation.z * transform2.mOrientation.z)); } // Return true if the two transforms are equal diff --git a/src/mathematics/Vector2.cpp b/src/mathematics/Vector2.cpp index 2f225636..271fd82f 100644 --- a/src/mathematics/Vector2.cpp +++ b/src/mathematics/Vector2.cpp @@ -30,21 +30,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor -Vector2::Vector2() : x(0.0), y(0.0) { - -} - -// Constructor with arguments -Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { - -} - -// Copy-constructor -Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { - -} - // Return the corresponding unit vector Vector2 Vector2::getUnit() const { decimal lengthVector = length(); diff --git a/src/mathematics/Vector2.h b/src/mathematics/Vector2.h index 4b0e32f1..bc5f8872 100644 --- a/src/mathematics/Vector2.h +++ b/src/mathematics/Vector2.h @@ -156,6 +156,22 @@ struct Vector2 { friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2); }; +// Constructor +inline Vector2::Vector2() : x(0.0), y(0.0) { + +} + +// Constructor with arguments +inline Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) { + +} + +// Copy-constructor +inline Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) { + +} + + // Set the vector to zero inline void Vector2::setToZero() { x = 0; diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index ab2d126d..33b760ba 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -31,21 +31,6 @@ // Namespaces using namespace reactphysics3d; -// Constructor of the class Vector3D -Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { - -} - -// Constructor with arguments -Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { - -} - -// Copy-constructor -Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { - -} - // Return the corresponding unit vector Vector3 Vector3::getUnit() const { decimal lengthVector = length(); diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 1ca1adfd..95255fdb 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -168,6 +168,21 @@ struct Vector3 { friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2); }; +// Constructor of the class Vector3D +inline Vector3::Vector3() : x(0.0), y(0.0), z(0.0) { + +} + +// Constructor with arguments +inline Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) { + +} + +// Copy-constructor +inline Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) { + +} + // Set the vector to zero inline void Vector3::setToZero() { x = 0; diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp old mode 100644 new mode 100755 index 97bf6dcf..7e4a1427 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -1,59 +1,410 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "mathematics_functions.h" -#include "Vector3.h" - -using namespace reactphysics3d; - -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -/// This method uses the technique described in the book Real-Time collision detection by -/// Christer Ericson. -void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w) { - const Vector3 v0 = b - a; - const Vector3 v1 = c - a; - const Vector3 v2 = p - a; - - decimal d00 = v0.dot(v0); - decimal d01 = v0.dot(v1); - decimal d11 = v1.dot(v1); - decimal d20 = v2.dot(v0); - decimal d21 = v2.dot(v1); - - decimal denom = d00 * d11 - d01 * d01; - v = (d11 * d20 - d01 * d21) / denom; - w = (d00 * d21 - d01 * d20) / denom; - u = decimal(1.0) - v - w; -} - -// Clamp a vector such that it is no longer than a given maximum length -Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { - if (vector.lengthSquare() > maxLength * maxLength) { - return vector.getUnit() * maxLength; - } - return vector; -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "mathematics_functions.h" +#include "Vector3.h" +#include "Vector2.h" +#include +#include + +using namespace reactphysics3d; + + +// Function to test if two vectors are (almost) equal +bool reactphysics3d::approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) && + approxEqual(vec1.z, vec2.z, epsilon); +} + +// Function to test if two vectors are (almost) equal +bool reactphysics3d::approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) { + return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon); +} + +// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +// This method uses the technique described in the book Real-Time collision detection by +// Christer Ericson. +void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& p, decimal& u, decimal& v, decimal& w) { + const Vector3 v0 = b - a; + const Vector3 v1 = c - a; + const Vector3 v2 = p - a; + + decimal d00 = v0.dot(v0); + decimal d01 = v0.dot(v1); + decimal d11 = v1.dot(v1); + decimal d20 = v2.dot(v0); + decimal d21 = v2.dot(v1); + + decimal denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = decimal(1.0) - v - w; +} + +// Clamp a vector such that it is no longer than a given maximum length +Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { + if (vector.lengthSquare() > maxLength * maxLength) { + return vector.getUnit() * maxLength; + } + return vector; +} + +// Return true if two vectors are parallel +bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) { + return vector1.cross(vector2).lengthSquare() < decimal(0.00001); +} + +// Return true if two vectors are orthogonal +bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { + return std::abs(vector1.dot(vector2)) < decimal(0.001); +} + +// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" +Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { + + const Vector3 ab = segPointB - segPointA; + + decimal abLengthSquare = ab.lengthSquare(); + + // If the segment has almost zero length + if (abLengthSquare < MACHINE_EPSILON) { + + // Return one end-point of the segment as the closest point + return segPointA; + } + + // Project point C onto "AB" line + decimal t = (pointC - segPointA).dot(ab) / abLengthSquare; + + // If projected point onto the line is outside the segment, clamp it to the segment + if (t < decimal(0.0)) t = decimal(0.0); + if (t > decimal(1.0)) t = decimal(1.0); + + // Return the closest point on the segment + return segPointA + t * ab; +} + +// Compute the closest points between two segments +// This method uses the technique described in the book Real-Time +// collision detection by Christer Ericson. +void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2) { + + const Vector3 d1 = seg1PointB - seg1PointA; + const Vector3 d2 = seg2PointB - seg2PointA; + const Vector3 r = seg1PointA - seg2PointA; + decimal a = d1.lengthSquare(); + decimal e = d2.lengthSquare(); + decimal f = d2.dot(r); + decimal s, t; + + // If both segments degenerate into points + if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) { + + closestPointSeg1 = seg1PointA; + closestPointSeg2 = seg2PointA; + return; + } + if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point + + s = decimal(0.0); + + // Compute the closest point on second segment + t = clamp(f / e, decimal(0.0), decimal(1.0)); + } + else { + + decimal c = d1.dot(r); + + // If the second segment degenerates into a point + if (e <= MACHINE_EPSILON) { + + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else { + + decimal b = d1.dot(d2); + decimal denom = a * e - b * b; + + // If the segments are not parallel + if (denom != decimal(0.0)) { + + // Compute the closest point on line 1 to line 2 and + // clamp to first segment. + s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0)); + } + else { + + // Pick an arbitrary point on first segment + s = decimal(0.0); + } + + // Compute the point on line 2 closest to the closest point + // we have just found + t = (b * s + f) / e; + + // If this closest point is inside second segment (t in [0, 1]), we are done. + // Otherwise, we clamp the point to the second segment and compute again the + // closest point on segment 1 + if (t < decimal(0.0)) { + t = decimal(0.0); + s = clamp(-c / a, decimal(0.0), decimal(1.0)); + } + else if (t > decimal(1.0)) { + t = decimal(1.0); + s = clamp((b - c) / a, decimal(0.0), decimal(1.0)); + } + } + } + + // Compute the closest points on both segments + closestPointSeg1 = seg1PointA + d1 * s; + closestPointSeg2 = seg2PointA + d2 * t; +} + +// Compute the intersection between a plane and a segment +// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method +// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such +// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise, +// there is no intersection between the plane and the segment. +decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) { + + const decimal parallelEpsilon = decimal(0.0001); + decimal t = decimal(-1); + + decimal nDotAB = planeNormal.dot(segB - segA); + + // If the segment is not parallel to the plane + if (std::abs(nDotAB) > parallelEpsilon) { + t = (planeD - planeNormal.dot(segA)) / nDotAB; + } + + return t; +} + +// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB" +decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) { + + decimal distAB = (linePointB - linePointA).length(); + + if (distAB < MACHINE_EPSILON) { + return (point - linePointA).length(); + } + + return ((point - linePointA).cross(point - linePointB)).length() / distAB; +} + +// Clip a segment against multiple planes and return the clipped segment vertices +// This method implements the Sutherland–Hodgman clipping algorithm +List reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const List& planesPoints, + const List& planesNormals, + MemoryAllocator& allocator) { + + assert(planesPoints.size() == planesNormals.size()); + + List list1(allocator, 2); + List list2(allocator, 2); + + List* inputVertices = &list1; + List* outputVertices = &list2; + + inputVertices->add(segA); + inputVertices->add(segB); + + // For each clipping plane + for (uint p=0; psize() == 0) return *inputVertices; + + assert(inputVertices->size() == 2); + + outputVertices->clear(); + + Vector3& v1 = (*inputVertices)[0]; + Vector3& v2 = (*inputVertices)[1]; + + decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); + decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); + + // If the second vertex is in front of the clippling plane + if (v2DotN >= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outputVertices->add(v1 + t * (v2 - v1)); + } + else { + outputVertices->add(v2); + } + } + else { + outputVertices->add(v1); + } + + // Add the second vertex + outputVertices->add(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + outputVertices->add(v1); + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outputVertices->add(v1 + t * (v2 - v1)); + } + } + } + + inputVertices = outputVertices; + outputVertices = p % 2 == 0 ? &list1 : &list2; + } + + return *outputVertices; +} + +// Clip a polygon against multiple planes and return the clipped polygon vertices +// This method implements the Sutherland–Hodgman clipping algorithm +List reactphysics3d::clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, + const List& planesNormals, MemoryAllocator& allocator) { + + assert(planesPoints.size() == planesNormals.size()); + + uint nbMaxElements = polygonVertices.size() + planesPoints.size(); + List list1(allocator, nbMaxElements); + List list2(allocator, nbMaxElements); + + const List* inputVertices = &polygonVertices; + List* outputVertices = &list2; + + // For each clipping plane + for (uint p=0; pclear(); + + uint nbInputVertices = inputVertices->size(); + uint vStart = nbInputVertices - 1; + + // For each edge of the polygon + for (uint vEnd = 0; vEnd= decimal(0.0)) { + + // If the first vertex is not in front of the clippling plane + if (v1DotN < decimal(0.0)) { + + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]); + + if (t >= decimal(0) && t <= decimal(1.0)) { + outputVertices->add(v1 + t * (v2 - v1)); + } + else { + outputVertices->add(v2); + } + } + + // Add the second vertex + outputVertices->add(v2); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (v1DotN >= decimal(0.0)) { + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outputVertices->add(v1 + t * (v2 - v1)); + } + else { + outputVertices->add(v1); + } + } + } + + vStart = vEnd; + } + + inputVertices = outputVertices; + outputVertices = p % 2 == 0 ? &list1 : &list2; + } + + return *outputVertices; +} + +// Project a point onto a plane that is given by a point and its unit length normal +Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector3& unitPlaneNormal, const Vector3& planePoint) { + return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal; +} + +// Return true if the given number is prime +bool reactphysics3d::isPrimeNumber(int number) { + + // If it's a odd number + if ((number & 1) != 0) { + + int limit = static_cast(std::sqrt(number)); + + for (int divisor = 3; divisor <= limit; divisor += 2) { + + // If we have found a divisor + if ((number % divisor) == 0) { + + // It is not a prime number + return false; + } + } + + return true; + } + + return number == 2; +} + + diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h old mode 100644 new mode 100755 index 7efe6d44..a72b0077 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -1,87 +1,133 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H -#define REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H - -// Libraries -#include "configuration.h" -#include "decimal.h" -#include -#include -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -struct Vector3; - -// ---------- Mathematics functions ---------- // - -/// Function to test if two real numbers are (almost) equal -/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] -inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { - return (std::fabs(a - b) < epsilon); -} - -/// Function that returns the result of the "value" clamped by -/// two others values "lowerLimit" and "upperLimit" -inline int clamp(int value, int lowerLimit, int upperLimit) { - assert(lowerLimit <= upperLimit); - return std::min(std::max(value, lowerLimit), upperLimit); -} - -/// Function that returns the result of the "value" clamped by -/// two others values "lowerLimit" and "upperLimit" -inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { - assert(lowerLimit <= upperLimit); - return std::min(std::max(value, lowerLimit), upperLimit); -} - -/// Return the minimum value among three values -inline decimal min3(decimal a, decimal b, decimal c) { - return std::min(std::min(a, b), c); -} - -/// Return the maximum value among three values -inline decimal max3(decimal a, decimal b, decimal c) { - return std::max(std::max(a, b), c); -} - -/// Return true if two values have the same sign -inline bool sameSign(decimal a, decimal b) { - return a * b >= decimal(0.0); -} - -/// Clamp a vector such that it is no longer than a given maximum length -Vector3 clamp(const Vector3& vector, decimal maxLength); - -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, - const Vector3& p, decimal& u, decimal& v, decimal& w); - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H +#define REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H + +// Libraries +#include "configuration.h" +#include "decimal.h" +#include +#include +#include +#include +#include "containers/List.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +struct Vector3; +struct Vector2; + +// ---------- Mathematics functions ---------- // + +/// Function to test if two real numbers are (almost) equal +/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] +inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) { + return (std::fabs(a - b) < epsilon); +} + +/// Function to test if two vectors are (almost) equal +bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON); + +/// Function to test if two vectors are (almost) equal +bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON); + +/// Function that returns the result of the "value" clamped by +/// two others values "lowerLimit" and "upperLimit" +inline int clamp(int value, int lowerLimit, int upperLimit) { + assert(lowerLimit <= upperLimit); + return std::min(std::max(value, lowerLimit), upperLimit); +} + +/// Function that returns the result of the "value" clamped by +/// two others values "lowerLimit" and "upperLimit" +inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) { + assert(lowerLimit <= upperLimit); + return std::min(std::max(value, lowerLimit), upperLimit); +} + +/// Return the minimum value among three values +inline decimal min3(decimal a, decimal b, decimal c) { + return std::min(std::min(a, b), c); +} + +/// Return the maximum value among three values +inline decimal max3(decimal a, decimal b, decimal c) { + return std::max(std::max(a, b), c); +} + +/// Return true if two values have the same sign +inline bool sameSign(decimal a, decimal b) { + return a * b >= decimal(0.0); +} + +/// Return true if two vectors are parallel +bool areParallelVectors(const Vector3& vector1, const Vector3& vector2); + +/// Return true if two vectors are orthogonal +bool areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2); + +/// Clamp a vector such that it is no longer than a given maximum length +Vector3 clamp(const Vector3& vector, decimal maxLength); + +// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" +Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC); + +// Compute the closest points between two segments +void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB, + const Vector3& seg2PointA, const Vector3& seg2PointB, + Vector3& closestPointSeg1, Vector3& closestPointSeg2); + +/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) +void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& p, decimal& u, decimal& v, decimal& w); + +/// Compute the intersection between a plane and a segment +decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal); + +/// Compute the distance between a point and a line +decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point); + +/// Clip a segment against multiple planes and return the clipped segment vertices +List clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB, + const List& planesPoints, + const List& planesNormals, + MemoryAllocator& allocator); + +/// Clip a polygon against multiple planes and return the clipped polygon vertices +List clipPolygonWithPlanes(const List& polygonVertices, const List& planesPoints, + const List& planesNormals, MemoryAllocator& allocator); + +/// Project a point onto a plane that is given by a point and its unit length normal +Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint); + +/// Return true if the given number is prime +bool isPrimeNumber(int number); + +} + + +#endif diff --git a/src/memory/DefaultAllocator.h b/src/memory/DefaultAllocator.h new file mode 100644 index 00000000..ac4ddd04 --- /dev/null +++ b/src/memory/DefaultAllocator.h @@ -0,0 +1,64 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_DEFAULT_ALLOCATOR_H +#define REACTPHYSICS3D_DEFAULT_ALLOCATOR_H + +// Libraries +#include "memory/MemoryAllocator.h" +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class DefaultAllocator +/** + * This class represents a default memory allocator that uses default malloc/free methods + */ +class DefaultAllocator : public MemoryAllocator { + + public: + + /// Destructor + virtual ~DefaultAllocator() = default; + + /// Assignment operator + DefaultAllocator& operator=(DefaultAllocator& allocator) = default; + + /// Allocate memory of a given size (in bytes) and return a pointer to the + /// allocated memory. + virtual void* allocate(size_t size) override { + return malloc(size); + } + + /// Release previously allocated memory. + virtual void release(void* pointer, size_t size) override { + free(pointer); + } +}; + +} + +#endif diff --git a/src/memory/Allocator.h b/src/memory/MemoryAllocator.h similarity index 87% rename from src/memory/Allocator.h rename to src/memory/MemoryAllocator.h index f7b9ac73..c0d86a33 100644 --- a/src/memory/Allocator.h +++ b/src/memory/MemoryAllocator.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_ALLOCATOR_H -#define REACTPHYSICS3D_ALLOCATOR_H +#ifndef REACTPHYSICS3D_MEMORY_ALLOCATOR_H +#define REACTPHYSICS3D_MEMORY_ALLOCATOR_H // Libraries #include @@ -32,16 +32,22 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -// Class Allocator +// Class MemoryAllocator /** * Abstract class with the basic interface of all the derived memory allocators */ -class Allocator { +class MemoryAllocator { public: + /// Constructor + MemoryAllocator() = default; + /// Destructor - virtual ~Allocator() = default; + virtual ~MemoryAllocator() = default; + + /// Assignment operator + MemoryAllocator& operator=(MemoryAllocator& allocator) = default; /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. diff --git a/src/collision/narrowphase/EPA/TrianglesStore.cpp b/src/memory/MemoryManager.cpp similarity index 89% rename from src/collision/narrowphase/EPA/TrianglesStore.cpp rename to src/memory/MemoryManager.cpp index 93fb95a6..99a32655 100644 --- a/src/collision/narrowphase/EPA/TrianglesStore.cpp +++ b/src/memory/MemoryManager.cpp @@ -1,6 +1,6 @@ /******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * +* Copyright (c) 2010-2015 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * @@ -24,12 +24,11 @@ ********************************************************************************/ // Libraries -#include "TrianglesStore.h" +#include "MemoryManager.h" -// We use the ReactPhysics3D namespace using namespace reactphysics3d; -// Constructor -TrianglesStore::TrianglesStore() : mNbTriangles(0) { - -} +// Static variables +DefaultAllocator MemoryManager::mDefaultAllocator; +MemoryAllocator* MemoryManager::mBaseAllocator = &mDefaultAllocator; + diff --git a/src/memory/MemoryManager.h b/src/memory/MemoryManager.h new file mode 100644 index 00000000..8d986a31 --- /dev/null +++ b/src/memory/MemoryManager.h @@ -0,0 +1,144 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_MEMORY_MANAGER_H +#define REACTPHYSICS3D_MEMORY_MANAGER_H + +// Libraries +#include "memory/MemoryAllocator.h" +#include "memory/DefaultAllocator.h" +#include "memory/PoolAllocator.h" +#include "memory/SingleFrameAllocator.h" + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class MemoryManager +/** + * The memory manager is used to store the different memory allocators that are used + * by the library. + */ +class MemoryManager { + + private: + + /// Default malloc/free memory allocator + static DefaultAllocator mDefaultAllocator; + + /// Pointer to the base memory allocator to use + static MemoryAllocator* mBaseAllocator; + + /// Memory pool allocator + PoolAllocator mPoolAllocator; + + /// Single frame stack allocator + SingleFrameAllocator mSingleFrameAllocator; + + public: + + /// Memory allocation types + enum class AllocationType { + Base, // Base memory allocator + Pool, // Memory pool allocator + Frame, // Single frame memory allocator + }; + + /// Constructor + MemoryManager() = default; + + /// Destructor + ~MemoryManager() = default; + + /// Allocate memory of a given type + void* allocate(AllocationType allocationType, size_t size); + + /// Release previously allocated memory. + void release(AllocationType allocationType, void* pointer, size_t size); + + /// Return the pool allocator + PoolAllocator& getPoolAllocator(); + + /// Return the single frame stack allocator + SingleFrameAllocator& getSingleFrameAllocator(); + + /// Return the base memory allocator + static MemoryAllocator& getBaseAllocator(); + + /// Set the base memory allocator + static void setBaseAllocator(MemoryAllocator* memoryAllocator); + + /// Reset the single frame allocator + void resetFrameAllocator(); +}; + +// Allocate memory of a given type +inline void* MemoryManager::allocate(AllocationType allocationType, size_t size) { + + switch (allocationType) { + case AllocationType::Base: return mBaseAllocator->allocate(size); break; + case AllocationType::Pool: return mPoolAllocator.allocate(size); break; + case AllocationType::Frame: return mSingleFrameAllocator.allocate(size); break; + } +} + +// Release previously allocated memory. +inline void MemoryManager::release(AllocationType allocationType, void* pointer, size_t size) { + + switch (allocationType) { + case AllocationType::Base: mBaseAllocator->release(pointer, size); break; + case AllocationType::Pool: mPoolAllocator.release(pointer, size); break; + case AllocationType::Frame: mSingleFrameAllocator.release(pointer, size); break; + } +} + +// Return the pool allocator +inline PoolAllocator& MemoryManager::getPoolAllocator() { + return mPoolAllocator; +} + +// Return the single frame stack allocator +inline SingleFrameAllocator& MemoryManager::getSingleFrameAllocator() { + return mSingleFrameAllocator; +} + +// Return the base memory allocator +inline MemoryAllocator& MemoryManager::getBaseAllocator() { + return *mBaseAllocator; +} + +// Set the base memory allocator +inline void MemoryManager::setBaseAllocator(MemoryAllocator* baseAllocator) { + mBaseAllocator = baseAllocator; +} + +// Reset the single frame allocator +inline void MemoryManager::resetFrameAllocator() { + mSingleFrameAllocator.reset(); +} + +} + +#endif + diff --git a/src/memory/PoolAllocator.cpp b/src/memory/PoolAllocator.cpp index dc1306bc..2a9507c4 100644 --- a/src/memory/PoolAllocator.cpp +++ b/src/memory/PoolAllocator.cpp @@ -25,6 +25,7 @@ // Libraries #include "PoolAllocator.h" +#include "MemoryManager.h" #include #include @@ -42,7 +43,7 @@ PoolAllocator::PoolAllocator() { mNbAllocatedMemoryBlocks = 64; mNbCurrentMemoryBlocks = 0; const size_t sizeToAllocate = mNbAllocatedMemoryBlocks * sizeof(MemoryBlock); - mMemoryBlocks = (MemoryBlock*) malloc(sizeToAllocate); + mMemoryBlocks = static_cast(MemoryManager::getBaseAllocator().allocate(sizeToAllocate)); memset(mMemoryBlocks, 0, sizeToAllocate); memset(mFreeMemoryUnits, 0, sizeof(mFreeMemoryUnits)); @@ -55,7 +56,7 @@ PoolAllocator::PoolAllocator() { // Initialize the array that contains the sizes the memory units that will // be allocated in each different heap - for (int i=0; i < NB_HEAPS; i++) { + for (uint i=0; i < NB_HEAPS; i++) { mUnitSizes[i] = (i+1) * 8; } @@ -82,16 +83,17 @@ PoolAllocator::~PoolAllocator() { // Release the memory allocated for each block for (uint i=0; i MAX_UNIT_SIZE) { - // Allocate memory using standard malloc() function - return malloc(size); + // Allocate memory using default allocation + return MemoryManager::getBaseAllocator().allocate(size); } // Get the index of the heap that will take care of the allocation request @@ -126,13 +128,13 @@ void* PoolAllocator::allocate(size_t size) { } else { // If there is no more free memory units in the corresponding heap - // If we need to allocate more memory to containsthe blocks + // If we need to allocate more memory to contains the blocks if (mNbCurrentMemoryBlocks == mNbAllocatedMemoryBlocks) { // Allocate more memory to contain the blocks MemoryBlock* currentMemoryBlocks = mMemoryBlocks; mNbAllocatedMemoryBlocks += 64; - mMemoryBlocks = (MemoryBlock*) malloc(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock)); + mMemoryBlocks = static_cast(MemoryManager::getBaseAllocator().allocate(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock))); memcpy(mMemoryBlocks, currentMemoryBlocks,mNbCurrentMemoryBlocks * sizeof(MemoryBlock)); memset(mMemoryBlocks + mNbCurrentMemoryBlocks, 0, 64 * sizeof(MemoryBlock)); free(currentMemoryBlocks); @@ -141,17 +143,22 @@ void* PoolAllocator::allocate(size_t size) { // Allocate a new memory blocks for the corresponding heap and divide it in many // memory units MemoryBlock* newBlock = mMemoryBlocks + mNbCurrentMemoryBlocks; - newBlock->memoryUnits = (MemoryUnit*) malloc(BLOCK_SIZE); + newBlock->memoryUnits = static_cast(MemoryManager::getBaseAllocator().allocate(BLOCK_SIZE)); assert(newBlock->memoryUnits != nullptr); size_t unitSize = mUnitSizes[indexHeap]; uint nbUnits = BLOCK_SIZE / unitSize; assert(nbUnits * unitSize <= BLOCK_SIZE); + void* memoryUnitsStart = static_cast(newBlock->memoryUnits); + char* memoryUnitsStartChar = static_cast(memoryUnitsStart); for (size_t i=0; i < nbUnits - 1; i++) { - MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i); - MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1)); + void* unitPointer = static_cast(memoryUnitsStartChar + unitSize * i); + void* nextUnitPointer = static_cast(memoryUnitsStartChar + unitSize * (i+1)); + MemoryUnit* unit = static_cast(unitPointer); + MemoryUnit* nextUnit = static_cast(nextUnitPointer); unit->nextUnit = nextUnit; } - MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1)); + void* lastUnitPointer = static_cast(memoryUnitsStartChar + unitSize*(nbUnits-1)); + MemoryUnit* lastUnit = static_cast(lastUnitPointer); lastUnit->nextUnit = nullptr; // Add the new allocated block into the list of free memory units in the heap @@ -176,8 +183,8 @@ void PoolAllocator::release(void* pointer, size_t size) { // If the size is larger than the maximum memory unit size if (size > MAX_UNIT_SIZE) { - // Release the memory using the standard free() function - free(pointer); + // Release the memory using the default deallocation + MemoryManager::getBaseAllocator().release(pointer, size); return; } @@ -187,7 +194,7 @@ void PoolAllocator::release(void* pointer, size_t size) { // Insert the released memory unit into the list of free memory units of the // corresponding heap - MemoryUnit* releasedUnit = (MemoryUnit*) pointer; + MemoryUnit* releasedUnit = static_cast(pointer); releasedUnit->nextUnit = mFreeMemoryUnits[indexHeap]; mFreeMemoryUnits[indexHeap] = releasedUnit; } diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h index 71e11714..25f31198 100644 --- a/src/memory/PoolAllocator.h +++ b/src/memory/PoolAllocator.h @@ -28,7 +28,7 @@ // Libraries #include "configuration.h" -#include "Allocator.h" +#include "MemoryAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -40,7 +40,7 @@ namespace reactphysics3d { * efficiently. This implementation is inspired by the small block allocator * described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp */ -class PoolAllocator : public Allocator { +class PoolAllocator : public MemoryAllocator { private : @@ -94,7 +94,7 @@ class PoolAllocator : public Allocator { /// Size of the memory units that each heap is responsible to allocate static size_t mUnitSizes[NB_HEAPS]; - /// Lookup table that mape size to allocate to the index of the + /// Lookup table that map the size to allocate to the index of the /// corresponding heap we will use for the allocation. static int mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; @@ -131,13 +131,15 @@ class PoolAllocator : public Allocator { /// Destructor virtual ~PoolAllocator() override; + /// Assignment operator + PoolAllocator& operator=(PoolAllocator& allocator) = default; + /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. virtual void* allocate(size_t size) override; /// Release previously allocated memory. virtual void release(void* pointer, size_t size) override; - }; } diff --git a/src/memory/SingleFrameAllocator.cpp b/src/memory/SingleFrameAllocator.cpp index a2e436cd..c9d6533a 100644 --- a/src/memory/SingleFrameAllocator.cpp +++ b/src/memory/SingleFrameAllocator.cpp @@ -25,17 +25,19 @@ // Libraries #include "SingleFrameAllocator.h" +#include "MemoryManager.h" #include #include using namespace reactphysics3d; // Constructor -SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes) - : mTotalSizeBytes(totalSizeBytes), mCurrentOffset(0) { +SingleFrameAllocator::SingleFrameAllocator() + : mTotalSizeBytes(INIT_SINGLE_FRAME_ALLOCATOR_BYTES), + mCurrentOffset(0), mNbFramesTooMuchAllocated(0), mNeedToAllocatedMore(false) { // Allocate a whole block of memory at the beginning - mMemoryBufferStart = static_cast(malloc(mTotalSizeBytes)); + mMemoryBufferStart = static_cast(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes)); assert(mMemoryBufferStart != nullptr); } @@ -43,7 +45,7 @@ SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes) SingleFrameAllocator::~SingleFrameAllocator() { // Release the memory allocated at the beginning - free(mMemoryBufferStart); + MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes); } @@ -52,14 +54,13 @@ SingleFrameAllocator::~SingleFrameAllocator() { void* SingleFrameAllocator::allocate(size_t size) { // Check that there is enough remaining memory in the buffer - if (static_cast(mCurrentOffset) + size > mTotalSizeBytes) { + if (mCurrentOffset + size > mTotalSizeBytes) { - // This should never occur. If it does, you must increase the initial - // size of memory of this allocator - assert(false); + // We need to allocate more memory next time reset() is called + mNeedToAllocatedMore = true; - // Return null - return nullptr; + // Return default memory allocation + return MemoryManager::getBaseAllocator().allocate(size); } // Next available memory location @@ -72,9 +73,63 @@ void* SingleFrameAllocator::allocate(size_t size) { return nextAvailableMemory; } +// Release previously allocated memory. +void SingleFrameAllocator::release(void* pointer, size_t size) { + + // If allocated memory is not within the single frame allocation range + char* p = static_cast(pointer); + if (p < mMemoryBufferStart || p > mMemoryBufferStart + mTotalSizeBytes) { + + // Use default deallocation + MemoryManager::getBaseAllocator().release(pointer, size); + } +} + // Reset the marker of the current allocated memory void SingleFrameAllocator::reset() { + // If too much memory is allocated + if (mCurrentOffset < mTotalSizeBytes / 2) { + + mNbFramesTooMuchAllocated++; + + if (mNbFramesTooMuchAllocated > NB_FRAMES_UNTIL_SHRINK) { + + // Release the memory allocated at the beginning + MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes); + + // Divide the total memory to allocate by two + mTotalSizeBytes /= 2; + if (mTotalSizeBytes <= 0) mTotalSizeBytes = 1; + + // Allocate a whole block of memory at the beginning + mMemoryBufferStart = static_cast(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes)); + assert(mMemoryBufferStart != nullptr); + + mNbFramesTooMuchAllocated = 0; + } + } + else { + mNbFramesTooMuchAllocated = 0; + } + + // If we need to allocate more memory + if (mNeedToAllocatedMore) { + + // Release the memory allocated at the beginning + MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes); + + // Multiply the total memory to allocate by two + mTotalSizeBytes *= 2; + + // Allocate a whole block of memory at the beginning + mMemoryBufferStart = static_cast(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes)); + assert(mMemoryBufferStart != nullptr); + + mNeedToAllocatedMore = false; + mNbFramesTooMuchAllocated = 0; + } + // Reset the current offset at the beginning of the block mCurrentOffset = 0; } diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h index cc030daa..d3142908 100644 --- a/src/memory/SingleFrameAllocator.h +++ b/src/memory/SingleFrameAllocator.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H // Libraries -#include "Allocator.h" +#include "MemoryAllocator.h" #include "configuration.h" /// ReactPhysics3D namespace @@ -38,10 +38,16 @@ namespace reactphysics3d { * This class represent a memory allocator used to efficiently allocate * memory on the heap that is used during a single frame. */ -class SingleFrameAllocator : public Allocator { +class SingleFrameAllocator : public MemoryAllocator { private : + // -------------------- Constants -------------------- // + + /// Number of frames to wait before shrinking the allocated + /// memory if too much is allocated + static const int NB_FRAMES_UNTIL_SHRINK = 120; + // -------------------- Attributes -------------------- // /// Total size (in bytes) of memory of the allocator @@ -51,29 +57,36 @@ class SingleFrameAllocator : public Allocator { char* mMemoryBufferStart; /// Pointer to the next available memory location in the buffer - int mCurrentOffset; + size_t mCurrentOffset; + + /// Current number of frames since we detected too much memory + /// is allocated + size_t mNbFramesTooMuchAllocated; + + /// True if we need to allocate more memory in the next reset() call + bool mNeedToAllocatedMore; public : // -------------------- Methods -------------------- // /// Constructor - SingleFrameAllocator(size_t totalSizeBytes); + SingleFrameAllocator(); /// Destructor virtual ~SingleFrameAllocator() override; + /// Assignment operator + SingleFrameAllocator& operator=(SingleFrameAllocator& allocator) = default; + /// Allocate memory of a given size (in bytes) virtual void* allocate(size_t size) override; /// Release previously allocated memory. - virtual void release(void* pointer, size_t size) override { } + virtual void release(void* pointer, size_t size) override; /// Reset the marker of the current allocated memory virtual void reset(); - - - }; } diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 6f1e42f4..dd2a6265 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -46,17 +46,20 @@ #include "collision/shapes/CollisionShape.h" #include "collision/shapes/BoxShape.h" #include "collision/shapes/SphereShape.h" -#include "collision/shapes/ConeShape.h" -#include "collision/shapes/CylinderShape.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/ConvexMeshShape.h" #include "collision/shapes/ConcaveMeshShape.h" #include "collision/shapes/HeightFieldShape.h" +#include "collision/PolyhedronMesh.h" #include "collision/shapes/AABB.h" #include "collision/ProxyShape.h" #include "collision/RaycastInfo.h" #include "collision/TriangleMesh.h" +#include "collision/PolyhedronMesh.h" #include "collision/TriangleVertexArray.h" +#include "collision/PolygonVertexArray.h" +#include "collision/CollisionCallback.h" +#include "collision/OverlapCallback.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h" #include "constraint/HingeJoint.h" diff --git a/test/Test.h b/test/Test.h index 37fa9dee..1288813d 100644 --- a/test/Test.h +++ b/test/Test.h @@ -35,7 +35,7 @@ namespace reactphysics3d { // Macros -#define test(condition) applyTest(condition, #condition, __FILE__, __LINE__) +#define test(condition) applyTest(condition, #condition, __FILE__, __LINE__); #define fail(text) applyFail(text, __FILE__, __LINE__); // Class Test diff --git a/test/main.cpp b/test/main.cpp index bf949030..8bb62ea4 100644 --- a/test/main.cpp +++ b/test/main.cpp @@ -37,6 +37,10 @@ #include "tests/collision/TestCollisionWorld.h" #include "tests/collision/TestAABB.h" #include "tests/collision/TestDynamicAABBTree.h" +#include "tests/collision/TestHalfEdgeStructure.h" +#include "tests/collision/TestTriangleVertexArray.h" +#include "tests/containers/TestList.h" +#include "tests/containers/TestMap.h" using namespace reactphysics3d; @@ -44,6 +48,11 @@ int main() { TestSuite testSuite("ReactPhysics3D Tests"); + // ---------- Containers tests ---------- // + + testSuite.addTest(new TestList("List")); + testSuite.addTest(new TestMap("Map")); + // ---------- Mathematics tests ---------- // testSuite.addTest(new TestVector2("Vector2")); @@ -58,9 +67,11 @@ int main() { testSuite.addTest(new TestAABB("AABB")); testSuite.addTest(new TestPointInside("IsPointInside")); + testSuite.addTest(new TestTriangleVertexArray("TriangleVertexArray")); testSuite.addTest(new TestRaycast("Raycasting")); testSuite.addTest(new TestCollisionWorld("CollisionWorld")); testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree")); + testSuite.addTest(new TestHalfEdgeStructure("HalfEdgeStructure")); // Run the tests testSuite.run(); diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h index 32d03f45..4328625c 100644 --- a/test/tests/collision/TestAABB.h +++ b/test/tests/collision/TestAABB.h @@ -72,7 +72,7 @@ class TestAABB : public Test { } /// Destructor - ~TestAABB() { + virtual ~TestAABB() { } diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h old mode 100644 new mode 100755 index 94d44a98..ad0c92cf --- a/test/tests/collision/TestCollisionWorld.h +++ b/test/tests/collision/TestCollisionWorld.h @@ -40,23 +40,108 @@ enum CollisionCategory { CATEGORY_3 = 0x0004 }; -// TODO : Add test for concave shape collision here +// Contact point collision data +struct CollisionPointData { + + Vector3 localPointBody1; + Vector3 localPointBody2; + decimal penetrationDepth; + + CollisionPointData(const Vector3& point1, const Vector3& point2, decimal penDepth) { + localPointBody1 = point1; + localPointBody2 = point2; + penetrationDepth = penDepth; + } + + bool isContactPointSimilarTo(const Vector3& pointBody1, const Vector3& pointBody2, decimal penDepth, decimal epsilon = 0.001) { + + return approxEqual(pointBody1, localPointBody1, epsilon) && + approxEqual(pointBody2, localPointBody2, epsilon) && + approxEqual(penetrationDepth, penDepth, epsilon); + } +}; + +// Contact manifold collision data +struct CollisionManifoldData { + + std::vector contactPoints; + + int getNbContactPoints() const { + return contactPoints.size(); + } + + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + + std::vector::iterator it; + for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { + + if (it->isContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { + return true; + } + } + + return false; + } + +}; + +// Collision data between two proxy shapes +struct CollisionData { + + std::pair proxyShapes; + std::pair bodies; + std::vector contactManifolds; + + int getNbContactManifolds() const { + return contactManifolds.size(); + } + + int getTotalNbContactPoints() const { + + int nbPoints = 0; + + std::vector::const_iterator it; + for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) { + + nbPoints += it->getNbContactPoints(); + } + + return nbPoints; + } + + bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) { + + std::vector::iterator it; + for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) { + + if (it->hasContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) { + return true; + } + } + + return false; + } + +}; // Class class WorldCollisionCallback : public CollisionCallback { + private: + + std::vector> mCollisionData; + + std::pair getCollisionKeyPair(std::pair pair) const { + + if (pair.first > pair.second) { + return std::make_pair(pair.second, pair.first); + } + + return pair; + } + public: - bool boxCollideWithSphere1; - bool boxCollideWithCylinder; - bool sphere1CollideWithCylinder; - bool sphere1CollideWithSphere2; - - CollisionBody* boxBody; - CollisionBody* sphere1Body; - CollisionBody* sphere2Body; - CollisionBody* cylinderBody; - WorldCollisionCallback() { reset(); @@ -64,38 +149,79 @@ class WorldCollisionCallback : public CollisionCallback void reset() { - boxCollideWithSphere1 = false; - boxCollideWithCylinder = false; - sphere1CollideWithCylinder = false; - sphere1CollideWithSphere2 = false; + mCollisionData.clear(); } - // This method will be called for contact + bool hasContacts() const { + return mCollisionData.size() > 0; + } + + bool areProxyShapesColliding(const ProxyShape* proxyShape1, const ProxyShape* proxyShape2) { + return std::find(mCollisionData.begin(), mCollisionData.end(), getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))) != mCollisionData.end(); + } + + // This method will be called for each contact virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) { - boxCollideWithSphere1 = true; - } - else if (isContactBetweenBodies(boxBody, cylinderBody, collisionCallbackInfo)) { - boxCollideWithCylinder = true; - } - else if (isContactBetweenBodies(sphere1Body, cylinderBody, collisionCallbackInfo)) { - sphere1CollideWithCylinder = true; - } - else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) { - sphere1CollideWithSphere2 = true; - } - } + CollisionData collisionData; + collisionData.bodies = std::make_pair(collisionCallbackInfo.body1, collisionCallbackInfo.body2); + collisionData.proxyShapes = std::make_pair(collisionCallbackInfo.proxyShape1, collisionCallbackInfo.proxyShape2); - bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2, - const CollisionCallbackInfo& collisionCallbackInfo) { - return (collisionCallbackInfo.body1->getID() == body1->getID() && - collisionCallbackInfo.body2->getID() == body2->getID()) || - (collisionCallbackInfo.body2->getID() == body1->getID() && - collisionCallbackInfo.body1->getID() == body2->getID()); + ContactManifoldListElement* element = collisionCallbackInfo.contactManifoldElements; + while (element != nullptr) { + + ContactManifold* contactManifold = element->getContactManifold(); + + CollisionManifoldData collisionManifold; + + ContactPoint* contactPoint = contactManifold->getContactPoints(); + while (contactPoint != nullptr) { + + CollisionPointData collisionPoint(contactPoint->getLocalPointOnShape1(), contactPoint->getLocalPointOnShape2(), contactPoint->getPenetrationDepth()); + collisionManifold.contactPoints.push_back(collisionPoint); + + contactPoint = contactPoint->getNext(); + } + + collisionData.contactManifolds.push_back(collisionManifold); + + element = element->getNext(); + } } }; +/// Overlap callback +class WorldOverlapCallback : public OverlapCallback { + + private: + + CollisionBody* mOverlapBody; + + public: + + /// Destructor + virtual ~WorldOverlapCallback() { + reset(); + } + + /// This method will be called for each reported overlapping bodies + virtual void notifyOverlap(CollisionBody* collisionBody) override { + + } + + void reset() { + mOverlapBody = nullptr; + } + + bool hasOverlap() const { + return mOverlapBody != nullptr; + } + + CollisionBody* getOverlapBody() { + return mOverlapBody; + } +}; + // Class TestCollisionWorld /** * Unit test for the CollisionWorld class. @@ -110,25 +236,29 @@ class TestCollisionWorld : public Test { CollisionWorld* mWorld; // Bodies - CollisionBody* mBoxBody; - CollisionBody* mSphere1Body; - CollisionBody* mSphere2Body; - CollisionBody* mCylinderBody; + CollisionBody* mBoxBody1; + CollisionBody* mBoxBody2; + CollisionBody* mSphereBody1; + CollisionBody* mSphereBody2; // Collision shapes - BoxShape* mBoxShape; - SphereShape* mSphereShape; - CylinderShape* mCylinderShape; + BoxShape* mBoxShape1; + BoxShape* mBoxShape2; + SphereShape* mSphereShape1; + SphereShape* mSphereShape2; // Proxy shapes - ProxyShape* mBoxProxyShape; - ProxyShape* mSphere1ProxyShape; - ProxyShape* mSphere2ProxyShape; - ProxyShape* mCylinderProxyShape; + ProxyShape* mBoxProxyShape1; + ProxyShape* mBoxProxyShape2; + ProxyShape* mSphereProxyShape1; + ProxyShape* mSphereProxyShape2; - // Collision callback class + // Collision callback WorldCollisionCallback mCollisionCallback; + // Overlap callback + WorldOverlapCallback mOverlapCallback; + public : // ---------- Methods ---------- // @@ -136,193 +266,243 @@ class TestCollisionWorld : public Test { /// Constructor TestCollisionWorld(const std::string& name) : Test(name) { - // Create the world + // Create the collision world mWorld = new CollisionWorld(); - // Create the bodies - Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity()); - mBoxBody = mWorld->createCollisionBody(boxTransform); - mBoxShape = new BoxShape(Vector3(3, 3, 3)); - mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity()); + // ---------- Boxes ---------- // + Transform boxTransform1(Vector3(-20, 20, 0), Quaternion::identity()); + mBoxBody1 = mWorld->createCollisionBody(boxTransform1); + mBoxShape1 = new BoxShape(Vector3(3, 3, 3)); + mBoxProxyShape1 = mBoxBody1->addCollisionShape(mBoxShape1, Transform::identity()); - mSphereShape = new SphereShape(3.0); - Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity()); - mSphere1Body = mWorld->createCollisionBody(sphere1Transform); - mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity()); + Transform boxTransform2(Vector3(-10, 20, 0), Quaternion::identity()); + mBoxBody2 = mWorld->createCollisionBody(boxTransform2); + mBoxShape2 = new BoxShape(Vector3(2, 2, 2)); + mBoxProxyShape2 = mBoxBody2->addCollisionShape(mBoxShape1, Transform::identity()); - Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity()); - mSphere2Body = mWorld->createCollisionBody(sphere2Transform); - mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity()); + // ---------- Spheres ---------- // + mSphereShape1 = new SphereShape(3.0); + Transform sphereTransform1(Vector3(10, 20, 0), Quaternion::identity()); + mSphereBody1 = mWorld->createCollisionBody(sphereTransform1); + mSphereProxyShape1 = mSphereBody1->addCollisionShape(mSphereShape1, Transform::identity()); - Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity()); - mCylinderBody = mWorld->createCollisionBody(cylinderTransform); - mCylinderShape = new CylinderShape(2, 5); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity()); - - // Assign collision categories to proxy shapes - mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); - mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); - mCylinderProxyShape->setCollisionCategoryBits(CATEGORY_3); - - mCollisionCallback.boxBody = mBoxBody; - mCollisionCallback.sphere1Body = mSphere1Body; - mCollisionCallback.sphere2Body = mSphere2Body; - mCollisionCallback.cylinderBody = mCylinderBody; + mSphereShape2 = new SphereShape(5.0); + Transform sphereTransform2(Vector3(20, 20, 0), Quaternion::identity()); + mSphereBody2 = mWorld->createCollisionBody(sphereTransform2); + mSphereProxyShape2 = mSphereBody2->addCollisionShape(mSphereShape2, Transform::identity()); } /// Destructor - ~TestCollisionWorld() { - delete mBoxShape; - delete mSphereShape; - delete mCylinderShape; + virtual ~TestCollisionWorld() { + + delete mBoxShape1; + delete mBoxShape2; + + delete mSphereShape1; + delete mSphereShape2; + + delete mWorld; } /// Run the tests void run() { - testCollisions(); + testNoCollisions(); + testNoOverlap(); + testNoAABBOverlap(); + + testAABBOverlap(); + + testSphereVsSphereCollision(); + testSphereVsBoxCollision(); + + testMultipleCollisions(); } - void testCollisions() { + void testNoCollisions() { - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no collision at all. - test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); + // ---------- Global test ---------- // - test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); + mCollisionCallback.reset(); + mWorld->testCollision(&mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(mCylinderBody, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // ---------- Single body test ---------- // - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + // Two bodies test - mCollisionCallback.reset(); - mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody1, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, mSphereBody1, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + mCollisionCallback.reset(); + mWorld->testCollision(mBoxBody2, mSphereBody2, &mCollisionCallback); + test(!mCollisionCallback.hasContacts()); + + } + + void testNoOverlap() { + + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no overlap at all. + + // ---------- Single body test ---------- // + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody1, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mBoxBody2, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody1, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + mOverlapCallback.reset(); + mWorld->testOverlap(mSphereBody2, &mOverlapCallback); + test(!mOverlapCallback.hasOverlap()); + + // Two bodies test + + test(!mWorld->testOverlap(mBoxBody1, mBoxBody2)); + test(!mWorld->testOverlap(mSphereBody1, mSphereBody2)); + test(!mWorld->testOverlap(mBoxBody1, mSphereBody1)); + test(!mWorld->testOverlap(mBoxBody1, mSphereBody2)); + test(!mWorld->testOverlap(mBoxBody2, mSphereBody1)); + test(!mWorld->testOverlap(mBoxBody2, mSphereBody2)); + } + + void testNoAABBOverlap() { + + // All the shapes of the world are not touching when they are created. + // Here we test that at the beginning, there is no AABB overlap at all. + + // Two bodies test + + test(!mWorld->testAABBOverlap(mBoxBody1, mBoxBody2)); + test(!mWorld->testAABBOverlap(mSphereBody1, mSphereBody2)); + test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody1)); + test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody2)); + test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody1)); + test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody2)); + } + + void testAABBOverlap() { + + // TODO : Test the CollisionWorld::testAABBOverlap() method here + } + + void testSphereVsSphereCollision() { + + + + // Move sphere 1 to collide with sphere 2 + mSphereBody1->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + + } + + void testSphereVsBoxCollision() { // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + mSphereBody1->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); // --------- Test collision with inactive bodies --------- // mCollisionCallback.reset(); - mBoxBody->setIsActive(false); - mCylinderBody->setIsActive(false); - mSphere1Body->setIsActive(false); - mSphere2Body->setIsActive(false); + mBoxBody1->setIsActive(false); + mSphereBody1->setIsActive(false); + mSphereBody2->setIsActive(false); mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + - test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body)); - test(!mWorld->testAABBOverlap(mBoxBody, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody)); - test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body)); - - test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB())); - test(!mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB())); - test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB())); - - mBoxBody->setIsActive(true); - mCylinderBody->setIsActive(true); - mSphere1Body->setIsActive(true); - mSphere2Body->setIsActive(true); + mBoxBody1->setIsActive(true); + mSphereBody1->setIsActive(true); + mSphereBody2->setIsActive(true); // --------- Test collision with collision filtering -------- // - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); + //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3); + //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2); + //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(mCollisionCallback.boxCollideWithSphere1); + //test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with sphere 2 - mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); + //// Move sphere 1 to collide with sphere 2 + //mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity())); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(!mCollisionCallback.boxCollideWithSphere1); + //test(mCollisionCallback.sphere1CollideWithSphere2); - mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); - mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); - mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1); + //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2); + //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2); + //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3); - mCollisionCallback.reset(); - mWorld->testCollision(&mCollisionCallback); - test(!mCollisionCallback.boxCollideWithSphere1); - test(!mCollisionCallback.boxCollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithCylinder); - test(!mCollisionCallback.sphere1CollideWithSphere2); + //mCollisionCallback.reset(); + //mWorld->testCollision(&mCollisionCallback); + //test(!mCollisionCallback.boxCollideWithSphere1); + //test(!mCollisionCallback.sphere1CollideWithSphere2); - // Move sphere 1 to collide with box - mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); + //// Move sphere 1 to collide with box + //mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity())); - mBoxProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); - mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); - mCylinderProxyShape->setCollideWithMaskBits(0xFFFF); + //mBoxProxyShape->setCollideWithMaskBits(0xFFFF); + //mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF); + //mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF); } + + void testMultipleCollisions() { + + // TODO : Test collisions without categories set + + // TODO : Test colliisons with categories set + + // Assign collision categories to proxy shapes + //mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1); + //mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1); + //mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2); + } }; } diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h old mode 100644 new mode 100755 index 7ab3202b..53f5e618 --- a/test/tests/collision/TestDynamicAABBTree.h +++ b/test/tests/collision/TestDynamicAABBTree.h @@ -114,6 +114,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; + +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif int object1Data = 56; int object2Data = 23; @@ -152,6 +158,10 @@ class TestDynamicAABBTree : public Test { test(*(int*)(tree.getNodeDataPointer(object2Id)) == object2Data); test(*(int*)(tree.getNodeDataPointer(object3Id)) == object3Data); test(*(int*)(tree.getNodeDataPointer(object4Id)) == object4Data); + +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } void testOverlapping() { @@ -161,6 +171,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif + int object1Data = 56; int object2Data = 23; int object3Data = 13; @@ -342,6 +358,9 @@ class TestDynamicAABBTree : public Test { test(!mOverlapCallback.isOverlapping(object3Id)); test(!mOverlapCallback.isOverlapping(object4Id)); +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } void testRaycast() { @@ -351,6 +370,12 @@ class TestDynamicAABBTree : public Test { // Dynamic AABB Tree DynamicAABBTree tree; +#ifdef IS_PROFILING_ACTIVE + /// Pointer to the profiler + Profiler* profiler = new Profiler(); + tree.setProfiler(profiler); +#endif + int object1Data = 56; int object2Data = 23; int object3Data = 13; @@ -513,6 +538,10 @@ class TestDynamicAABBTree : public Test { test(!mRaycastCallback.isHit(object2Id)); test(mRaycastCallback.isHit(object3Id)); test(mRaycastCallback.isHit(object4Id)); + +#ifdef IS_PROFILING_ACTIVE + delete profiler; +#endif } }; diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h new file mode 100644 index 00000000..35f357b3 --- /dev/null +++ b/test/tests/collision/TestHalfEdgeStructure.h @@ -0,0 +1,263 @@ +#ifndef TEST_HALF_EDGE_STRUCTURE_H +#define TEST_HALF_EDGE_STRUCTURE_H + +// Libraries +#include "reactphysics3d.h" +#include "Test.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + + +// Class TestHalfEdgeStructure +/** + * Unit test for the HalfEdgeStructure class. + */ +class TestHalfEdgeStructure : public Test { + + private : + + // ---------- Atributes ---------- // + + /// Memory allocator + DefaultAllocator mAllocator; + + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestHalfEdgeStructure(const std::string& name) : Test(name) { + + } + + /// Destructor + virtual ~TestHalfEdgeStructure() { + + } + + /// Run the tests + void run() { + testCube(); + testTetrahedron(); + } + + void testCube() { + + // Create the half-edge structure for a cube + rp3d::HalfEdgeStructure cubeStructure(mAllocator, 6, 8, 24); + + rp3d::Vector3 vertices[8] = { + rp3d::Vector3(-0.5, -0.5, 0.5), + rp3d::Vector3(0.5, -0.5, 0.5), + rp3d::Vector3(0.5, 0.5, 0.5), + rp3d::Vector3(-0.5, 0.5, 0.5), + rp3d::Vector3(-0.5, -0.5, -0.5), + rp3d::Vector3(0.5, -0.5, -0.5), + rp3d::Vector3(0.5, 0.5, -0.5), + rp3d::Vector3(-0.5, 0.5, -0.5) + }; + + // Vertices + cubeStructure.addVertex(0); + cubeStructure.addVertex(1); + cubeStructure.addVertex(2); + cubeStructure.addVertex(3); + cubeStructure.addVertex(4); + cubeStructure.addVertex(5); + cubeStructure.addVertex(6); + cubeStructure.addVertex(7); + + // Faces + List face0(mAllocator, 4); + face0.add(0); face0.add(1); face0.add(2); face0.add(3); + List face1(mAllocator, 4); + face1.add(1); face1.add(5); face1.add(6); face1.add(2); + List face2(mAllocator, 4); + face2.add(5); face2.add(4); face2.add(7); face2.add(6); + List face3(mAllocator, 4); + face3.add(4); face3.add(0); face3.add(3); face3.add(7); + List face4(mAllocator, 4); + face4.add(0); face4.add(4); face4.add(5); face4.add(1); + List face5(mAllocator, 4); + face5.add(2); face5.add(6); face5.add(7); face5.add(3); + + cubeStructure.addFace(face0); + cubeStructure.addFace(face1); + cubeStructure.addFace(face2); + cubeStructure.addFace(face3); + cubeStructure.addFace(face4); + cubeStructure.addFace(face5); + + cubeStructure.init(); + + // --- Test that the half-edge structure of the cube is valid --- // + + test(cubeStructure.getNbFaces() == 6); + test(cubeStructure.getNbVertices() == 8); + test(cubeStructure.getNbHalfEdges() == 24); + + // Test vertices + test(vertices[cubeStructure.getVertex(0).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(0).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(0).vertexPointIndex].z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(0).edgeIndex).vertexIndex == 0); + + test(vertices[cubeStructure.getVertex(1).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(1).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(1).vertexPointIndex].z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(1).edgeIndex).vertexIndex == 1); + + test(vertices[cubeStructure.getVertex(2).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(2).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(2).vertexPointIndex].z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(2).edgeIndex).vertexIndex == 2); + + test(vertices[cubeStructure.getVertex(3).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(3).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(3).vertexPointIndex].z == 0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(3).edgeIndex).vertexIndex == 3); + + test(vertices[cubeStructure.getVertex(4).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(4).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(4).vertexPointIndex].z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(4).edgeIndex).vertexIndex == 4); + + test(vertices[cubeStructure.getVertex(5).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(5).vertexPointIndex].y == -0.5); + test(vertices[cubeStructure.getVertex(5).vertexPointIndex].z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(5).edgeIndex).vertexIndex == 5); + + test(vertices[cubeStructure.getVertex(6).vertexPointIndex].x == 0.5); + test(vertices[cubeStructure.getVertex(6).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(6).vertexPointIndex].z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(6).edgeIndex).vertexIndex == 6); + + test(vertices[cubeStructure.getVertex(7).vertexPointIndex].x == -0.5); + test(vertices[cubeStructure.getVertex(7).vertexPointIndex].y == 0.5); + test(vertices[cubeStructure.getVertex(7).vertexPointIndex].z == -0.5); + test(cubeStructure.getHalfEdge(cubeStructure.getVertex(7).edgeIndex).vertexIndex == 7); + + // Test faces + for (uint f=0; f<6; f++) { + test(cubeStructure.getHalfEdge(cubeStructure.getFace(f).edgeIndex).faceIndex == f); + } + + // Test edges + for (uint f=0; f<6; f++) { + + + uint edgeIndex = cubeStructure.getFace(f).edgeIndex; + const uint firstEdgeIndex = edgeIndex; + + // For each half-edge of the face + for (uint e=0; e<4; e++) { + + rp3d::HalfEdgeStructure::Edge edge = cubeStructure.getHalfEdge(edgeIndex); + + test(cubeStructure.getHalfEdge(edge.twinEdgeIndex).twinEdgeIndex == edgeIndex); + test(edge.faceIndex == f); + + // Go to the next edge + edgeIndex = edge.nextEdgeIndex; + } + + test(firstEdgeIndex == edgeIndex); + } + } + + void testTetrahedron() { + + // Create the half-edge structure for a tetrahedron + std::vector> faces; + rp3d::HalfEdgeStructure tetrahedron(mAllocator, 4, 4, 12); + + // Vertices + rp3d::Vector3 vertices[4] = { + rp3d::Vector3(1, -1, -1), + rp3d::Vector3(-1, -1, -1), + rp3d::Vector3(0, -1, 1), + rp3d::Vector3(0, 1, 0) + }; + + tetrahedron.addVertex(0); + tetrahedron.addVertex(1); + tetrahedron.addVertex(2); + tetrahedron.addVertex(3); + + // Faces + List face0(mAllocator, 3); + face0.add(0); face0.add(1); face0.add(2); + List face1(mAllocator, 3); + face1.add(0); face1.add(3); face1.add(1); + List face2(mAllocator, 3); + face2.add(1); face2.add(3); face2.add(2); + List face3(mAllocator, 3); + face3.add(0); face3.add(2); face3.add(3); + + tetrahedron.addFace(face0); + tetrahedron.addFace(face1); + tetrahedron.addFace(face2); + tetrahedron.addFace(face3); + + tetrahedron.init(); + + // --- Test that the half-edge structure of the tetrahedron is valid --- // + + test(tetrahedron.getNbFaces() == 4); + test(tetrahedron.getNbVertices() == 4); + test(tetrahedron.getNbHalfEdges() == 12); + + // Test vertices + test(vertices[tetrahedron.getVertex(0).vertexPointIndex].x == 1); + test(vertices[tetrahedron.getVertex(0).vertexPointIndex].y == -1); + test(vertices[tetrahedron.getVertex(0).vertexPointIndex].z == -1); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(0).edgeIndex).vertexIndex == 0); + + test(vertices[tetrahedron.getVertex(1).vertexPointIndex].x == -1); + test(vertices[tetrahedron.getVertex(1).vertexPointIndex].y == -1); + test(vertices[tetrahedron.getVertex(1).vertexPointIndex].z == -1); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(1).edgeIndex).vertexIndex == 1); + + test(vertices[tetrahedron.getVertex(2).vertexPointIndex].x == 0); + test(vertices[tetrahedron.getVertex(2).vertexPointIndex].y == -1); + test(vertices[tetrahedron.getVertex(2).vertexPointIndex].z == 1); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(2).edgeIndex).vertexIndex == 2); + + test(vertices[tetrahedron.getVertex(3).vertexPointIndex].x == 0); + test(vertices[tetrahedron.getVertex(3).vertexPointIndex].y == 1); + test(vertices[tetrahedron.getVertex(3).vertexPointIndex].z == 0); + test(tetrahedron.getHalfEdge(tetrahedron.getVertex(3).edgeIndex).vertexIndex == 3); + + // Test faces + for (uint f=0; f<4; f++) { + test(tetrahedron.getHalfEdge(tetrahedron.getFace(f).edgeIndex).faceIndex == f); + } + + // Test edges + for (uint f=0; f<4; f++) { + + uint edgeIndex = tetrahedron.getFace(f).edgeIndex; + const uint firstEdgeIndex = edgeIndex; + + // For each half-edge of the face + for (uint e=0; e<3; e++) { + + rp3d::HalfEdgeStructure::Edge edge = tetrahedron.getHalfEdge(edgeIndex); + + test(tetrahedron.getHalfEdge(edge.twinEdgeIndex).twinEdgeIndex == edgeIndex); + test(edge.faceIndex == f); + + // Go to the next edge + edgeIndex = edge.nextEdgeIndex; + } + + test(firstEdgeIndex == edgeIndex); + } + } + }; + +} + +#endif diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h index 68345739..afdb415c 100644 --- a/test/tests/collision/TestPointInside.h +++ b/test/tests/collision/TestPointInside.h @@ -31,9 +31,7 @@ #include "collision/shapes/BoxShape.h" #include "collision/shapes/SphereShape.h" #include "collision/shapes/CapsuleShape.h" -#include "collision/shapes/ConeShape.h" #include "collision/shapes/ConvexMeshShape.h" -#include "collision/shapes/CylinderShape.h" /// Reactphysics3D namespace namespace reactphysics3d { @@ -65,10 +63,8 @@ class TestPointInside : public Test { BoxShape* mBoxShape; SphereShape* mSphereShape; CapsuleShape* mCapsuleShape; - ConeShape* mConeShape; ConvexMeshShape* mConvexMeshShape; ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo; - CylinderShape* mCylinderShape; // Transform Transform mBodyTransform; @@ -97,7 +93,7 @@ class TestPointInside : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); mBodyTransform = Transform(position, orientation); // Create the bodies @@ -112,14 +108,14 @@ class TestPointInside : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space mLocalShapeToWorld = mBodyTransform * mShapeTransform; // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxShape = new BoxShape(Vector3(2, 3, 4)); mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); mSphereShape = new SphereShape(3); @@ -128,10 +124,8 @@ class TestPointInside : public Test { mCapsuleShape = new CapsuleShape(2, 10); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - mConeShape = new ConeShape(2, 6, 0); - mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); - - mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) + // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again + /*mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4) mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); mConvexMeshShape->addVertex(Vector3(2, -3, -4)); mConvexMeshShape->addVertex(Vector3(2, -3, 4)); @@ -167,28 +161,24 @@ class TestPointInside : public Test { mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( mConvexMeshShapeBodyEdgesInfo, mShapeTransform); + */ - mCylinderShape = new CylinderShape(3, 8, 0); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); - - // Compound shape is a cylinder and a sphere + // Compound shape is a capsule and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; - mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); + mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); } /// Destructor - ~TestPointInside() { + virtual ~TestPointInside() { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - delete mConeShape; - delete mConvexMeshShape; - delete mConvexMeshShapeBodyEdgesInfo; - delete mCylinderShape; + //delete mConvexMeshShape; + //delete mConvexMeshShapeBodyEdgesInfo; } /// Run the tests @@ -196,9 +186,7 @@ class TestPointInside : public Test { testBox(); testSphere(); testCapsule(); - testCone(); testConvexMesh(); - testCylinder(); testCompound(); } @@ -407,89 +395,13 @@ class TestPointInside : public Test { test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7))); } - /// Test the ProxyConeShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testCone() { - - // Tests with CollisionBody - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); - - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); - - // Tests with ProxyConeShape - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4))); - test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4))); - - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5))); - test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5))); - } - /// Test the ProxyConvexMeshShape::testPointInside() and /// CollisionBody::testPointInside() methods void testConvexMesh() { // ----- Tests without using edges information ----- // + /* // Tests with CollisionBody test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0))); @@ -595,129 +507,14 @@ class TestPointInside : public Test { test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5))); test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5))); test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5))); - } - - /// Test the ProxyCylinderShape::testPointInside() and - /// CollisionBody::testPointInside() methods - void testCylinder() { - - // Tests with CollisionBody - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); - - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); - - // Tests with ProxyCylinderShape - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7))); - test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7))); - - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2))); - test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2))); + */ } /// Test the CollisionBody::testPointInside() method void testCompound() { - // Points on the cylinder + // Points on the capsule + // TODO : Previous it was a cylinder (not a capsule). Maybe those tests are wrong now test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0))); test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0))); diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index 7295a8d9..d02ab847 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -33,9 +33,7 @@ #include "collision/shapes/BoxShape.h" #include "collision/shapes/SphereShape.h" #include "collision/shapes/CapsuleShape.h" -#include "collision/shapes/ConeShape.h" #include "collision/shapes/ConvexMeshShape.h" -#include "collision/shapes/CylinderShape.h" #include "collision/shapes/TriangleShape.h" #include "collision/shapes/ConcaveMeshShape.h" #include "collision/shapes/HeightFieldShape.h" @@ -101,6 +99,8 @@ class TestRaycast : public Test { // Raycast callback class WorldRaycastCallback mCallback; + DefaultAllocator mAllocator; + // Epsilon decimal epsilon; @@ -111,9 +111,7 @@ class TestRaycast : public Test { CollisionBody* mBoxBody; CollisionBody* mSphereBody; CollisionBody* mCapsuleBody; - CollisionBody* mConeBody; CollisionBody* mConvexMeshBody; - CollisionBody* mConvexMeshBodyEdgesInfo; CollisionBody* mCylinderBody; CollisionBody* mCompoundBody; CollisionBody* mTriangleBody; @@ -130,10 +128,7 @@ class TestRaycast : public Test { BoxShape* mBoxShape; SphereShape* mSphereShape; CapsuleShape* mCapsuleShape; - ConeShape* mConeShape; ConvexMeshShape* mConvexMeshShape; - ConvexMeshShape* mConvexMeshShapeEdgesInfo; - CylinderShape* mCylinderShape; TriangleShape* mTriangleShape; ConcaveShape* mConcaveMeshShape; HeightFieldShape* mHeightFieldShape; @@ -142,12 +137,9 @@ class TestRaycast : public Test { ProxyShape* mBoxProxyShape; ProxyShape* mSphereProxyShape; ProxyShape* mCapsuleProxyShape; - ProxyShape* mConeProxyShape; ProxyShape* mConvexMeshProxyShape; - ProxyShape* mConvexMeshProxyShapeEdgesInfo; - ProxyShape* mCylinderProxyShape; ProxyShape* mCompoundSphereProxyShape; - ProxyShape* mCompoundCylinderProxyShape; + ProxyShape* mCompoundCapsuleProxyShape; ProxyShape* mTriangleProxyShape; ProxyShape* mConcaveMeshProxyShape; ProxyShape* mHeightFieldProxyShape; @@ -159,6 +151,11 @@ class TestRaycast : public Test { std::vector mConcaveMeshIndices; TriangleVertexArray* mConcaveMeshVertexArray; float mHeightFieldData[100]; + PolygonVertexArray::PolygonFace mPolygonFaces[6]; + PolygonVertexArray* mPolygonVertexArray; + PolyhedronMesh* mPolyhedronMesh; + Vector3 mPolyhedronVertices[8]; + int mPolyhedronIndices[4 * 6]; public : @@ -174,16 +171,14 @@ class TestRaycast : public Test { // Body transform Vector3 position(-3, 2, 7); - Quaternion orientation(PI / 5, PI / 6, PI / 7); + Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7); mBodyTransform = Transform(position, orientation); // Create the bodies mBoxBody = mWorld->createCollisionBody(mBodyTransform); mSphereBody = mWorld->createCollisionBody(mBodyTransform); mCapsuleBody = mWorld->createCollisionBody(mBodyTransform); - mConeBody = mWorld->createCollisionBody(mBodyTransform); mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform); - mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform); mCylinderBody = mWorld->createCollisionBody(mBodyTransform); mCompoundBody = mWorld->createCollisionBody(mBodyTransform); mTriangleBody = mWorld->createCollisionBody(mBodyTransform); @@ -192,78 +187,71 @@ class TestRaycast : public Test { // Collision shape transform Vector3 shapePosition(1, -4, -3); - Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3); + Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3); mShapeTransform = Transform(shapePosition, shapeOrientation); // Compute the the transform from a local shape point to world-space mLocalShapeToWorld = mBodyTransform * mShapeTransform; // Create collision shapes - mBoxShape = new BoxShape(Vector3(2, 3, 4), 0); + mBoxShape = new BoxShape(Vector3(2, 3, 4)); mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform); mSphereShape = new SphereShape(3); mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); - const Vector3 triangleVertex1(100, 100, 0); - const Vector3 triangleVertex2(105, 100, 0); - const Vector3 triangleVertex3(100, 103, 0); - mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3); + Vector3 triangleVertices[3]; + triangleVertices[0] = Vector3(100, 100, 0); + triangleVertices[1] = Vector3(105, 100, 0); + triangleVertices[2] = Vector3(100, 103, 0); + Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)}; + mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0, mAllocator); mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); mCapsuleShape = new CapsuleShape(2, 5); mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform); - mConeShape = new ConeShape(2, 6, 0); - mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform); + // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again + // Box of extents (2, 3, 4) + mPolyhedronVertices[0] = Vector3(-2, -3, -4); + mPolyhedronVertices[1] = Vector3(2, -3, -4); + mPolyhedronVertices[2] = Vector3(2, -3, 4); + mPolyhedronVertices[3] = Vector3(-2, -3, 4); + mPolyhedronVertices[4] = Vector3(-2, 3, -4); + mPolyhedronVertices[5] = Vector3(2, 3, -4); + mPolyhedronVertices[6] = Vector3(2, 3, 4); + mPolyhedronVertices[7] = Vector3(-2, 3, 4); - // Box of dimension (2, 3, 4) - mConvexMeshShape = new ConvexMeshShape(0.0); - mConvexMeshShape->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, -4)); - mConvexMeshShape->addVertex(Vector3(2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, -4)); - mConvexMeshShape->addVertex(Vector3(2, 3, 4)); - mConvexMeshShape->addVertex(Vector3(-2, 3, 4)); + mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 1; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 3; + mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 2; + mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 4; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 1; + mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 7; mPolyhedronIndices[15] = 4; + mPolyhedronIndices[16] = 3; mPolyhedronIndices[17] = 2; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 7; + mPolyhedronIndices[20] = 2; mPolyhedronIndices[21] = 5; mPolyhedronIndices[22] = 4; mPolyhedronIndices[23] = 7; + + // Polygon faces descriptions for the polyhedron + for (int f=0; f < 8; f++) { + PolygonVertexArray::PolygonFace& face = mPolygonFaces[f]; + face.indexBase = f * 4; + face.nbVertices = 4; + } + + // Create the polygon vertex array + mPolygonVertexArray = new PolygonVertexArray(8, mPolyhedronVertices, sizeof(Vector3), + mPolyhedronIndices, sizeof(int), 6, mPolygonFaces, + PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + mPolyhedronMesh = new PolyhedronMesh(mPolygonVertexArray); + mConvexMeshShape = new ConvexMeshShape(mPolyhedronMesh); mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform); - mConvexMeshShapeEdgesInfo = new ConvexMeshShape(0.0); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, -4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, 4)); - mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, 4)); - mConvexMeshShapeEdgesInfo->addEdge(0, 1); - mConvexMeshShapeEdgesInfo->addEdge(1, 2); - mConvexMeshShapeEdgesInfo->addEdge(2, 3); - mConvexMeshShapeEdgesInfo->addEdge(0, 3); - mConvexMeshShapeEdgesInfo->addEdge(4, 5); - mConvexMeshShapeEdgesInfo->addEdge(5, 6); - mConvexMeshShapeEdgesInfo->addEdge(6, 7); - mConvexMeshShapeEdgesInfo->addEdge(4, 7); - mConvexMeshShapeEdgesInfo->addEdge(0, 4); - mConvexMeshShapeEdgesInfo->addEdge(1, 5); - mConvexMeshShapeEdgesInfo->addEdge(2, 6); - mConvexMeshShapeEdgesInfo->addEdge(3, 7); - mConvexMeshShapeEdgesInfo->setIsEdgesInformationUsed(true); - mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape( - mConvexMeshShapeEdgesInfo, - mShapeTransform); - - mCylinderShape = new CylinderShape(2, 5, 0); - mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform); - // Compound shape is a cylinder and a sphere Vector3 positionShape2(Vector3(4, 2, -3)); - Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); + Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13); Transform shapeTransform2(positionShape2, orientationShape2); mLocalShape2ToWorld = mBodyTransform * shapeTransform2; - mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform); + mCompoundCapsuleProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform); mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2); // Concave Mesh shape @@ -292,9 +280,8 @@ class TestRaycast : public Test { TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE; mConcaveMeshVertexArray = new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3), - 12, &(mConcaveMeshIndices[0]), sizeof(uint), - vertexType, - TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + 12, &(mConcaveMeshIndices[0]), 3 * sizeof(uint), + vertexType, TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); // Add the triangle vertex array of the subpart to the triangle mesh @@ -312,31 +299,28 @@ class TestRaycast : public Test { mBoxProxyShape->setCollisionCategoryBits(CATEGORY1); mSphereProxyShape->setCollisionCategoryBits(CATEGORY1); mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1); - mConeProxyShape->setCollisionCategoryBits(CATEGORY2); mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2); - mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2); - mCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2); - mCompoundCylinderProxyShape->setCollisionCategoryBits(CATEGORY2); + mCompoundCapsuleProxyShape->setCollisionCategoryBits(CATEGORY2); mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1); mConcaveMeshProxyShape->setCollisionCategoryBits(CATEGORY2); mHeightFieldProxyShape->setCollisionCategoryBits(CATEGORY2); } /// Destructor - ~TestRaycast() { + virtual ~TestRaycast() { delete mBoxShape; delete mSphereShape; delete mCapsuleShape; - delete mConeShape; delete mConvexMeshShape; - delete mConvexMeshShapeEdgesInfo; - delete mCylinderShape; delete mTriangleShape; delete mConcaveMeshShape; delete mHeightFieldShape; delete mConcaveMeshVertexArray; + + delete mPolygonVertexArray; + delete mPolyhedronMesh; } /// Run the tests @@ -344,9 +328,7 @@ class TestRaycast : public Test { testBox(); testSphere(); testCapsule(); - testCone(); testConvexMesh(); - testCylinder(); testCompound(); testTriangle(); testConcaveMesh(); @@ -1313,253 +1295,6 @@ class TestRaycast : public Test { mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, decimal(0.8)), &mCallback); } - /// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCone() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 0, 3); - Vector3 point1B = mLocalShapeToWorld * Vector3(0, 0, -7); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 0, 1); - - Vector3 point2A = mLocalShapeToWorld * Vector3(1 , -5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(1, 5, 0); - Ray rayBottom(point2A, point2B); - Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, -3, 0); - - mCallback.shapeToTest = mConeProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConeBody); - test(mCallback.raycastInfo.proxyShape == mConeProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mConeBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mConeBody); - test(raycastInfo2.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mConeProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mConeBody); - test(raycastInfo3.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - mCallback.reset(); - mWorld->raycast(rayBottom, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mConeBody); - test(mCallback.raycastInfo.proxyShape == mConeProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon)); - - // CollisionBody::raycast() - RaycastInfo raycastInfo5; - test(mConeBody->raycast(rayBottom, raycastInfo5)); - test(raycastInfo5.body == mConeBody); - test(raycastInfo5.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo6; - test(mConeProxyShape->raycast(rayBottom, raycastInfo6)); - test(raycastInfo6.body == mConeBody); - test(raycastInfo6.proxyShape == mConeProxyShape); - test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(-1, -2, 1), mLocalShapeToWorld * Vector3(-13, -2, 22)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -1), mLocalShapeToWorld * Vector3(-26, 1, -1)); - Ray ray6(mLocalShapeToWorld * Vector3(3, 4, 1), mLocalShapeToWorld * Vector3(3, -16, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 3), mLocalShapeToWorld * Vector3(1, -4, -17)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(26, 4, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 46, -7)); - Ray ray10(mLocalShapeToWorld * Vector3(-3, -2, -6), mLocalShapeToWorld * Vector3(-3, -2, 74)); - Ray ray11(mLocalShapeToWorld * Vector3(3, -1, 0.5), mLocalShapeToWorld * Vector3(-27, -1, 0.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -26, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, -2, 3), mLocalShapeToWorld * Vector3(-1, -2, -27)); - Ray ray14(mLocalShapeToWorld * Vector3(-2, 0, 0.8), mLocalShapeToWorld * Vector3(30, 0, 0.8)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-0.9, 0, -4), mLocalShapeToWorld * Vector3(-0.9, 0, 30)); - - // ----- Test raycast miss ----- // - test(!mConeBody->raycast(ray1, raycastInfo3)); - test(!mConeProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray2, raycastInfo3)); - test(!mConeProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray3, raycastInfo3)); - test(!mConeProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray4, raycastInfo3)); - test(!mConeProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray5, raycastInfo3)); - test(!mConeProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray6, raycastInfo3)); - test(!mConeProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray7, raycastInfo3)); - test(!mConeProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray8, raycastInfo3)); - test(!mConeProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray9, raycastInfo3)); - test(!mConeProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mConeBody->raycast(ray10, raycastInfo3)); - test(!mConeProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mConeBody->raycast(ray11, raycastInfo3)); - test(mConeProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray12, raycastInfo3)); - test(mConeProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray13, raycastInfo3)); - test(mConeProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray14, raycastInfo3)); - test(mConeProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray15, raycastInfo3)); - test(mConeProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mConeBody->raycast(ray16, raycastInfo3)); - test(mConeProxyShape->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - /// Test the ProxyConvexMeshShape::raycast(), CollisionBody::raycast() and /// CollisionWorld::raycast() methods. void testConvexMesh() { @@ -1603,16 +1338,6 @@ class TestRaycast : public Test { test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mConvexMeshBodyEdgesInfo); - test(raycastInfo3.proxyShape == mConvexMeshProxyShapeEdgesInfo); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - // ProxyCollisionShape::raycast() RaycastInfo raycastInfo4; test(mConvexMeshProxyShape->raycast(ray, raycastInfo4)); @@ -1623,16 +1348,6 @@ class TestRaycast : public Test { test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon)); test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon)); - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray, raycastInfo5)); - test(raycastInfo5.body == mConvexMeshBodyEdgesInfo); - test(raycastInfo5.proxyShape == mConvexMeshProxyShapeEdgesInfo); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon)); - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28)); Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24)); @@ -1651,10 +1366,9 @@ class TestRaycast : public Test { Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); // ----- Test raycast miss ----- // + RaycastInfo raycastInfo3; test(!mConvexMeshBody->raycast(ray1, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray1, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray1, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray1, &mCallback); test(!mCallback.isHit); @@ -1666,73 +1380,55 @@ class TestRaycast : public Test { test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray2, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray2, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray2, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray2, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray2, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray3, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray3, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray3, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray3, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray3, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray4, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray4, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray4, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray4, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray4, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray5, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray5, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray5, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray5, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray5, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray6, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray6, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray6, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray6, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray6, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray7, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray7, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray7, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray7, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray7, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray8, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray8, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray8, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray8, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray8, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray9, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray9, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray9, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray9, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray9, &mCallback); test(!mCallback.isHit); test(!mConvexMeshBody->raycast(ray10, raycastInfo3)); - test(!mConvexMeshBodyEdgesInfo->raycast(ray10, raycastInfo3)); test(!mConvexMeshProxyShape->raycast(ray10, raycastInfo3)); - test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray10, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray10, &mCallback); test(!mCallback.isHit); @@ -1758,10 +1454,8 @@ class TestRaycast : public Test { // ----- Test raycast hits ----- // test(mConvexMeshBody->raycast(ray11, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray11, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray11, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray11, raycastInfo3)); - mCallback.reset(); + mCallback.reset(); mWorld->raycast(ray11, &mCallback); test(mCallback.isHit); mCallback.reset(); @@ -1769,9 +1463,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray12, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray12, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray12, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray12, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray12, &mCallback); test(mCallback.isHit); @@ -1780,9 +1472,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray13, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray13, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray13, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray13, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray13, &mCallback); test(mCallback.isHit); @@ -1791,9 +1481,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray14, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray14, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray14, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray14, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray14, &mCallback); test(mCallback.isHit); @@ -1802,9 +1490,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray15, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray15, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray15, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray15, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray15, &mCallback); test(mCallback.isHit); @@ -1813,251 +1499,7 @@ class TestRaycast : public Test { test(mCallback.isHit); test(mConvexMeshBody->raycast(ray16, raycastInfo3)); - test(mConvexMeshBodyEdgesInfo->raycast(ray16, raycastInfo3)); test(mConvexMeshProxyShape->raycast(ray16, raycastInfo3)); - test(mConvexMeshProxyShapeEdgesInfo->raycast(ray16, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray16, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - } - - /// Test the ProxyCylinderShape::raycast(), CollisionBody::raycast() and - /// CollisionWorld::raycast() methods. - void testCylinder() { - - // ----- Test feedback data ----- // - Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0); - Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0); - Ray ray(point1A, point1B); - Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0); - - Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 4.5, 0); - Vector3 point2B = mLocalShapeToWorld * Vector3(0, -5.5, 0); - Ray rayTop(point2A, point2B); - Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, decimal(2.5), 0); - - Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -4.5, 0); - Vector3 point3B = mLocalShapeToWorld * Vector3(0, 5.5, 0); - Ray rayBottom(point3A, point3B); - Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, decimal(-2.5), 0); - - mCallback.shapeToTest = mCylinderProxyShape; - - // CollisionWorld::raycast() - mCallback.reset(); - mWorld->raycast(ray, &mCallback); - test(mCallback.isHit); - test(mCallback.raycastInfo.body == mCylinderBody); - test(mCallback.raycastInfo.proxyShape == mCylinderProxyShape); - test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon)); - - // Correct category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY2); - test(mCallback.isHit); - - // Wrong category filter mask - mCallback.reset(); - mWorld->raycast(ray, &mCallback, CATEGORY1); - test(!mCallback.isHit); - - // CollisionBody::raycast() - RaycastInfo raycastInfo2; - test(mCylinderBody->raycast(ray, raycastInfo2)); - test(raycastInfo2.body == mCylinderBody); - test(raycastInfo2.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo2.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo3; - test(mCylinderProxyShape->raycast(ray, raycastInfo3)); - test(raycastInfo3.body == mCylinderBody); - test(raycastInfo3.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo3.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon)); - test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo5; - test(mCylinderProxyShape->raycast(rayTop, raycastInfo5)); - test(raycastInfo5.body == mCylinderBody); - test(raycastInfo5.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo5.worldPoint.x, hitPointTop.x, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.y, hitPointTop.y, epsilon)); - test(approxEqual(raycastInfo5.worldPoint.z, hitPointTop.z, epsilon)); - - // ProxyCollisionShape::raycast() - RaycastInfo raycastInfo6; - test(mCylinderProxyShape->raycast(rayBottom, raycastInfo6)); - test(raycastInfo6.body == mCylinderBody); - test(raycastInfo6.proxyShape == mCylinderProxyShape); - test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon)); - test(approxEqual(raycastInfo6.worldPoint.x, hitPointBottom.x, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.y, hitPointBottom.y, epsilon)); - test(approxEqual(raycastInfo6.worldPoint.z, hitPointBottom.z, epsilon)); - - Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1)); - Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 20, 28)); - Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-11,3, 20)); - Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31)); - Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5)); - Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, -30, 1)); - Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, -30)); - Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(30, 9, 0)); - Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, 30, -4)); - Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30)); - Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5)); - Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1)); - Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); - Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7)); - Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); - Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - - // ----- Test raycast miss ----- // - test(!mCylinderBody->raycast(ray1, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray1, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray1, &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray2, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray2, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray2, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray3, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray3, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray3, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray4, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray4, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray4, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray5, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray5, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray5, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray6, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray6, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray6, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray7, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray7, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray7, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray8, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray8, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray8, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray9, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray9, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray9, &mCallback); - test(!mCallback.isHit); - - test(!mCylinderBody->raycast(ray10, raycastInfo3)); - test(!mCylinderProxyShape->raycast(ray10, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray10, &mCallback); - test(!mCallback.isHit); - - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback); - test(!mCallback.isHit); - - // ----- Test raycast hits ----- // - test(mCylinderBody->raycast(ray11, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray11, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray11, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray12, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray12, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray12, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray13, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray13, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray13, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray14, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray14, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray14, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray15, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray15, raycastInfo3)); - mCallback.reset(); - mWorld->raycast(ray15, &mCallback); - test(mCallback.isHit); - mCallback.reset(); - mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback); - test(mCallback.isHit); - - test(mCylinderBody->raycast(ray16, raycastInfo3)); - test(mCylinderProxyShape->raycast(ray16, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray16, &mCallback); test(mCallback.isHit); @@ -2141,7 +1583,8 @@ class TestRaycast : public Test { mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback); test(mCallback.isHit); - // Raycast hit agains the cylinder shape + // Raycast hit agains the capsule shape + // TODO : Previous it was a cylinder, now it is a capsule shape, maybe those tests are wrong now Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1.5, 2)); Ray ray12(mLocalShapeToWorld * Vector3(1.5, 9, -1), mLocalShapeToWorld * Vector3(1.5, -30, -1)); Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30)); @@ -2149,7 +1592,7 @@ class TestRaycast : public Test { Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1)); Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30)); - mCallback.shapeToTest = mCompoundCylinderProxyShape; + mCallback.shapeToTest = mCompoundCapsuleProxyShape; test(mCompoundBody->raycast(ray11, raycastInfo)); mCallback.reset(); @@ -2291,7 +1734,7 @@ class TestRaycast : public Test { // ----- Test raycast miss ----- // test(!mConcaveMeshBody->raycast(ray1, raycastInfo3)); - test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); + //test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3)); mCallback.reset(); mWorld->raycast(ray1, &mCallback); test(!mCallback.isHit); diff --git a/test/tests/collision/TestTriangleVertexArray.h b/test/tests/collision/TestTriangleVertexArray.h new file mode 100644 index 00000000..282b7d55 --- /dev/null +++ b/test/tests/collision/TestTriangleVertexArray.h @@ -0,0 +1,261 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef TEST_TRIANGLE_VERTEX_ARRAY_H +#define TEST_TRIANGLE_VERTEX_ARRAY_H + +// Libraries +#include "reactphysics3d.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + + +// Class TestTriangleVertexArray +/** + * Unit test for the TestTriangleArray class. + */ +class TestTriangleVertexArray : public Test { + + private : + + // ---------- Atributes ---------- // + + float mVertices1[4*3]; + double mVertices2[4*3]; + float mNormals2[4*3]; + uint mIndices1[6]; + short mIndices2[6]; + TriangleVertexArray* mTriangleVertexArray1; + TriangleVertexArray* mTriangleVertexArray2; + + Vector3 mVertex0; + Vector3 mVertex1; + Vector3 mVertex2; + Vector3 mVertex3; + Vector3 mVertex4; + Vector3 mVertex5; + Vector3 mVertex6; + Vector3 mVertex7; + + Vector3 mNormal0; + Vector3 mNormal1; + Vector3 mNormal2; + Vector3 mNormal3; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestTriangleVertexArray(const std::string& name) : Test(name) { + + mVertex0 = Vector3(0, 0, 4); + mVertex1 = Vector3(0, 0, -3); + mVertex2 = Vector3(-2, 0, 0); + mVertex3 = Vector3(0, -5, 0); + + // Initialize data + mVertices1[0] = mVertex0.x; mVertices1[1] = mVertex0.y; mVertices1[2] = mVertex0.z; + mVertices1[3] = mVertex1.x; mVertices1[4] = mVertex1.y; mVertices1[5] = mVertex1.z; + mVertices1[6] = mVertex2.x; mVertices1[7] = mVertex2.y; mVertices1[8] = mVertex2.z; + mVertices1[9] = mVertex3.x; mVertices1[10] = mVertex3.y; mVertices1[11] = mVertex3.z; + + mIndices1[0] = 0; mIndices1[1] = 1; mIndices1[2] = 2; + mIndices1[3] = 0; mIndices1[4] = 3; mIndices1[5] = 1; + + mVertex4 = Vector3(0, 0, 5); + mVertex5 = Vector3(0, 0, -7); + mVertex6 = Vector3(-2, 0, 0); + mVertex7 = Vector3(0, -5, 0); + + mVertices2[0] = static_cast(mVertex4.x); mVertices2[1] = static_cast(mVertex4.y); mVertices2[2] = static_cast(mVertex4.z); + mVertices2[3] = static_cast(mVertex5.x); mVertices2[4] = static_cast(mVertex5.y); mVertices2[5] = static_cast(mVertex5.z); + mVertices2[6] = static_cast(mVertex6.x); mVertices2[7] = static_cast(mVertex6.y); mVertices2[8] = static_cast(mVertex6.z); + mVertices2[9] = static_cast(mVertex7.x); mVertices2[10] = static_cast(mVertex7.y); mVertices2[11] = static_cast(mVertex7.z); + + mIndices2[0] = 0; mIndices2[1] = 1; mIndices2[2] = 2; + mIndices2[3] = 0; mIndices2[4] = 3; mIndices2[5] = 1; + + mNormal0 = Vector3(2, 4, 6); + mNormal1 = Vector3(1, 6, -3); + mNormal2 = Vector3(-2, 4, 7); + mNormal3 = Vector3(-5, 2, 9); + mNormal0.normalize(); + mNormal1.normalize(); + mNormal2.normalize(); + mNormal3.normalize(); + mNormals2[0] = mNormal0.x; mNormals2[1] = mNormal0.y; mNormals2[2] = mNormal0.z; + mNormals2[3] = mNormal1.x; mNormals2[4] = mNormal1.y; mNormals2[5] = mNormal1.z; + mNormals2[6] = mNormal2.x; mNormals2[7] = mNormal2.y; mNormals2[8] = mNormal2.z; + mNormals2[9] = mNormal3.x; mNormals2[10] = mNormal3.y; mNormals2[11] = mNormal3.z; + + // Create triangle vertex array with automatic normals computation + mTriangleVertexArray1 = new TriangleVertexArray(4, static_cast(mVertices1), 3 * sizeof(float), + 2, static_cast(mIndices1), 3 * sizeof(uint), + TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + // Create triangle vertex array with normals defined by the user + mTriangleVertexArray2 = new TriangleVertexArray(4, static_cast(mVertices2), 3 * sizeof(double), + static_cast(mNormals2), 3 * sizeof(float), + 2, static_cast(mIndices2), 3 * sizeof(short), + TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE, + TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE, + TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE); + + } + + /// Destructor + virtual ~TestTriangleVertexArray() { + delete mTriangleVertexArray1; + delete mTriangleVertexArray2; + } + + /// Run the tests + void run() { + + // ----- First triangle vertex array ----- // + + test(mTriangleVertexArray1->getVertexDataType() == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE); + test(mTriangleVertexArray1->getIndexDataType() == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + test(mTriangleVertexArray1->getVertexNormalDataType() == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE); + test(mTriangleVertexArray1->getNbTriangles() == 2); + test(mTriangleVertexArray1->getNbVertices() == 4); + test(mTriangleVertexArray1->getIndicesStart() == static_cast(mIndices1)); + test(mTriangleVertexArray1->getVerticesStart() == static_cast(mVertices1)); + test(mTriangleVertexArray1->getIndicesStride() == (3 * sizeof(uint))); + test(mTriangleVertexArray1->getVerticesStride() == (3 * sizeof(float))); + + // Get triangle indices + + uint triangle0Indices[3]; + mTriangleVertexArray1->getTriangleVerticesIndices(0, triangle0Indices); + + test(triangle0Indices[0] == mIndices1[0]); + test(triangle0Indices[1] == mIndices1[1]); + test(triangle0Indices[2] == mIndices1[2]); + + uint triangle1Indices[3]; + mTriangleVertexArray1->getTriangleVerticesIndices(1, triangle1Indices); + + test(triangle1Indices[0] == mIndices1[3]); + test(triangle1Indices[1] == mIndices1[4]); + test(triangle1Indices[2] == mIndices1[5]); + + // Get triangle vertices + + Vector3 triangle0Vertices[3]; + mTriangleVertexArray1->getTriangleVertices(0, triangle0Vertices); + + test(approxEqual(triangle0Vertices[0], mVertex0, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[1], mVertex1, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[2], mVertex2, decimal(0.0000001))); + + Vector3 triangle1Vertices[3]; + mTriangleVertexArray1->getTriangleVertices(1, triangle1Vertices); + + test(approxEqual(triangle1Vertices[0], mVertex0, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[1], mVertex3, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[2], mVertex1, decimal(0.0000001))); + + // Get triangle normals + + Vector3 triangle0Normals[3]; + mTriangleVertexArray1->getTriangleVerticesNormals(0, triangle0Normals); + + Vector3 triangle1Normals[3]; + mTriangleVertexArray1->getTriangleVerticesNormals(1, triangle1Normals); + + const Vector3 normal0Test(decimal(0.9792), decimal(0.20268), 0); + const Vector3 normal2Test(0, 1, 0); + const Vector3 normal3Test(1, 0, 0); + + test(approxEqual(triangle0Normals[0], normal0Test, decimal(0.0001))); + test(approxEqual(triangle0Normals[2], normal2Test, decimal(0.0001))); + test(approxEqual(triangle1Normals[1], normal3Test, decimal(0.0001))); + + // ----- Second triangle vertex array ----- // + + test(mTriangleVertexArray2->getVertexDataType() == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE); + test(mTriangleVertexArray2->getIndexDataType() == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE); + test(mTriangleVertexArray2->getVertexNormalDataType() == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE); + test(mTriangleVertexArray2->getNbTriangles() == 2); + test(mTriangleVertexArray2->getNbVertices() == 4); + test(mTriangleVertexArray2->getIndicesStart() == static_cast(mIndices2)); + test(mTriangleVertexArray2->getVerticesStart() == static_cast(mVertices2)); + test(mTriangleVertexArray2->getVerticesNormalsStart() == static_cast(mNormals2)); + test(mTriangleVertexArray2->getIndicesStride() == (3 * sizeof(short))); + test(mTriangleVertexArray2->getVerticesStride() == (3 * sizeof(double))); + test(mTriangleVertexArray2->getVerticesNormalsStride() == (3 * sizeof(float))); + + // Get triangle indices + + mTriangleVertexArray2->getTriangleVerticesIndices(0, triangle0Indices); + + test(triangle0Indices[0] == mIndices2[0]); + test(triangle0Indices[1] == mIndices2[1]); + test(triangle0Indices[2] == mIndices2[2]); + + mTriangleVertexArray2->getTriangleVerticesIndices(1, triangle1Indices); + + test(triangle1Indices[0] == mIndices2[3]); + test(triangle1Indices[1] == mIndices2[4]); + test(triangle1Indices[2] == mIndices2[5]); + + // Get triangle vertices + + mTriangleVertexArray2->getTriangleVertices(0, triangle0Vertices); + + test(approxEqual(triangle0Vertices[0], mVertex4, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[1], mVertex5, decimal(0.0000001))); + test(approxEqual(triangle0Vertices[2], mVertex6, decimal(0.0000001))); + + mTriangleVertexArray2->getTriangleVertices(1, triangle1Vertices); + + test(approxEqual(triangle1Vertices[0], mVertex4, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[1], mVertex7, decimal(0.0000001))); + test(approxEqual(triangle1Vertices[2], mVertex5, decimal(0.0000001))); + + // Get triangle normals + + mTriangleVertexArray2->getTriangleVerticesNormals(0, triangle0Normals); + mTriangleVertexArray2->getTriangleVerticesNormals(1, triangle1Normals); + + test(approxEqual(triangle0Normals[0], mNormal0, decimal(0.000001))); + test(approxEqual(triangle0Normals[1], mNormal1, decimal(0.000001))); + test(approxEqual(triangle0Normals[2], mNormal2, decimal(0.000001))); + + test(approxEqual(triangle1Normals[0], mNormal0, decimal(0.000001))); + test(approxEqual(triangle1Normals[1], mNormal3, decimal(0.000001))); + test(approxEqual(triangle1Normals[2], mNormal1, decimal(0.000001))); + } + +}; + +} + +#endif + diff --git a/test/tests/containers/TestList.h b/test/tests/containers/TestList.h new file mode 100644 index 00000000..ba1fbf61 --- /dev/null +++ b/test/tests/containers/TestList.h @@ -0,0 +1,331 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef TEST_LIST_H +#define TEST_LIST_H + +// Libraries +#include "Test.h" +#include "containers/List.h" +#include "memory/DefaultAllocator.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestList +/** + * Unit test for the List class + */ +class TestList : public Test { + + private : + + // ---------- Atributes ---------- // + + DefaultAllocator mAllocator; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestList(const std::string& name) : Test(name) { + + } + + /// Run the tests + void run() { + + testConstructors(); + testAddRemoveClear(); + testAssignment(); + testIndexing(); + testEquality(); + testReserve(); + testIteration(); + } + + void testConstructors() { + + // ----- Constructors ----- // + + List list1(mAllocator); + test(list1.capacity() == 0); + test(list1.size() == 0); + + List list2(mAllocator, 100); + test(list2.capacity() == 100); + test(list2.size() == 0); + + List list3(mAllocator); + list3.add(1); + list3.add(2); + list3.add(3); + test(list3.capacity() == 4); + test(list3.size() == 3); + + // ----- Copy Constructors ----- // + + List list4(list1); + test(list4.capacity() == 0); + test(list4.size() == 0); + + List list5(list3); + test(list5.capacity() == list3.size()); + test(list5.size() == list3.size()); + for (uint i=0; i list6(mAllocator, 20); + test(list6.capacity() == 20); + for (uint i=0; i<20; i++) { + list6.add("test"); + } + test(list6.capacity() == 20); + list6.add("test"); + test(list6.capacity() == 40); + } + + void testAddRemoveClear() { + + // ----- Test add() ----- // + + List list1(mAllocator); + list1.add(4); + test(list1.size() == 1); + test(list1[0] == 4); + list1.add(9); + test(list1.size() == 2); + test(list1[0] == 4); + test(list1[1] == 9); + + const int arraySize = 15; + int arrayTest[arraySize] = {3, 145, -182, 34, 12, 95, -1834, 4143, -111, -111, 4343, 234, 22983, -3432, 753}; + List list2(mAllocator); + for (uint i=0; i list3(mAllocator); + list3.add(1); + list3.add(2); + list3.add(3); + list3.add(4); + + list3.remove(3); + test(list3.size() == 3); + test(list3.capacity() == 4); + test(list3[0] = 1); + test(list3[1] = 2); + test(list3[2] = 3); + + list3.remove(1); + test(list3.size() == 2); + test(list3.capacity() == 4); + test(list3[0] = 1); + test(list3[1] = 3); + + list3.remove(0); + test(list3.size() == 1); + test(list3.capacity() == 4); + test(list3[0] = 3); + + list3.remove(0); + test(list3.size() == 0); + test(list3.capacity() == 4); + + // ----- Test addRange() ----- // + + List list4(mAllocator); + list4.add(1); + list4.add(2); + list4.add(3); + + List list5(mAllocator); + list5.add(4); + list5.add(5); + + List list6(mAllocator); + list6.addRange(list5); + test(list6.size() == list5.size()); + test(list6[0] == 4); + test(list6[1] == 5); + + list4.addRange(list5); + test(list4.size() == 3 + list5.size()); + test(list4[0] == 1); + test(list4[1] == 2); + test(list4[2] == 3); + test(list4[3] == 4); + test(list4[4] == 5); + + // ----- Test clear() ----- // + + List list7(mAllocator); + list7.add("test1"); + list7.add("test2"); + list7.add("test3"); + list7.clear(); + test(list7.size() == 0); + list7.add("new"); + test(list7.size() == 1); + test(list7[0] == "new"); + } + + void testAssignment() { + + List list1(mAllocator); + list1.add(1); + list1.add(2); + list1.add(3); + + List list2(mAllocator); + list2.add(5); + list2.add(6); + + List list3(mAllocator); + List list4(mAllocator); + list4.add(1); + list4.add(2); + + List list5(mAllocator); + list5.add(1); + list5.add(2); + list5.add(3); + + list3 = list2; + test(list2.size() == list3.size()); + test(list2[0] == list3[0]); + test(list2[1] == list3[1]); + + list4 = list1; + test(list4.size() == list1.size()) + test(list4[0] = list1[0]); + test(list4[1] = list1[1]); + test(list4[2] = list1[2]); + + list5 = list2; + test(list5.size() == list2.size()); + test(list5[0] = list2[0]); + test(list5[1] = list2[1]); + } + + void testIndexing() { + + List list1(mAllocator); + list1.add(1); + list1.add(2); + list1.add(3); + + test(list1[0] == 1); + test(list1[1] == 2); + test(list1[2] == 3); + + list1[0] = 6; + list1[1] = 7; + list1[2] = 8; + + test(list1[0] == 6); + test(list1[1] == 7); + test(list1[2] == 8); + + const int a = list1[0]; + const int b = list1[1]; + test(a == 6); + test(b == 7); + + list1[0]++; + list1[1]++; + test(list1[0] == 7); + test(list1[1] == 8); + } + + void testEquality() { + + List list1(mAllocator); + list1.add(1); + list1.add(2); + list1.add(3); + + List list2(mAllocator); + list2.add(1); + list2.add(2); + + List list3(mAllocator); + list3.add(1); + list3.add(2); + list3.add(3); + + List list4(mAllocator); + list4.add(1); + list4.add(5); + list4.add(3); + + test(list1 == list1); + test(list1 != list2); + test(list1 == list3); + test(list1 != list4); + test(list2 != list4); + } + + void testReserve() { + + List list1(mAllocator); + list1.reserve(10); + test(list1.size() == 0); + test(list1.capacity() == 10); + list1.add(1); + list1.add(2); + test(list1.capacity() == 10); + test(list1.size() == 2); + test(list1[0] == 1); + test(list1[1] == 2); + + list1.reserve(1); + test(list1.capacity() == 10); + + list1.reserve(100); + test(list1.capacity() == 100); + test(list1[0] == 1); + test(list1[1] == 2); + } + + void testIteration() { + // TODO : Implement this + } + + }; + +} + +#endif diff --git a/test/tests/containers/TestMap.h b/test/tests/containers/TestMap.h new file mode 100644 index 00000000..d7e084dc --- /dev/null +++ b/test/tests/containers/TestMap.h @@ -0,0 +1,319 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef TEST_MAP_H +#define TEST_MAP_H + +// Libraries +#include "Test.h" +#include "containers/Map.h" +#include "memory/DefaultAllocator.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestMap +/** + * Unit test for the Map class + */ +class TestMap : public Test { + + private : + + // ---------- Atributes ---------- // + + DefaultAllocator mAllocator; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestMap(const std::string& name) : Test(name) { + + } + + /// Run the tests + void run() { + + testConstructors(); + testReserve(); + testAddRemoveClear(); + testContainsKey(); + testIndexing(); + testEquality(); + testAssignment(); + testIteration(); + } + + void testConstructors() { + + // ----- Constructors ----- // + + Map map1(mAllocator); + test(map1.capacity() == 0); + test(map1.size() == 0); + + Map map2(mAllocator, 100); + test(map2.capacity() >= 100); + test(map2.size() == 0); + + // ----- Copy Constructors ----- // +/* + Map map3(map1); + test(map3.capacity() == map1.capacity()); + test(map3.size() == map1.size()); + + Map map4(mAllocator); + map4.add(std::make_pair(1, 10)); + map4.add(std::make_pair(2, 20)); + map4.add(std::make_pair(3, 30)); + test(map4.capacity() >= 3); + test(map4.size() == 3); + + Map map5(map4); + test(map5.capacity() == map4.capacity()); + test(map5.size() == map4.size()); + test(map5[1] == 10); + test(map5[2] == 20); + test(map5[3] == 30); + */ + } + + void testReserve() { + + Map map1(mAllocator); + map1.reserve(15); + test(map1.capacity() >= 15); + map1.add(std::make_pair(1, "test1")); + map1.add(std::make_pair(2, "test2")); + test(map1.capacity() >= 15); + + map1.reserve(10); + test(map1.capacity() >= 15); + + map1.reserve(100); + test(map1.capacity() >= 100); + test(map1[1] == "test1"); + test(map1[2] == "test2"); + } + + void testAddRemoveClear() { + + // ----- Test add() ----- // + + Map map1(mAllocator); + map1.add(std::make_pair(1, 10)); + map1.add(std::make_pair(8, 80)); + map1.add(std::make_pair(13, 130)); + test(map1[1] == 10); + test(map1[8] == 80); + test(map1[13] == 130); + test(map1.size() == 3); + + Map map2(mAllocator, 15); + for (int i = 0; i < 1000000; i++) { + map2.add(std::make_pair(i, i * 100)); + } + bool isValid = true; + for (int i = 0; i < 1000000; i++) { + if (map2[i] != i * 100) isValid = false; + } + test(isValid); + + map1.remove(1); + map1.add(std::make_pair(1, 10)); + test(map1.size() == 3); + test(map1[1] == 10); + + // ----- Test remove() ----- // + + map1.remove(1); + test(!map1.containsKey(1)); + test(map1.containsKey(8)); + test(map1.containsKey(13)); + test(map1.size() == 2); + + map1.remove(13); + test(!map1.containsKey(8)); + test(map1.containsKey(13)); + test(map1.size() == 1); + + map1.remove(8); + test(!map1.containsKey(8)); + test(map1.size() == 0); + + isValid = true; + for (int i = 0; i < 1000000; i++) { + map2.remove(i); + } + for (int i = 0; i < 1000000; i++) { + if (map2.containsKey(i)) isValid = false; + } + test(isValid); + test(map2.size() == 0); + + Map map3(mAllocator); + for (int i=0; i < 1000000; i++) { + map3.add(std::make_pair(i, i * 10)); + map3.remove(i); + } + + // ----- Test clear() ----- // + + Map map4(mAllocator); + map4.add(std::make_pair(2, 20)); + map4.add(std::make_pair(4, 40)); + map4.add(std::make_pair(6, 60)); + map4.clear(); + test(map4.size() == 0); + map4.add(std::make_pair(2, 20)); + test(map4.size() == 1); + test(map4[2] == 20); + map4.clear(); + test(map4.size() == 0); + + Map map5(mAllocator); + map5.clear(); + test(map5.size() == 0); + } + + void testContainsKey() { + + Map map1(mAllocator); + + test(!map1.containsKey(2)); + test(!map1.containsKey(4)); + test(!map1.containsKey(6)); + + map1.add(std::make_pair(2, 20)); + map1.add(std::make_pair(4, 40)); + map1.add(std::make_pair(6, 60)); + + test(map1.containsKey(2)); + test(map1.containsKey(4)); + test(map1.containsKey(6)); + + map1.remove(4); + test(!map1.containsKey(4)); + test(map1.containsKey(2)); + test(map1.containsKey(6)); + + map1.clear(); + test(!map1.containsKey(2)); + test(!map1.containsKey(6)); + } + + void testIndexing() { + + Map map1(mAllocator); + map1.add(std::make_pair(2, 20)); + map1.add(std::make_pair(4, 40)); + map1.add(std::make_pair(6, 60)); + test(map1[2] == 20); + test(map1[4] == 40); + test(map1[6] == 60); + + map1[2] = 10; + map1[4] = 20; + map1[6] = 30; + + test(map1[2] == 10); + test(map1[4] == 20); + test(map1[6] == 30); + } + + void testEquality() { + + Map map1(mAllocator, 10); + Map map2(mAllocator, 2); + + test(map1 == map2); + + map1.add(std::make_pair("a", 1)); + map1.add(std::make_pair("b", 2)); + map1.add(std::make_pair("c", 3)); + + map2.add(std::make_pair("a", 1)); + map2.add(std::make_pair("b", 2)); + map2.add(std::make_pair("c", 4)); + + test(map1 == map1); + test(map2 == map2); + test(map1 != map2); + + map2["c"] = 3; + + test(map1 == map2); + + Map map3(mAllocator); + map3.add(std::make_pair("a", 1)); + + test(map1 != map3); + test(map2 != map3); + } + + void testAssignment() { + + Map map1(mAllocator); + map1.add(std::make_pair(1, 3)); + map1.add(std::make_pair(2, 6)); + map1.add(std::make_pair(10, 30)); +/* + Map map2(mAllocator); + map2 = map1; + test(map2.size() == map1.size()); + test(map2[1] == 3); + test(map2[2] == 6); + test(map2[10] == 30); + + Map map3(mAllocator, 100); + map3 = map1; + test(map3.size() == map1.size()); + test(map3[1] == 3); + test(map3[2] == 6); + test(map3[10] == 30); + + Map map4(mAllocator); + map3 = map4; + test(map3.size() == 0); +*/ + Map map5(mAllocator); + map5.add(std::make_pair(7, 8)); + map5.add(std::make_pair(19, 70)); + map1 = map5; + test(map5.size() == map1.size()); + test(map1[7] == 8); + test(map1[19] == 70); + } + + void testIteration() { + + } + }; + +} + +#endif diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h index 5041fd48..b76cb78c 100644 --- a/test/tests/mathematics/TestMathematicsFunctions.h +++ b/test/tests/mathematics/TestMathematicsFunctions.h @@ -1,133 +1,266 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef TEST_MATHEMATICS_FUNCTIONS_H -#define TEST_MATHEMATICS_FUNCTIONS_H - -// Libraries -#include "Test.h" -#include "mathematics/mathematics_functions.h" - -/// Reactphysics3D namespace -namespace reactphysics3d { - -// Class TestMathematicsFunctions -/** - * Unit test for mathematics functions - */ -class TestMathematicsFunctions : public Test { - - private : - - // ---------- Atributes ---------- // - - - - public : - - // ---------- Methods ---------- // - - /// Constructor - TestMathematicsFunctions(const std::string& name): Test(name) {} - - /// Run the tests - void run() { - - // Test approxEqual() - test(approxEqual(2, 7, 5.2)); - test(approxEqual(7, 2, 5.2)); - test(approxEqual(6, 6)); - test(!approxEqual(1, 5)); - test(!approxEqual(1, 5, 3)); - test(approxEqual(-2, -2)); - test(approxEqual(-2, -7, 6)); - test(!approxEqual(-2, 7, 2)); - test(approxEqual(-3, 8, 12)); - test(!approxEqual(-3, 8, 6)); - - // Test clamp() - test(clamp(4, -3, 5) == 4); - test(clamp(-3, 1, 8) == 1); - test(clamp(45, -6, 7) == 7); - test(clamp(-5, -2, -1) == -2); - test(clamp(-5, -9, -1) == -5); - test(clamp(6, 6, 9) == 6); - test(clamp(9, 6, 9) == 9); - test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4)); - test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1)); - test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7)); - test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2)); - test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5)); - test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6)); - test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9)); - - // Test min3() - test(min3(1, 5, 7) == 1); - test(min3(-4, 2, 4) == -4); - test(min3(-1, -5, -7) == -7); - test(min3(13, 5, 47) == 5); - test(min3(4, 4, 4) == 4); - - // Test max3() - test(max3(1, 5, 7) == 7); - test(max3(-4, 2, 4) == 4); - test(max3(-1, -5, -7) == -1); - test(max3(13, 5, 47) == 47); - test(max3(4, 4, 4) == 4); - - // Test sameSign() - test(sameSign(4, 53)); - test(sameSign(-4, -8)); - test(!sameSign(4, -7)); - test(!sameSign(-4, 53)); - - // Test computeBarycentricCoordinatesInTriangle() - Vector3 a(0, 0, 0); - Vector3 b(5, 0, 0); - Vector3 c(0, 0, 5); - Vector3 testPoint(4, 0, 1); - decimal u,v,w; - computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w); - test(approxEqual(u, 1.0, 0.000001)); - test(approxEqual(v, 0.0, 0.000001)); - test(approxEqual(w, 0.0, 0.000001)); - computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w); - test(approxEqual(u, 0.0, 0.000001)); - test(approxEqual(v, 1.0, 0.000001)); - test(approxEqual(w, 0.0, 0.000001)); - computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w); - test(approxEqual(u, 0.0, 0.000001)); - test(approxEqual(v, 0.0, 0.000001)); - test(approxEqual(w, 1.0, 0.000001)); - - computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); - test(approxEqual(u + v + w, 1.0, 0.000001)); - } - - }; - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef TEST_MATHEMATICS_FUNCTIONS_H +#define TEST_MATHEMATICS_FUNCTIONS_H + +// Libraries +#include "containers/List.h" +#include "memory/DefaultAllocator.h" + +/// Reactphysics3D namespace +namespace reactphysics3d { + +// Class TestMathematicsFunctions +/** + * Unit test for mathematics functions + */ +class TestMathematicsFunctions : public Test { + + private : + + // ---------- Atributes ---------- // + + DefaultAllocator mAllocator; + + public : + + // ---------- Methods ---------- // + + /// Constructor + TestMathematicsFunctions(const std::string& name): Test(name) {} + + /// Run the tests + void run() { + + // Test approxEqual() + test(approxEqual(2, 7, 5.2)); + test(approxEqual(7, 2, 5.2)); + test(approxEqual(6, 6)); + test(!approxEqual(1, 5)); + test(!approxEqual(1, 5, 3)); + test(approxEqual(-2, -2)); + test(approxEqual(-2, -7, 6)); + test(!approxEqual(-2, 7, 2)); + test(approxEqual(-3, 8, 12)); + test(!approxEqual(-3, 8, 6)); + + // Test clamp() + test(clamp(4, -3, 5) == 4); + test(clamp(-3, 1, 8) == 1); + test(clamp(45, -6, 7) == 7); + test(clamp(-5, -2, -1) == -2); + test(clamp(-5, -9, -1) == -5); + test(clamp(6, 6, 9) == 6); + test(clamp(9, 6, 9) == 9); + test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4)); + test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1)); + test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7)); + test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2)); + test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5)); + test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6)); + test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9)); + + // Test min3() + test(min3(1, 5, 7) == 1); + test(min3(-4, 2, 4) == -4); + test(min3(-1, -5, -7) == -7); + test(min3(13, 5, 47) == 5); + test(min3(4, 4, 4) == 4); + + // Test max3() + test(max3(1, 5, 7) == 7); + test(max3(-4, 2, 4) == 4); + test(max3(-1, -5, -7) == -1); + test(max3(13, 5, 47) == 47); + test(max3(4, 4, 4) == 4); + + // Test sameSign() + test(sameSign(4, 53)); + test(sameSign(-4, -8)); + test(!sameSign(4, -7)); + test(!sameSign(-4, 53)); + + // Test computeBarycentricCoordinatesInTriangle() + Vector3 a(0, 0, 0); + Vector3 b(5, 0, 0); + Vector3 c(0, 0, 5); + Vector3 testPoint(4, 0, 1); + decimal u,v,w; + computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w); + test(approxEqual(u, 1.0, 0.000001)); + test(approxEqual(v, 0.0, 0.000001)); + test(approxEqual(w, 0.0, 0.000001)); + computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w); + test(approxEqual(u, 0.0, 0.000001)); + test(approxEqual(v, 1.0, 0.000001)); + test(approxEqual(w, 0.0, 0.000001)); + computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w); + test(approxEqual(u, 0.0, 0.000001)); + test(approxEqual(v, 0.0, 0.000001)); + test(approxEqual(w, 1.0, 0.000001)); + + computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w); + test(approxEqual(u + v + w, 1.0, 0.000001)); + + // Test computeClosestPointBetweenTwoSegments() + Vector3 closestSeg1, closestSeg2; + computeClosestPointBetweenTwoSegments(Vector3(4, 0, 0), Vector3(6, 0, 0), Vector3(8, 0, 0), Vector3(8, 6, 0), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 6.0, 0.000001)); + test(approxEqual(closestSeg1.y, 0.0, 0.000001)); + test(approxEqual(closestSeg1.z, 0.0, 0.000001)); + test(approxEqual(closestSeg2.x, 8.0, 0.000001)); + test(approxEqual(closestSeg2.y, 0.0, 0.000001)); + test(approxEqual(closestSeg2.z, 0.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(4, 6, 5), Vector3(4, 6, 5), Vector3(8, 3, -9), Vector3(8, 3, -9), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 4.0, 0.000001)); + test(approxEqual(closestSeg1.y, 6.0, 0.000001)); + test(approxEqual(closestSeg1.z, 5.0, 0.000001)); + test(approxEqual(closestSeg2.x, 8.0, 0.000001)); + test(approxEqual(closestSeg2.y, 3.0, 0.000001)); + test(approxEqual(closestSeg2.z, -9.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(0, -5, 0), Vector3(0, 8, 0), Vector3(6, 3, 0), Vector3(10, -3, 0), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 0.0, 0.000001)); + test(approxEqual(closestSeg1.y, 3.0, 0.000001)); + test(approxEqual(closestSeg1.z, 0.0, 0.000001)); + test(approxEqual(closestSeg2.x, 6.0, 0.000001)); + test(approxEqual(closestSeg2.y, 3.0, 0.000001)); + test(approxEqual(closestSeg2.z, 0.0, 0.000001)); + computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2); + test(approxEqual(closestSeg1.x, 1.0, 0.000001)); + test(approxEqual(closestSeg1.y, 4.0, 0.000001)); + test(approxEqual(closestSeg1.z, -5.0, 0.000001)); + test(approxEqual(closestSeg2.x, 1.0, 0.000001)); + test(approxEqual(closestSeg2.y, 5.0, 0.000001)); + test(approxEqual(closestSeg2.z, -5.0, 0.000001)); + + // Test computePlaneSegmentIntersection(); + test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001)); + test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001)); + decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0)); + test(tIntersect < 0.0 || tIntersect > 1.0); + + // Test computePointToLineDistance() + test(approxEqual(computePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001)); + test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001)); + + // Test clipSegmentWithPlanes() + std::vector segmentVertices; + segmentVertices.push_back(Vector3(-6, 3, 0)); + segmentVertices.push_back(Vector3(8, 3, 0)); + + List planesNormals(mAllocator, 2); + List planesPoints(mAllocator, 2); + planesNormals.add(Vector3(-1, 0, 0)); + planesPoints.add(Vector3(4, 0, 0)); + + List clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], + planesPoints, planesNormals, mAllocator); + test(clipSegmentVertices.size() == 2); + test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); + test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[1].x, 4, 0.000001)); + test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); + + segmentVertices.clear(); + segmentVertices.push_back(Vector3(8, 3, 0)); + segmentVertices.push_back(Vector3(-6, 3, 0)); + + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); + test(clipSegmentVertices.size() == 2); + test(approxEqual(clipSegmentVertices[0].x, 4, 0.000001)); + test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[1].x, -6, 0.000001)); + test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); + + segmentVertices.clear(); + segmentVertices.push_back(Vector3(-6, 3, 0)); + segmentVertices.push_back(Vector3(3, 3, 0)); + + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); + test(clipSegmentVertices.size() == 2); + test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001)); + test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001)); + test(approxEqual(clipSegmentVertices[1].x, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001)); + test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001)); + + segmentVertices.clear(); + segmentVertices.push_back(Vector3(5, 3, 0)); + segmentVertices.push_back(Vector3(8, 3, 0)); + + clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator); + test(clipSegmentVertices.size() == 0); + + // Test clipPolygonWithPlanes() + List polygonVertices(mAllocator); + polygonVertices.add(Vector3(-4, 2, 0)); + polygonVertices.add(Vector3(7, 2, 0)); + polygonVertices.add(Vector3(7, 4, 0)); + polygonVertices.add(Vector3(-4, 4, 0)); + + List polygonPlanesNormals(mAllocator); + List polygonPlanesPoints(mAllocator); + polygonPlanesNormals.add(Vector3(1, 0, 0)); + polygonPlanesPoints.add(Vector3(0, 0, 0)); + polygonPlanesNormals.add(Vector3(0, 1, 0)); + polygonPlanesPoints.add(Vector3(0, 0, 0)); + polygonPlanesNormals.add(Vector3(-1, 0, 0)); + polygonPlanesPoints.add(Vector3(10, 0, 0)); + polygonPlanesNormals.add(Vector3(0, -1, 0)); + polygonPlanesPoints.add(Vector3(10, 5, 0)); + + List clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator); + test(clipPolygonVertices.size() == 4); + test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001)); + test(approxEqual(clipPolygonVertices[0].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[1].x, 7, 0.000001)); + test(approxEqual(clipPolygonVertices[1].y, 2, 0.000001)); + test(approxEqual(clipPolygonVertices[1].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[2].x, 7, 0.000001)); + test(approxEqual(clipPolygonVertices[2].y, 4, 0.000001)); + test(approxEqual(clipPolygonVertices[2].z, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001)); + test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001)); + test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001)); + + } + + }; + +} + +#endif diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 5fe0ccd0..dfc65955 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -94,7 +94,7 @@ class TestQuaternion : public Test { const decimal PI_OVER_2 = PI * decimal(0.5); const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5); - Quaternion quaternion5(PI_OVER_2, 0, 0); + Quaternion quaternion5 = Quaternion::fromEulerAngles(PI_OVER_2, 0, 0); Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); quaternionTest5.normalize(); test(approxEqual(quaternion5.x, quaternionTest5.x)); @@ -102,7 +102,7 @@ class TestQuaternion : public Test { test(approxEqual(quaternion5.z, quaternionTest5.z)); test(approxEqual(quaternion5.w, quaternionTest5.w)); - Quaternion quaternion6(0, PI_OVER_2, 0); + Quaternion quaternion6 = Quaternion::fromEulerAngles(0, PI_OVER_2, 0); Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); quaternionTest6.normalize(); test(approxEqual(quaternion6.x, quaternionTest6.x)); @@ -110,7 +110,7 @@ class TestQuaternion : public Test { test(approxEqual(quaternion6.z, quaternionTest6.z)); test(approxEqual(quaternion6.w, quaternionTest6.w)); - Quaternion quaternion7(Vector3(0, 0, PI_OVER_2)); + Quaternion quaternion7 = Quaternion::fromEulerAngles(Vector3(0, 0, PI_OVER_2)); Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); quaternionTest7.normalize(); test(approxEqual(quaternion7.x, quaternionTest7.x)); @@ -124,7 +124,7 @@ class TestQuaternion : public Test { // Test method that returns the length Quaternion quaternion(2, 3, -4, 5); - test(approxEqual(quaternion.length(), sqrt(decimal(54.0)))); + test(approxEqual(quaternion.length(), std::sqrt(decimal(54.0)))); // Test method that returns a unit quaternion test(approxEqual(quaternion.getUnit().length(), 1.0)); diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h index 581b9c5e..815e3d54 100644 --- a/test/tests/mathematics/TestTransform.h +++ b/test/tests/mathematics/TestTransform.h @@ -61,13 +61,16 @@ class TestTransform : public Test { mIdentityTransform.setToIdentity(); - decimal sinA = sin(PI/8.0f); - decimal cosA = cos(PI/8.0f); - mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA, sinA, sinA, cosA)); + Vector3 unitVec(1, 1, 1); + unitVec.normalize(); - decimal sinB = sin(PI/3.0f); - decimal cosB = cos(PI/3.0f); - mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB, sinB, sinB, cosB)); + decimal sinA = std::sin(PI/8.0f); + decimal cosA = std::cos(PI/8.0f); + mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA * unitVec, cosA)); + + decimal sinB = std::sin(PI/3.0f); + decimal cosB = std::cos(PI/3.0f); + mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB * unitVec, cosB)); } /// Run the tests diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt index 808ded2f..818cb026 100644 --- a/testbed/CMakeLists.txt +++ b/testbed/CMakeLists.txt @@ -74,8 +74,6 @@ SET(TESTBED_SOURCES SET(COMMON_SOURCES common/Box.h common/Box.cpp - common/Cone.h - common/Cone.cpp common/Sphere.h common/Sphere.cpp common/Line.h @@ -86,8 +84,6 @@ SET(COMMON_SOURCES common/ConvexMesh.cpp common/ConcaveMesh.h common/ConcaveMesh.cpp - common/Cylinder.h - common/Cylinder.cpp common/Dumbbell.h common/Dumbbell.cpp common/HeightField.h @@ -98,6 +94,8 @@ SET(COMMON_SOURCES common/VisualContactPoint.cpp common/PerlinNoise.h common/PerlinNoise.cpp + common/AABB.h + common/AABB.cpp ) # Examples scenes source files @@ -110,10 +108,14 @@ SET(SCENES_SOURCES scenes/raycast/RaycastScene.cpp scenes/collisionshapes/CollisionShapesScene.h scenes/collisionshapes/CollisionShapesScene.cpp + scenes/collisiondetection/CollisionDetectionScene.h + scenes/collisiondetection/CollisionDetectionScene.cpp scenes/concavemesh/ConcaveMeshScene.h scenes/concavemesh/ConcaveMeshScene.cpp scenes/heightfield/HeightFieldScene.h scenes/heightfield/HeightFieldScene.cpp + scenes/cubestack/CubeStackScene.h + scenes/cubestack/CubeStackScene.cpp ) # Add .user file to set debug path in Visual Studio diff --git a/testbed/common/AABB.cpp b/testbed/common/AABB.cpp new file mode 100644 index 00000000..8c43700c --- /dev/null +++ b/testbed/common/AABB.cpp @@ -0,0 +1,202 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "AABB.h" + +// Macros +#define MEMBER_OFFSET(s,m) ((char *)nullptr + (offsetof(s,m))) + +// Initialize static variables +openglframework::VertexBufferObject AABB::mVBOVertices(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject AABB::mVBONormals(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject AABB::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); +openglframework::VertexArrayObject AABB::mVAO; + + +// Initialize the data to render AABBs +void AABB::init() { + createVBOAndVAO(); +} + +// Destroy the data used to render AABBs +void AABB::destroy() { + + // Destroy the VBOs and VAO + mVBOVertices.destroy(); + mVBONormals.destroy(); + mVAO.destroy(); +} + +// Render the AABB +void AABB::render(const openglframework::Vector3& position, const openglframework::Vector3& dimension, + openglframework::Color color, openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix) { + + // Render in wireframe mode + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + + // Bind the shader + shader.bind(); + + // Transform matrix + openglframework::Matrix4 transformMatrix = openglframework::Matrix4::identity(); + transformMatrix = openglframework::Matrix4::translationMatrix(position) * transformMatrix; + + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * transformMatrix; + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + + + // Compute the scaling matrix + openglframework::Matrix4 scalingMatrix = openglframework::Matrix4(dimension.x, 0, 0, 0, + 0, dimension.y, 0, 0, + 0, 0, dimension.z, 0, + 0, 0, 0, 1); + + transformMatrix = transformMatrix * scalingMatrix; + + // Set the model to camera matrix + shader.setMatrix4x4Uniform("localToWorldMatrix", transformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + + // Set the vertex color + openglframework::Vector4 colorVec(color.r, color.g, color.b, color.a); + shader.setVector4Uniform("vertexColor", colorVec, false); + + // Bind the VAO + mVAO.bind(); + + mVBOVertices.bind(); + + // Get the location of shader attribute variables + GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); + + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); + + mVBONormals.bind(); + + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); + if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); + + // Draw the geometry + glDrawElements(GL_LINES, 12 * 2, GL_UNSIGNED_INT, (char*)nullptr); + + glDisableVertexAttribArray(vertexPositionLoc); + if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc); + + mVBONormals.unbind(); + mVBOVertices.unbind(); + + // Unbind the VAO + mVAO.unbind(); + + // Unbind the shader + shader.unbind(); + + // Disable wireframe mode + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); +} + +// Create the Vertex Buffer Objects used to render to box with OpenGL. +/// We create two VBOs (one for vertices and one for indices) +void AABB::createVBOAndVAO() { + + std::vector vertices; + std::vector normals; + std::vector indices; + + // Vertices + vertices.push_back(openglframework::Vector3(-0.5, -0.5, 0.5)); + vertices.push_back(openglframework::Vector3(0.5, -0.5, 0.5)); + vertices.push_back(openglframework::Vector3(0.5, 0.5, 0.5)); + vertices.push_back(openglframework::Vector3(-0.5, 0.5, 0.5)); + vertices.push_back(openglframework::Vector3(0.5, -0.5, -0.5)); + vertices.push_back(openglframework::Vector3(0.5, 0.5, -0.5)); + vertices.push_back(openglframework::Vector3(-0.5, 0.5, -0.5)); + vertices.push_back(openglframework::Vector3(-0.5, -0.5, -0.5)); + + // Normals + normals.push_back(openglframework::Vector3(1, -1, 1)); + normals.push_back(openglframework::Vector3(1, 1, 1)); + normals.push_back(openglframework::Vector3(-1, 1, 1)); + normals.push_back(openglframework::Vector3(-1, -1, 1)); + normals.push_back(openglframework::Vector3(1, -1, -1)); + normals.push_back(openglframework::Vector3(1, 1, -1)); + normals.push_back(openglframework::Vector3(-1, 1, -1)); + normals.push_back(openglframework::Vector3(-1, -1, -1)); + + // Indices + indices.push_back(0); indices.push_back(1); indices.push_back(1); indices.push_back(2); + indices.push_back(2); indices.push_back(3); indices.push_back(3); indices.push_back(0); + + indices.push_back(4); indices.push_back(5); indices.push_back(5); indices.push_back(6); + indices.push_back(6); indices.push_back(7); indices.push_back(7); indices.push_back(4); + + indices.push_back(1); indices.push_back(4); indices.push_back(2); indices.push_back(5); + indices.push_back(0); indices.push_back(7); indices.push_back(3); indices.push_back(6); + + // Create the VBO for the vertices data + mVBOVertices.create(); + mVBOVertices.bind(); + GLsizei sizeVertices = static_cast(vertices.size() * sizeof(openglframework::Vector3)); + mVBOVertices.copyDataIntoVBO(sizeVertices, &(vertices[0]), GL_STATIC_DRAW); + mVBOVertices.unbind(); + + // Create the VBO for the normals data + mVBONormals.create(); + mVBONormals.bind(); + GLsizei sizeNormals = static_cast(normals.size() * sizeof(openglframework::Vector3)); + mVBONormals.copyDataIntoVBO(sizeNormals, &(normals[0]), GL_STATIC_DRAW); + mVBONormals.unbind(); + + // Create th VBO for the indices data + mVBOIndices.create(); + mVBOIndices.bind(); + GLsizei sizeIndices = static_cast(indices.size() * sizeof(unsigned int)); + mVBOIndices.copyDataIntoVBO(sizeIndices, &(indices[0]), GL_STATIC_DRAW); + mVBOIndices.unbind(); + + // Create the VAO for both VBOs + mVAO.create(); + mVAO.bind(); + + // Bind the VBO of vertices + mVBOVertices.bind(); + + // Bind the VBO of normals + mVBONormals.bind(); + + // Bind the VBO of indices + mVBOIndices.bind(); + + // Unbind the VAO + mVAO.unbind(); +} diff --git a/testbed/common/AABB.h b/testbed/common/AABB.h new file mode 100644 index 00000000..2e8f82e1 --- /dev/null +++ b/testbed/common/AABB.h @@ -0,0 +1,76 @@ +/******************************************************************************** + * ReactPhysics3D physics library, http://www.reactphysics3d.com * + * Copyright (c) 2010-2016 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. * + * * + ********************************************************************************/ + +#ifndef AABB_H +#define AABB_H + +// Libraries +#include "openglframework.h" +#include "reactphysics3d.h" + +// Class AABB +class AABB { + + private : + + // -------------------- Attributes -------------------- // + + /// Scaling matrix (applied to a cube to obtain the correct box dimensions) + openglframework::Matrix4 mScalingMatrix; + + /// Vertex Buffer Object for the vertices data + static openglframework::VertexBufferObject mVBOVertices; + + /// Vertex Buffer Object for the normals data + static openglframework::VertexBufferObject mVBONormals; + + /// Vertex Buffer Object for the indices + static openglframework::VertexBufferObject mVBOIndices; + + /// Vertex Array Object for the vertex data + static openglframework::VertexArrayObject mVAO; + + // -------------------- Methods -------------------- // + + /// Create a the VAO and VBOs to render to box with OpenGL + static void createVBOAndVAO(); + + public : + + // -------------------- Methods -------------------- // + + /// Initialize the data to render AABBs + static void init(); + + /// Destroy the data used to render AABBs + static void destroy(); + + /// Render the cube at the correct position and with the correct orientation + static void render(const openglframework::Vector3& position, const openglframework::Vector3& dimension, + openglframework::Color color, openglframework::Shader& shader, + const openglframework::Matrix4& worldToCameraMatrix); +}; + +#endif diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp index 549b86bb..508fa516 100644 --- a/testbed/common/Box.cpp +++ b/testbed/common/Box.cpp @@ -32,88 +32,15 @@ // Initialize static variables openglframework::VertexBufferObject Box::mVBOVertices(GL_ARRAY_BUFFER); openglframework::VertexBufferObject Box::mVBONormals(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject Box::mVBOTextureCoords(GL_ARRAY_BUFFER); +openglframework::VertexBufferObject Box::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); openglframework::VertexArrayObject Box::mVAO; int Box::totalNbBoxes = 0; -GLfloat Box::mCubeVertices[108] = { - -1.0f,-1.0f,-1.0f, // triangle 1 : begin - -1.0f,-1.0f, 1.0f, - -1.0f, 1.0f, 1.0f, // triangle 1 : end - 1.0f, 1.0f,-1.0f, // triangle 2 : begin - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f,-1.0f, // triangle 2 : end - 1.0f,-1.0f, 1.0f, - -1.0f,-1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - -1.0f, 1.0f,-1.0f, - 1.0f,-1.0f, 1.0f, - -1.0f,-1.0f, 1.0f, - -1.0f,-1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - -1.0f,-1.0f, 1.0f, - 1.0f,-1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f,-1.0f, - 1.0f,-1.0f,-1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f,-1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - 1.0f, 1.0f,-1.0f, - -1.0f, 1.0f,-1.0f, - 1.0f, 1.0f, 1.0f, - -1.0f, 1.0f,-1.0f, - -1.0f, 1.0f, 1.0f, - 1.0f, 1.0f, 1.0f, - -1.0f, 1.0f, 1.0f, - 1.0f,-1.0f, 1.0f -}; -GLfloat Box::mCubeNormals[108] = { - -1.0f, 0.0f, 0.0f, // triangle 1 : begin - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, // triangle 1 : end - 0.0f, 0.0f,-1.0f, // triangle 2 : begin - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f, // triangle 2 : end - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f,// - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f, - 0.0f, 0.0f,-1.0f,// - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f, 0.0f, - -1.0f, 0.0f,0.0f,// - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f, - 0.0f,-1.0f, 0.0f,// - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f,// - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f,// - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f, - 1.0f, 0.0f, 0.0f,// - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f,// - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 1.0f, 0.0f,// - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f, - 0.0f, 0.0f, 1.0f// -}; + // Constructor -Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world) - : openglframework::Object3D() { +Box::Box(const openglframework::Vector3& size, reactphysics3d::CollisionWorld* world, + const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "cube.obj") { // Initialize the size of the box mSize[0] = size.x * 0.5f; @@ -126,23 +53,16 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p 0, 0, mSize[2], 0, 0, 0, 0, 1); - // Initialize the position where the cube will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (box shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); + //mBoxShape->setLocalScaling(rp3d::Vector3(2, 2, 2)); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add the collision shape to the body mProxyShape = mBody->addCollisionShape(mBoxShape, rp3d::Transform::identity()); @@ -160,9 +80,15 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p } // Constructor -Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* world) - : openglframework::Object3D() { +Box::Box(const openglframework::Vector3& size, float mass, reactphysics3d::DynamicsWorld* world, + const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "cube.obj") { + + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this); + + // Calculate the normals of the mesh + calculateNormals(); // Initialize the size of the box mSize[0] = size.x * 0.5f; @@ -175,23 +101,15 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& p 0, 0, mSize[2], 0, 0, 0, 0, 1); - // Initialize the position where the cube will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (box shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2])); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body in the dynamics world - rp3d::RigidBody* body = world->createRigidBody(transform); + rp3d::RigidBody* body = world->createRigidBody(mPreviousTransform); // Add the collision shape to the body mProxyShape = body->addCollisionShape(mBoxShape, rp3d::Transform::identity(), mass); @@ -225,17 +143,11 @@ Box::~Box() { } // Render the cube at the correct position and with the correct orientation -void Box::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the VAO - mVAO.bind(); +void Box::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); - mVBOVertices.bind(); - // Set the model to camera matrix shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); @@ -252,20 +164,27 @@ void Box::render(openglframework::Shader& shader, openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); shader.setVector4Uniform("vertexColor", color, false); + // Bind the VAO + mVAO.bind(); + + mVBOVertices.bind(); + // Get the location of shader attribute variables GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, nullptr); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); mVBONormals.bind(); + if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, nullptr); - // Draw the geometry of the box - glDrawArrays(GL_TRIANGLES, 0, 36); + // For each part of the mesh + for (unsigned int i=0; isetTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Box::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Box.h b/testbed/common/Box.h index 56f8daf5..42d234f9 100644 --- a/testbed/common/Box.h +++ b/testbed/common/Box.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Box -class Box : public openglframework::Object3D, public PhysicsObject { +class Box : public PhysicsObject { private : @@ -47,20 +47,20 @@ class Box : public openglframework::Object3D, public PhysicsObject { /// Scaling matrix (applied to a cube to obtain the correct box dimensions) openglframework::Matrix4 mScalingMatrix; - /// Vertex Buffer Object for the vertices data used to render the box with OpenGL - static openglframework::VertexBufferObject mVBOVertices; + /// Vertex Buffer Object for the vertices data + static openglframework::VertexBufferObject mVBOVertices; - /// Vertex Buffer Object for the normales used to render the box with OpenGL - static openglframework::VertexBufferObject mVBONormals; + /// Vertex Buffer Object for the normals data + static openglframework::VertexBufferObject mVBONormals; - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; + /// Vertex Buffer Object for the texture coords + static openglframework::VertexBufferObject mVBOTextureCoords; - /// Vertices coordinates of the triangles of the box - static GLfloat mCubeVertices[108]; + /// Vertex Buffer Object for the indices + static openglframework::VertexBufferObject mVBOIndices; - /// Vertices normals of the triangles of the box - static GLfloat mCubeNormals[108]; + /// Vertex Array Object for the vertex data + static openglframework::VertexArrayObject mVAO; /// Total number of boxes created static int totalNbBoxes; @@ -68,34 +68,29 @@ class Box : public openglframework::Object3D, public PhysicsObject { // -------------------- Methods -------------------- // /// Create a the VAO and VBOs to render to box with OpenGL - static void createVBOAndVAO(); + void createVBOAndVAO(); public : // -------------------- Methods -------------------- // /// Constructor - Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world); + Box(const openglframework::Vector3& size, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Box(const openglframework::Vector3& size, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld *world); + Box(const openglframework::Vector3& size, float mass, reactphysics3d::DynamicsWorld *world, const std::string& meshFolderPath); /// Destructor ~Box(); /// Render the cube at the correct position and with the correct orientation - void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp index 7b83f6b3..b48baf69 100644 --- a/testbed/common/Capsule.cpp +++ b/testbed/common/Capsule.cpp @@ -34,16 +34,8 @@ openglframework::VertexArrayObject Capsule::mVAO; int Capsule::totalNbCapsules = 0; // Constructor -Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); +Capsule::Capsule(float radius, float height, rp3d::CollisionWorld* world, const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -51,23 +43,16 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos 0, 0, mRadius, 0, 0, 0, 0, 1.0f); - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); + //mCapsuleShape->setLocalScaling(rp3d::Vector3(2, 2, 2)); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = mBody->addCollisionShape(mCapsuleShape, rp3d::Transform::identity()); @@ -83,16 +68,9 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos } // Constructor -Capsule::Capsule(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, +Capsule::Capsule(float radius, float height, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -100,21 +78,15 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos 0, 0, mRadius, 0, 0, 0, 0, 1.0f); - // Initialize the position where the sphere will be rendered - translateWorld(position); + mPreviousTransform = rp3d::Transform::identity(); // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - // Create a rigid body corresponding in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity(), mass); @@ -264,24 +236,6 @@ void Capsule::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Capsule::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Capsule::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h index ee380914..d0e74436 100644 --- a/testbed/common/Capsule.h +++ b/testbed/common/Capsule.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Sphere -class Capsule : public openglframework::Mesh, public PhysicsObject { +class Capsule : public PhysicsObject { private : @@ -82,29 +82,23 @@ class Capsule : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Capsule(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath); + Capsule(float radius, float height, rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Capsule(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, + Capsule(float radius, float height, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Destructor ~Capsule(); /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp index 68f7a56e..f6b04b9c 100644 --- a/testbed/common/ConcaveMesh.cpp +++ b/testbed/common/ConcaveMesh.cpp @@ -27,22 +27,11 @@ #include "ConcaveMesh.h" // Constructor -ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), +ConcaveMesh::ConcaveMesh(rp3d::CollisionWorld* world, const std::string& meshPath) + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -52,7 +41,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, // Vertex and Indices array for the triangle mesh (data in shared and not copied) rp3d::TriangleVertexArray* vertexArray = new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(i), &(mIndices[i][0]), sizeof(int), + getNbFaces(i), &(mIndices[i][0]), 3 * sizeof(int), rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); @@ -64,15 +53,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, // do not forget to delete it at the end mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = mBody->addCollisionShape(mConcaveShape, rp3d::Transform::identity()); @@ -84,22 +68,11 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, } // Constructor -ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), +ConcaveMesh::ConcaveMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -109,7 +82,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, // Vertex and Indices array for the triangle mesh (data in shared and not copied) rp3d::TriangleVertexArray* vertexArray = new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(i), &(mIndices[i][0]), sizeof(int), + getNbFaces(i), &(mIndices[i][0]), 3 * sizeof(int), rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); @@ -121,15 +94,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass, // do not forget to delete it at the end mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh); - mConcaveShape->setIsSmoothMeshCollisionEnabled(false); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = body->addCollisionShape(mConcaveShape, rp3d::Transform::identity(), mass); @@ -165,7 +133,7 @@ ConcaveMesh::~ConcaveMesh() { // Render the sphere at the correct position and with the correct orientation void ConcaveMesh::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { + const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -277,24 +245,6 @@ void ConcaveMesh::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void ConcaveMesh::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void ConcaveMesh::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h index 087c030c..17755352 100644 --- a/testbed/common/ConcaveMesh.h +++ b/testbed/common/ConcaveMesh.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class ConcaveMesh -class ConcaveMesh : public openglframework::Mesh, public PhysicsObject { +class ConcaveMesh : public PhysicsObject { private : @@ -76,28 +76,23 @@ class ConcaveMesh : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - ConcaveMesh(const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshPath); + ConcaveMesh(rp3d::CollisionWorld* world, const std::string& meshPath); /// Constructor - ConcaveMesh(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); + ConcaveMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); /// Destructor ~ConcaveMesh(); /// Render the mesh at the correct position and with the correct orientation void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp deleted file mode 100644 index 1a971922..00000000 --- a/testbed/common/Cone.cpp +++ /dev/null @@ -1,294 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "Cone.h" - -openglframework::VertexBufferObject Cone::mVBOVertices(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cone::mVBONormals(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cone::mVBOTextureCoords(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cone::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); -openglframework::VertexArrayObject Cone::mVAO; -int Cone::totalNbCones = 0; - -// Constructor -Cone::Cone(float radius, float height, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cone will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cone shape) and do - // not forget to delete it at the end - mConeShape = new rp3d::ConeShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; - - // Create a rigid body corresponding to the cone in the dynamics world - mBody = world->createCollisionBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = mBody->addCollisionShape(mConeShape, rp3d::Transform::identity()); - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - // Create the VBOs and VAO - if (totalNbCones == 0) { - createVBOAndVAO(); - } - - totalNbCones++; -} - -// Constructor -Cone::Cone(float radius, float height, const openglframework::Vector3 &position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cone will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cone shape) and do not - // forget to delete it at the end - mConeShape = new rp3d::ConeShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - // Create a rigid body corresponding to the cone in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = body->addCollisionShape(mConeShape, rp3d::Transform::identity(), mass); - - mBody = body; - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - // Create the VBOs and VAO - if (totalNbCones == 0) { - createVBOAndVAO(); - } - - totalNbCones++; -} - -// Destructor -Cone::~Cone() { - - if (totalNbCones == 1) { - // Destroy the mesh - destroy(); - - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mConeShape; - totalNbCones--; -} - -// Render the cone at the correct position and with the correct orientation -void Cone::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); - - // Bind the VAO - mVAO.bind(); - - mVBOVertices.bind(); - - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - - mVBONormals.bind(); - - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - -// Set the scaling of the object -void Cone::setScaling(const openglframework::Vector3& scaling) { - - // Scale the collision shape - mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, - 0, 0, 0, 1); -} - diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h deleted file mode 100644 index e886c66b..00000000 --- a/testbed/common/Cone.h +++ /dev/null @@ -1,113 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef CONE_H -#define CONE_H - -// Libraries -#include "openglframework.h" -#include "reactphysics3d.h" -#include "PhysicsObject.h" - -// Class Cone -class Cone : public openglframework::Mesh, public PhysicsObject { - - private : - - // -------------------- Attributes -------------------- // - - /// Radius of the cone - float mRadius; - - /// Height of the cone - float mHeight; - - /// Collision shape - rp3d::ConeShape* mConeShape; - rp3d::ProxyShape* mProxyShape; - - /// Scaling matrix (applied to a sphere to obtain the correct cone dimensions) - openglframework::Matrix4 mScalingMatrix; - - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; - - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; - - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; - - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; - - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; - - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; - - // Total number of cones created - static int totalNbCones; - - // -------------------- Methods -------------------- // - - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshFolderPath); - - /// Constructor - Cone(float radius, float height, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); - - /// Destructor - ~Cone(); - - /// Render the cone at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor) override; - - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); -}; - -inline void Cone::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); -} - -#endif diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp index 1ded48a2..a59c2f3d 100644 --- a/testbed/common/ConvexMesh.cpp +++ b/testbed/common/ConvexMesh.cpp @@ -27,46 +27,42 @@ #include "ConvexMesh.h" // Constructor -ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), +ConvexMesh::ConvexMesh(rp3d::CollisionWorld* world, const std::string& meshPath) + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); + // Polygon faces descriptions for the polyhedron + mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)]; + rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces; + for (int f=0; f < getNbFaces(0); f++) { + face->indexBase = f * 3; + face->nbVertices = 3; + face++; + } - // Vertex and Indices array for the triangle mesh (data in shared and not copied) - mPhysicsTriangleVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(0), &(mIndices[0][0]), sizeof(int), - rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, - rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + // Create the polygon vertex array + mPolygonVertexArray = + new rp3d::PolygonVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), + &(mIndices[0][0]), sizeof(int), + getNbFaces(0), mPolygonFaces, + rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + // Create the polyhedron mesh + mPolyhedronMesh = new rp3d::PolyhedronMesh(mPolygonVertexArray); // Create the collision shape for the rigid body (convex mesh shape) and // do not forget to delete it at the end - mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); + mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = mBody->addCollisionShape(mConvexShape, rp3d::Transform::identity()); @@ -78,43 +74,42 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, } // Constructor -ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshPath) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), +ConvexMesh::ConvexMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath) + : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); - // Vertex and Indices array for the triangle mesh (data in shared and not copied) - mPhysicsTriangleVertexArray = - new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), - getNbFaces(0), &(mIndices[0][0]), sizeof(int), - rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, - rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + // Polygon faces descriptions for the polyhedron + mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)]; + rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces; + for (int f=0; f < getNbFaces(0); f++) { + face->indexBase = f * 3; + face->nbVertices = 3; + face++; + } + + // Create the polygon vertex array + mPolygonVertexArray = + new rp3d::PolygonVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3), + &(mIndices[0][0]), sizeof(int), + getNbFaces(0), mPolygonFaces, + rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE, + rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE); + + // Create the polyhedron mesh + mPolyhedronMesh = new rp3d::PolyhedronMesh(mPolygonVertexArray); // Create the collision shape for the rigid body (convex mesh shape) and do // not forget to delete it at the end - mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray); + mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = body->addCollisionShape(mConvexShape, rp3d::Transform::identity(), mass); @@ -140,7 +135,9 @@ ConvexMesh::~ConvexMesh() { mVBOTextureCoords.destroy(); mVAO.destroy(); - delete mPhysicsTriangleVertexArray; + delete mPolyhedronMesh; + delete mPolygonVertexArray; + delete[] mPolygonFaces; delete mConvexShape; } @@ -258,24 +255,6 @@ void ConvexMesh::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void ConvexMesh::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void ConvexMesh::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h index 842d187f..865921ca 100644 --- a/testbed/common/ConvexMesh.h +++ b/testbed/common/ConvexMesh.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class ConvexMesh -class ConvexMesh : public openglframework::Mesh, public PhysicsObject { +class ConvexMesh : public PhysicsObject { private : @@ -41,7 +41,11 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject { /// Previous transform (for interpolation) rp3d::Transform mPreviousTransform; - rp3d::TriangleVertexArray* mPhysicsTriangleVertexArray; + rp3d::PolygonVertexArray::PolygonFace* mPolygonFaces; + + rp3d::PolygonVertexArray* mPolygonVertexArray; + + rp3d::PolyhedronMesh* mPolyhedronMesh; /// Collision shape rp3d::ConvexMeshShape* mConvexShape; @@ -75,28 +79,22 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - ConvexMesh(const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshPath); + ConvexMesh(rp3d::CollisionWorld* world, const std::string& meshPath); /// Constructor - ConvexMesh(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); + ConvexMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath); /// Destructor ~ConvexMesh(); /// Render the mesh at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp deleted file mode 100644 index f791317b..00000000 --- a/testbed/common/Cylinder.cpp +++ /dev/null @@ -1,294 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "Cylinder.h" - -openglframework::VertexBufferObject Cylinder::mVBOVertices(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cylinder::mVBONormals(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cylinder::mVBOTextureCoords(GL_ARRAY_BUFFER); -openglframework::VertexBufferObject Cylinder::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER); -openglframework::VertexArrayObject Cylinder::mVAO; -int Cylinder::totalNbCylinders = 0; - -// Constructor -Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, - reactphysics3d::CollisionWorld* world, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cylinder will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cylinder shape) and do not - // forget to delete it at the end - mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; - - // Create a rigid body corresponding to the cylinder in the dynamics world - mBody = world->createCollisionBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = mBody->addCollisionShape(mCylinderShape, rp3d::Transform::identity()); - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - // Create the VBOs and VAO - if (totalNbCylinders == 0) { - createVBOAndVAO(); - } - - totalNbCylinders++; -} - -// Constructor -Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position, - float mass, reactphysics3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius), mHeight(height) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); - - // Compute the scaling matrix - mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, - 0, mHeight, 0, 0, - 0, 0, mRadius, 0, - 0, 0, 0, 1); - - // Initialize the position where the cylinder will be rendered - translateWorld(position); - - // Create the collision shape for the rigid body (cylinder shape) and do - // not forget to delete it at the end - mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - // Create a rigid body corresponding to the cylinder in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); - - // Add a collision shape to the body and specify the mass of the shape - mProxyShape = body->addCollisionShape(mCylinderShape, rp3d::Transform::identity(), mass); - - mTransformMatrix = mTransformMatrix * mScalingMatrix; - - mBody = body; - - // Create the VBOs and VAO - if (totalNbCylinders == 0) { - createVBOAndVAO(); - } - - totalNbCylinders++; -} - -// Destructor -Cylinder::~Cylinder() { - - if (totalNbCylinders == 1) { - - // Destroy the mesh - destroy(); - - // Destroy the VBOs and VAO - mVBOIndices.destroy(); - mVBOVertices.destroy(); - mVBONormals.destroy(); - mVBOTextureCoords.destroy(); - mVAO.destroy(); - } - delete mCylinderShape; - totalNbCylinders--; -} - -// Render the cylinder at the correct position and with the correct orientation -void Cylinder::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Set the model to camera matrix - shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - - // Set the vertex color - openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor; - openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a); - shader.setVector4Uniform("vertexColor", color, false); - - // Bind the VAO - mVAO.bind(); - - mVBOVertices.bind(); - - // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); - GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); - - glEnableVertexAttribArray(vertexPositionLoc); - glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - - mVBONormals.bind(); - - if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr); - if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc); - - // For each part of the mesh - for (unsigned int i=0; isetTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != nullptr) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - -// Set the scaling of the object -void Cylinder::setScaling(const openglframework::Vector3& scaling) { - - // Scale the collision shape - mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z)); - - // Scale the graphics object - mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0, - 0, mHeight * scaling.y, 0, 0, - 0, 0, mRadius * scaling.z, 0, - 0, 0, 0, 1); -} diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h deleted file mode 100644 index 2e75084c..00000000 --- a/testbed/common/Cylinder.h +++ /dev/null @@ -1,114 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef CYLINDER_H -#define CYLINDER_H - -// Libraries -#include "openglframework.h" -#include "reactphysics3d.h" -#include "PhysicsObject.h" - -// Class Cylinder -class Cylinder : public openglframework::Mesh, public PhysicsObject { - - private : - - // -------------------- Attributes -------------------- // - - /// Radius of the cylinder - float mRadius; - - /// Height of the cylinder - float mHeight; - - /// Scaling matrix (applied to a sphere to obtain the correct cylinder dimensions) - openglframework::Matrix4 mScalingMatrix; - - /// Previous transform (for interpolation) - rp3d::Transform mPreviousTransform; - - /// Collision shape - rp3d::CylinderShape* mCylinderShape; - rp3d::ProxyShape* mProxyShape; - - /// Vertex Buffer Object for the vertices data - static openglframework::VertexBufferObject mVBOVertices; - - /// Vertex Buffer Object for the normals data - static openglframework::VertexBufferObject mVBONormals; - - /// Vertex Buffer Object for the texture coords - static openglframework::VertexBufferObject mVBOTextureCoords; - - /// Vertex Buffer Object for the indices - static openglframework::VertexBufferObject mVBOIndices; - - /// Vertex Array Object for the vertex data - static openglframework::VertexArrayObject mVAO; - - // Total number of capsules created - static int totalNbCylinders; - - // -------------------- Methods -------------------- // - - // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string &meshFolderPath); - - /// Constructor - Cylinder(float radius, float height, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string &meshFolderPath); - - /// Destructor - ~Cylinder(); - - /// Render the cylinder at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); - - /// Update the transform matrix of the object - virtual void updateTransform(float interpolationFactor) override; - - /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); -}; - -// Update the transform matrix of the object -inline void Cylinder::updateTransform(float interpolationFactor) { - mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix); -} - -#endif diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp index 92f50223..f90f0cc9 100644 --- a/testbed/common/Dumbbell.cpp +++ b/testbed/common/Dumbbell.cpp @@ -34,24 +34,14 @@ openglframework::VertexArrayObject Dumbbell::mVAO; int Dumbbell::totalNbDumbbells = 0; // Constructor -Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) - : openglframework::Mesh() { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); +Dumbbell::Dumbbell(rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "dumbbell.obj") { // Identity scaling matrix mScalingMatrix.setToIdentity(); mDistanceBetweenSphere = 8.0f; - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create a sphere collision shape for the two ends of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() @@ -59,21 +49,15 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, const rp3d::decimal massSphere = rp3d::decimal(2.0); mSphereShape = new rp3d::SphereShape(radiusSphere); - // Create a cylinder collision shape for the middle of the dumbbell + // Create a capsule collision shape for the middle of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusCylinder = rp3d::decimal(0.5); - const rp3d::decimal heightCylinder = rp3d::decimal(8.0); + const rp3d::decimal radiusCapsule = rp3d::decimal(0.5); + const rp3d::decimal heightCapsule = rp3d::decimal(7.0); const rp3d::decimal massCylinder = rp3d::decimal(1.0); - mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); + mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::decimal angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transformBody(initPosition, initOrientation); - - mPreviousTransform = transformBody; + mPreviousTransform = rp3d::Transform::identity(); // Initial transform of the first sphere collision shape of the dumbbell (in local-space) rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); @@ -85,12 +69,12 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); // Create a rigid body corresponding to the dumbbell in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transformBody); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add the three collision shapes to the body and specify the mass and transform of the shapes mProxyShapeSphere1 = body->addCollisionShape(mSphereShape, transformSphereShape1, massSphere); mProxyShapeSphere2 = body->addCollisionShape(mSphereShape, transformSphereShape2, massSphere); - mProxyShapeCylinder = body->addCollisionShape(mCylinderShape, transformCylinderShape, massCylinder); + mProxyShapeCapsule = body->addCollisionShape(mCapsuleShape, transformCylinderShape, massCylinder); mBody = body; @@ -105,24 +89,14 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, } // Constructor -Dumbbell::Dumbbell(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh() { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); +Dumbbell::Dumbbell(rp3d::CollisionWorld* world, const std::string& meshFolderPath) + : PhysicsObject(meshFolderPath + "dumbbell.obj"){ // Identity scaling matrix mScalingMatrix.setToIdentity(); mDistanceBetweenSphere = 8.0f; - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create a sphere collision shape for the two ends of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() @@ -132,15 +106,9 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Create a cylinder collision shape for the middle of the dumbbell // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() - const rp3d::decimal radiusCylinder = rp3d::decimal(0.5); - const rp3d::decimal heightCylinder = rp3d::decimal(8.0); - mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder); - - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::decimal angleAroundX = 0;//rp3d::PI / 2; - rp3d::Quaternion initOrientation(angleAroundX, 0, 0); - rp3d::Transform transformBody(initPosition, initOrientation); + const rp3d::decimal radiusCapsule = rp3d::decimal(0.5); + const rp3d::decimal heightCapsule = rp3d::decimal(7.0); + mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule); // Initial transform of the first sphere collision shape of the dumbbell (in local-space) rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity()); @@ -151,13 +119,15 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position, // Initial transform of the cylinder collision shape of the dumbell (in local-space) rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); + mPreviousTransform = rp3d::Transform::identity(); + // Create a rigid body corresponding to the dumbbell in the dynamics world - mBody = world->createCollisionBody(transformBody); + mBody = world->createCollisionBody(mPreviousTransform); // Add the three collision shapes to the body and specify the mass and transform of the shapes mProxyShapeSphere1 = mBody->addCollisionShape(mSphereShape, transformSphereShape1); mProxyShapeSphere2 = mBody->addCollisionShape(mSphereShape, transformSphereShape2); - mProxyShapeCylinder = mBody->addCollisionShape(mCylinderShape, transformCylinderShape); + mProxyShapeCapsule = mBody->addCollisionShape(mCapsuleShape, transformCylinderShape); mTransformMatrix = mTransformMatrix * mScalingMatrix; @@ -185,7 +155,7 @@ Dumbbell::~Dumbbell() { mVAO.destroy(); } delete mSphereShape; - delete mCylinderShape; + delete mCapsuleShape; totalNbDumbbells--; } @@ -303,30 +273,12 @@ void Dumbbell::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Dumbbell::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Dumbbell::setScaling(const openglframework::Vector3& scaling) { // Scale the collision shape rp3d::Vector3 newScaling(scaling.x, scaling.y, scaling.z); - mProxyShapeCylinder->setLocalScaling(newScaling); + mProxyShapeCapsule->setLocalScaling(newScaling); mProxyShapeSphere1->setLocalScaling(newScaling); mProxyShapeSphere2->setLocalScaling(newScaling); diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h index 7982819d..135beabc 100644 --- a/testbed/common/Dumbbell.h +++ b/testbed/common/Dumbbell.h @@ -31,8 +31,8 @@ #include "reactphysics3d.h" #include "PhysicsObject.h" -// Class Sphere -class Dumbbell : public openglframework::Mesh, public PhysicsObject { +// Class Dumbbell +class Dumbbell : public PhysicsObject { private : @@ -42,9 +42,9 @@ class Dumbbell : public openglframework::Mesh, public PhysicsObject { float mDistanceBetweenSphere; /// Collision shapes - rp3d::CylinderShape* mCylinderShape; + rp3d::CapsuleShape* mCapsuleShape; rp3d::SphereShape* mSphereShape; - rp3d::ProxyShape* mProxyShapeCylinder; + rp3d::ProxyShape* mProxyShapeCapsule; rp3d::ProxyShape* mProxyShapeSphere1; rp3d::ProxyShape* mProxyShapeSphere2; @@ -82,29 +82,22 @@ class Dumbbell : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld, - const std::string& meshFolderPath); + Dumbbell(rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Constructor - Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world, - const std::string& meshFolderPath); - + Dumbbell(rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Destructor ~Dumbbell(); /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp index f65123ee..e979936d 100644 --- a/testbed/common/HeightField.cpp +++ b/testbed/common/HeightField.cpp @@ -28,15 +28,11 @@ #include "PerlinNoise.h" // Constructor -HeightField::HeightField(const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), +HeightField::HeightField(rp3d::CollisionWorld* world) + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -51,15 +47,10 @@ HeightField::HeightField(const openglframework::Vector3 &position, mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, mHeightData, rp3d::HeightFieldShape::HeightDataType::HEIGHT_FLOAT_TYPE); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = mBody->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity()); @@ -71,15 +62,11 @@ HeightField::HeightField(const openglframework::Vector3 &position, } // Constructor -HeightField::HeightField(const openglframework::Vector3 &position, float mass, - reactphysics3d::DynamicsWorld* dynamicsWorld) - : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER), +HeightField::HeightField(float mass, rp3d::DynamicsWorld* dynamicsWorld) + : mVBOVertices(GL_ARRAY_BUFFER), mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER), mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) { - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4::identity(); @@ -94,13 +81,10 @@ HeightField::HeightField(const openglframework::Vector3 &position, float mass, mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight, mHeightData, rp3d::HeightFieldShape::HeightDataType::HEIGHT_FLOAT_TYPE); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform); + rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the collision shape mProxyShape = body->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity(), mass); @@ -320,24 +304,6 @@ void HeightField::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void HeightField::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void HeightField::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h index bf1cb148..1ff390ee 100644 --- a/testbed/common/HeightField.h +++ b/testbed/common/HeightField.h @@ -33,7 +33,7 @@ // Class HeightField -class HeightField : public openglframework::Mesh, public PhysicsObject { +class HeightField : public PhysicsObject { private : @@ -89,28 +89,23 @@ class HeightField : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - HeightField(const openglframework::Vector3& position, - rp3d::CollisionWorld* world); + HeightField(rp3d::CollisionWorld* world); /// Constructor - HeightField(const openglframework::Vector3& position, float mass, - rp3d::DynamicsWorld* dynamicsWorld); + HeightField(float mass, rp3d::DynamicsWorld* dynamicsWorld); /// Destructor ~HeightField(); /// Render the mesh at the correct position and with the correct orientation void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/PhysicsObject.cpp b/testbed/common/PhysicsObject.cpp index c176bb5d..9955725c 100644 --- a/testbed/common/PhysicsObject.cpp +++ b/testbed/common/PhysicsObject.cpp @@ -27,12 +27,22 @@ #include "PhysicsObject.h" /// Constructor -PhysicsObject::PhysicsObject() { +PhysicsObject::PhysicsObject() : openglframework::Mesh() { mColor = openglframework::Color(1, 1, 1, 1); mSleepingColor = openglframework::Color(1, 0, 0, 1); } +/// Constructor +PhysicsObject::PhysicsObject(const std::string& meshPath) : PhysicsObject() { + + // Load the mesh from a file + openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this); + + // Calculate the normals of the mesh + calculateNormals(); +} + // Compute the new transform matrix openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFactor, const openglframework::Matrix4& scalingMatrix) { @@ -57,3 +67,21 @@ openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFact // Apply the scaling matrix to have the correct box dimensions return newMatrix * scalingMatrix; } + +// Reset the transform +void PhysicsObject::setTransform(const rp3d::Transform& transform) { + + // Reset the transform + mBody->setTransform(transform); + + mBody->setIsSleeping(false); + + // Reset the velocity of the rigid body + rp3d::RigidBody* rigidBody = dynamic_cast(mBody); + if (rigidBody != nullptr) { + rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); + rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); + } + + updateTransform(1.0f); +} diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h index 5a54d6d0..e1714048 100644 --- a/testbed/common/PhysicsObject.h +++ b/testbed/common/PhysicsObject.h @@ -31,7 +31,7 @@ #include "reactphysics3d.h" // Class PhysicsObject -class PhysicsObject { +class PhysicsObject : public openglframework::Mesh { protected: @@ -56,15 +56,27 @@ class PhysicsObject { /// Constructor PhysicsObject(); + /// Constructor + PhysicsObject(const std::string& meshPath); + /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor)=0; + /// Render the sphere at the correct position and with the correct orientation + virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix)=0; + /// Set the color of the box void setColor(const openglframework::Color& color); /// Set the sleeping color of the box void setSleepingColor(const openglframework::Color& color); + /// Get the transform + const rp3d::Transform& getTransform() const; + + /// Set the transform + void setTransform(const rp3d::Transform& transform); + /// Return a pointer to the collision body of the box reactphysics3d::CollisionBody* getCollisionBody(); @@ -85,6 +97,11 @@ inline void PhysicsObject::setSleepingColor(const openglframework::Color& color) mSleepingColor = color; } +// Get the transform +inline const rp3d::Transform& PhysicsObject::getTransform() const { + return mBody->getTransform(); +} + // Return a pointer to the collision body of the box inline rp3d::CollisionBody* PhysicsObject::getCollisionBody() { return mBody; diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp index e7f880ea..f7c53579 100644 --- a/testbed/common/Sphere.cpp +++ b/testbed/common/Sphere.cpp @@ -34,16 +34,9 @@ openglframework::VertexArrayObject Sphere::mVAO; int Sphere::totalNbSpheres = 0; // Constructor -Sphere::Sphere(float radius, const openglframework::Vector3 &position, - reactphysics3d::CollisionWorld* world, +Sphere::Sphere(float radius, rp3d::CollisionWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -51,23 +44,16 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, 0, 0, mRadius, 0, 0, 0, 0, 1); - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCollisionShape = new rp3d::SphereShape(mRadius); + //mCollisionShape->setLocalScaling(rp3d::Vector3(2, 2, 2)); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); - - mPreviousTransform = transform; + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - mBody = world->createCollisionBody(transform); + mBody = world->createCollisionBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = mBody->addCollisionShape(mCollisionShape, rp3d::Transform::identity()); @@ -83,16 +69,9 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, } // Constructor -Sphere::Sphere(float radius, const openglframework::Vector3 &position, - float mass, reactphysics3d::DynamicsWorld* world, +Sphere::Sphere(float radius, float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) - : openglframework::Mesh(), mRadius(radius) { - - // Load the mesh from a file - openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this); - - // Calculate the normals of the mesh - calculateNormals(); + : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) { // Compute the scaling matrix mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0, @@ -100,21 +79,15 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position, 0, 0, mRadius, 0, 0, 0, 0, 1); - // Initialize the position where the sphere will be rendered - translateWorld(position); - // Create the collision shape for the rigid body (sphere shape) // ReactPhysics3D will clone this object to create an internal one. Therefore, // it is OK if this object is destroyed right after calling RigidBody::addCollisionShape() mCollisionShape = new rp3d::SphereShape(mRadius); - // Initial position and orientation of the rigid body - rp3d::Vector3 initPosition(position.x, position.y, position.z); - rp3d::Quaternion initOrientation = rp3d::Quaternion::identity(); - rp3d::Transform transform(initPosition, initOrientation); + mPreviousTransform = rp3d::Transform::identity(); // Create a rigid body corresponding to the sphere in the dynamics world - rp3d::RigidBody* body = world->createRigidBody(transform); + rp3d::RigidBody* body = world->createRigidBody(mPreviousTransform); // Add a collision shape to the body and specify the mass of the shape mProxyShape = body->addCollisionShape(mCollisionShape, rp3d::Transform::identity(), mass); @@ -150,8 +123,7 @@ Sphere::~Sphere() { } // Render the sphere at the correct position and with the correct orientation -void Sphere::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { +void Sphere::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the shader shader.bind(); @@ -263,24 +235,6 @@ void Sphere::createVBOAndVAO() { mVAO.unbind(); } -// Reset the transform -void Sphere::resetTransform(const rp3d::Transform& transform) { - - // Reset the transform - mBody->setTransform(transform); - - mBody->setIsSleeping(false); - - // Reset the velocity of the rigid body - rp3d::RigidBody* rigidBody = dynamic_cast(mBody); - if (rigidBody != NULL) { - rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0)); - rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0)); - } - - updateTransform(1.0f); -} - // Set the scaling of the object void Sphere::setScaling(const openglframework::Vector3& scaling) { diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h index 70270026..fe2b3f83 100644 --- a/testbed/common/Sphere.h +++ b/testbed/common/Sphere.h @@ -32,7 +32,7 @@ #include "PhysicsObject.h" // Class Sphere -class Sphere : public openglframework::Mesh, public PhysicsObject { +class Sphere : public PhysicsObject { private : @@ -79,28 +79,22 @@ class Sphere : public openglframework::Mesh, public PhysicsObject { // -------------------- Methods -------------------- // /// Constructor - Sphere(float radius, const openglframework::Vector3& position, - rp3d::CollisionWorld* world, const std::string& meshFolderPath); + Sphere(float radius, rp3d::CollisionWorld* world, const std::string& meshFolderPath); /// Constructor - Sphere(float radius, const openglframework::Vector3& position, - float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); + Sphere(float radius, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath); /// Destructor ~Sphere(); /// Render the sphere at the correct position and with the correct orientation - void render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix); - - /// Set the position of the box - void resetTransform(const rp3d::Transform& transform); + void virtual render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override; /// Update the transform matrix of the object virtual void updateTransform(float interpolationFactor) override; /// Set the scaling of the object - void setScaling(const openglframework::Vector3& scaling); + void setScaling(const openglframework::Vector3& scaling) override; }; // Update the transform matrix of the object diff --git a/testbed/common/VisualContactPoint.cpp b/testbed/common/VisualContactPoint.cpp index 7661587e..03ed864b 100644 --- a/testbed/common/VisualContactPoint.cpp +++ b/testbed/common/VisualContactPoint.cpp @@ -36,17 +36,24 @@ openglframework::Mesh VisualContactPoint::mMesh; bool VisualContactPoint::mStaticDataCreated = false; // Constructor -VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position, - const std::string& meshFolderPath) - : mColor(1.0f, 0.0f, 0.0f, 1.0f) { +VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position, const std::string& meshFolderPath, + const openglframework::Vector3& normalLineEndPointLocal, const openglframework::Color& color) + : mColor(color), mVBOVerticesNormalLine(GL_ARRAY_BUFFER) { + + mContactNormalLinePoints[0] = openglframework::Vector3(0, 0, 0); + mContactNormalLinePoints[1] = (normalLineEndPointLocal - position) * 0.5f; // Initialize the position where the mesh will be rendered translateWorld(position); + + // Create the VBO and VAO to render the contact normal line + createContactNormalLineVBOAndVAO(); } // Destructor VisualContactPoint::~VisualContactPoint() { - + mVAONormalLine.destroy(); + mVBOVerticesNormalLine.destroy(); } // Load and initialize the mesh for all the contact points @@ -84,8 +91,7 @@ void VisualContactPoint::destroyStaticData() { } // Render the sphere at the correct position and with the correct orientation -void VisualContactPoint::render(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { +void VisualContactPoint::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the VAO mVAO.bind(); @@ -93,6 +99,10 @@ void VisualContactPoint::render(openglframework::Shader& shader, // Bind the shader shader.bind(); + // Set the vertex color + openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); + shader.setVector4Uniform("vertexColor", color, false); + mVBOVertices.bind(); // Set the model to camera matrix @@ -106,10 +116,6 @@ void VisualContactPoint::render(openglframework::Shader& shader, localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); - // Set the vertex color - openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a); - shader.setVector4Uniform("vertexColor", color, false); - // Get the location of shader attribute variables GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false); @@ -138,9 +144,56 @@ void VisualContactPoint::render(openglframework::Shader& shader, // Unbind the shader shader.unbind(); + + // Render the contact normal line + renderContactNormalLine(shader, worldToCameraMatrix); } -// Create the Vertex Buffer Objects used to render with OpenGL. +void VisualContactPoint::renderContactNormalLine(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { + + // Bind the VAO + mVAONormalLine.bind(); + + // Bind the shader + shader.bind(); + + mVBOVerticesNormalLine.bind(); + + // Set the model to camera matrix + const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix; + shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix); + shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); + + // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the + // model-view matrix) + const openglframework::Matrix3 normalMatrix = + localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); + shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + + // Set the vertex color + openglframework::Vector4 color(0, 1, 0, 1); + shader.setVector4Uniform("vertexColor", color, false); + + // Get the location of shader attribute variables + GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + + glEnableVertexAttribArray(vertexPositionLoc); + glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); + + // Draw the lines + glDrawArrays(GL_LINES, 0, 2); + + glDisableVertexAttribArray(vertexPositionLoc); + + mVBOVerticesNormalLine.unbind(); + + // Unbind the VAO + mVAONormalLine.unbind(); + + shader.unbind(); +} + +// Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL. /// We create two VBOs (one for vertices and one for indices) void VisualContactPoint::createVBOAndVAO() { @@ -181,3 +234,24 @@ void VisualContactPoint::createVBOAndVAO() { // Unbind the VAO mVAO.unbind(); } + +// Create the Vertex Buffer Objects used to render the contact normal line +void VisualContactPoint::createContactNormalLineVBOAndVAO() { + + // Create the VBO for the vertices data + mVBOVerticesNormalLine.create(); + mVBOVerticesNormalLine.bind(); + size_t sizeNormalLineVertices = 2 * sizeof(openglframework::Vector3); + mVBOVerticesNormalLine.copyDataIntoVBO(sizeNormalLineVertices, &mContactNormalLinePoints[0], GL_STATIC_DRAW); + mVBOVerticesNormalLine.unbind(); + + // Create the VAO for both VBOs + mVAONormalLine.create(); + mVAONormalLine.bind(); + + // Bind the VBO of vertices + mVBOVerticesNormalLine.bind(); + + // Unbind the VAO + mVAONormalLine.unbind(); +} diff --git a/testbed/common/VisualContactPoint.h b/testbed/common/VisualContactPoint.h index 944caf21..b540e751 100644 --- a/testbed/common/VisualContactPoint.h +++ b/testbed/common/VisualContactPoint.h @@ -28,6 +28,7 @@ // Libraries #include "openglframework.h" +#include "Line.h" const float VISUAL_CONTACT_POINT_RADIUS = 0.2f; @@ -56,15 +57,30 @@ class VisualContactPoint : public openglframework::Object3D { /// Vertex Array Object for the vertex data static openglframework::VertexArrayObject mVAO; + /// Vertex Buffer Object for the vertices data + openglframework::VertexBufferObject mVBOVerticesNormalLine; + + /// Vertex Array Object for the vertex data + openglframework::VertexArrayObject mVAONormalLine; + /// True if static data (VBO, VAO) has been created already static bool mStaticDataCreated; + // Two end-points of the contact normal line + openglframework::Vector3 mContactNormalLinePoints[2]; + /// Color openglframework::Color mColor; - // Create the Vertex Buffer Objects used to render with OpenGL. + // Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL. static void createVBOAndVAO(); + // Create the Vertex Buffer Objects used to render the contact normal line with OpenGL. + void createContactNormalLineVBOAndVAO(); + + /// Render the contact normal line + void renderContactNormalLine(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); + // -------------------- Methods -------------------- // public : @@ -72,8 +88,8 @@ class VisualContactPoint : public openglframework::Object3D { // -------------------- Methods -------------------- // /// Constructor - VisualContactPoint(const openglframework::Vector3& position, - const std::string &meshFolderPath); + VisualContactPoint(const openglframework::Vector3& position, const std::string &meshFolderPath, + const openglframework::Vector3& normalLineEndPointLocal, const openglframework::Color& color); /// Destructor ~VisualContactPoint(); @@ -84,7 +100,7 @@ class VisualContactPoint : public openglframework::Object3D { /// Destroy the mesh for the contact points static void destroyStaticData(); - /// Render the sphere at the correct position and with the correct orientation + /// Render the sphere contact point at the correct position and with the correct orientation void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); }; diff --git a/testbed/meshes/cube.obj b/testbed/meshes/cube.obj new file mode 100644 index 00000000..167925a5 --- /dev/null +++ b/testbed/meshes/cube.obj @@ -0,0 +1,17 @@ +# Blender v2.72 (sub 0) OBJ File: '' +# www.blender.org +v -1.000000 -1.000000 1.000000 +v -1.000000 -1.000000 -1.000000 +v 1.000000 -1.000000 -1.000000 +v 1.000000 -1.000000 1.000000 +v -1.000000 1.000000 1.000000 +v -1.000000 1.000000 -1.000000 +v 1.000000 1.000000 -1.000000 +v 1.000000 1.000000 1.000000 +s off +f 5 6 2 1 +f 6 7 3 2 +f 7 8 4 3 +f 8 5 1 4 +f 1 2 3 4 +f 8 7 6 5 diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp new file mode 100755 index 00000000..02e8acc1 --- /dev/null +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -0,0 +1,315 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "CollisionDetectionScene.h" + +// Namespaces +using namespace openglframework; +using namespace collisiondetectionscene; + +// Constructor +CollisionDetectionScene::CollisionDetectionScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), + mContactManager(mPhongShader, mMeshFolderPath), + mAreNormalsDisplayed(false) { + + mSelectedShapeIndex = 0; + mIsContactPointsDisplayed = true; + mIsWireframeEnabled = true; + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 0, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Create the dynamics world for the physics simulation + mPhysicsWorld = new rp3d::CollisionWorld(); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif + + // ---------- Sphere 1 ---------- // + + // Create a sphere and a corresponding collision body in the dynamics world + mSphere1 = new Sphere(4, mPhysicsWorld, mMeshFolderPath); + mAllShapes.push_back(mSphere1); + + // Set the color + mSphere1->setColor(mGreyColorDemo); + mSphere1->setSleepingColor(mRedColorDemo); + //mSphere1->setScaling(0.5f); + mPhysicsObjects.push_back(mSphere1); + + // ---------- Sphere 2 ---------- // + + // Create a sphere and a corresponding collision body in the dynamics world + mSphere2 = new Sphere(2, mPhysicsWorld, mMeshFolderPath); + mAllShapes.push_back(mSphere2); + + // Set the color + mSphere2->setColor(mGreyColorDemo); + mSphere2->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mSphere2); + + + // ---------- Capsule 1 ---------- // + + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath); + mAllShapes.push_back(mCapsule1); + + // Set the color + mCapsule1->setColor(mGreyColorDemo); + mCapsule1->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mCapsule1); + + // ---------- Capsule 2 ---------- // + + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath); + mAllShapes.push_back(mCapsule2); + + // Set the color + mCapsule2->setColor(mGreyColorDemo); + mCapsule2->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mCapsule2); + + // ---------- Concave Mesh ---------- // + + // Create a convex mesh and a corresponding collision body in the dynamics world + mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj"); + mAllShapes.push_back(mConcaveMesh); + + // Set the color + mConcaveMesh->setColor(mGreyColorDemo); + mConcaveMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConcaveMesh); + + // ---------- Box 1 ---------- // + + // Create a cylinder and a corresponding collision body in the dynamics world + mBox1 = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath); + mAllShapes.push_back(mBox1); + + // Set the color + mBox1->setColor(mGreyColorDemo); + mBox1->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox1); + + // ---------- Box 2 ---------- // + + // Create a cylinder and a corresponding collision body in the dynamics world + mBox2 = new Box(openglframework::Vector3(3, 2, 5), mPhysicsWorld, mMeshFolderPath); + mAllShapes.push_back(mBox2); + + // Set the color + mBox2->setColor(mGreyColorDemo); + mBox2->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox2); + + // ---------- Convex Mesh ---------- // + + // Create a convex mesh and a corresponding collision body in the dynamics world + mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj"); + mAllShapes.push_back(mConvexMesh); + + // Set the color + mConvexMesh->setColor(mGreyColorDemo); + mConvexMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConvexMesh); + + // ---------- Heightfield ---------- // + + // Create a convex mesh and a corresponding collision body in the dynamics world + mHeightField = new HeightField(mPhysicsWorld); + + // Set the color + mHeightField->setColor(mGreyColorDemo); + mHeightField->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mHeightField); + + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Reset the scene +void CollisionDetectionScene::reset() { + + mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(15, 5, 0), rp3d::Quaternion::identity())); + mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(0, 6, 0), rp3d::Quaternion::identity())); + mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-8, 7, 0), rp3d::Quaternion::identity())); + mCapsule2->setTransform(rp3d::Transform(rp3d::Vector3(11, -8, 0), rp3d::Quaternion::identity())); + mBox1->setTransform(rp3d::Transform(rp3d::Vector3(-4, -7, 0), rp3d::Quaternion::identity())); + mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 9, 0), rp3d::Quaternion::identity())); + mConvexMesh->setTransform(rp3d::Transform(rp3d::Vector3(-5, 0, 0), rp3d::Quaternion::identity())); + mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 15, 0), rp3d::Quaternion::identity())); + mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -22, 0), rp3d::Quaternion::identity())); +} + +// Destructor +CollisionDetectionScene::~CollisionDetectionScene() { + + // Destroy the box rigid body from the dynamics world + //mPhysicsWorld->destroyCollisionBody(mBox->getCollisionBody()); + //delete mBox; + + // Destroy the spheres + mPhysicsWorld->destroyCollisionBody(mSphere1->getCollisionBody()); + delete mSphere1; + + mPhysicsWorld->destroyCollisionBody(mSphere2->getCollisionBody()); + delete mSphere2; + + mPhysicsWorld->destroyCollisionBody(mCapsule1->getCollisionBody()); + delete mCapsule1; + + mPhysicsWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); + delete mCapsule2; + + mPhysicsWorld->destroyCollisionBody(mBox1->getCollisionBody()); + delete mBox1; + + mPhysicsWorld->destroyCollisionBody(mBox2->getCollisionBody()); + delete mBox2; + + mPhysicsWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + delete mConvexMesh; + + mPhysicsWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + delete mConcaveMesh; + + mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + delete mHeightField; + + mContactManager.resetPoints(); + + // Destroy the static data for the visual contact points + VisualContactPoint::destroyStaticData(); + + // Destroy the collision world + delete mPhysicsWorld; +} + +// Take a step for the simulation +void CollisionDetectionScene::update() { + + mContactManager.resetPoints(); + + mPhysicsWorld->testCollision(&mContactManager); + + SceneDemo::update(); +} + +void CollisionDetectionScene::selectNextShape() { + + uint previousIndex = mSelectedShapeIndex; + mSelectedShapeIndex++; + if (mSelectedShapeIndex >= mAllShapes.size()) { + mSelectedShapeIndex = 0; + } + + mAllShapes[previousIndex]->setColor(mGreyColorDemo); + mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo); +} + +// Called when a keyboard event occurs +bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) { + + // If the space key has been pressed + if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) { + selectNextShape(); + return true; + } + + float stepDist = 0.2f; + float stepAngle = 15 * (3.14f / 180.0f); + + if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_UP && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_Z && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_H && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist)); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_A && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_D && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, -stepAngle, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_W && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_S && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(-stepAngle, 0, 0) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_F && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + else if (key == GLFW_KEY_G && action == GLFW_PRESS) { + rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform(); + transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, -stepAngle) * transform.getOrientation()); + mAllShapes[mSelectedShapeIndex]->setTransform(transform); + } + + return false; +} diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h new file mode 100644 index 00000000..9a90f25c --- /dev/null +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -0,0 +1,214 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef COLLISION_DETECTION_SCENE_H +#define COLLISION_DETECTION_SCENE_H + +// Libraries +#include +#include "openglframework.h" +#include "reactphysics3d.h" +#include "SceneDemo.h" +#include "Sphere.h" +#include "Box.h" +#include "Capsule.h" +#include "Line.h" +#include "ConvexMesh.h" +#include "ConcaveMesh.h" +#include "HeightField.h" +#include "Dumbbell.h" +#include "VisualContactPoint.h" + +namespace collisiondetectionscene { + +// Constants +const float SCENE_RADIUS = 30.0f; +const openglframework::Vector3 BOX_SIZE(4, 2, 1); +const float SPHERE_RADIUS = 3.0f; +const float CONE_RADIUS = 3.0f; +const float CONE_HEIGHT = 5.0f; +const float CYLINDER_RADIUS = 3.0f; +const float CYLINDER_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 1.0f; +const float CAPSULE_HEIGHT = 1.0f; +const float DUMBBELL_HEIGHT = 5.0f; +const int NB_RAYS = 100; +const float RAY_LENGTH = 30.0f; +const int NB_BODIES = 9; + +// Contact manager +class ContactManager : public rp3d::CollisionCallback { + + private: + + /// All the visual contact points + std::vector mContactPoints; + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + public: + + ContactManager(openglframework::Shader& shader, const std::string& meshFolderPath) + : mMeshFolderPath(meshFolderPath) { + + } + + /// This method will be called for each reported contact point + virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { + + // For each contact manifold + rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements; + while (manifoldElement != nullptr) { + + // Get the contact manifold + rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold(); + + // For each contact point + rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints(); + while (contactPoint != nullptr) { + + // Contact normal + rp3d::Vector3 normal = contactPoint->getNormal(); + openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z); + + rp3d::Vector3 point1 = contactPoint->getLocalPointOnShape1(); + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red())); + + rp3d::Vector3 point2 = contactPoint->getLocalPointOnShape2(); + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue())); + + contactPoint = contactPoint->getNext(); + } + + manifoldElement = manifoldElement->getNext(); + } + } + + void resetPoints() { + + mContactPoints.clear(); + } + + std::vector getContactPoints() const { + return mContactPoints; + } +}; + +// Class CollisionDetectionScene +class CollisionDetectionScene : public SceneDemo { + + private : + + // -------------------- Attributes -------------------- // + + /// Contact point mesh folder path + std::string mMeshFolderPath; + + /// Contact manager + ContactManager mContactManager; + + bool mAreNormalsDisplayed; + + /// All objects on the scene + //Box* mBox; + Sphere* mSphere1; + Sphere* mSphere2; + Capsule* mCapsule1; + Capsule* mCapsule2; + Box* mBox1; + Box* mBox2; + ConvexMesh* mConvexMesh; + //Dumbbell* mDumbbell; + ConcaveMesh* mConcaveMesh; + HeightField* mHeightField; + + std::vector mAllShapes; + + unsigned int mSelectedShapeIndex; + + /// Select the next shape + void selectNextShape(); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CollisionDetectionScene(const std::string& name, EngineSettings& settings); + + /// Destructor + virtual ~CollisionDetectionScene() override; + + /// Take a step for the simulation + virtual void update() override; + + /// Reset the scene + virtual void reset() override; + + /// Display or not the surface normals at hit points + void showHideNormals(); + + /// Called when a keyboard event occurs + virtual bool keyboardEvent(int key, int scancode, int action, int mods) override; + + /// Enabled/Disable the shadow mapping + virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; + + /// Display/Hide the contact points + virtual void setIsContactPointsDisplayed(bool display) override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Display or not the surface normals at hit points +inline void CollisionDetectionScene::showHideNormals() { + mAreNormalsDisplayed = !mAreNormalsDisplayed; +} + +// Enabled/Disable the shadow mapping +inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { + SceneDemo::setIsShadowMappingEnabled(false); +} + +// Display/Hide the contact points +inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) { + SceneDemo::setIsContactPointsDisplayed(true); +} + +// Return all the contact points of the scene +inline std::vector CollisionDetectionScene::getContactPoints() const { + return mContactManager.getContactPoints(); +} + +} + +#endif diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index 5078cd14..8efc13de 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace collisionshapesscene; // Constructor -CollisionShapesScene::CollisionShapesScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { +CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { std::string meshFolderPath("meshes/"); @@ -43,23 +43,22 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) setScenePosition(center, SCENE_RADIUS); // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, -9.81, 0); + rp3d::Vector3 gravity(0, -9.81f, 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif - float radius = 3.0f; for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -71,19 +70,14 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the mesh the list of dumbbells in the scene mDumbbells.push_back(dumbbell); + mPhysicsObjects.push_back(dumbbell); } // Create all the boxes of the scene for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -95,23 +89,17 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the sphere the list of sphere in the scene mBoxes.push_back(box); + mPhysicsObjects.push_back(box); } // Create all the spheres of the scene for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + sphere->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); // Set the box color sphere->setColor(mDemoColors[i % mNbDemoColors]); @@ -123,78 +111,17 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the sphere the list of sphere in the scene mSpheres.push_back(sphere); - } - - // Create all the cones of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); - - // Set the box color - cone->setColor(mDemoColors[i % mNbDemoColors]); - cone->setSleepingColor(mRedColorDemo); - - // Change the material properties of the rigid body - rp3d::Material& material = cone->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - - // Add the cone the list of sphere in the scene - mCones.push_back(cone); - } - - // Create all the cylinders of the scene - for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); - - // Set the box color - cylinder->setColor(mDemoColors[i % mNbDemoColors]); - cylinder->setSleepingColor(mRedColorDemo); - - // Change the material properties of the rigid body - rp3d::Material& material = cylinder->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - - // Add the cylinder the list of sphere in the scene - mCylinders.push_back(cylinder); + mPhysicsObjects.push_back(sphere); } // Create all the capsules of the scene for (int i=0; igetRigidBody()->getMaterial().setRollingResistance(0.08); + capsule->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08f)); // Set the box color capsule->setColor(mDemoColors[i % mNbDemoColors]); @@ -206,19 +133,14 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the cylinder the list of sphere in the scene mCapsules.push_back(capsule); + mPhysicsObjects.push_back(capsule); } // Create all the convex meshes of the scene for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); @@ -230,12 +152,13 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Add the mesh the list of sphere in the scene mConvexMeshes.push_back(mesh); + mPhysicsObjects.push_back(mesh); } // ---------- Create the floor --------- - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); + mPhysicsObjects.push_back(mFloor); // Set the box color mFloor->setColor(mGreyColorDemo); @@ -248,390 +171,100 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.2)); - // ---------- Create the triangular mesh ---------- // - - /* - // Position - openglframework::Vector3 position(0, 0, 0); - rp3d::decimal mass = 1.0; - - // Create a convex mesh and a corresponding rigid in the dynamics world - mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath); - - // Set the mesh as beeing static - mConcaveMesh->getRigidBody()->setType(rp3d::STATIC); - - // Set the box color - mConcaveMesh->setColor(mDemoColors[0]); - mConcaveMesh->setSleepingColor(mRedColorDemo); - - // Change the material properties of the rigid body - rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); - */ - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor CollisionShapesScene::~CollisionShapesScene() { - // Destroy all the boxes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + // Destroy all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); - // Destroy the box + // Destroy the object delete (*it); } - // Destroy all the sphere of the scene - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - - // Destroy all the cones of the scene - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - - // Destroy all the cylinders of the scene - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - - // Destroy all the capsules of the scene - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the sphere - delete (*it); - } - - // Destroy all the convex meshes of the scene - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the convex mesh - delete (*it); - } - - // Destroy all the dumbbell of the scene - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the convex mesh - delete (*it); - } - - // Destroy the rigid body of the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); - //mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); - - // Destroy the floor - delete mFloor; - - // Destroy the convex mesh - //delete mConcaveMesh; - // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void CollisionShapesScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Take a step for the simulation -void CollisionShapesScene::update() { - - SceneDemo::update(); - - // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the sphere - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the cones - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the cylinders - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the capsules - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the convex meshes - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the dumbbells - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - //mConcaveMesh->updateTransform(mInterpolationFactor); - - mFloor->updateTransform(mInterpolationFactor); -} - -// Render the scene -void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Render all the boxes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render all the sphere of the scene - for (std::vector::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render all the cones of the scene - for (std::vector::iterator it = mCones.begin(); it != mCones.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render all the cylinders of the scene - for (std::vector::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render all the capsules of the scene - for (std::vector::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render all the convex meshes of the scene - for (std::vector::iterator it = mConvexMeshes.begin(); - it != mConvexMeshes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render all the dumbbells of the scene - for (std::vector::iterator it = mDumbbells.begin(); - it != mDumbbells.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render the floor - mFloor->render(shader, worldToCameraMatrix); - - //mConcaveMesh->render(shader, worldToCameraMatrix); - - // Unbind the shader - shader.unbind(); + delete mPhysicsWorld; } /// Reset the scene void CollisionShapesScene::reset() { - float radius = 3.0f; + const float radius = 3.0f; - for (int i=0; iresetTransform(transform); + mDumbbells[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the boxes of the scene - for (int i=0; iresetTransform(transform); + mBoxes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the spheres of the scene - for (int i=0; iresetTransform(transform); - } - - // Create all the cones of the scene - for (int i=0; iresetTransform(transform); - } - - // Create all the cylinders of the scene - for (int i=0; iresetTransform(transform); + mSpheres[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the capsules of the scene - for (int i=0; iresetTransform(transform); + mCapsules[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } // Create all the convex meshes of the scene - for (int i=0; iresetTransform(transform); + mConvexMeshes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } + + // ---------- Create the triangular mesh ---------- // + + mFloor->setTransform(rp3d::Transform::identity()); } diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h index 93d14051..b22d8429 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.h +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h @@ -32,8 +32,6 @@ #include "SceneDemo.h" #include "Sphere.h" #include "Box.h" -#include "Cone.h" -#include "Cylinder.h" #include "Capsule.h" #include "ConvexMesh.h" #include "ConcaveMesh.h" @@ -46,10 +44,8 @@ namespace collisionshapesscene { const float SCENE_RADIUS = 30.0f; const int NB_BOXES = 5; const int NB_SPHERES = 5; -const int NB_CONES = 5; -const int NB_CYLINDERS = 5; const int NB_CAPSULES = 5; -const int NB_MESHES = 3; +const int NB_MESHES = 4; const int NB_COMPOUND_SHAPES = 3; const openglframework::Vector3 BOX_SIZE(2, 2, 2); const float SPHERE_RADIUS = 1.5f; @@ -75,15 +71,11 @@ class CollisionShapesScene : public SceneDemo { // -------------------- Attributes -------------------- // - /// All the spheres of the scene + /// All the boxes of the scene std::vector mBoxes; std::vector mSpheres; - std::vector mCones; - - std::vector mCylinders; - std::vector mCapsules; /// All the convex meshes of the scene @@ -95,30 +87,16 @@ class CollisionShapesScene : public SceneDemo { /// Box for the floor Box* mFloor; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - public: // -------------------- Methods -------------------- // /// Constructor - CollisionShapesScene(const std::string& name); + CollisionShapesScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~CollisionShapesScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Take a step for the simulation - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; @@ -128,7 +106,7 @@ class CollisionShapesScene : public SceneDemo { // Return all the contact points of the scene inline std::vector CollisionShapesScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp index 41234f3f..77656f92 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace trianglemeshscene; // Constructor -ConcaveMeshScene::ConcaveMeshScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { +ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { std::string meshFolderPath("meshes/"); @@ -46,38 +46,122 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif // ---------- Create the boxes ----------- // + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + dumbbell->setSleepingColor(mRedColorDemo); - // Position - openglframework::Vector3 boxPosition(-NB_BOXES_X * BOX_SIZE * BOXES_SPACE / 2 + i * BOX_SIZE * BOXES_SPACE, 30, -NB_BOXES_Z * BOX_SIZE * BOXES_SPACE / 2 + j * BOX_SIZE * BOXES_SPACE); + // Change the material properties of the rigid body + rp3d::Material& material = dumbbell->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); - // Create a sphere and a corresponding rigid in the dynamics world - mBoxes[i * NB_BOXES_Z + j] = new Box(Vector3(BOX_SIZE, BOX_SIZE, BOX_SIZE) * 0.5f, boxPosition, 80.1, mDynamicsWorld); + // Add the mesh the list of dumbbells in the scene + mDumbbells.push_back(dumbbell); + mPhysicsObjects.push_back(dumbbell); + } - // Set the sphere color - mBoxes[i * NB_BOXES_Z + j]->setColor(mDemoColors[0]); - mBoxes[i * NB_BOXES_Z + j]->setSleepingColor(mRedColorDemo); + // Create all the boxes of the scene + for (int i = 0; igetRigidBody()->getMaterial(); - boxMaterial.setBounciness(rp3d::decimal(0.2)); - } + // Create a sphere and a corresponding rigid in the dynamics world + Box* box = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + + // Set the box color + box->setColor(mDemoColors[i % mNbDemoColors]); + box->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = box->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mBoxes.push_back(box); + mPhysicsObjects.push_back(box); + } + + // Create all the spheres of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); + + // Set the box color + sphere->setColor(mDemoColors[i % mNbDemoColors]); + sphere->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = sphere->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mSpheres.push_back(sphere); + mPhysicsObjects.push_back(sphere); + } + + // Create all the capsules of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08)); + + // Set the box color + capsule->setColor(mDemoColors[i % mNbDemoColors]); + capsule->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = capsule->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the cylinder the list of sphere in the scene + mCapsules.push_back(capsule); + mPhysicsObjects.push_back(capsule); + } + + // Create all the convex meshes of the scene + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + mesh->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = mesh->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the mesh the list of sphere in the scene + mConvexMeshes.push_back(mesh); + mPhysicsObjects.push_back(mesh); } // ---------- Create the triangular mesh ---------- // // Position - openglframework::Vector3 position(0, 0, 0); rp3d::decimal mass = 1.0; // Create a convex mesh and a corresponding rigid in the dynamics world - mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath + "city.obj"); + mConcaveMesh = new ConcaveMesh(mass, getDynamicsWorld(), meshFolderPath + "city.obj"); // Set the mesh as beeing static mConcaveMesh->getRigidBody()->setType(rp3d::BodyType::STATIC); @@ -86,106 +170,107 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name) mConcaveMesh->setColor(mGreyColorDemo); mConcaveMesh->setSleepingColor(mGreyColorDemo); + mPhysicsObjects.push_back(mConcaveMesh); + // Change the material properties of the rigid body rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); + material.setFrictionCoefficient(rp3d::decimal(0.1)); // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor ConcaveMeshScene::~ConcaveMeshScene() { - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody()); + // Destroy all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { - // Destroy the boxes - for (int i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); - delete mBoxes[i]; + // Destroy the corresponding rigid body from the dynamics world + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); + + // Destroy the object + delete (*it); } - // Destroy the convex mesh - delete mConcaveMesh; - // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void ConcaveMeshScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Update the scene -void ConcaveMeshScene::update() { - - SceneDemo::update(); - - // Update the transform used for the rendering - mConcaveMesh->updateTransform(mInterpolationFactor); - - for (int i=0; iupdateTransform(mInterpolationFactor); - } -} - -// Render the scene in a single pass -void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - mConcaveMesh->render(shader, worldToCameraMatrix); - - for (int i=0; irender(shader, worldToCameraMatrix); - } - - // Unbind the shader - shader.unbind(); + delete getDynamicsWorld(); } // Reset the scene void ConcaveMeshScene::reset() { - // Reset the transform - rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()); - mConcaveMesh->resetTransform(transform); + const float radius = 15.0f; - for (int i=0; iresetTransform(boxTransform); - } + mDumbbells[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } + // Create all the boxes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the spheres of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the capsules of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the convex meshes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // ---------- Create the triangular mesh ---------- // + + mConcaveMesh->setTransform(rp3d::Transform::identity()); } diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h index 5943fe6c..beca8f71 100644 --- a/testbed/scenes/concavemesh/ConcaveMeshScene.h +++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h @@ -33,15 +33,34 @@ #include "SceneDemo.h" #include "ConcaveMesh.h" #include "Box.h" +#include "Capsule.h" +#include "Dumbbell.h" +#include "Sphere.h" +#include "ConvexMesh.h" namespace trianglemeshscene { // Constants const float SCENE_RADIUS = 70.0f; // Radius of the scene in meters -const int NB_BOXES_X = 8; -const int NB_BOXES_Z = 8; -const float BOX_SIZE = 3.0f; -const float BOXES_SPACE = 2.0f; +static const int NB_BOXES = 50; +static const int NB_SPHERES = 40; +static const int NB_CAPSULES = 20; +static const int NB_MESHES = 15; +static const int NB_COMPOUND_SHAPES = 3; +const openglframework::Vector3 BOX_SIZE(2, 2, 2); +const float SPHERE_RADIUS = 1.5f; +const float CONE_RADIUS = 2.0f; +const float CONE_HEIGHT = 3.0f; +const float CYLINDER_RADIUS = 1.0f; +const float CYLINDER_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 1.0f; +const float CAPSULE_HEIGHT = 1.0f; +const float DUMBBELL_HEIGHT = 1.0f; +const float BOX_MASS = 1.0f; +const float CONE_MASS = 1.0f; +const float CYLINDER_MASS = 1.0f; +const float CAPSULE_MASS = 1.0f; +const float MESH_MASS = 1.0f; // Class TriangleMeshScene class ConcaveMeshScene : public SceneDemo { @@ -50,35 +69,31 @@ class ConcaveMeshScene : public SceneDemo { // -------------------- Attributes -------------------- // - Box* mBoxes[NB_BOXES_X * NB_BOXES_Z]; + std::vector mBoxes; + + std::vector mSpheres; + + std::vector mCapsules; + + /// All the convex meshes of the scene + std::vector mConvexMeshes; + + /// All the dumbbell of the scene + std::vector mDumbbells; /// Concave triangles mesh ConcaveMesh* mConcaveMesh; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - public: // -------------------- Methods -------------------- // /// Constructor - ConcaveMeshScene(const std::string& name); + ConcaveMeshScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~ConcaveMeshScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Update the scene (take a simulation step) - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; @@ -88,7 +103,7 @@ class ConcaveMeshScene : public SceneDemo { // Return all the contact points of the scene inline std::vector ConcaveMeshScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp index f9c8ad77..9eebb351 100644 --- a/testbed/scenes/cubes/CubesScene.cpp +++ b/testbed/scenes/cubes/CubesScene.cpp @@ -1,195 +1,142 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "CubesScene.h" - -// Namespaces -using namespace openglframework; -using namespace cubesscene; - -// Constructor -CubesScene::CubesScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { - - // Compute the radius and the center of the scene - openglframework::Vector3 center(0, 5, 0); - - // Set the center of the scene - setScenePosition(center, SCENE_RADIUS); - - // Gravity vector in the dynamics world - rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); - - // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - - float radius = 2.0f; - - // Create all the cubes of the scene - for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); - cube->setSleepingColor(mRedColorDemo); - - // Change the material properties of the rigid body - rp3d::Material& material = cube->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.4)); - - // Add the box the list of box in the scene - mBoxes.push_back(cube); - } - - // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); - mFloor->setColor(mGreyColorDemo); - mFloor->setSleepingColor(mGreyColorDemo); - - // The floor must be a static rigid body - mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); - - // Change the material properties of the floor rigid body - rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); - material.setBounciness(rp3d::decimal(0.3)); - - // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); - mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); -} - -// Destructor -CubesScene::~CubesScene() { - - // Destroy all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Destroy the corresponding rigid body from the dynamics world - mDynamicsWorld->destroyRigidBody((*it)->getRigidBody()); - - // Destroy the cube - delete (*it); - } - - // Destroy the rigid body of the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); - - // Destroy the floor - delete mFloor; - - // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void CubesScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Update the scene -void CubesScene::update() { - - SceneDemo::update(); - - // Update the position and orientation of the boxes - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - - // Update the transform used for the rendering - (*it)->updateTransform(mInterpolationFactor); - } - - mFloor->updateTransform(mInterpolationFactor); -} - -// Render the scene in a single pass -void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Render all the cubes of the scene - for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); - } - - // Render the floor - mFloor->render(shader, worldToCameraMatrix); - - // Unbind the shader - shader.unbind(); -} - -// Reset the scene -void CubesScene::reset() { - - float radius = 2.0f; - - for (int i=0; iresetTransform(transform); - } -} +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "CubesScene.h" + +// Namespaces +using namespace openglframework; +using namespace cubesscene; + +// Constructor +CubesScene::CubesScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 5, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + + // Create the dynamics world for the physics simulation + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif + + // Create all the cubes of the scene + for (int i=0; isetColor(mDemoColors[i % mNbDemoColors]); + cube->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = cube->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.4)); + + // Add the box the list of box in the scene + mBoxes.push_back(cube); + mPhysicsObjects.push_back(cube); + } + + // ------------------------- FLOOR ----------------------- // + + // Create the floor + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); + mFloor->setColor(mGreyColorDemo); + mFloor->setSleepingColor(mGreyColorDemo); + + // The floor must be a static rigid body + mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); + mPhysicsObjects.push_back(mFloor); + + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); +} + +// Destructor +CubesScene::~CubesScene() { + + // Destroy all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Destroy the corresponding rigid body from the dynamics world + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); + + // Destroy the cube + delete (*it); + } + + // Destroy the rigid body of the floor + getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody()); + + // Destroy the floor + delete mFloor; + + // Destroy the dynamics world + delete getDynamicsWorld(); +} + +// Reset the scene +void CubesScene::reset() { + + float radius = 2.0f; + + // Create all the cubes of the scene + std::vector::iterator it; + int i = 0; + for (it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Position of the cubes + float angle = i * 30.0f; + rp3d::Vector3 position(radius * std::cos(angle), + 10 + i * (BOX_SIZE.y + 0.3f), + 0); + + (*it)->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + + i++; + } + + mFloor->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity())); +} diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h old mode 100644 new mode 100755 index a07c74d2..6d553835 --- a/testbed/scenes/cubes/CubesScene.h +++ b/testbed/scenes/cubes/CubesScene.h @@ -1,96 +1,82 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef CUBES_SCENE_H -#define CUBES_SCENE_H - -// Libraries -#include "openglframework.h" -#include "reactphysics3d.h" -#include "Box.h" -#include "SceneDemo.h" - -namespace cubesscene { - -// Constants -const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters -const int NB_CUBES = 30; // Number of boxes in the scene -const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters -const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters -const float BOX_MASS = 1.0f; // Box mass in kilograms -const float FLOOR_MASS = 100.0f; // Floor mass in kilograms - -// Class CubesScene -class CubesScene : public SceneDemo { - - protected : - - // -------------------- Attributes -------------------- // - - /// All the boxes of the scene - std::vector mBoxes; - - /// Box for the floor - Box* mFloor; - - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - CubesScene(const std::string& name); - - /// Destructor - virtual ~CubesScene() override; - - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Update the scene (take a simulation step) - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - - /// Reset the scene - virtual void reset() override; - - /// Return all the contact points of the scene - virtual std::vector getContactPoints() const override; -}; - -// Return all the contact points of the scene -inline std::vector CubesScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); -} - -} - -#endif +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef CUBES_SCENE_H +#define CUBES_SCENE_H + +// Libraries +#include "openglframework.h" +#include "reactphysics3d.h" +#include "Box.h" +#include "SceneDemo.h" + +namespace cubesscene { + +// Constants +const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters +const int NB_CUBES = 30; // Number of boxes in the scene +const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(50, 1, 50); // Floor dimensions in meters +const float BOX_MASS = 1.0f; // Box mass in kilograms +const float FLOOR_MASS = 100.0f; // Floor mass in kilograms + +// Class CubesScene +class CubesScene : public SceneDemo { + + protected : + + // -------------------- Attributes -------------------- // + + /// All the boxes of the scene + std::vector mBoxes; + + /// Box for the floor + Box* mFloor; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CubesScene(const std::string& name, EngineSettings& settings); + + /// Destructor + virtual ~CubesScene() override; + + /// Reset the scene + virtual void reset() override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Return all the contact points of the scene +inline std::vector CubesScene::getContactPoints() const { + return computeContactPointsOfWorld(getDynamicsWorld()); +} + +} + +#endif diff --git a/testbed/scenes/cubestack/CubeStackScene.cpp b/testbed/scenes/cubestack/CubeStackScene.cpp new file mode 100644 index 00000000..e4a0f859 --- /dev/null +++ b/testbed/scenes/cubestack/CubeStackScene.cpp @@ -0,0 +1,146 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "CubeStackScene.h" + +// Namespaces +using namespace openglframework; +using namespace cubestackscene; + +// Constructor +CubeStackScene::CubeStackScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { + + // Compute the radius and the center of the scene + openglframework::Vector3 center(0, 5, 0); + + // Set the center of the scene + setScenePosition(center, SCENE_RADIUS); + + // Gravity vector in the dynamics world + rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); + + // Create the dynamics world for the physics simulation + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif + + // Create all the cubes of the scene + for (int i=1; i<=NB_FLOORS; i++) { + + for (int j=0; jsetColor(mDemoColors[i % mNbDemoColors]); + cube->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = cube->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.4)); + + // Add the box the list of box in the scene + mBoxes.push_back(cube); + mPhysicsObjects.push_back(cube); + } + } + + // ------------------------- FLOOR ----------------------- // + + // Create the floor + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); + mFloor->setColor(mGreyColorDemo); + mFloor->setSleepingColor(mGreyColorDemo); + + // The floor must be a static rigid body + mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC); + mPhysicsObjects.push_back(mFloor); + + // Get the physics engine parameters + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); + mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); +} + +// Destructor +CubeStackScene::~CubeStackScene() { + + // Destroy all the cubes of the scene + for (std::vector::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) { + + // Destroy the corresponding rigid body from the dynamics world + getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody()); + + // Destroy the cube + delete (*it); + } + + // Destroy the rigid body of the floor + getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody()); + + // Destroy the floor + delete mFloor; + + // Destroy the dynamics world + delete getDynamicsWorld(); +} + +// Reset the scene +void CubeStackScene::reset() { + + int index = 0; + for (int i=NB_FLOORS; i > 0; i--) { + + for (int j=0; jsetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + + index++; + } + } + + mFloor->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity())); +} diff --git a/testbed/scenes/cubestack/CubeStackScene.h b/testbed/scenes/cubestack/CubeStackScene.h new file mode 100644 index 00000000..9bbc1c01 --- /dev/null +++ b/testbed/scenes/cubestack/CubeStackScene.h @@ -0,0 +1,82 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef CUBESTACK_SCENE_H +#define CUBESTACK_SCENE_H + +// Libraries +#include "openglframework.h" +#include "reactphysics3d.h" +#include "Box.h" +#include "SceneDemo.h" + +namespace cubestackscene { + +// Constants +const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters +const int NB_FLOORS = 15; // Number of boxes in the scene +const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters +const openglframework::Vector3 FLOOR_SIZE(50, 1, 50); // Floor dimensions in meters +const float BOX_MASS = 1.0f; // Box mass in kilograms +const float FLOOR_MASS = 100.0f; // Floor mass in kilograms + +// Class CubeStackScene +class CubeStackScene : public SceneDemo { + + protected : + + // -------------------- Attributes -------------------- // + + /// All the boxes of the scene + std::vector mBoxes; + + /// Box for the floor + Box* mFloor; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + CubeStackScene(const std::string& name, EngineSettings& settings); + + /// Destructor + virtual ~CubeStackScene() override; + + /// Reset the scene + virtual void reset() override; + + /// Return all the contact points of the scene + virtual std::vector getContactPoints() const override; +}; + +// Return all the contact points of the scene +inline std::vector CubeStackScene::getContactPoints() const { + return computeContactPointsOfWorld(getDynamicsWorld()); +} + +} + +#endif diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp index 3395ac17..0d13d40d 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -31,7 +31,10 @@ using namespace openglframework; using namespace heightfieldscene; // Constructor -HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) { +HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { + + std::string meshFolderPath("meshes/"); // Compute the radius and the center of the scene openglframework::Vector3 center(0, 5, 0); @@ -43,40 +46,128 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); - // ---------- Create the boxes ----------- // +#ifdef IS_PROFILING_ACTIVE - // For each box - for (int i=0; isetProfilerName(name + "_profiler"); - // Position - openglframework::Vector3 position(15, 10 + 6 * i, 0); +#endif - // Create a box and a corresponding rigid in the dynamics world - mBoxes[i] = new Box(Vector3(3, 3, 3), position, 80.1, mDynamicsWorld); - // Set the box color - mBoxes[i]->setColor(mDemoColors[2]); - mBoxes[i]->setSleepingColor(mRedColorDemo); + for (int i = 0; igetRigidBody()->getMaterial(); - boxMaterial.setBounciness(rp3d::decimal(0.2)); - } + // Create a convex mesh and a corresponding rigid in the dynamics world + Dumbbell* dumbbell = new Dumbbell(getDynamicsWorld(), meshFolderPath); + + // Set the box color + dumbbell->setColor(mDemoColors[i % mNbDemoColors]); + dumbbell->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = dumbbell->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the mesh the list of dumbbells in the scene + mDumbbells.push_back(dumbbell); + mPhysicsObjects.push_back(dumbbell); + } + + // Create all the boxes of the scene + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + box->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = box->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mBoxes.push_back(box); + mPhysicsObjects.push_back(box); + } + + // Create all the spheres of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(0.08f); + + // Set the box color + sphere->setColor(mDemoColors[i % mNbDemoColors]); + sphere->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = sphere->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the sphere the list of sphere in the scene + mSpheres.push_back(sphere); + mPhysicsObjects.push_back(sphere); + } + + // Create all the capsules of the scene + for (int i = 0; igetRigidBody()->getMaterial().setRollingResistance(0.08f); + + // Set the box color + capsule->setColor(mDemoColors[i % mNbDemoColors]); + capsule->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = capsule->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the cylinder the list of sphere in the scene + mCapsules.push_back(capsule); + mPhysicsObjects.push_back(capsule); + } + + // Create all the convex meshes of the scene + for (int i = 0; isetColor(mDemoColors[i % mNbDemoColors]); + mesh->setSleepingColor(mRedColorDemo); + + // Change the material properties of the rigid body + rp3d::Material& material = mesh->getRigidBody()->getMaterial(); + material.setBounciness(rp3d::decimal(0.2)); + + // Add the mesh the list of sphere in the scene + mConvexMeshes.push_back(mesh); + mPhysicsObjects.push_back(mesh); + } // ---------- Create the height field ---------- // // Position - openglframework::Vector3 position(0, 0, 0); rp3d::decimal mass = 1.0; // Create a convex mesh and a corresponding rigid in the dynamics world - mHeightField = new HeightField(position, mass, mDynamicsWorld); + mHeightField = new HeightField(mass, getDynamicsWorld()); // Set the mesh as beeing static mHeightField->getRigidBody()->setType(rp3d::BodyType::STATIC); + mPhysicsObjects.push_back(mHeightField); + // Set the color mHeightField->setColor(mGreyColorDemo); mHeightField->setSleepingColor(mGreyColorDemo); @@ -84,100 +175,102 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Change the material properties of the rigid body rp3d::Material& material = mHeightField->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.2)); - material.setFrictionCoefficient(0.1); + material.setFrictionCoefficient(0.1f); // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor HeightFieldScene::~HeightFieldScene() { - // Destroy the corresponding rigid body from the dynamics world - for (int i=0; idestroyRigidBody(mBoxes[i]->getRigidBody()); - } - mDynamicsWorld->destroyRigidBody(mHeightField->getRigidBody()); + // Destroy all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { - for (int i=0; idestroyRigidBody((*it)->getRigidBody()); - // Destroy the convex mesh - delete mHeightField; + // Destroy the object + delete (*it); + } // Destroy the dynamics world - delete mDynamicsWorld; -} - -// Update the physics world (take a simulation step) -void HeightFieldScene::updatePhysics() { - - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Update the scene -void HeightFieldScene::update() { - - SceneDemo::update(); - - // Update the transform used for the rendering - mHeightField->updateTransform(mInterpolationFactor); - - for (int i=0; iupdateTransform(mInterpolationFactor); - } -} - -// Render the scene in a single pass -void HeightFieldScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - mHeightField->render(shader, worldToCameraMatrix); - - for (int i=0; irender(shader, worldToCameraMatrix); - } - - // Unbind the shader - shader.unbind(); + delete getDynamicsWorld(); } // Reset the scene void HeightFieldScene::reset() { - // Reset the transform - rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()); - mHeightField->resetTransform(transform); + const float radius = 3.0f; - float heightFieldWidth = 10.0f; - float stepDist = heightFieldWidth / (NB_BOXES + 1); - for (int i=0; iresetTransform(boxTransform); + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); } + + // Create all the boxes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the spheres of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the capsules of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // Create all the convex meshes of the scene + for (uint i = 0; isetTransform(rp3d::Transform(position, rp3d::Quaternion::identity())); + } + + // ---------- Create the triangular mesh ---------- // + + mHeightField->setTransform(rp3d::Transform::identity()); } diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h index 7eb9527d..2a3e68f5 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.h +++ b/testbed/scenes/heightfield/HeightFieldScene.h @@ -30,6 +30,10 @@ #include "openglframework.h" #include "reactphysics3d.h" #include "Box.h" +#include "Sphere.h" +#include "ConvexMesh.h" +#include "Capsule.h" +#include "Dumbbell.h" #include "SceneDemo.h" #include "HeightField.h" @@ -37,45 +41,61 @@ namespace heightfieldscene { // Constants const float SCENE_RADIUS = 50.0f; +static const int NB_BOXES = 10; +static const int NB_SPHERES = 5; +static const int NB_CAPSULES = 5; +static const int NB_MESHES = 4; +static const int NB_COMPOUND_SHAPES = 3; +const openglframework::Vector3 BOX_SIZE(2, 2, 2); +const float SPHERE_RADIUS = 1.5f; +const float CONE_RADIUS = 2.0f; +const float CONE_HEIGHT = 3.0f; +const float CYLINDER_RADIUS = 1.0f; +const float CYLINDER_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 1.0f; +const float CAPSULE_HEIGHT = 1.0f; +const float DUMBBELL_HEIGHT = 1.0f; +const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters +const float BOX_MASS = 1.0f; +const float CONE_MASS = 1.0f; +const float CYLINDER_MASS = 1.0f; +const float CAPSULE_MASS = 1.0f; +const float MESH_MASS = 1.0f; +const float FLOOR_MASS = 100.0f; // Class HeightFieldScene class HeightFieldScene : public SceneDemo { - static const int NB_BOXES = 10; - protected : // -------------------- Attributes -------------------- // - Box* mBoxes[NB_BOXES]; + /// All the boxes of the scene + std::vector mBoxes; + + std::vector mSpheres; + + std::vector mCapsules; + + /// All the convex meshes of the scene + std::vector mConvexMeshes; + + /// All the dumbbell of the scene + std::vector mDumbbells; /// Height field HeightField* mHeightField; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - public: // -------------------- Methods -------------------- // /// Constructor - HeightFieldScene(const std::string& name); + HeightFieldScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~HeightFieldScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - - /// Update the scene (take a simulation step) - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override ; - /// Reset the scene virtual void reset() override ; @@ -85,7 +105,7 @@ class HeightFieldScene : public SceneDemo { // Return all the contact points of the scene inline std::vector HeightFieldScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp index b1891627..271cf1f9 100644 --- a/testbed/scenes/joints/JointsScene.cpp +++ b/testbed/scenes/joints/JointsScene.cpp @@ -32,8 +32,8 @@ using namespace openglframework; using namespace jointsscene; // Constructor -JointsScene::JointsScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS) { +JointsScene::JointsScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS) { // Compute the radius and the center of the scene openglframework::Vector3 center(0, 5, 0); @@ -45,7 +45,13 @@ JointsScene::JointsScene(const std::string& name) rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0); // Create the dynamics world for the physics simulation - mDynamicsWorld = new rp3d::DynamicsWorld(gravity); + mPhysicsWorld = new rp3d::DynamicsWorld(gravity); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif // Create the Ball-and-Socket joint createBallAndSocketJoints(); @@ -63,37 +69,37 @@ JointsScene::JointsScene(const std::string& name) createFloor(); // Get the physics engine parameters - mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled(); - rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity(); + mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled(); + rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity(); mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z); - mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled(); - mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity(); - mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity(); - mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver(); - mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver(); - mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep(); + mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled(); + mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity(); + mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity(); + mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver(); + mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver(); + mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep(); } // Destructor JointsScene::~JointsScene() { // Destroy the joints - mDynamicsWorld->destroyJoint(mSliderJoint); - mDynamicsWorld->destroyJoint(mPropellerHingeJoint); - mDynamicsWorld->destroyJoint(mFixedJoint1); - mDynamicsWorld->destroyJoint(mFixedJoint2); + getDynamicsWorld()->destroyJoint(mSliderJoint); + getDynamicsWorld()->destroyJoint(mPropellerHingeJoint); + getDynamicsWorld()->destroyJoint(mFixedJoint1); + getDynamicsWorld()->destroyJoint(mFixedJoint2); for (int i=0; idestroyJoint(mBallAndSocketJoints[i]); + getDynamicsWorld()->destroyJoint(mBallAndSocketJoints[i]); } // Destroy all the rigid bodies of the scene - mDynamicsWorld->destroyRigidBody(mSliderJointBottomBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mSliderJointTopBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mPropellerBox->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mFixedJointBox1->getRigidBody()); - mDynamicsWorld->destroyRigidBody(mFixedJointBox2->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mSliderJointBottomBox->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mSliderJointTopBox->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mPropellerBox->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mFixedJointBox1->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mFixedJointBox2->getRigidBody()); for (int i=0; idestroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody()); } delete mSliderJointBottomBox; @@ -106,77 +112,21 @@ JointsScene::~JointsScene() { } // Destroy the floor - mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); + getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody()); delete mFloor; // Destroy the dynamics world - delete mDynamicsWorld; + delete getDynamicsWorld(); } // Update the physics world (take a simulation step) void JointsScene::updatePhysics() { - // Update the physics engine parameters - mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled); - rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, - mEngineSettings.gravity.z); - mDynamicsWorld->setGravity(gravity); - mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled); - mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); - mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); - mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); - mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); - mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); - // Update the motor speed of the Slider Joint (to move up and down) - long double motorSpeed = 2 * cos(mEngineSettings.elapsedTime * 1.5); + double motorSpeed = 2.0 * std::cos(static_cast(mEngineSettings.elapsedTime) * 1.5); mSliderJoint->setMotorSpeed(rp3d::decimal(motorSpeed)); - // Take a simulation step - mDynamicsWorld->update(mEngineSettings.timeStep); -} - -// Take a step for the simulation -void JointsScene::update() { - - SceneDemo::update(); - - // Update the position and orientation of the boxes - mSliderJointBottomBox->updateTransform(mInterpolationFactor); - mSliderJointTopBox->updateTransform(mInterpolationFactor); - mPropellerBox->updateTransform(mInterpolationFactor); - mFixedJointBox1->updateTransform(mInterpolationFactor); - mFixedJointBox2->updateTransform(mInterpolationFactor); - for (int i=0; iupdateTransform(mInterpolationFactor); - } - - // Update the position and orientation of the floor - mFloor->updateTransform(mInterpolationFactor); -} - -// Render the scene -void JointsScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { - - // Bind the shader - shader.bind(); - - // Render all the boxes - mSliderJointBottomBox->render(shader, worldToCameraMatrix); - mSliderJointTopBox->render(shader, worldToCameraMatrix); - mPropellerBox->render(shader, worldToCameraMatrix); - mFixedJointBox1->render(shader, worldToCameraMatrix); - mFixedJointBox2->render(shader, worldToCameraMatrix); - for (int i=0; irender(shader, worldToCameraMatrix); - } - - // Render the floor - mFloor->render(shader, worldToCameraMatrix); - - // Unbind the shader - shader.unbind(); + SceneDemo::updatePhysics(); } // Reset the scene @@ -193,7 +143,7 @@ void JointsScene::reset() { rp3d::Transform transform(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mBallAndSocketJointChainBoxes[i]->resetTransform(transform); + mBallAndSocketJointChainBoxes[i]->setTransform(transform); positionBox.y -= boxDimension.y + 0.5f; } @@ -207,7 +157,7 @@ void JointsScene::reset() { rp3d::Transform transformBottomBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mSliderJointBottomBox->resetTransform(transformBottomBox); + mSliderJointBottomBox->setTransform(transformBottomBox); // Position of the box openglframework::Vector3 positionBox2(0, 4.2f, 0); @@ -216,7 +166,7 @@ void JointsScene::reset() { rp3d::Transform transformTopBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mSliderJointTopBox->resetTransform(transformTopBox); + mSliderJointTopBox->setTransform(transformTopBox); // --------------- Propeller Hinge joint --------------- // @@ -227,7 +177,7 @@ void JointsScene::reset() { rp3d::Transform transformHingeBox(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mPropellerBox->resetTransform(transformHingeBox); + mPropellerBox->setTransform(transformHingeBox); // --------------- Fixed joint --------------- // @@ -238,7 +188,7 @@ void JointsScene::reset() { rp3d::Transform transformFixedBox1(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox1->resetTransform(transformFixedBox1); + mFixedJointBox1->setTransform(transformFixedBox1); // Position of the box positionBox2 = openglframework::Vector3(-5, 7, 0); @@ -247,7 +197,7 @@ void JointsScene::reset() { rp3d::Transform transformFixedBox2(initPosition, initOrientation); // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox2->resetTransform(transformFixedBox2); + mFixedJointBox2->setTransform(transformFixedBox2); } // Create the boxes and joints for the Ball-and-Socket joint example @@ -255,15 +205,16 @@ void JointsScene::createBallAndSocketJoints() { // --------------- Create the boxes --------------- // - openglframework::Vector3 positionBox(0, 15, 5); + rp3d::Vector3 positionBox(0, 15, 5); openglframework::Vector3 boxDimension(1, 1, 1); const float boxMass = 0.5f; for (int i=0; isetTransform(rp3d::Transform(positionBox, rp3d::Quaternion::identity())); // Set the box color mBallAndSocketJointChainBoxes[i]->setColor(mDemoColors[i % mNbDemoColors]); @@ -281,6 +232,8 @@ void JointsScene::createBallAndSocketJoints() { rp3d::Material& material = mBallAndSocketJointChainBoxes[i]->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mBallAndSocketJointChainBoxes[i]); + positionBox.y -= boxDimension.y + 0.5f; } @@ -298,7 +251,7 @@ void JointsScene::createBallAndSocketJoints() { // Create the joint in the dynamics world mBallAndSocketJoints[i] = dynamic_cast( - mDynamicsWorld->createJoint(jointInfo)); + getDynamicsWorld()->createJoint(jointInfo)); } } @@ -308,11 +261,12 @@ void JointsScene::createSliderJoint() { // --------------- Create the first box --------------- // // Position of the box - openglframework::Vector3 positionBox1(0, 2.1f, 0); + rp3d::Vector3 positionBox1(0, 2.1f, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 box1Dimension(2, 4, 2); - mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld); + mSliderJointBottomBox = new Box(box1Dimension , BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mSliderJointBottomBox->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity())); // Set the box color mSliderJointBottomBox->setColor(mBlueColorDemo); @@ -324,15 +278,17 @@ void JointsScene::createSliderJoint() { // Change the material properties of the rigid body rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial(); material1.setBounciness(0.4f); + mPhysicsObjects.push_back(mSliderJointBottomBox); // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(0, 4.2f, 0); + rp3d::Vector3 positionBox2(0, 4.2f, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f); - mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld); + mSliderJointTopBox = new Box(box2Dimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mSliderJointTopBox->setTransform(rp3d::Transform(positionBox2, rp3d::Quaternion::identity())); // Set the box color mSliderJointTopBox->setColor(mOrangeColorDemo); @@ -341,6 +297,7 @@ void JointsScene::createSliderJoint() { // Change the material properties of the rigid body rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial(); material2.setBounciness(0.4f); + mPhysicsObjects.push_back(mSliderJointTopBox); // --------------- Create the joint --------------- // @@ -359,7 +316,7 @@ void JointsScene::createSliderJoint() { jointInfo.isCollisionEnabled = false; // Create the joint in the dynamics world - mSliderJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); + mSliderJoint = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo)); } /// Create the boxes and joint for the Hinge joint example @@ -368,11 +325,12 @@ void JointsScene::createPropellerHingeJoint() { // --------------- Create the propeller box --------------- // // Position of the box - openglframework::Vector3 positionBox1(0, 7, 0); + rp3d::Vector3 positionBox1(0, 7, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 boxDimension(10, 1, 1); - mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + mPropellerBox = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mPropellerBox->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity())); // Set the box color mPropellerBox->setColor(mYellowColorDemo); @@ -381,6 +339,7 @@ void JointsScene::createPropellerHingeJoint() { // Change the material properties of the rigid body rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mPropellerBox); // --------------- Create the Hinge joint --------------- // @@ -398,7 +357,7 @@ void JointsScene::createPropellerHingeJoint() { jointInfo.isCollisionEnabled = false; // Create the joint in the dynamics world - mPropellerHingeJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); + mPropellerHingeJoint = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo)); } /// Create the boxes and joints for the fixed joints @@ -407,11 +366,12 @@ void JointsScene::createFixedJoints() { // --------------- Create the first box --------------- // // Position of the box - openglframework::Vector3 positionBox1(5, 7, 0); + rp3d::Vector3 positionBox1(5, 7, 0); // Create a box and a corresponding rigid in the dynamics world openglframework::Vector3 boxDimension(1.5, 1.5, 1.5); - mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld); + mFixedJointBox1 = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mFixedJointBox1->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity())); // Set the box color mFixedJointBox1->setColor(mPinkColorDemo); @@ -420,14 +380,16 @@ void JointsScene::createFixedJoints() { // Change the material properties of the rigid body rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial(); material1.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mFixedJointBox1); // --------------- Create the second box --------------- // // Position of the box - openglframework::Vector3 positionBox2(-5, 7, 0); + rp3d::Vector3 positionBox2(-5, 7, 0); // Create a box and a corresponding rigid in the dynamics world - mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld); + mFixedJointBox2 = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath); + mFixedJointBox2->setTransform(rp3d::Transform(positionBox2, rp3d::Quaternion::identity())); // Set the box color mFixedJointBox2->setColor(mBlueColorDemo); @@ -436,6 +398,7 @@ void JointsScene::createFixedJoints() { // Change the material properties of the rigid body rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial(); material2.setBounciness(rp3d::decimal(0.4)); + mPhysicsObjects.push_back(mFixedJointBox2); // --------------- Create the first fixed joint --------------- // @@ -447,7 +410,7 @@ void JointsScene::createFixedJoints() { jointInfo1.isCollisionEnabled = false; // Create the joint in the dynamics world - mFixedJoint1 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo1)); + mFixedJoint1 = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo1)); // --------------- Create the second fixed joint --------------- // @@ -458,15 +421,15 @@ void JointsScene::createFixedJoints() { jointInfo2.isCollisionEnabled = false; // Create the joint in the dynamics world - mFixedJoint2 = dynamic_cast(mDynamicsWorld->createJoint(jointInfo2)); + mFixedJoint2 = dynamic_cast(getDynamicsWorld()->createJoint(jointInfo2)); } // Create the floor void JointsScene::createFloor() { // Create the floor - openglframework::Vector3 floorPosition(0, 0, 0); - mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld); + rp3d::Vector3 floorPosition(0, 0, 0); + mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath); // Set the box color mFloor->setColor(mGreyColorDemo); @@ -478,4 +441,5 @@ void JointsScene::createFloor() { // Change the material properties of the rigid body rp3d::Material& material = mFloor->getRigidBody()->getMaterial(); material.setBounciness(rp3d::decimal(0.3)); + mPhysicsObjects.push_back(mFloor); } diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h index a9474393..dad9af38 100644 --- a/testbed/scenes/joints/JointsScene.h +++ b/testbed/scenes/joints/JointsScene.h @@ -92,9 +92,6 @@ class JointsScene : public SceneDemo { /// Box for the floor Box* mFloor; - /// Dynamics world used for the physics simulation - rp3d::DynamicsWorld* mDynamicsWorld; - // -------------------- Methods -------------------- // /// Create the boxes and joints for the Ball-and-Socket joint example @@ -117,7 +114,7 @@ class JointsScene : public SceneDemo { // -------------------- Methods -------------------- // /// Constructor - JointsScene(const std::string& name); + JointsScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~JointsScene() override ; @@ -126,13 +123,6 @@ class JointsScene : public SceneDemo { /// Can be called several times per frame virtual void updatePhysics() override; - /// Take a step for the simulation - virtual void update() override; - - /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) override; - /// Reset the scene virtual void reset() override; @@ -142,7 +132,7 @@ class JointsScene : public SceneDemo { // Return all the contact points of the scene inline std::vector JointsScene::getContactPoints() const { - return computeContactPointsOfWorld(mDynamicsWorld); + return computeContactPointsOfWorld(getDynamicsWorld()); } } diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp index 479e0915..5705627d 100644 --- a/testbed/scenes/raycast/RaycastScene.cpp +++ b/testbed/scenes/raycast/RaycastScene.cpp @@ -31,8 +31,8 @@ using namespace openglframework; using namespace raycastscene; // Constructor -RaycastScene::RaycastScene(const std::string& name) - : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), +RaycastScene::RaycastScene(const std::string& name, EngineSettings& settings) + : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"), mRaycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1), mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) { @@ -45,108 +45,91 @@ RaycastScene::RaycastScene(const std::string& name) setScenePosition(center, SCENE_RADIUS); // Create the dynamics world for the physics simulation - mCollisionWorld = new rp3d::CollisionWorld(); + mPhysicsWorld = new rp3d::CollisionWorld(); + +#ifdef IS_PROFILING_ACTIVE + + mPhysicsWorld->setProfilerName(name + "_profiler"); + +#endif // ---------- Dumbbell ---------- // - openglframework::Vector3 position1(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mDumbbell = new Dumbbell(position1, mCollisionWorld, mMeshFolderPath); + mDumbbell = new Dumbbell(mPhysicsWorld, mMeshFolderPath); // Set the box color mDumbbell->setColor(mGreyColorDemo); mDumbbell->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mDumbbell); // ---------- Box ---------- // - openglframework::Vector3 position2(0, 0, 0); // Create a box and a corresponding collision body in the dynamics world - mBox = new Box(BOX_SIZE, position2, mCollisionWorld); + mBox = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath); mBox->getCollisionBody()->setIsActive(false); // Set the box color mBox->setColor(mGreyColorDemo); mBox->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mBox); // ---------- Sphere ---------- // - openglframework::Vector3 position3(0, 0, 0); // Create a sphere and a corresponding collision body in the dynamics world - mSphere = new Sphere(SPHERE_RADIUS, position3, mCollisionWorld, - mMeshFolderPath); + mSphere = new Sphere(SPHERE_RADIUS, mPhysicsWorld, mMeshFolderPath); // Set the color mSphere->setColor(mGreyColorDemo); mSphere->setSleepingColor(mRedColorDemo); - - // ---------- Cone ---------- // - openglframework::Vector3 position4(0, 0, 0); - - // Create a cone and a corresponding collision body in the dynamics world - mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld, - mMeshFolderPath); - - // Set the color - mCone->setColor(mGreyColorDemo); - mCone->setSleepingColor(mRedColorDemo); - - // ---------- Cylinder ---------- // - openglframework::Vector3 position5(0, 0, 0); - - // Create a cylinder and a corresponding collision body in the dynamics world - mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5, - mCollisionWorld, mMeshFolderPath); - - // Set the color - mCylinder->setColor(mGreyColorDemo); - mCylinder->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mSphere); // ---------- Capsule ---------- // openglframework::Vector3 position6(0, 0, 0); // Create a cylinder and a corresponding collision body in the dynamics world - mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 , - mCollisionWorld, mMeshFolderPath); + mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath); // Set the color mCapsule->setColor(mGreyColorDemo); mCapsule->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mCapsule); // ---------- Convex Mesh ---------- // - openglframework::Vector3 position7(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj"); + mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj"); // Set the color mConvexMesh->setColor(mGreyColorDemo); mConvexMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConvexMesh); // ---------- Concave Mesh ---------- // - openglframework::Vector3 position8(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj"); + mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj"); // Set the color mConcaveMesh->setColor(mGreyColorDemo); mConcaveMesh->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mConcaveMesh); // ---------- Heightfield ---------- // - openglframework::Vector3 position9(0, 0, 0); // Create a convex mesh and a corresponding collision body in the dynamics world - mHeightField = new HeightField(position9, mCollisionWorld); + mHeightField = new HeightField(mPhysicsWorld); // Set the color mHeightField->setColor(mGreyColorDemo); mHeightField->setSleepingColor(mRedColorDemo); + mPhysicsObjects.push_back(mHeightField); // Create the lines that will be used for raycasting createLines(); // Create the VBO and VAO to render the lines - createVBOAndVAO(mPhongShader); + createVBOAndVAO(); changeBody(); } @@ -154,7 +137,7 @@ RaycastScene::RaycastScene(const std::string& name) // Create the raycast lines void RaycastScene::createLines() { - int nbRaysOneDimension = std::sqrt(float(NB_RAYS)); + int nbRaysOneDimension = static_cast(std::sqrt(float(NB_RAYS))); for (int i=0; igetCollisionBody()->setIsActive(false); mBox->getCollisionBody()->setIsActive(false); - mCone->getCollisionBody()->setIsActive(false); - mCylinder->getCollisionBody()->setIsActive(false); mCapsule->getCollisionBody()->setIsActive(false); mConvexMesh->getCollisionBody()->setIsActive(false); mDumbbell->getCollisionBody()->setIsActive(false); @@ -201,19 +182,15 @@ void RaycastScene::changeBody() { break; case 1: mBox->getCollisionBody()->setIsActive(true); break; - case 2: mCone->getCollisionBody()->setIsActive(true); + case 2: mCapsule->getCollisionBody()->setIsActive(true); break; - case 3: mCylinder->getCollisionBody()->setIsActive(true); + case 3: mConvexMesh->getCollisionBody()->setIsActive(true); break; - case 4: mCapsule->getCollisionBody()->setIsActive(true); + case 4: mDumbbell->getCollisionBody()->setIsActive(true); break; - case 5: mConvexMesh->getCollisionBody()->setIsActive(true); + case 5: mConcaveMesh->getCollisionBody()->setIsActive(true); break; - case 6: mDumbbell->getCollisionBody()->setIsActive(true); - break; - case 7: mConcaveMesh->getCollisionBody()->setIsActive(true); - break; - case 8: mHeightField->getCollisionBody()->setIsActive(true); + case 6: mHeightField->getCollisionBody()->setIsActive(true); break; } @@ -222,58 +199,49 @@ void RaycastScene::changeBody() { // Reset the scene void RaycastScene::reset() { + std::vector::iterator it; + for (it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + (*it)->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity())); + } } // Destructor RaycastScene::~RaycastScene() { - // Destroy the shader - mPhongShader.destroy(); - // Destroy the box rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mBox->getCollisionBody()); delete mBox; // Destroy the sphere - mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mSphere->getCollisionBody()); delete mSphere; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); - delete mCone; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody()); - - // Destroy the sphere - delete mCylinder; - - // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mCapsule->getCollisionBody()); // Destroy the sphere delete mCapsule; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mConvexMesh->getCollisionBody()); // Destroy the convex mesh delete mConvexMesh; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mDumbbell->getCollisionBody()); // Destroy the dumbbell delete mDumbbell; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody()); // Destroy the convex mesh delete mConcaveMesh; // Destroy the corresponding rigid body from the dynamics world - mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody()); + mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody()); // Destroy the convex mesh delete mHeightField; @@ -284,7 +252,7 @@ RaycastScene::~RaycastScene() { VisualContactPoint::destroyStaticData(); // Destroy the collision world - delete mCollisionWorld; + delete mPhysicsWorld; // Destroy the lines for (std::vector::iterator it = mLines.begin(); it != mLines.end(); @@ -297,12 +265,6 @@ RaycastScene::~RaycastScene() { mVAO.destroy(); } -// Update the physics world (take a simulation step) -void RaycastScene::updatePhysics() { - - -} - // Take a step for the simulation void RaycastScene::update() { @@ -324,47 +286,40 @@ void RaycastScene::update() { // Perform a raycast query on the physics world by passing a raycast // callback class in argument. - mCollisionWorld->raycast(ray, &mRaycastManager); + mPhysicsWorld->raycast(ray, &mRaycastManager); } SceneDemo::update(); } // Render the scene -void RaycastScene::renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { +void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { // Bind the VAO mVAO.bind(); // Bind the shader - shader.bind(); + mColorShader.bind(); mVBOVertices.bind(); // Set the model to camera matrix const Matrix4 localToCameraMatrix = Matrix4::identity(); - shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); - shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); - - // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the - // model-view matrix) - const openglframework::Matrix3 normalMatrix = - localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose(); - shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false); + mColorShader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix); + mColorShader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix); // Set the vertex color - openglframework::Vector4 color(1, 0, 0, 1); - shader.setVector4Uniform("vertexColor", color, false); + openglframework::Vector4 color(1, 0.55f, 0, 1); + mColorShader.setVector4Uniform("vertexColor", color, false); // Get the location of shader attribute variables - GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition"); + GLint vertexPositionLoc = mColorShader.getAttribLocation("vertexPosition"); glEnableVertexAttribArray(vertexPositionLoc); glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL); // Draw the lines - glDrawArrays(GL_LINES, 0, NB_RAYS); + glDrawArrays(GL_LINES, 0, mLinePoints.size() * 2); glDisableVertexAttribArray(vertexPositionLoc); @@ -373,28 +328,25 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, // Unbind the VAO mVAO.unbind(); - shader.unbind(); + mColorShader.unbind(); - // Render the shapes - if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); - if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix); - if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix); - if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix); - if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix); - if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix); - if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix); - if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix); - if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix); + // Bind the shader + shader.bind(); - shader.unbind(); + // Render all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + if ((*it)->getCollisionBody()->isActive()) { + (*it)->render(shader, worldToCameraMatrix); + } + } + + // Unbind the shader + shader.unbind(); } // Create the Vertex Buffer Objects used to render with OpenGL. /// We create two VBOs (one for vertices and one for indices) -void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) { - - // Bind the shader - shader.bind(); +void RaycastScene::createVBOAndVAO() { // Create the VBO for the vertices data mVBOVertices.create(); @@ -412,9 +364,6 @@ void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) { // Unbind the VAO mVAO.unbind(); - - // Unbind the shader - shader.unbind(); } // Called when a keyboard event occurs diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h index 56ebb565..8024ff1a 100644 --- a/testbed/scenes/raycast/RaycastScene.h +++ b/testbed/scenes/raycast/RaycastScene.h @@ -34,8 +34,6 @@ #include "SceneDemo.h" #include "Sphere.h" #include "Box.h" -#include "Cone.h" -#include "Cylinder.h" #include "Capsule.h" #include "Line.h" #include "ConvexMesh.h" @@ -59,7 +57,7 @@ const float CAPSULE_HEIGHT = 5.0f; const float DUMBBELL_HEIGHT = 5.0f; const int NB_RAYS = 100; const float RAY_LENGTH = 30.0f; -const int NB_BODIES = 9; +const int NB_BODIES = 7; // Raycast manager class RaycastManager : public rp3d::RaycastCallback { @@ -84,13 +82,16 @@ class RaycastManager : public rp3d::RaycastCallback { } virtual rp3d::decimal notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) override { + + rp3d::Vector3 n = raycastInfo.worldNormal; + openglframework::Vector3 normal(n.x, n.y, n.z); + rp3d::Vector3 hitPos = raycastInfo.worldPoint; openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z); - mHitPoints.push_back(ContactPoint(position)); + mHitPoints.push_back(ContactPoint(position, normal, openglframework::Color::red())); // Create a line to display the normal at hit point - rp3d::Vector3 n = raycastInfo.worldNormal; - openglframework::Vector3 normal(n.x, n.y, n.z); + // TODO : Remove the mNormals because the VisualContactPoint is now able to display the contact normal on its own Line* normalLine = new Line(position, position + normal); mNormals.push_back(normalLine); @@ -136,22 +137,15 @@ class RaycastScene : public SceneDemo { /// True if the hit points normals are displayed bool mAreNormalsDisplayed; - /// Raycast manager - /// All objects on the scene Box* mBox; Sphere* mSphere; - Cone* mCone; - Cylinder* mCylinder; Capsule* mCapsule; ConvexMesh* mConvexMesh; Dumbbell* mDumbbell; ConcaveMesh* mConcaveMesh; HeightField* mHeightField; - /// Collision world used for the physics simulation - rp3d::CollisionWorld* mCollisionWorld; - /// All the points to render the lines std::vector mLinePoints; @@ -165,7 +159,7 @@ class RaycastScene : public SceneDemo { void createLines(); // Create the Vertex Buffer Objects used to render with OpenGL. - void createVBOAndVAO(openglframework::Shader& shader); + void createVBOAndVAO(); public: @@ -173,15 +167,11 @@ class RaycastScene : public SceneDemo { // -------------------- Methods -------------------- // /// Constructor - RaycastScene(const std::string& name); + RaycastScene(const std::string& name, EngineSettings& settings); /// Destructor virtual ~RaycastScene() override; - /// Update the physics world (take a simulation step) - /// Can be called several times per frame - virtual void updatePhysics() override; - /// Take a step for the simulation virtual void update() override; diff --git a/testbed/shaders/color.frag b/testbed/shaders/color.frag new file mode 100644 index 00000000..7ba710f0 --- /dev/null +++ b/testbed/shaders/color.frag @@ -0,0 +1,38 @@ +#version 330 + +/******************************************************************************** +* OpenGL-Framework * +* Copyright (c) 2015 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. * +* * +********************************************************************************/ + +// Uniform variables +uniform vec4 vertexColor; // Vertex color + +// Out variable +out vec4 color; // Output color + +void main() { + + // Compute the final color + color = vertexColor; +} diff --git a/testbed/shaders/color.vert b/testbed/shaders/color.vert new file mode 100644 index 00000000..72083abc --- /dev/null +++ b/testbed/shaders/color.vert @@ -0,0 +1,43 @@ +#version 330 + +/******************************************************************************** +* OpenGL-Framework * +* Copyright (c) 2015 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. * +* * +********************************************************************************/ + +// Uniform variables +uniform mat4 localToWorldMatrix; // Local-space to world-space matrix +uniform mat4 worldToCameraMatrix; // World-space to camera-space matrix +uniform mat4 projectionMatrix; // Projection matrix + +// In variables +in vec4 vertexPosition; + +void main() { + + // Compute the vertex position + vec4 positionCameraSpace = worldToCameraMatrix * localToWorldMatrix * vertexPosition; + + // Compute the clip-space vertex coordinates + gl_Position = projectionMatrix * positionCameraSpace; +} diff --git a/testbed/src/Gui.cpp b/testbed/src/Gui.cpp index 499f1fb3..6ad24335 100644 --- a/testbed/src/Gui.cpp +++ b/testbed/src/Gui.cpp @@ -388,6 +388,14 @@ void Gui::createSettingsPanel() { mApp->mIsContactPointsDisplayed = value; }); + + // Display/Hide the AABBs + CheckBox* checkboxAABBs = new CheckBox(mRenderingPanel, "AABBs"); + checkboxAABBs->setChecked(mApp->mIsAABBsDisplayed); + checkboxAABBs->setCallback([&](bool value) { + mApp->mIsAABBsDisplayed = value; + }); + // Enabled/Disable VSync CheckBox* checkboxVSync = new CheckBox(mRenderingPanel, "V-Sync"); checkboxVSync->setChecked(mApp->mIsVSyncEnabled); @@ -402,6 +410,13 @@ void Gui::createSettingsPanel() { mApp->mIsShadowMappingEnabled = value; }); + // Enable/Disable wireframe mode + CheckBox* checkboxWireframe = new CheckBox(mRenderingPanel, "Wireframe"); + checkboxWireframe->setChecked(mApp->mIsWireframeEnabled); + checkboxWireframe->setCallback([&](bool value) { + mApp->mIsWireframeEnabled = value; + }); + mPhysicsPanel->setVisible(true); mRenderingPanel->setVisible(false); } diff --git a/testbed/src/Scene.cpp b/testbed/src/Scene.cpp index 8031c1f8..b6668649 100644 --- a/testbed/src/Scene.cpp +++ b/testbed/src/Scene.cpp @@ -30,10 +30,10 @@ using namespace openglframework; // Constructor -Scene::Scene(const std::string& name, bool isShadowMappingEnabled) - : mName(name), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0), +Scene::Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled) + : mName(name), mEngineSettings(engineSettings), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0), mViewportWidth(0), mViewportHeight(0), mIsShadowMappingEnabled(isShadowMappingEnabled), - mIsContactPointsDisplayed(true) { + mIsContactPointsDisplayed(true), mIsAABBsDisplayed(false), mIsWireframeEnabled(false) { } @@ -74,14 +74,14 @@ bool Scene::mapMouseCoordinatesToSphere(double xMouse, double yMouse, if ((xMouse >= 0) && (xMouse <= mWindowWidth) && (yMouse >= 0) && (yMouse <= mWindowHeight)) { float x = float(xMouse - 0.5f * mWindowWidth) / float(mWindowWidth); float y = float(0.5f * mWindowHeight - yMouse) / float(mWindowHeight); - float sinx = sin(PI * x * 0.5f); - float siny = sin(PI * y * 0.5f); + float sinx = std::sin(PI * x * 0.5f); + float siny = std::sin(PI * y * 0.5f); float sinx2siny2 = sinx * sinx + siny * siny; // Compute the point on the sphere spherePoint.x = sinx; spherePoint.y = siny; - spherePoint.z = (sinx2siny2 < 1.0) ? sqrt(1.0f - sinx2siny2) : 0.0f; + spherePoint.z = (sinx2siny2 < 1.0f) ? std::sqrt(1.0f - sinx2siny2) : 0.0f; return true; } @@ -175,9 +175,9 @@ void Scene::rotate(int xMouse, int yMouse) { float cosAngle = mLastPointOnSphere.dot(newPoint3D); float epsilon = std::numeric_limits::epsilon(); - if (fabs(cosAngle) < 1.0f && axis.length() > epsilon) { + if (std::abs(cosAngle) < 1.0f && axis.length() > epsilon) { axis.normalize(); - float angle = 2.0f * acos(cosAngle); + float angle = 2.0f * std::acos(cosAngle); // Rotate the camera around the center of the scene mCamera.rotateAroundLocalPoint(axis, -angle, mCenterScene); diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h index bfac3ecb..89a9afdd 100644 --- a/testbed/src/Scene.h +++ b/testbed/src/Scene.h @@ -28,15 +28,19 @@ // Libraries #include "openglframework.h" +#include "reactphysics3d.h" // Structure ContactPoint struct ContactPoint { public: openglframework::Vector3 point; + openglframework::Vector3 normal; + openglframework::Color color; /// Constructor - ContactPoint(const openglframework::Vector3& pointWorld) : point(pointWorld) { + ContactPoint(const openglframework::Vector3& pointWorld, const openglframework::Vector3& normalWorld, const openglframework::Color colorPoint) + : point(pointWorld), normal(normalWorld), color(colorPoint) { } }; @@ -49,8 +53,8 @@ struct EngineSettings { long double elapsedTime; // Elapsed time (in seconds) float timeStep; // Current time step (in seconds) - int nbVelocitySolverIterations; // Nb of velocity solver iterations - int nbPositionSolverIterations; // Nb of position solver iterations + unsigned int nbVelocitySolverIterations; // Nb of velocity solver iterations + unsigned int nbPositionSolverIterations; // Nb of position solver iterations bool isSleepingEnabled; // True if sleeping technique is enabled float timeBeforeSleep; // Time of inactivity before a body sleep float sleepLinearVelocity; // Sleep linear velocity @@ -62,6 +66,23 @@ struct EngineSettings { EngineSettings() : elapsedTime(0.0f), timeStep(0.0f) { } + + /// Return default engine settings + static EngineSettings defaultSettings() { + + EngineSettings defaultSettings; + + defaultSettings.timeStep = 1.0f / 60.0f; + defaultSettings.nbVelocitySolverIterations = rp3d::DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS; + defaultSettings.nbPositionSolverIterations = rp3d::DEFAULT_POSITION_SOLVER_NB_ITERATIONS; + defaultSettings.isSleepingEnabled = rp3d::SLEEPING_ENABLED; + defaultSettings.timeBeforeSleep = rp3d::DEFAULT_TIME_BEFORE_SLEEP; + defaultSettings.sleepLinearVelocity = rp3d::DEFAULT_SLEEP_LINEAR_VELOCITY; + defaultSettings.sleepAngularVelocity = rp3d::DEFAULT_SLEEP_ANGULAR_VELOCITY; + defaultSettings.isGravityEnabled = true; + + return defaultSettings; + } }; // Class Scene @@ -76,7 +97,7 @@ class Scene { std::string mName; /// Physics engine settings - EngineSettings mEngineSettings; + EngineSettings& mEngineSettings; /// Camera openglframework::Camera mCamera; @@ -108,6 +129,12 @@ class Scene { /// True if contact points are displayed bool mIsContactPointsDisplayed; + /// True if the AABBs of the phycis objects are displayed + bool mIsAABBsDisplayed; + + /// True if we render shapes in wireframe mode + bool mIsWireframeEnabled; + // -------------------- Methods -------------------- // /// Set the scene position (where the camera needs to look at) @@ -134,7 +161,7 @@ class Scene { // -------------------- Methods -------------------- // /// Constructor - Scene(const std::string& name, bool isShadowMappingEnabled = false); + Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled = false); /// Destructor virtual ~Scene(); @@ -178,12 +205,6 @@ class Scene { /// Return a reference to the camera const openglframework::Camera& getCamera() const; - /// Get the engine settings - EngineSettings getEngineSettings() const; - - /// Set the engine settings - void setEngineSettings(const EngineSettings& settings); - /// Set the interpolation factor void setInterpolationFactor(float interpolationFactor); @@ -199,6 +220,15 @@ class Scene { /// Display/Hide the contact points void virtual setIsContactPointsDisplayed(bool display); + /// Display/Hide the AABBs + void setIsAABBsDisplayed(bool display); + + /// Return true if wireframe rendering is enabled + bool getIsWireframeEnabled() const; + + /// Enable/disbale wireframe rendering + void setIsWireframeEnabled(bool isEnabled); + /// Return all the contact points of the scene std::vector virtual getContactPoints() const; }; @@ -232,16 +262,6 @@ inline void Scene::setViewport(int x, int y, int width, int height) { mViewportHeight = height; } -// Get the engine settings -inline EngineSettings Scene::getEngineSettings() const { - return mEngineSettings; -} - -// Set the engine settings -inline void Scene::setEngineSettings(const EngineSettings& settings) { - mEngineSettings = settings; -} - // Set the interpolation factor inline void Scene::setInterpolationFactor(float interpolationFactor) { mInterpolationFactor = interpolationFactor; @@ -267,6 +287,21 @@ inline void Scene::setIsContactPointsDisplayed(bool display) { mIsContactPointsDisplayed = display; } +// Display/Hide the AABBs +inline void Scene::setIsAABBsDisplayed(bool display) { + mIsAABBsDisplayed = display; +} + +// Return true if wireframe rendering is enabled +inline bool Scene::getIsWireframeEnabled() const { + return mIsWireframeEnabled; +} + +// Enable/disbale wireframe rendering +inline void Scene::setIsWireframeEnabled(bool isEnabled) { + mIsWireframeEnabled = isEnabled; +} + // Return all the contact points of the scene inline std::vector Scene::getContactPoints() const { diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp index 1906d9f4..5ef6fb95 100644 --- a/testbed/src/SceneDemo.cpp +++ b/testbed/src/SceneDemo.cpp @@ -26,25 +26,27 @@ // Libraries #include "SceneDemo.h" #include +#include "AABB.h" using namespace openglframework; int SceneDemo::shadowMapTextureLevel = 0; -openglframework::Color SceneDemo::mGreyColorDemo = Color(0.70f, 0.70f, 0.7f, 1.0); -openglframework::Color SceneDemo::mYellowColorDemo = Color(0.9, 0.88, 0.145, 1.0); -openglframework::Color SceneDemo::mBlueColorDemo = Color(0, 0.66, 0.95, 1.0); -openglframework::Color SceneDemo::mOrangeColorDemo = Color(0.9, 0.35, 0, 1.0); -openglframework::Color SceneDemo::mPinkColorDemo = Color(0.83, 0.48, 0.64, 1.0); -openglframework::Color SceneDemo::mRedColorDemo = Color(0.95, 0, 0, 1.0); +openglframework::Color SceneDemo::mGreyColorDemo = Color(0.70f, 0.70f, 0.7f, 1.0f); +openglframework::Color SceneDemo::mYellowColorDemo = Color(0.9f, 0.88f, 0.145f, 1.0f); +openglframework::Color SceneDemo::mBlueColorDemo = Color(0, 0.66f, 0.95f, 1.0f); +openglframework::Color SceneDemo::mOrangeColorDemo = Color(0.9f, 0.35f, 0, 1.0f); +openglframework::Color SceneDemo::mPinkColorDemo = Color(0.83f, 0.48f, 0.64f, 1.0f); +openglframework::Color SceneDemo::mRedColorDemo = Color(0.95f, 0, 0, 1.0f); int SceneDemo::mNbDemoColors = 4; openglframework::Color SceneDemo::mDemoColors[] = {SceneDemo::mYellowColorDemo, SceneDemo::mBlueColorDemo, SceneDemo::mOrangeColorDemo, SceneDemo::mPinkColorDemo}; // Constructor -SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled) - : Scene(name, isShadowMappingEnabled), mIsShadowMappingInitialized(false), +SceneDemo::SceneDemo(const std::string& name, EngineSettings& settings, float sceneRadius, bool isShadowMappingEnabled) + : Scene(name, settings, isShadowMappingEnabled), mIsShadowMappingInitialized(false), mDepthShader("shaders/depth.vert", "shaders/depth.frag"), mPhongShader("shaders/phong.vert", "shaders/phong.frag"), + mColorShader("shaders/color.vert", "shaders/color.frag"), mQuadShader("shaders/quad.vert", "shaders/quad.frag"), mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/") { @@ -74,19 +76,30 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa createQuadVBO(); + // Init rendering for the AABBs + AABB::init(); + VisualContactPoint::createStaticData(mMeshFolderPath); } // Destructor SceneDemo::~SceneDemo() { - + mShadowMapTexture.destroy(); mFBOShadowMap.destroy(); mVBOQuad.destroy(); + mDepthShader.destroy(); + mPhongShader.destroy(); + mQuadShader.destroy(); + mColorShader.destroy(); + // Destroy the contact points removeAllContactPoints(); + // Destroy rendering data for the AABB + AABB::destroy(); + VisualContactPoint::destroyStaticData(); } @@ -95,6 +108,36 @@ void SceneDemo::update() { // Update the contact points updateContactPoints(); + + // Update the position and orientation of the physics objects + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + + // Update the transform used for the rendering + (*it)->updateTransform(mInterpolationFactor); + } +} + +// Update the physics world (take a simulation step) +// Can be called several times per frame +void SceneDemo::updatePhysics() { + + if (getDynamicsWorld() != nullptr) { + + // Update the physics engine parameters + getDynamicsWorld()->setIsGratityEnabled(mEngineSettings.isGravityEnabled); + rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y, + mEngineSettings.gravity.z); + getDynamicsWorld()->setGravity(gravity); + getDynamicsWorld()->enableSleeping(mEngineSettings.isSleepingEnabled); + getDynamicsWorld()->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity); + getDynamicsWorld()->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity); + getDynamicsWorld()->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations); + getDynamicsWorld()->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations); + getDynamicsWorld()->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep); + + // Take a simulation step + getDynamicsWorld()->update(mEngineSettings.timeStep); + } } // Render the scene (in multiple passes for shadow mapping) @@ -156,7 +199,7 @@ void SceneDemo::render() { if (mIsShadowMappingEnabled) mShadowMapTexture.bind(); const GLuint textureUnit = 0; - // Set the variables of the shader + // Set the variables of the phong shader mPhongShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix()); mPhongShader.setMatrix4x4Uniform("shadowMapProjectionMatrix", mShadowMapBiasMatrix * shadowMapProjMatrix); mPhongShader.setMatrix4x4Uniform("worldToLight0CameraMatrix", worldToLightCameraMatrix); @@ -166,6 +209,12 @@ void SceneDemo::render() { mPhongShader.setIntUniform("shadowMapSampler", textureUnit); mPhongShader.setIntUniform("isShadowEnabled", mIsShadowMappingEnabled); mPhongShader.setVector2Uniform("shadowMapDimension", Vector2(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT)); + mPhongShader.unbind(); + + // Set the variables of the color shader + mColorShader.bind(); + mColorShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix()); + mColorShader.unbind(); // Set the viewport to render the scene glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight); @@ -184,12 +233,40 @@ void SceneDemo::render() { renderContactPoints(mPhongShader, worldToCameraMatrix); } + // Render the AABBs + if (mIsAABBsDisplayed) { + renderAABBs(worldToCameraMatrix); + } + if (mIsShadowMappingEnabled) mShadowMapTexture.unbind(); mPhongShader.unbind(); //drawTextureQuad(); } +// Render the scene in a single pass +void SceneDemo::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { + + if (mIsWireframeEnabled) { + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + } + + // Bind the shader + shader.bind(); + + // Render all the physics objects of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + (*it)->render(mIsWireframeEnabled ? mColorShader : shader, worldToCameraMatrix); + } + + // Unbind the shader + shader.unbind(); + + if (mIsWireframeEnabled) { + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + } +} + // Create the Shadow map FBO and texture void SceneDemo::createShadowMapFBOAndTexture() { @@ -289,20 +366,44 @@ void SceneDemo::updateContactPoints() { for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { // Create a visual contact point for rendering - VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath); + VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath, it->point + it->normal, it->color); mContactPoints.push_back(point); } } } // Render the contact points -void SceneDemo::renderContactPoints(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix) { +void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) { - // Render all the raycast hit points + // Render all the contact points for (std::vector::iterator it = mContactPoints.begin(); it != mContactPoints.end(); ++it) { - (*it)->render(shader, worldToCameraMatrix); + (*it)->render(mColorShader, worldToCameraMatrix); + } +} + +// Render the AABBs +void SceneDemo::renderAABBs(const openglframework::Matrix4& worldToCameraMatrix) { + + // For each physics object of the scene + for (std::vector::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) { + + // For each proxy shape of the object + rp3d::ProxyShape* proxyShape = (*it)->getCollisionBody()->getProxyShapesList(); + while (proxyShape != nullptr) { + + // Get the broad-phase AABB corresponding to the proxy shape + rp3d::AABB aabb = mPhysicsWorld->getWorldAABB(proxyShape); + + openglframework::Vector3 aabbCenter(aabb.getCenter().x, aabb.getCenter().y, aabb.getCenter().z); + openglframework::Vector3 aabbMin(aabb.getMin().x, aabb.getMin().y, aabb.getMin().z); + openglframework::Vector3 aabbMax(aabb.getMax().x, aabb.getMax().y, aabb.getMax().z); + + // Render the AABB + AABB::render(aabbCenter, aabbMax - aabbMin, Color::green(), mColorShader, worldToCameraMatrix); + + proxyShape = proxyShape->getNext(); + } } } @@ -331,12 +432,16 @@ std::vector SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn const rp3d::ContactManifold* manifold = *it; // For each contact point of the manifold - for (uint i=0; igetNbContactPoints(); i++) { + rp3d::ContactPoint* contactPoint = manifold->getContactPoints(); + while (contactPoint != nullptr) { - rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i); - rp3d::Vector3 point = contactPoint->getWorldPointOnBody1(); - ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z)); + rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnShape1(); + rp3d::Vector3 normalWorld = contactPoint->getNormal(); + openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z); + ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red()); contactPoints.push_back(contact); + + contactPoint = contactPoint->getNext(); } } diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h index c64c8a9b..359c2374 100644 --- a/testbed/src/SceneDemo.h +++ b/testbed/src/SceneDemo.h @@ -30,6 +30,7 @@ #include "Scene.h" #include "VisualContactPoint.h" #include "reactphysics3d.h" +#include "PhysicsObject.h" // Constants const int SHADOWMAP_WIDTH = 2048; @@ -73,6 +74,9 @@ class SceneDemo : public Scene { /// Phong shader openglframework::Shader mPhongShader; + /// Constant color shader + openglframework::Shader mColorShader; + // TODO : Delete this openglframework::Shader mQuadShader; @@ -92,6 +96,10 @@ class SceneDemo : public Scene { std::string mMeshFolderPath; + std::vector mPhysicsObjects; + + rp3d::CollisionWorld* mPhysicsWorld; + // -------------------- Methods -------------------- // // Create the Shadow map FBO and texture @@ -110,14 +118,25 @@ class SceneDemo : public Scene { void renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); + + /// Render the AABBs + void renderAABBs(const openglframework::Matrix4& worldToCameraMatrix); + + /// Remove all contact points void removeAllContactPoints(); + /// Return a reference to the dynamics world + rp3d::DynamicsWorld* getDynamicsWorld(); + + /// Return a reference to the dynamics world + const rp3d::DynamicsWorld* getDynamicsWorld() const; + public: // -------------------- Methods -------------------- // /// Constructor - SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled = true); + SceneDemo(const std::string& name, EngineSettings& settings, float sceneRadius, bool isShadowMappingEnabled = true); /// Destructor virtual ~SceneDemo() override; @@ -125,12 +144,15 @@ class SceneDemo : public Scene { /// Update the scene virtual void update() override; + /// Update the physics world (take a simulation step) + /// Can be called several times per frame + virtual void updatePhysics() override; + /// Render the scene (possibly in multiple passes for shadow mapping) virtual void render() override; /// Render the scene in a single pass - virtual void renderSinglePass(openglframework::Shader& shader, - const openglframework::Matrix4& worldToCameraMatrix)=0 ; + virtual void renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix); /// Enabled/Disable the shadow mapping virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override; @@ -149,6 +171,16 @@ inline void SceneDemo::setIsShadowMappingEnabled(bool isShadowMappingEnabled) { } } +// Return a reference to the dynamics world +inline rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() { + return dynamic_cast(mPhysicsWorld); +} + +// Return a reference to the dynamics world +inline const rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() const { + return dynamic_cast(mPhysicsWorld); +} + #endif diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp index f57aa829..63230e0c 100644 --- a/testbed/src/TestbedApplication.cpp +++ b/testbed/src/TestbedApplication.cpp @@ -30,11 +30,13 @@ #include #include #include "cubes/CubesScene.h" +#include "collisiondetection/CollisionDetectionScene.h" #include "joints/JointsScene.h" #include "collisionshapes/CollisionShapesScene.h" #include "heightfield/HeightFieldScene.h" #include "raycast/RaycastScene.h" #include "concavemesh/ConcaveMeshScene.h" +#include "cubestack/CubeStackScene.h" using namespace openglframework; using namespace jointsscene; @@ -43,6 +45,8 @@ using namespace raycastscene; using namespace collisionshapesscene; using namespace trianglemeshscene; using namespace heightfieldscene; +using namespace collisiondetectionscene; +using namespace cubestackscene; // Initialization of static variables const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f; @@ -57,12 +61,15 @@ TestbedApplication::TestbedApplication(bool isFullscreen) mIsMultisamplingActive = true; mWidth = 1280; mHeight = 720; + mEngineSettings = EngineSettings::defaultSettings(); mSinglePhysicsStepEnabled = false; mSinglePhysicsStepDone = false; mWindowToFramebufferRatio = Vector2(1, 1); mIsShadowMappingEnabled = true; - mIsVSyncEnabled = false; + mIsVSyncEnabled = true; mIsContactPointsDisplayed = false; + mIsAABBsDisplayed = false; + mIsWireframeEnabled = false; init(); @@ -94,35 +101,42 @@ void TestbedApplication::init() { void TestbedApplication::createScenes() { // Cubes scene - CubesScene* cubeScene = new CubesScene("Cubes"); + CubesScene* cubeScene = new CubesScene("Cubes", mEngineSettings); mScenes.push_back(cubeScene); + // Cube Stack scene + CubeStackScene* cubeStackScene = new CubeStackScene("Cube Stack", mEngineSettings); + mScenes.push_back(cubeStackScene); + // Joints scene - JointsScene* jointsScene = new JointsScene("Joints"); + JointsScene* jointsScene = new JointsScene("Joints", mEngineSettings); mScenes.push_back(jointsScene); // Collision shapes scene - CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes"); + CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes", mEngineSettings); mScenes.push_back(collisionShapesScene); // Heightfield shape scene - HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield"); + HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield", mEngineSettings); mScenes.push_back(heightFieldScene); // Raycast scene - RaycastScene* raycastScene = new RaycastScene("Raycast"); + RaycastScene* raycastScene = new RaycastScene("Raycast", mEngineSettings); mScenes.push_back(raycastScene); - // Raycast scene - ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh"); + // Collision Detection scene + CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection", mEngineSettings); + mScenes.push_back(collisionDetectionScene); + + // Concave Mesh scene + ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh", mEngineSettings); mScenes.push_back(concaveMeshScene); assert(mScenes.size() > 0); - mCurrentScene = mScenes[0]; - // Get the engine settings from the scene - mEngineSettings = mCurrentScene->getEngineSettings(); - mEngineSettings.timeStep = DEFAULT_TIMESTEP; + const int firstSceneIndex = 0; + + switchScene(mScenes[firstSceneIndex]); } // Remove all the scenes @@ -145,9 +159,8 @@ void TestbedApplication::updateSinglePhysicsStep() { // Update the physics of the current scene void TestbedApplication::updatePhysics() { - // Set the engine settings + // Update the elapsed time mEngineSettings.elapsedTime = mTimer.getPhysicsTime(); - mCurrentScene->setEngineSettings(mEngineSettings); if (mTimer.isRunning()) { @@ -195,6 +208,12 @@ void TestbedApplication::update() { // Display/Hide contact points mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed); + // Display/Hide the AABBs + mCurrentScene->setIsAABBsDisplayed(mIsAABBsDisplayed); + + // Enable/Disable wireframe mode + mCurrentScene->setIsWireframeEnabled(mIsWireframeEnabled); + // Update the scene mCurrentScene->update(); } @@ -249,11 +268,6 @@ void TestbedApplication::switchScene(Scene* newScene) { mCurrentScene = newScene; - // Get the engine settings of the scene - float currentTimeStep = mEngineSettings.timeStep; - mEngineSettings = mCurrentScene->getEngineSettings(); - mEngineSettings.timeStep = currentTimeStep; - // Reset the scene mCurrentScene->reset(); diff --git a/testbed/src/TestbedApplication.h b/testbed/src/TestbedApplication.h index dcff0598..3a9fe071 100644 --- a/testbed/src/TestbedApplication.h +++ b/testbed/src/TestbedApplication.h @@ -38,9 +38,6 @@ using namespace nanogui; // Macro for OpenGL errors #define checkOpenGLErrors() checkOpenGLErrorsInternal(__FILE__,__LINE__) -// Constants -const float DEFAULT_TIMESTEP = 1.0f / 60.0f; - /// Class TestbedApplication class TestbedApplication : public Screen { @@ -109,6 +106,12 @@ class TestbedApplication : public Screen { /// True if contact points are displayed bool mIsContactPointsDisplayed; + /// True if the AABBs of physics objects are displayed + bool mIsAABBsDisplayed; + + /// True if the wireframe rendering is enabled + bool mIsWireframeEnabled; + /// True if vsync is enabled bool mIsVSyncEnabled; @@ -159,12 +162,6 @@ class TestbedApplication : public Screen { /// Set the variable to know if we need to take a single physics step void toggleTakeSinglePhysicsStep(); - /// Enable/Disable shadow mapping - void enableShadows(bool enable); - - /// Display/Hide contact points - void displayContactPoints(bool display); - public : // -------------------- Methods -------------------- // @@ -249,16 +246,6 @@ inline void TestbedApplication::toggleTakeSinglePhysicsStep() { } } -// Enable/Disable shadow mapping -inline void TestbedApplication::enableShadows(bool enable) { - mIsShadowMappingEnabled = enable; -} - -/// Display/Hide contact points -inline void TestbedApplication::displayContactPoints(bool display) { - mIsContactPointsDisplayed = display; -} - // Enable/Disable Vertical synchronization inline void TestbedApplication::enableVSync(bool enable) { mIsVSyncEnabled = enable;