diff --git a/CMakeLists.txt b/CMakeLists.txt index e85693d7..87a02105 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -147,7 +147,6 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/DynamicsWorld.h" "src/engine/DynamicsWorld.cpp" "src/engine/EventListener.h" - "src/engine/Impulse.h" "src/engine/Island.h" "src/engine/Island.cpp" "src/engine/Material.h" @@ -174,8 +173,10 @@ SET (REACTPHYSICS3D_SOURCES "src/mathematics/Vector3.h" "src/mathematics/Ray.h" "src/mathematics/Vector3.cpp" - "src/memory/MemoryAllocator.h" - "src/memory/MemoryAllocator.cpp" + "src/memory/PoolAllocator.h" + "src/memory/PoolAllocator.cpp" + "src/memory/SingleFrameAllocator.h" + "src/memory/SingleFrameAllocator.cpp" "src/memory/Stack.h" ) diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index daa4a71e..db22f606 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -70,7 +70,7 @@ 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.mMemoryAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, decimal(1)); @@ -116,7 +116,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -136,7 +136,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } elementToRemove->~ProxyShape(); - mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(elementToRemove, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -162,7 +162,7 @@ void CollisionBody::removeAllCollisionShapes() { } current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); // Get the next element in the list current = nextElement; @@ -181,7 +181,7 @@ void CollisionBody::resetContactManifoldsList() { // Delete the current element currentElement->~ContactManifoldListElement(); - mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + mWorld.mPoolAllocator.release(currentElement, sizeof(ContactManifoldListElement)); currentElement = nextElement; } diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 5fda6958..bd5c7818 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -34,7 +34,7 @@ #include "collision/shapes/AABB.h" #include "collision/shapes/CollisionShape.h" #include "collision/RaycastInfo.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "configuration.h" /// Namespace reactphysics3d diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index ff77b228..6e6a55e6 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -46,6 +46,9 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; + + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); } // Destructor @@ -90,11 +93,15 @@ void RigidBody::setType(BodyType type) { mMassInverse = decimal(0.0); mInertiaTensorLocal.setToZero(); mInertiaTensorLocalInverse.setToZero(); + mInertiaTensorInverseWorld.setToZero(); } else { // If it is a dynamic body mMassInverse = decimal(1.0) / mInitMass; mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); } // Awake the body @@ -125,6 +132,9 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { // Compute the inverse local inertia tensor mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); } // Set the local center of mass of the body (in local-space coordinates) @@ -166,7 +176,7 @@ void RigidBody::setMass(decimal mass) { } // Remove a joint from the joints list -void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) { +void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint) { assert(joint != nullptr); assert(mJointsList != nullptr); @@ -214,7 +224,7 @@ 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.mMemoryAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, mass); @@ -314,6 +324,9 @@ void RigidBody::setTransform(const Transform& transform) { // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); + // Update the broad-phase state of the body updateBroadPhaseState(); } @@ -326,6 +339,7 @@ void RigidBody::recomputeMassInformation() { mMassInverse = decimal(0.0); mInertiaTensorLocal.setToZero(); mInertiaTensorLocalInverse.setToZero(); + mInertiaTensorInverseWorld.setToZero(); mCenterOfMassLocal.setToZero(); // If it is STATIC or KINEMATIC body @@ -386,6 +400,9 @@ void RigidBody::recomputeMassInformation() { // Compute the local inverse inertia tensor mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); + // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); } diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 8fb36d0e..a38b53d5 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/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -83,6 +83,9 @@ class RigidBody : public CollisionBody { /// Inverse of the inertia tensor of the body Matrix3x3 mInertiaTensorLocalInverse; + /// Inverse of the world inertia tensor of the body + Matrix3x3 mInertiaTensorInverseWorld; + /// Inverse of the mass of the body decimal mMassInverse; @@ -104,7 +107,7 @@ class RigidBody : public CollisionBody { // -------------------- Methods -------------------- // /// Remove a joint from the joints list - void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); + void removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint); /// Update the transform of the body after a change of the center of mass void updateTransformWithCenterOfMass(); @@ -112,6 +115,9 @@ class RigidBody : public CollisionBody { /// Update the broad-phase state for this body (because it has moved for instance) virtual void updateBroadPhaseState() const override; + /// Update the world inverse inertia tensor of the body + void updateInertiaTensorInverseWorld(); + public : // -------------------- Methods -------------------- // @@ -291,12 +297,19 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { */ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { - // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE - // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES - // Compute and return the inertia tensor in world coordinates - return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse * - mTransform.getOrientation().getMatrix().getTranspose(); + return mInertiaTensorInverseWorld; +} + +// Update the world inverse inertia tensor of the body +/// The inertia tensor I_w in world coordinates is computed with the +/// local inverse inertia tensor I_b^-1 in body coordinates +/// by I_w = R * I_b^-1 * R^T +/// where R is the rotation matrix (and R^T its transpose) of the +/// current orientation quaternion of the body +inline void RigidBody::updateInertiaTensorInverseWorld() { + Matrix3x3 orientation = mTransform.getOrientation().getMatrix(); + mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose(); } // Return true if the gravity needs to be applied to this rigid body diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 7d9f6fb2..c3caa97a 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -41,7 +41,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) +CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator), mWorld(world), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { @@ -189,7 +189,7 @@ void CollisionDetection::computeNarrowPhase() { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -294,7 +294,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -370,8 +370,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro shape2->getCollisionShape()->getType()); // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator); + OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator); assert(newPair != nullptr); #ifndef NDEBUG @@ -400,7 +400,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); } else { @@ -434,7 +434,7 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { // Create a new contact - ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint))) + ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactInfo); // Add the contact to the contact manifold set of the corresponding overlapping pair @@ -477,7 +477,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); ContactManifoldListElement* listElement1 = new (allocatedMemory1) ContactManifoldListElement(contactManifold, body1->mContactManifoldsList); @@ -485,7 +485,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); ContactManifoldListElement* listElement2 = new (allocatedMemory2) ContactManifoldListElement(contactManifold, body2->mContactManifoldsList); @@ -520,8 +520,8 @@ EventListener* CollisionDetection::getWorldEventListener() { } /// Return a reference to the world memory allocator -MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() { - return mWorld->mMemoryAllocator; +PoolAllocator& CollisionDetection::getWorldMemoryAllocator() { + return mWorld->mPoolAllocator; } // Called by a narrow-phase collision algorithm when a new contact has been found diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 75f278ba..0fe486c4 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -32,7 +32,7 @@ #include "engine/OverlappingPair.h" #include "engine/EventListener.h" #include "narrowphase/DefaultCollisionDispatch.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "constraint/ContactPoint.h" #include #include @@ -93,7 +93,7 @@ class CollisionDetection : public NarrowPhaseCallback { NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Pointer to the physics world CollisionWorld* mWorld; @@ -143,7 +143,7 @@ class CollisionDetection : public NarrowPhaseCallback { // -------------------- Methods -------------------- // /// Constructor - CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator); + CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator); /// Destructor ~CollisionDetection() = default; @@ -220,7 +220,7 @@ class CollisionDetection : public NarrowPhaseCallback { EventListener* getWorldEventListener(); /// Return a reference to the world memory allocator - MemoryAllocator& getWorldMemoryAllocator(); + PoolAllocator& getWorldMemoryAllocator(); /// Called by a narrow-phase collision algorithm when a new contact has been found virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override; diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index cfa905ff..ff15bce6 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short normalDirectionId) + PoolAllocator& memoryAllocator, short normalDirectionId) : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index eb7716df..c36c1476 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -31,7 +31,7 @@ #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -102,7 +102,7 @@ class ContactManifold { short int mNormalDirectionId; /// Number of contacts in the cache - uint mNbContactPoints; + int8 mNbContactPoints; /// First friction vector of the contact manifold Vector3 mFrictionVector1; @@ -126,7 +126,7 @@ class ContactManifold { bool mIsAlreadyInIsland; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; // -------------------- Methods -------------------- // @@ -151,7 +151,7 @@ class ContactManifold { /// Constructor ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short int normalDirectionId); + PoolAllocator& memoryAllocator, short int normalDirectionId); /// Destructor ~ContactManifold(); @@ -187,7 +187,7 @@ class ContactManifold { void clear(); /// Return the number of contact points in the manifold - uint getNbContactPoints() const; + int8 getNbContactPoints() const; /// Return the first friction vector at the center of the contact manifold const Vector3& getFrictionVector1() const; @@ -264,7 +264,7 @@ inline short int ContactManifold::getNormalDirectionId() const { } // Return the number of contact points in the manifold -inline uint ContactManifold::getNbContactPoints() const { +inline int8 ContactManifold::getNbContactPoints() const { return mNbContactPoints; } diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 54e3614e..26f3a66f 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds) + PoolAllocator& memoryAllocator, int nbMaxManifolds) : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), mShape2(shape2), mMemoryAllocator(memoryAllocator) { assert(nbMaxManifolds >= 1); diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index b167f506..21944e36 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -61,7 +61,7 @@ class ContactManifoldSet { ProxyShape* mShape2; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Contact manifolds of the set ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; @@ -88,7 +88,7 @@ class ContactManifoldSet { /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds); + PoolAllocator& memoryAllocator, int nbMaxManifolds); /// Destructor ~ContactManifoldSet(); diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 8fa24f87..6e027721 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -51,7 +51,7 @@ class CollisionDispatch { /// Initialize the collision dispatch configuration virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 109b5de3..f6812d74 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; /// Initialize the collision dispatch configuration void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { // Initialize the collision algorithms mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator); diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 9bf15b11..5dd07bf2 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -63,7 +63,7 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Initialize the collision dispatch configuration virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) override; + PoolAllocator* memoryAllocator) override; /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index f585ac15..05a78eea 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -34,7 +34,7 @@ #include "collision/narrowphase/NarrowPhaseAlgorithm.h" #include "mathematics/mathematics.h" #include "TriangleEPA.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include /// ReactPhysics3D namespace @@ -88,7 +88,7 @@ class EPAAlgorithm { // -------------------- Attributes -------------------- // /// Reference to the memory allocator - MemoryAllocator* mMemoryAllocator; + PoolAllocator* mMemoryAllocator; /// Triangle comparison operator TriangleComparison mTriangleComparison; @@ -120,7 +120,7 @@ class EPAAlgorithm { EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete; /// Initalize the algorithm - void init(MemoryAllocator* memoryAllocator); + void init(PoolAllocator* memoryAllocator); /// Compute the penetration depth with EPA algorithm. bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, @@ -151,7 +151,7 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** } // Initalize the algorithm -inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) { +inline void EPAAlgorithm::init(PoolAllocator* memoryAllocator) { mMemoryAllocator = memoryAllocator; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index e81ee9b3..b6cce5c2 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -94,7 +94,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { /// Initalize the algorithm virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) override; + PoolAllocator* memoryAllocator) override; /// Compute a contact info if the two bounding volumes collide. virtual void testCollision(const CollisionShapeInfo& shape1Info, @@ -110,7 +110,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { // Initalize the algorithm inline void GJKAlgorithm::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator); mAlgoEPA.init(memoryAllocator); } diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp index 382ccc9b..590182ac 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp @@ -563,7 +563,6 @@ bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Ve if (squareDist < closestSquareDistance) { // Use it as the current closest point - closestSquareDistance = squareDist; baryCoordsAB.setAllValues(0.0, triangleBaryCoords[0]); baryCoordsCD.setAllValues(triangleBaryCoords[2], triangleBaryCoords[1]); bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 1, 3, 2); diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp index 0fbbe5fe..782f8d6f 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp @@ -36,7 +36,7 @@ NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() } // Initalize the algorithm -void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { +void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator) { mCollisionDetection = collisionDetection; mMemoryAllocator = memoryAllocator; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 30b216f0..0759d138 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -29,7 +29,7 @@ // Libraries #include "body/Body.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "engine/OverlappingPair.h" #include "collision/CollisionShapeInfo.h" @@ -71,7 +71,7 @@ class NarrowPhaseAlgorithm { CollisionDetection* mCollisionDetection; /// Pointer to the memory allocator - MemoryAllocator* mMemoryAllocator; + PoolAllocator* mMemoryAllocator; /// Overlapping pair of the bodies currently tested for collision OverlappingPair* mCurrentOverlappingPair; @@ -93,7 +93,7 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator); + virtual void init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator); /// Set the current overlapping pair of bodies void setCurrentOverlappingPair(OverlappingPair* overlappingPair); diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 1e2e3f14..b33ed4f1 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -34,7 +34,7 @@ #include "mathematics/Ray.h" #include "AABB.h" #include "collision/RaycastInfo.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/configuration.h b/src/configuration.h index 4364c548..04cb0956 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -30,6 +30,7 @@ #include #include #include +#include #include "decimal.h" // Windows platform @@ -51,10 +52,9 @@ using luint = long unsigned int; using bodyindex = luint; using bodyindexpair = std::pair; -using int16 = signed short; -using int32 = signed int; -using uint16 = unsigned short; -using uint32 = unsigned int; +using int8 = int8_t; +using int16 = int16_t; +using int32 = int32_t; // ------------------- Enumerations ------------------- // @@ -144,6 +144,9 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; /// least one concave collision shape. constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; +/// Size (in bytes) of the single frame allocator +constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb + } #endif diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 3be8476a..c0d567ea 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -45,9 +45,6 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) contactInfo.localPoint2), mIsRestingContact(false) { - mFrictionVectors[0] = Vector3(0, 0, 0); - mFrictionVectors[1] = Vector3(0, 0, 0); - - assert(mPenetrationDepth > 0.0); + assert(mPenetrationDepth > decimal(0.0)); } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index d2e8f189..18be17b0 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -128,21 +128,9 @@ class ContactPoint { /// True if the contact is a resting contact (exists for more than one time step) bool mIsRestingContact; - /// Two orthogonal vectors that span the tangential friction plane - Vector3 mFrictionVectors[2]; - /// Cached penetration impulse decimal mPenetrationImpulse; - /// Cached first friction impulse - decimal mFrictionImpulse1; - - /// Cached second friction impulse - decimal mFrictionImpulse2; - - /// Cached rolling resistance impulse - Vector3 mRollingResistanceImpulse; - public : // -------------------- Methods -------------------- // @@ -186,27 +174,9 @@ class ContactPoint { /// Return the cached penetration impulse decimal getPenetrationImpulse() const; - /// Return the cached first friction impulse - decimal getFrictionImpulse1() const; - - /// Return the cached second friction impulse - decimal getFrictionImpulse2() const; - - /// Return the cached rolling resistance impulse - Vector3 getRollingResistanceImpulse() const; - /// Set the cached penetration impulse void setPenetrationImpulse(decimal impulse); - /// Set the first cached friction impulse - void setFrictionImpulse1(decimal impulse); - - /// Set the second cached friction impulse - void setFrictionImpulse2(decimal impulse); - - /// Set the cached rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& impulse); - /// Set the contact world point on body 1 void setWorldPointOnBody1(const Vector3& worldPoint); @@ -219,18 +189,6 @@ class ContactPoint { /// Set the mIsRestingContact variable void setIsRestingContact(bool isRestingContact); - /// Get the first friction vector - Vector3 getFrictionVector1() const; - - /// Set the first friction vector - void setFrictionVector1(const Vector3& frictionVector1); - - /// Get the second friction vector - Vector3 getFrictionVector2() const; - - /// Set the second friction vector - void setFrictionVector2(const Vector3& frictionVector2); - /// Return the penetration depth decimal getPenetrationDepth() const; @@ -283,41 +241,11 @@ inline decimal ContactPoint::getPenetrationImpulse() const { return mPenetrationImpulse; } -// Return the cached first friction impulse -inline decimal ContactPoint::getFrictionImpulse1() const { - return mFrictionImpulse1; -} - -// Return the cached second friction impulse -inline decimal ContactPoint::getFrictionImpulse2() const { - return mFrictionImpulse2; -} - -// Return the cached rolling resistance impulse -inline Vector3 ContactPoint::getRollingResistanceImpulse() const { - return mRollingResistanceImpulse; -} - // Set the cached penetration impulse inline void ContactPoint::setPenetrationImpulse(decimal impulse) { mPenetrationImpulse = impulse; } -// Set the first cached friction impulse -inline void ContactPoint::setFrictionImpulse1(decimal impulse) { - mFrictionImpulse1 = impulse; -} - -// Set the second cached friction impulse -inline void ContactPoint::setFrictionImpulse2(decimal impulse) { - mFrictionImpulse2 = impulse; -} - -// Set the cached rolling resistance impulse -inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) { - mRollingResistanceImpulse = impulse; -} - // Set the contact world point on body 1 inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { mWorldPointOnBody1 = worldPoint; @@ -338,26 +266,6 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } -// Get the first friction vector -inline Vector3 ContactPoint::getFrictionVector1() const { - return mFrictionVectors[0]; -} - -// Set the first friction vector -inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { - mFrictionVectors[0] = frictionVector1; -} - -// Get the second friction vector -inline Vector3 ContactPoint::getFrictionVector2() const { - return mFrictionVectors[1]; -} - -// Set the second friction vector -inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { - mFrictionVectors[1] = frictionVector2; -} - // Return the penetration depth of the contact inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 68e37625..b1b004ec 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -33,7 +33,7 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0), + : mCollisionDetection(this, mPoolAllocator), mCurrentBodyID(0), mEventListener(nullptr) { } @@ -66,7 +66,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the collision body - CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody))) + CollisionBody* collisionBody = new (mPoolAllocator.allocate(sizeof(CollisionBody))) CollisionBody(transform, *this, bodyID); assert(collisionBody != nullptr); @@ -97,7 +97,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { mBodies.erase(collisionBody); // Free the object from the memory allocator - mMemoryAllocator.release(collisionBody, sizeof(CollisionBody)); + mPoolAllocator.release(collisionBody, sizeof(CollisionBody)); } // Return the next available body ID diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 77189e89..d04acb4e 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/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "EventListener.h" /// Namespace reactphysics3d @@ -72,8 +72,8 @@ class CollisionWorld { /// List of free ID for rigid bodies std::vector mFreeBodiesIDs; - /// Memory allocator - MemoryAllocator mMemoryAllocator; + /// Pool Memory allocator + PoolAllocator mPoolAllocator; /// Pointer to an event listener object EventListener* mEventListener; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 331e8045..5d61243a 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -36,20 +36,67 @@ using namespace std; // Constants initialization const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); -const decimal ContactSolver::SLOP= decimal(0.01); +const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) +ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, + SingleFrameAllocator& allocator) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), - mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), + mContactConstraints(nullptr), mSingleFrameAllocator(allocator), + mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mIsSplitImpulseActive(true), - mIsSolveFrictionAtContactManifoldCenterActive(true) { + mIsSplitImpulseActive(true) { } +// Initialize the contact constraints +void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { + + PROFILE("ContactSolver::init()"); + + mTimeStep = timeStep; + + // TODO : Try not to count manifolds and contact points here + uint nbContactManifolds = 0; + uint nbContactPoints = 0; + for (uint i = 0; i < nbIslands; i++) { + uint nbManifoldsInIsland = islands[i]->getNbContactManifolds(); + nbContactManifolds += nbManifoldsInIsland; + + for (uint j=0; j < nbManifoldsInIsland; j++) { + nbContactPoints += islands[i]->getContactManifolds()[j]->getNbContactPoints(); + } + } + + mNbContactManifolds = 0; + mNbContactPoints = 0; + + mContactConstraints = nullptr; + mContactPoints = nullptr; + + if (nbContactManifolds == 0 || nbContactPoints == 0) return; + + // TODO : Count exactly the number of constraints to allocate here + mContactPoints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints)); + assert(mContactPoints != nullptr); + + mContactConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds)); + assert(mContactConstraints != nullptr); + + // For each island of the world + for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { + + if (islands[islandIndex]->getNbContactManifolds() > 0) { + initializeForIsland(islands[islandIndex]); + } + } + + // Warmstarting + warmStart(); +} + // Initialize the constraint solver for a given island -void ContactSolver::initializeForIsland(decimal dt, Island* island) { +void ContactSolver::initializeForIsland(Island* island) { PROFILE("ContactSolver::initializeForIsland()"); @@ -59,22 +106,12 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); - // Set the current time step - mTimeStep = dt; - - mNbContactManifolds = island->getNbContactManifolds(); - - mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; - assert(mContactConstraints != nullptr); - // For each contact manifold of the island - ContactManifold** contactManifolds = island->getContactManifold(); - for (uint i=0; igetContactManifolds(); + for (uint i=0; igetNbContactManifolds(); i++) { ContactManifold* externalManifold = contactManifolds[i]; - ContactManifoldSolver& internalManifold = mContactConstraints[i]; - assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact @@ -89,31 +126,30 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { // Initialize the internal contact manifold structure using the external // contact manifold - internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; - internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalManifold.massInverseBody1 = body1->mMassInverse; - internalManifold.massInverseBody2 = body2->mMassInverse; - internalManifold.nbContacts = externalManifold->getNbContactPoints(); - internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); - internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); - internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); - internalManifold.externalContactManifold = externalManifold; - internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; + new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); + mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; + mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse; + mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints(); + mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); + mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold; + mContactConstraints[mNbContactManifolds].normal.setToZero(); + mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero(); + mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero(); - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 = Vector3::zero(); - internalManifold.frictionPointBody2 = Vector3::zero(); - } + // Get the velocities of the bodies + const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; + const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; + const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; + const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; // For each contact point of the contact manifold for (uint c=0; cgetNbContactPoints(); c++) { - ContactPointSolver& contactPoint = internalManifold.contacts[c]; - // Get a contact point ContactPoint* externalContact = externalManifold->getContactPoint(c); @@ -121,209 +157,105 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p2 = externalContact->getWorldPointOnBody2(); - contactPoint.externalContact = externalContact; - contactPoint.normal = externalContact->getNormal(); - contactPoint.r1 = p1 - x1; - contactPoint.r2 = p2 - x2; - contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); - contactPoint.isRestingContact = externalContact->getIsRestingContact(); + 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].penetrationDepth = externalContact->getPenetrationDepth(); + mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); - contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); - contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); - contactPoint.penetrationImpulse = 0.0; - contactPoint.friction1Impulse = 0.0; - contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); + mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse(); + mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0; - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 += p1; - internalManifold.frictionPointBody2 += p2; - } - } - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - - internalManifold.frictionPointBody1 /=static_cast(internalManifold.nbContacts); - internalManifold.frictionPointBody2 /=static_cast(internalManifold.nbContacts); - internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; - internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; - internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); - internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); - - // If warm starting is active - if (mIsWarmStartingActive) { - - // Initialize the accumulated impulses with the previous step accumulated impulses - internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); - internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); - internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); - } - else { - - // Initialize the accumulated impulses to zero - internalManifold.friction1Impulse = 0.0; - internalManifold.friction2Impulse = 0.0; - internalManifold.frictionTwistImpulse = 0.0; - internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0); - } - } - } - - // Fill-in all the matrices needed to solve the LCP problem - initializeContactConstraints(); -} - -// Initialize the contact constraints before solving the system -void ContactSolver::initializeContactConstraints() { - - // For each contact constraint - for (uint c=0; c 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / - massPenetration : - decimal(0.0); - - // If we do not solve the friction constraints at the center of the contact manifold - if (!mIsSolveFrictionAtContactManifoldCenterActive) { - - // Compute the friction vectors - computeFrictionVectors(deltaV, contactPoint); - - contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); - contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); - contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); - contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); - - // Compute the inverse mass matrix K for the friction - // constraints at each contact point - decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( - contactPoint.frictionVector1) + - ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( - contactPoint.frictionVector1); - decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( - contactPoint.frictionVector2) + - ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( - contactPoint.frictionVector2); - friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / - friction1Mass : - decimal(0.0); - friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / - friction2Mass : - decimal(0.0); - } + decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactPoints[mNbContactPoints].i1TimesR1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) + + ((mContactPoints[mNbContactPoints].i2TimesR2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal); + mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference // 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 - contactPoint.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contactPoint.normal); + mContactPoints[mNbContactPoints].restitutionBias = 0.0; + decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; + mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN; } - // If the warm starting of the contact solver is active - if (mIsWarmStartingActive) { + mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; - // Get the cached accumulated impulses from the previous step - contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); - contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); - contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); - contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); - } - - // Initialize the split impulses to zero - contactPoint.penetrationSplitImpulse = 0.0; - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - manifold.normal += contactPoint.normal; - } + mNbContactPoints++; } + 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].oldFrictionVector1 = externalManifold->getFrictionVector1(); + mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2(); + + // Initialize the accumulated impulses with the previous step accumulated impulses + mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1(); + mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2(); + mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); + // Compute the inverse K matrix for the rolling resistance constraint - manifold.inverseRollingResistance.setToZero(); - if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { - manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; - manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); + bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; + bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; + mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); + if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2; + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); } - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + mContactConstraints[mNbContactManifolds].normal.normalize(); - manifold.normal.normalize(); + Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - + v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); - Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - - v1 - w1.cross(manifold.r1Friction); + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]); - // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, manifold); + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + mContactConstraints[mNbContactManifolds].r1CrossT1 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1); + mContactConstraints[mNbContactManifolds].r1CrossT2 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2); + mContactConstraints[mNbContactManifolds].r2CrossT1 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1); + mContactConstraints[mNbContactManifolds].r2CrossT2 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2); + decimal friction1Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT1).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector1) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT1).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector1); + decimal friction2Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT2).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector2) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT2).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector2); + decimal frictionTwistMass = mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * + mContactConstraints[mNbContactManifolds].normal) + + mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * + mContactConstraints[mNbContactManifolds].normal); + mContactConstraints[mNbContactManifolds].inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0); + mContactConstraints[mNbContactManifolds].inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); + mContactConstraints[mNbContactManifolds].inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); - // Compute the inverse mass matrix K for the friction constraints at the center of - // the contact manifold - manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); - manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); - manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); - manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); - decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( - manifold.frictionVector1) + - ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( - manifold.frictionVector1); - decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + - ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( - manifold.frictionVector2) + - ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( - manifold.frictionVector2); - decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * - manifold.normal) + - manifold.normal.dot(manifold.inverseInertiaTensorBody2 * - manifold.normal); - friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass - : decimal(0.0); - friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass - : decimal(0.0); - frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / - frictionTwistMass : - decimal(0.0); - } + mNbContactManifolds++; } } @@ -333,169 +265,116 @@ void ContactSolver::initializeContactConstraints() { /// the solution of the linear system void ContactSolver::warmStart() { - // Check that warm starting is active - if (!mIsWarmStartingActive) return; + PROFILE("ContactSolver::warmStart()"); + + uint contactPointIndex = 0; // For each constraint for (uint c=0; c 0) { - - // Compute the impulse P = J^T * lambda - const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse, - Vector3::zero(), contactPoint.rollingResistanceImpulse); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); - } - } + // 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; } else { // If it is a new contact point // Initialize the accumulated impulses to zero - contactPoint.penetrationImpulse = 0.0; - contactPoint.friction1Impulse = 0.0; - contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); + mContactPoints[contactPointIndex].penetrationImpulse = 0.0; } + + contactPointIndex++; } // If we solve the friction constraints at the center of the contact manifold and there is // at least one resting contact point in the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { + if (atLeastOneRestingContactPoint) { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * - contactManifold.oldFrictionVector1 + - contactManifold.friction2Impulse * - contactManifold.oldFrictionVector2; - contactManifold.friction1Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector1); - contactManifold.friction2Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector2); + Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 + + mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2; + 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 linearImpulseBody1 = -contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * - contactManifold.friction1Impulse; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * - contactManifold.friction1Impulse; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * + mContactConstraints[c].friction1Impulse; + Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * + mContactConstraints[c].friction1Impulse; + Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * + mContactConstraints[c].friction1Impulse; - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifold ----- // // Compute the impulse P = J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody1 = -contactManifold.r1CrossT2 * - contactManifold.friction2Impulse; - linearImpulseBody2 = contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody2 = contactManifold.r2CrossT2 * - contactManifold.friction2Impulse; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse; + linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse; + angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse; - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + 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; + 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 - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; + angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse; - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 2 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Rolling resistance at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; - angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); + angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse; - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; } else { // If it is a new contact manifold // Initialize the accumulated impulses to zero - contactManifold.friction1Impulse = 0.0; - contactManifold.friction2Impulse = 0.0; - contactManifold.frictionTwistImpulse = 0.0; - contactManifold.rollingResistanceImpulse = Vector3::zero(); + mContactConstraints[c].friction1Impulse = 0.0; + mContactConstraints[c].friction2Impulse = 0.0; + mContactConstraints[c].frictionTwistImpulse = 0.0; + mContactConstraints[c].rollingResistanceImpulse.setToZero(); } } } @@ -507,268 +386,193 @@ void ContactSolver::solve() { decimal deltaLambda; decimal lambdaTemp; + uint contactPointIndex = 0; // For each contact manifold for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contactPoint.penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias; // Compute the Lagrange multiplier lambda if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + contactPoint.restitutionBias) * - contactPoint.inversePenetrationMass; + deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) * + mContactPoints[contactPointIndex].inversePenetrationMass; } else { - deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass; } - lambdaTemp = contactPoint.penetrationImpulse; - contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + + lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse; + mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse + deltaLambda, decimal(0.0)); - deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; + deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, - contactPoint); + Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda; - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, contactManifold); + // 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; - sumPenetrationImpulse += contactPoint.penetrationImpulse; + // 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; + + sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; // If the split impulse position correction is active if (mIsSplitImpulseActive) { // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - - v1Split - w1Split.cross(contactPoint.r1); - decimal JvSplit = deltaVSplit.dot(contactPoint.normal); + const Vector3& v1Split = mSplitLinearVelocities[mContactConstraints[c].indexBody1]; + 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); decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - contactPoint.inversePenetrationMass; - decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; - contactPoint.penetrationSplitImpulse = std::max( - contactPoint.penetrationSplitImpulse + + mContactPoints[contactPointIndex].inversePenetrationMass; + decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse; + mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max( + mContactPoints[contactPointIndex].penetrationSplitImpulse + deltaLambdaSplit, decimal(0.0)); - deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit; - // Compute the impulse P=J^T * lambda - const Impulse splitImpulsePenetration = computePenetrationImpulse( - deltaLambdaSplit, contactPoint); + Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit; - applySplitImpulse(splitImpulsePenetration, contactManifold); + // 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; + + // 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; } - // If we do not solve the friction constraints at the center of the contact manifold - if (!mIsSolveFrictionAtContactManifoldCenterActive) { - - // --------- Friction 1 --------- // - - // Compute J*v - deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); - Jv = deltaV.dot(contactPoint.frictionVector1); - - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv; - deltaLambda *= contactPoint.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; - lambdaTemp = contactPoint.friction1Impulse; - contactPoint.friction1Impulse = std::max(-frictionLimit, - std::min(contactPoint.friction1Impulse - + deltaLambda, frictionLimit)); - deltaLambda = contactPoint.friction1Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, - contactPoint); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); - - // --------- Friction 2 --------- // - - // Compute J*v - deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); - Jv = deltaV.dot(contactPoint.frictionVector2); - - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv; - deltaLambda *= contactPoint.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * - contactPoint.penetrationImpulse; - lambdaTemp = contactPoint.friction2Impulse; - contactPoint.friction2Impulse = std::max(-frictionLimit, - std::min(contactPoint.friction2Impulse - + deltaLambda, frictionLimit)); - deltaLambda = contactPoint.friction2Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, - contactPoint); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); - - // --------- Rolling resistance constraint --------- // - - if (contactManifold.rollingResistanceFactor > 0) { - - // Compute J*v - const Vector3 JvRolling = w2 - w1; - - // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); - decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; - Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; - contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; - - // Compute the impulse P=J^T * lambda - const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling, - Vector3::zero(), deltaLambdaRolling); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRolling, contactManifold); - } - } + contactPointIndex++; } - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + // ------ First friction constraint at the center of the contact manifol ------ // - // ------ 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); + + // Compute the Lagrange multiplier lambda + decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass; + decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].friction1Impulse; + mContactConstraints[c].friction1Impulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].friction1Impulse + + deltaLambda, frictionLimit)); + 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; + + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + 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; + 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); + + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass; + frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].friction2Impulse; + mContactConstraints[c].friction2Impulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].friction2Impulse + + deltaLambda, frictionLimit)); + 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; + + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; + 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; + 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); + + deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass); + frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].frictionTwistImpulse; + mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].frictionTwistImpulse + + deltaLambda, frictionLimit)); + deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda; + + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + + // --------- Rolling resistance constraint at the center of the contact manifold --------- // + + if (mContactConstraints[c].rollingResistanceFactor > 0) { // Compute J*v - Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - decimal Jv = deltaV.dot(contactManifold.frictionVector1); + const Vector3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda - decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction1Impulse; - contactManifold.friction1Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction1Impulse + - deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling); + decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse; + Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse; + mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse + + deltaLambdaRolling, rollingLimit); + deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling; - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling; - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); - - // ------ Second friction constraint at the center of the contact manifol ----- // - - // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - Jv = deltaV.dot(contactManifold.frictionVector2); - - // Compute the Lagrange multiplier lambda - deltaLambda = -Jv * contactManifold.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction2Impulse; - contactManifold.friction2Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction2Impulse + - deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction2Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); - - // ------ Twist friction constraint at the center of the contact manifol ------ // - - // Compute J*v - deltaV = w2 - w1; - Jv = deltaV.dot(contactManifold.normal); - - deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.frictionTwistImpulse; - contactManifold.frictionTwistImpulse = std::max(-frictionLimit, - std::min(contactManifold.frictionTwistImpulse - + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -contactManifold.normal * deltaLambda; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; - angularImpulseBody2 = contactManifold.normal * deltaLambda; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); - - // --------- Rolling resistance constraint at the center of the contact manifold --------- // - - if (contactManifold.rollingResistanceFactor > 0) { - - // Compute J*v - const Vector3 JvRolling = w2 - w1; - - // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); - decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; - Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; - contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; - - // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -deltaLambdaRolling; - angularImpulseBody2 = deltaLambdaRolling; - const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRolling, contactManifold); - } + // Update the velocities of the body 2 by applying the impulse P + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling; } } } @@ -777,103 +581,37 @@ void ContactSolver::solve() { // warm start the solver at the next iteration void ContactSolver::storeImpulses() { + PROFILE("ContactSolver::storeImpulses()"); + + uint contactPointIndex = 0; + // For each contact manifold for (uint c=0; csetPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse); - ContactPointSolver& contactPoint = manifold.contacts[i]; - - contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse); - contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); - contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); - contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); - - contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); + contactPointIndex++; } - manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); - manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); - manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); - manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); - manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); - manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); + mContactConstraints[c].externalContactManifold->setFrictionImpulse1(mContactConstraints[c].friction1Impulse); + mContactConstraints[c].externalContactManifold->setFrictionImpulse2(mContactConstraints[c].friction2Impulse); + mContactConstraints[c].externalContactManifold->setFrictionTwistImpulse(mContactConstraints[c].frictionTwistImpulse); + mContactConstraints[c].externalContactManifold->setRollingResistanceImpulse(mContactConstraints[c].rollingResistanceImpulse); + mContactConstraints[c].externalContactManifold->setFrictionVector1(mContactConstraints[c].frictionVector1); + mContactConstraints[c].externalContactManifold->setFrictionVector2(mContactConstraints[c].frictionVector2); } } -// Apply an impulse to the two bodies of a constraint -void ContactSolver::applyImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { - - // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - - // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; -} - -// Apply an impulse to the two bodies of a constraint -void ContactSolver::applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { - - // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - - // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; -} - -// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointSolver& contactPoint) const { - - assert(contactPoint.normal.length() > 0.0); - - // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; - - // If the velocty difference in the tangential plane is not zero - decimal lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > MACHINE_EPSILON) { - - // Compute the first friction vector in the direction of the tangent - // velocity difference - contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { - - // Get any orthogonal vector to the normal as the first friction vector - contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); - } - - // The second friction vector is computed by the cross product of the firs - // friction vector and the contact normal - contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); -} - // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { - assert(contact.normal.length() > 0.0); + PROFILE("ContactSolver::computeFrictionVectors()"); + + assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; @@ -897,12 +635,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // friction vector and the contact normal contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); } - -// Clean up the constraint solver -void ContactSolver::cleanup() { - - if (mContactConstraints != nullptr) { - delete[] mContactConstraints; - mContactConstraints = nullptr; - } -} diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index ae2005f4..40e0721d 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -32,7 +32,6 @@ #include "constraint/Joint.h" #include "collision/ContactManifold.h" #include "Island.h" -#include "Impulse.h" #include #include @@ -120,80 +119,41 @@ class ContactSolver { */ struct ContactPointSolver { - /// Accumulated normal impulse - decimal penetrationImpulse; - - /// Accumulated impulse in the 1st friction direction - decimal friction1Impulse; - - /// Accumulated impulse in the 2nd friction direction - decimal friction2Impulse; - - /// Accumulated split impulse for penetration correction - decimal penetrationSplitImpulse; - - /// Accumulated rolling resistance impulse - Vector3 rollingResistanceImpulse; + /// Pointer to the external contact + ContactPoint* externalContact; /// Normal vector of the contact Vector3 normal; - /// First friction vector in the tangent plane - Vector3 frictionVector1; - - /// Second friction vector in the tangent plane - Vector3 frictionVector2; - - /// Old first friction vector in the tangent plane - Vector3 oldFrictionVector1; - - /// Old second friction vector in the tangent plane - Vector3 oldFrictionVector2; - /// Vector from the body 1 center to the contact point Vector3 r1; /// Vector from the body 2 center to the contact point Vector3 r2; - /// Cross product of r1 with 1st friction vector - Vector3 r1CrossT1; - - /// Cross product of r1 with 2nd friction vector - Vector3 r1CrossT2; - - /// Cross product of r2 with 1st friction vector - Vector3 r2CrossT1; - - /// Cross product of r2 with 2nd friction vector - Vector3 r2CrossT2; - - /// Cross product of r1 with the contact normal - Vector3 r1CrossN; - - /// Cross product of r2 with the contact normal - Vector3 r2CrossN; - /// Penetration depth decimal penetrationDepth; /// Velocity restitution bias decimal restitutionBias; + /// Accumulated normal impulse + decimal penetrationImpulse; + + /// Accumulated split impulse for penetration correction + decimal penetrationSplitImpulse; + /// Inverse of the matrix K for the penenetration decimal inversePenetrationMass; - /// Inverse of the matrix K for the 1st friction - decimal inverseFriction1Mass; + /// Cross product of r1 with the contact normal + Vector3 i1TimesR1CrossN; - /// Inverse of the matrix K for the 2nd friction - decimal inverseFriction2Mass; + /// Cross product of r2 with the contact normal + Vector3 i2TimesR2CrossN; /// True if the contact was existing last time step bool isRestingContact; - - /// Pointer to the external contact - ContactPoint* externalContact; }; // Structure ContactManifoldSolver @@ -203,11 +163,14 @@ class ContactSolver { */ struct ContactManifoldSolver { + /// Pointer to the external contact manifold + ContactManifold* externalContactManifold; + /// Index of body 1 in the constraint solver - uint indexBody1; + int32 indexBody1; /// Index of body 2 in the constraint solver - uint indexBody2; + int32 indexBody2; /// Inverse of the mass of body 1 decimal massInverseBody1; @@ -221,30 +184,12 @@ class ContactSolver { /// Inverse inertia tensor of body 2 Matrix3x3 inverseInertiaTensorBody2; - /// Contact point constraints - ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - - /// Number of contact points - uint nbContacts; - - /// True if the body 1 is of type dynamic - bool isBody1DynamicType; - - /// True if the body 2 is of type dynamic - bool isBody2DynamicType; - - /// Mix of the restitution factor for two bodies - decimal restitutionFactor; - /// Mix friction coefficient for the two bodies decimal frictionCoefficient; /// Rolling resistance factor between the two bodies decimal rollingResistanceFactor; - /// Pointer to the external contact manifold - ContactManifold* externalContactManifold; - // - Variables used when friction constraints are apply at the center of the manifold-// /// Average normal vector of the contact manifold @@ -309,6 +254,9 @@ class ContactSolver { /// Rolling resistance impulse Vector3 rollingResistanceImpulse; + + /// Number of contact points + int8 nbContacts; }; // -------------------- Constants --------------------- // @@ -336,9 +284,18 @@ class ContactSolver { /// Contact constraints ContactManifoldSolver* mContactConstraints; + /// Contact points + ContactPointSolver* mContactPoints; + + /// Number of contact point constraints + uint mNbContactPoints; + /// Number of contact constraints uint mNbContactManifolds; + /// Single frame memory allocator + SingleFrameAllocator& mSingleFrameAllocator; + /// Array of linear velocities Vector3* mLinearVelocities; @@ -348,28 +305,11 @@ class ContactSolver { /// Reference to the map of rigid body to their index in the constrained velocities array const std::map& mMapBodyToConstrainedVelocityIndex; - /// True if the warm starting of the solver is active - bool mIsWarmStartingActive; - /// True if the split impulse position correction is active bool mIsSplitImpulseActive; - /// True if we solve 3 friction constraints at the contact manifold center only - /// instead of 2 friction constraints at each contact point - bool mIsSolveFrictionAtContactManifoldCenterActive; - // -------------------- Methods -------------------- // - /// Initialize the contact constraints before solving the system - void initializeContactConstraints(); - - /// Apply an impulse to the two bodies of a constraint - void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); - - /// Apply an impulse to the two bodies of a constraint - void applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold); - /// Compute the collision restitution factor from the restitution factor of each body decimal computeMixedRestitutionFactor(RigidBody *body1, RigidBody *body2) const; @@ -381,42 +321,31 @@ class ContactSolver { /// Compute th mixed rolling resistance factor between two bodies decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; - /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction - /// plane for a contact point. The two vectors have to be - /// such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointSolver &contactPoint) const; - /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// plane for a contact manifold. The two vectors have to be /// such that : t1 x t2 = contactNormal. void computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contactPoint) const; - /// Compute a penetration constraint impulse - const Impulse computePenetrationImpulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; - - /// Compute the first friction constraint impulse - const Impulse computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; - - /// Compute the second friction constraint impulse - const Impulse computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; + /// Warm start the solver. + void warmStart(); public: // -------------------- Methods -------------------- // /// Constructor - ContactSolver(const std::map& mapBodyToVelocityIndex); + ContactSolver(const std::map& mapBodyToVelocityIndex, + SingleFrameAllocator& allocator); /// Destructor ~ContactSolver() = default; + /// Initialize the contact constraints + void init(Island** islands, uint nbIslands, decimal timeStep); + /// Initialize the constraint solver for a given island - void initializeForIsland(decimal dt, Island* island); + void initializeForIsland(Island* island); /// Set the split velocities arrays void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, @@ -426,9 +355,6 @@ class ContactSolver { void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, Vector3* constrainedAngularVelocities); - /// Warm start the solver. - void warmStart(); - /// Store the computed impulses to use them to /// warm start the solver at the next iteration void storeImpulses(); @@ -441,13 +367,6 @@ class ContactSolver { /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); - - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - - /// Clean up the constraint solver - void cleanup(); }; // Set the split velocities arrays @@ -482,12 +401,6 @@ inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { mIsSplitImpulseActive = isActive; } -// Activate or deactivate the solving of friction constraints at the center of -// the contact manifold instead of solving them at each contact point -inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mIsSolveFrictionAtContactManifoldCenterActive = isActive; -} - // Compute the collision restitution factor from the restitution factor of each body inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) const { @@ -502,7 +415,7 @@ inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, RigidBody *body2) const { // Use the geometric mean to compute the mixed friction coefficient - return sqrt(body1->getMaterial().getFrictionCoefficient() * + return std::sqrt(body1->getMaterial().getFrictionCoefficient() * body2->getMaterial().getFrictionCoefficient()); } @@ -512,34 +425,6 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } -// Compute a penetration constraint impulse -inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, - contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); -} - -// Compute the first friction constraint impulse -inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector1 * deltaLambda, - -contactPoint.r1CrossT1 * deltaLambda, - contactPoint.frictionVector1 * deltaLambda, - contactPoint.r2CrossT1 * deltaLambda); -} - -// Compute the second friction constraint impulse -inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector2 * deltaLambda, - -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionVector2 * deltaLambda, - contactPoint.r2CrossT2 * deltaLambda); -} - } #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 2f601edd..d52348f8 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -40,7 +40,8 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mContactSolver(mMapBodyToConstrainedVelocityIndex), + mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES), + mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator), mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), @@ -48,8 +49,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), - mConstrainedOrientations(nullptr), mNbIslands(0), - mNbIslandsCapacity(0), mIslands(nullptr), mNbBodiesCapacity(0), + mConstrainedOrientations(nullptr), mNbIslands(0), mIslands(nullptr), mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { @@ -75,29 +75,6 @@ DynamicsWorld::~DynamicsWorld() { destroyRigidBody(*itToRemove); } - // Release the memory allocated for the islands - for (uint i=0; i~Island(); - - // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); - } - if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); - } - - // Release the memory allocated for the bodies velocity arrays - if (mNbBodiesCapacity > 0) { - delete[] mSplitLinearVelocities; - delete[] mSplitAngularVelocities; - delete[] mConstrainedLinearVelocities; - delete[] mConstrainedAngularVelocities; - delete[] mConstrainedPositions; - delete[] mConstrainedOrientations; - } - assert(mJoints.size() == 0); assert(mRigidBodies.size() == 0); @@ -161,6 +138,9 @@ void DynamicsWorld::update(decimal timeStep) { // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); + + // Reset the single frame memory allocator + mSingleFrameAllocator.reset(); } // Integrate position and orientation of the rigid bodies. @@ -232,6 +212,9 @@ void DynamicsWorld::updateBodiesState() { // Update the transform of the body (using the new center of mass and new orientation) bodies[b]->updateTransformWithCenterOfMass(); + // Update the world inverse inertia tensor of the body + bodies[b]->updateInertiaTensorInverseWorld(); + // Update the broad-phase state of the body bodies[b]->updateBroadPhaseState(); } @@ -241,31 +224,26 @@ void DynamicsWorld::updateBodiesState() { // Initialize the bodies velocities arrays for the next simulation step. void DynamicsWorld::initVelocityArrays() { + PROFILE("DynamicsWorld::initVelocityArrays()"); + // Allocate memory for the bodies velocity arrays uint nbBodies = mRigidBodies.size(); - if (mNbBodiesCapacity != nbBodies && nbBodies > 0) { - if (mNbBodiesCapacity > 0) { - delete[] mSplitLinearVelocities; - delete[] mSplitAngularVelocities; - } - mNbBodiesCapacity = nbBodies; - // TODO : Use better memory allocation here - mSplitLinearVelocities = new Vector3[mNbBodiesCapacity]; - mSplitAngularVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedPositions = new Vector3[mNbBodiesCapacity]; - mConstrainedOrientations = new Quaternion[mNbBodiesCapacity]; - assert(mSplitLinearVelocities != nullptr); - assert(mSplitAngularVelocities != nullptr); - assert(mConstrainedLinearVelocities != nullptr); - assert(mConstrainedAngularVelocities != nullptr); - assert(mConstrainedPositions != nullptr); - assert(mConstrainedOrientations != nullptr); - } + + 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))); + assert(mSplitLinearVelocities != nullptr); + assert(mSplitAngularVelocities != nullptr); + assert(mConstrainedLinearVelocities != nullptr); + assert(mConstrainedAngularVelocities != nullptr); + assert(mConstrainedPositions != nullptr); + assert(mConstrainedOrientations != nullptr); // Reset the velocities arrays - for (uint i=0; igetNbJoints() > 0; - bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - if (!isConstraintsToSolve && !isContactsToSolve) continue; + //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; + //if (!isConstraintsToSolve && !isContactsToSolve) continue; // If there are contacts in the current island - if (isContactsToSolve) { +// if (isContactsToSolve) { - // Initialize the solver - mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); +// // Initialize the solver +// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - // Warm start the contact solver - mContactSolver.warmStart(); - } +// // Warm start the contact solver +// if (mContactSolver.IsWarmStartingActive()) { +// mContactSolver.warmStart(); +// } +// } // If there are constraints if (isConstraintsToSolve) { @@ -388,26 +371,32 @@ void DynamicsWorld::solveContactsAndConstraints() { // Initialize the constraint solver mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); } + } - // For each iteration of the velocity solver - for (uint i=0; igetNbJoints() > 0; if (isConstraintsToSolve) { mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]); } - - // Solve the contacts - if (isContactsToSolve) mContactSolver.solve(); - } - - // Cache the lambda values in order to use them in the next - // step and cleanup the contact solver - if (isContactsToSolve) { - mContactSolver.storeImpulses(); - mContactSolver.cleanup(); } + + mContactSolver.solve(); + + // Solve the contacts +// if (isContactsToSolve) { + +// mContactSolver.resetTotalPenetrationImpulse(); + +// mContactSolver.solvePenetrationConstraints(); +// mContactSolver.solveFrictionConstraints(); +// } } + + mContactSolver.storeImpulses(); } // Solve the position error correction of the constraints @@ -446,7 +435,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the rigid body - RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, + RigidBody* rigidBody = new (mPoolAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, *this, bodyID); assert(rigidBody != nullptr); @@ -487,7 +476,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mRigidBodies.erase(rigidBody); // Free the object from the memory allocator - mMemoryAllocator.release(rigidBody, sizeof(RigidBody)); + mPoolAllocator.release(rigidBody, sizeof(RigidBody)); } // Create a joint between two bodies in the world and return a pointer to the new joint @@ -505,7 +494,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Ball-and-Socket joint case JointType::BALLSOCKETJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); newJoint = new (allocatedMemory) BallAndSocketJoint(info); @@ -515,7 +504,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Slider joint case JointType::SLIDERJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) SliderJoint(info); break; @@ -524,7 +513,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Hinge joint case JointType::HINGEJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) HingeJoint(info); break; @@ -533,7 +522,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Fixed joint case JointType::FIXEDJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) FixedJoint(info); break; @@ -586,8 +575,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(mMemoryAllocator, joint); - joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint); + joint->mBody1->removeJointFromJointsList(mPoolAllocator, joint); + joint->mBody2->removeJointFromJointsList(mPoolAllocator, joint); size_t nbBytes = joint->getSizeInBytes(); @@ -595,7 +584,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { joint->~Joint(); // Release the allocated memory - mMemoryAllocator.release(joint, nbBytes); + mPoolAllocator.release(joint, nbBytes); } // Add the joint to the list of joints of the two bodies involved in the joint @@ -604,13 +593,13 @@ 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 = mMemoryAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory1 = mPoolAllocator.allocate(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 = mMemoryAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory2 = mPoolAllocator.allocate(sizeof(JointListElement)); JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, joint->mBody2->mJointsList); joint->mBody2->mJointsList = jointListElement2; @@ -629,24 +618,9 @@ void DynamicsWorld::computeIslands() { uint nbBodies = mRigidBodies.size(); - // Clear all the islands - for (uint i=0; i~Island(); - - // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); - } - - // Allocate and create the array of islands - if (mNbIslandsCapacity != nbBodies && nbBodies > 0) { - if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); - } - mNbIslandsCapacity = nbBodies; - mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); - } + // 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)); mNbIslands = 0; int nbContactManifolds = 0; @@ -662,7 +636,7 @@ 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 = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack); + RigidBody** stackBodiesToVisit = static_cast(mSingleFrameAllocator.allocate(nbBytesStack)); // For each rigid body of the world for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { @@ -685,10 +659,9 @@ void DynamicsWorld::computeIslands() { body->mIsAlreadyInIsland = true; // Create the new island - void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); - mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, - nbContactManifolds, - mJoints.size(), mMemoryAllocator); + void* allocatedMemoryIsland = mSingleFrameAllocator.allocate(sizeof(Island)); + mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(), + mSingleFrameAllocator); // While there are still some bodies to visit in the stack while (stackIndex > 0) { @@ -778,9 +751,6 @@ void DynamicsWorld::computeIslands() { mNbIslands++; } - - // Release the allocated memory for the stack of bodies to visit - mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack); } // Put bodies to sleep if needed. diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 7dc715c9..746e8255 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -50,6 +50,9 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Attributes -------------------- // + /// Single frame Memory allocator + SingleFrameAllocator mSingleFrameAllocator; + /// Contact solver ContactSolver mContactSolver; @@ -106,15 +109,9 @@ class DynamicsWorld : public CollisionWorld { /// Number of islands in the world uint mNbIslands; - /// Current allocated capacity for the islands - uint mNbIslandsCapacity; - /// Array with all the islands of awaken bodies Island** mIslands; - /// Current allocated capacity for the bodies - uint mNbBodiesCapacity; - /// Sleep linear velocity threshold decimal mSleepLinearVelocity; @@ -207,10 +204,6 @@ class DynamicsWorld : public CollisionWorld { /// Set the position correction technique used for joints void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Create a rigid body into the physics world. RigidBody* createRigidBody(const Transform& transform); @@ -370,16 +363,6 @@ inline void DynamicsWorld::setJointsPositionCorrectionTechnique( } } -// Activate or deactivate the solving of friction constraints at the center of -// the contact manifold instead of solving them at each contact point -/** - * @param isActive True if you want the friction to be solved at the center of - * the contact manifold and false otherwise - */ -inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); -} - // Return the gravity vector of the world /** * @return The current gravity vector (in meter per seconds squared) diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index 41560bde..19eef304 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -29,26 +29,18 @@ using namespace reactphysics3d; // Constructor -Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator) +Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, SingleFrameAllocator& allocator) : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0), - mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { + mNbContactManifolds(0), mNbJoints(0) { - // Allocate memory for the arrays - mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies; - mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies); - mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds; - mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate( - mNbAllocatedBytesContactManifolds); - mNbAllocatedBytesJoints = sizeof(Joint*) * nbMaxJoints; - mJoints = (Joint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints); + // 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)); } // Destructor Island::~Island() { - - // Release the memory of the arrays - mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies); - mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds); - mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints); + // This destructor is never called because memory is allocated on the + // single frame allocator } diff --git a/src/engine/Island.h b/src/engine/Island.h index 1b086939..5f370004 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_ISLAND_H // Libraries -#include "memory/MemoryAllocator.h" +#include "memory/SingleFrameAllocator.h" #include "body/RigidBody.h" #include "constraint/Joint.h" #include "collision/ContactManifold.h" @@ -63,25 +63,13 @@ class Island { /// Current number of joints in the island uint mNbJoints; - /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; - - /// Number of bytes allocated for the bodies array - size_t mNbAllocatedBytesBodies; - - /// Number of bytes allocated for the contact manifolds array - size_t mNbAllocatedBytesContactManifolds; - - /// Number of bytes allocated for the joints array - size_t mNbAllocatedBytesJoints; - public: // -------------------- Methods -------------------- // /// Constructor Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator); + SingleFrameAllocator& allocator); /// Destructor ~Island(); @@ -114,7 +102,7 @@ class Island { RigidBody** getBodies(); /// Return a pointer to the array of contact manifolds - ContactManifold** getContactManifold(); + ContactManifold** getContactManifolds(); /// Return a pointer to the array of joints Joint** getJoints(); @@ -164,7 +152,7 @@ inline RigidBody** Island::getBodies() { } // Return a pointer to the array of contact manifolds -inline ContactManifold** Island::getContactManifold() { +inline ContactManifold** Island::getContactManifolds() { return mContactManifolds; } diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 7efca599..48658f5c 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator) + int nbMaxContactManifolds, PoolAllocator& memoryAllocator) : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), mCachedSeparatingAxis(0.0, 1.0, 0.0) { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 7995b0d2..70ee1c3f 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -63,7 +63,7 @@ class OverlappingPair { /// Constructor OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator); + int nbMaxContactManifolds, PoolAllocator& memoryAllocator); /// Destructor ~OverlappingPair() = default; diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index ed29a37f..ab2d126d 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -65,17 +65,17 @@ Vector3 Vector3::getOneUnitOrthogonalVector() const { assert(length() > MACHINE_EPSILON); // Get the minimum element of the vector - Vector3 vectorAbs(fabs(x), fabs(y), fabs(z)); + Vector3 vectorAbs(std::fabs(x), std::fabs(y), std::fabs(z)); int minElement = vectorAbs.getMinAxis(); if (minElement == 0) { - return Vector3(0.0, -z, y) / sqrt(y*y + z*z); + return Vector3(0.0, -z, y) / std::sqrt(y*y + z*z); } else if (minElement == 1) { - return Vector3(-z, 0.0, x) / sqrt(x*x + z*z); + return Vector3(-z, 0.0, x) / std::sqrt(x*x + z*z); } else { - return Vector3(-y, x, 0.0) / sqrt(x*x + y*y); + return Vector3(-y, x, 0.0) / std::sqrt(x*x + y*y); } } diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 4f30391d..1ca1adfd 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -184,7 +184,7 @@ inline void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) { // Return the length of the vector inline decimal Vector3::length() const { - return sqrt(x*x + y*y + z*z); + return std::sqrt(x*x + y*y + z*z); } // Return the square of the length of the vector diff --git a/src/memory/MemoryAllocator.cpp b/src/memory/PoolAllocator.cpp similarity index 95% rename from src/memory/MemoryAllocator.cpp rename to src/memory/PoolAllocator.cpp index 46c47e14..dc1306bc 100644 --- a/src/memory/MemoryAllocator.cpp +++ b/src/memory/PoolAllocator.cpp @@ -24,19 +24,19 @@ ********************************************************************************/ // Libraries -#include "MemoryAllocator.h" +#include "PoolAllocator.h" #include #include using namespace reactphysics3d; // Initialization of static variables -bool MemoryAllocator::isMapSizeToHeadIndexInitialized = false; -size_t MemoryAllocator::mUnitSizes[NB_HEAPS]; -int MemoryAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; +bool PoolAllocator::isMapSizeToHeadIndexInitialized = false; +size_t PoolAllocator::mUnitSizes[NB_HEAPS]; +int PoolAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; // Constructor -MemoryAllocator::MemoryAllocator() { +PoolAllocator::PoolAllocator() { // Allocate some memory to manage the blocks mNbAllocatedMemoryBlocks = 64; @@ -78,7 +78,7 @@ MemoryAllocator::MemoryAllocator() { } // Destructor -MemoryAllocator::~MemoryAllocator() { +PoolAllocator::~PoolAllocator() { // Release the memory allocated for each block for (uint i=0; i @@ -33,14 +33,14 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -// Class MemoryAllocator +// Class PoolAllocator /** * This class is used to efficiently allocate memory on the heap. * It allows us to allocate small blocks of memory (smaller or equal to 1024 bytes) * efficiently. This implementation is inspired by the small block allocator * described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp */ -class MemoryAllocator { +class PoolAllocator { private : @@ -126,10 +126,10 @@ class MemoryAllocator { // -------------------- Methods -------------------- // /// Constructor - MemoryAllocator(); + PoolAllocator(); /// Destructor - ~MemoryAllocator(); + ~PoolAllocator(); /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. diff --git a/src/memory/SingleFrameAllocator.cpp b/src/memory/SingleFrameAllocator.cpp new file mode 100644 index 00000000..a2e436cd --- /dev/null +++ b/src/memory/SingleFrameAllocator.cpp @@ -0,0 +1,80 @@ +/******************************************************************************** +* 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 "SingleFrameAllocator.h" +#include +#include + +using namespace reactphysics3d; + +// Constructor +SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes) + : mTotalSizeBytes(totalSizeBytes), mCurrentOffset(0) { + + // Allocate a whole block of memory at the beginning + mMemoryBufferStart = static_cast(malloc(mTotalSizeBytes)); + assert(mMemoryBufferStart != nullptr); +} + +// Destructor +SingleFrameAllocator::~SingleFrameAllocator() { + + // Release the memory allocated at the beginning + free(mMemoryBufferStart); +} + + +// Allocate memory of a given size (in bytes) and return a pointer to the +// allocated memory. +void* SingleFrameAllocator::allocate(size_t size) { + + // Check that there is enough remaining memory in the buffer + if (static_cast(mCurrentOffset) + size > mTotalSizeBytes) { + + // This should never occur. If it does, you must increase the initial + // size of memory of this allocator + assert(false); + + // Return null + return nullptr; + } + + // Next available memory location + void* nextAvailableMemory = mMemoryBufferStart + mCurrentOffset; + + // Increment the offset + mCurrentOffset += size; + + // Return the next available memory location + return nextAvailableMemory; +} + +// Reset the marker of the current allocated memory +void SingleFrameAllocator::reset() { + + // Reset the current offset at the beginning of the block + mCurrentOffset = 0; +} diff --git a/src/engine/Impulse.h b/src/memory/SingleFrameAllocator.h similarity index 58% rename from src/engine/Impulse.h rename to src/memory/SingleFrameAllocator.h index 89e36429..e3151f0a 100644 --- a/src/engine/Impulse.h +++ b/src/memory/SingleFrameAllocator.h @@ -23,63 +23,52 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_IMPULSE_H -#define REACTPHYSICS3D_IMPULSE_H +#ifndef REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H +#define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H // Libraries -#include "mathematics/mathematics.h" +#include +#include "configuration.h" +/// ReactPhysics3D namespace namespace reactphysics3d { -// Structure Impulse +// Class SingleFrameAllocator /** - * Represents an impulse that we can apply to bodies in the contact or constraint solver. + * This class represent a memory allocator used to efficiently allocate + * memory on the heap that is used during a single frame. */ -struct Impulse { +class SingleFrameAllocator { - private: - - // -------------------- Methods -------------------- // - - public: + private : // -------------------- Attributes -------------------- // - /// Linear impulse applied to the first body - const Vector3 linearImpulseBody1; + /// Total size (in bytes) of memory of the allocator + size_t mTotalSizeBytes; - /// Angular impulse applied to the first body - const Vector3 angularImpulseBody1; + /// Pointer to the beginning of the allocated memory block + char* mMemoryBufferStart; - /// Linear impulse applied to the second body - const Vector3 linearImpulseBody2; + /// Pointer to the next available memory location in the buffer + int mCurrentOffset; - /// Angular impulse applied to the second body - const Vector3 angularImpulseBody2; + public : // -------------------- Methods -------------------- // /// Constructor - Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1, - const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2) - : linearImpulseBody1(initLinearImpulseBody1), - angularImpulseBody1(initAngularImpulseBody1), - linearImpulseBody2(initLinearImpulseBody2), - angularImpulseBody2(initAngularImpulseBody2) { + SingleFrameAllocator(size_t totalSizeBytes); - } + /// Destructor + ~SingleFrameAllocator(); - /// Copy-constructor - Impulse(const Impulse& impulse) - : linearImpulseBody1(impulse.linearImpulseBody1), - angularImpulseBody1(impulse.angularImpulseBody1), - linearImpulseBody2(impulse.linearImpulseBody2), - angularImpulseBody2(impulse.angularImpulseBody2) { + /// Allocate memory of a given size (in bytes) + void* allocate(size_t size); - } + /// Reset the marker of the current allocated memory + void reset(); - /// Deleted assignment operator - Impulse& operator=(const Impulse& impulse) = delete; }; }