mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Refactor memory allocator and refactor contact solver

Browse Source
This commit is contained in:
Daniel Chappuis 2016-09-21 22:03:45 +02:00
parent 92460791e6
commit e014f00afc
31 changed files with 201 additions and 1085 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -174,8 +174,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"
)

10
src/body/CollisionBody.cpp
View File

@ -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;
}

2
src/body/CollisionBody.h
View File

@ -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

4
src/body/RigidBody.cpp
View File

@ -166,7 +166,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 +214,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);

4
src/body/RigidBody.h
View File

@ -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 {
@ -104,7 +104,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();

22
src/collision/CollisionDetection.cpp
View File

@ -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

8
src/collision/CollisionDetection.h
View File

@ -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 <vector>
#include <set>
@ -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;

2
src/collision/ContactManifold.cpp
View File

@ -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),

6
src/collision/ContactManifold.h
View File

@ -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 {
@ -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();

2
src/collision/ContactManifoldSet.cpp
View File

@ -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);

4
src/collision/ContactManifoldSet.h
View File

@ -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();

2
src/collision/narrowphase/CollisionDispatch.h
View File

@ -51,7 +51,7 @@ class CollisionDispatch {
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
PoolAllocator* memoryAllocator) {
}

2
src/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

@ -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);

2
src/collision/narrowphase/DefaultCollisionDispatch.h
View File

@ -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.

8
src/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -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 <algorithm>
/// 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;
}

4
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -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);
}

1
src/collision/narrowphase/GJK/VoronoiSimplex.cpp
View File

@ -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);

2
src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
View File

@ -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;
}

6
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -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);

2
src/collision/shapes/CollisionShape.h
View File

@ -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 {

3
src/configuration.h
View File

@ -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 = 1048576;
}
#endif

6
src/engine/CollisionWorld.cpp
View File

@ -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<reactphysics3d::bodyindex>::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

6
src/engine/CollisionWorld.h
View File

@ -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<luint> mFreeBodiesIDs;
/// Memory allocator
MemoryAllocator mMemoryAllocator;
/// Pool Memory allocator
PoolAllocator mPoolAllocator;
/// Pointer to an event listener object
EventListener* mEventListener;

805
src/engine/ContactSolver.cpp
View File

@ -39,55 +39,75 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP= decimal(0.01);
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
SingleFrameAllocator& singleFrameAllocator)
:mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
mContactConstraints(nullptr), mPenetrationConstraints(nullptr),
mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mSingleFrameAllocator(singleFrameAllocator),
mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr),
mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
}
// Initialize the contact constraints
void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
mTimeStep = timeStep;
// TODO : Try not to count manifolds here
// Count the contact manifolds
uint nbContactManifolds = 0;
for (uint i = 0; i < nbIslands; i++) {
nbContactManifolds += islands[i]->getNbContactManifolds();
}
mNbFrictionConstraints = 0;
mNbPenetrationConstraints = 0;
mPenetrationConstraints = nullptr;
mFrictionConstraints = nullptr;
if (nbContactManifolds == 0) return;
// TODO : Count exactly the number of constraints to allocate here
uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD;
mPenetrationConstraints = static_cast<PenetrationConstraint*>(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints));
//mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints];
assert(mPenetrationConstraints != nullptr);
//mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
mFrictionConstraints = static_cast<FrictionConstraint*>(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds));
//mFrictionConstraints = new FrictionConstraint[nbContactManifolds];
assert(mFrictionConstraints != nullptr);
//mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
// For each island of the world
for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
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()");
assert(island != nullptr);
assert(island->getNbBodies() > 0);
assert(island->getNbContactManifolds() > 0);
assert(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr);
// Set the current time step
mTimeStep = dt;
mNbContactManifolds = island->getNbContactManifolds();
mNbFrictionConstraints = 0;
mNbPenetrationConstraints = 0;
// TODO : Try to do faster allocation here
mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
assert(mContactConstraints != nullptr);
// TODO : Count exactly the number of constraints to allocate here (do not reallocate each frame)
mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
assert(mPenetrationConstraints != nullptr);
// TODO : Do not reallocate each frame)
mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
assert(mFrictionConstraints != nullptr);
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifolds();
for (uint i=0; i<mNbContactManifolds; i++) {
for (uint i=0; i<island->getNbContactManifolds(); i++) {
ContactManifold* externalManifold = contactManifolds[i];
ContactManifoldSolver& internalManifold = mContactConstraints[i];
assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact
@ -100,6 +120,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
uint indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
new (mFrictionConstraints + mNbFrictionConstraints) FrictionConstraint();
mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1;
mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2;
mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold;
@ -129,7 +150,6 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
internalManifold.externalContactManifold = externalManifold;
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
//internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
//internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
@ -159,6 +179,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
// Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c);
new (mPenetrationConstraints + mNbPenetrationConstraints) PenetrationConstraint();
mPenetrationConstraints[mNbPenetrationConstraints].indexBody1 = indexBody1;
mPenetrationConstraints[mNbPenetrationConstraints].indexBody2 = indexBody2;
mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 = I1;
@ -301,168 +322,19 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
frictionTwistMass :
decimal(0.0);
//}
mNbFrictionConstraints++;
}
// Fill-in all the matrices needed to solve the LCP problem
//initializeContactConstraints();
}
// TODO : Delete this method
// Initialize the contact constraints before solving the system
void ContactSolver::initializeContactConstraints() {
// Solve the contact constraints of one iteration of the solve
void ContactSolver::solve() {
PROFILE("ContactSolver::initializeContactConstraints()");
// For each contact constraint
//for (uint c=0; c<mNbContactManifolds; c++) {
assert(mTimeStep > decimal(0.0));
// ContactManifoldSolver& manifold = mContactConstraints[c];
// // Get the inertia tensors of both bodies
// Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
// Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold
// if (mIsSolveFrictionAtContactManifoldCenterActive) {
// manifold.normal = Vector3(0.0, 0.0, 0.0);
// }
// Get the velocities of the bodies
// const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
// const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
// const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
// const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
// For each contact point constraint
//for (uint i=0; i<manifold.nbContacts; i++) {
//ContactPointSolver& contactPoint = manifold.contacts[i];
//ContactPoint* externalContact = contactPoint.externalContact;
// // Compute the velocity difference
// Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
// contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
// contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
// // Compute the inverse mass matrix K for the penetration constraint
// decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
// ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
// ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
// massPenetration > 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);
// }
// 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);
// if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
// contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
// }
// // If the warm starting of the contact solver is active
// if (mIsWarmStartingActive) {
// // 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;
// }
//}
// // 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();
// }
// If we solve the friction constraints at the center of the contact manifold
//if (mIsSolveFrictionAtContactManifoldCenterActive) {
// manifold.normal.normalize();
// Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
// v1 - w1.cross(manifold.r1Friction);
// // Compute the friction vectors
// computeFrictionVectors(deltaVFrictionPoint, manifold);
// // 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);
//}
//}
resetTotalPenetrationImpulse();
solvePenetrationConstraints();
solveFrictionConstraints();
}
// Warm start the solver.
@ -576,177 +448,6 @@ void ContactSolver::warmStart() {
mFrictionConstraints[i].rollingResistanceImpulse.setToZero();
}
}
/*
// Check that warm starting is active
if (!mIsWarmStartingActive) return;
// For each constraint
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
bool atLeastOneRestingContactPoint = false;
for (uint i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
// If it is not a new contact (this contact was already existing at last time step)
if (contactPoint.isRestingContact) {
atLeastOneRestingContactPoint = true;
// --------- Penetration --------- //
// Compute the impulse P = J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(
contactPoint.penetrationImpulse, contactPoint);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// Project the old friction impulses (with old friction vectors) into
// the new friction vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
contactPoint.oldFrictionVector1 +
contactPoint.friction2Impulse *
contactPoint.oldFrictionVector2;
contactPoint.friction1Impulse = oldFrictionImpulse.dot(
contactPoint.frictionVector1);
contactPoint.friction2Impulse = oldFrictionImpulse.dot(
contactPoint.frictionVector2);
// --------- Friction 1 --------- //
// Compute the impulse P = J^T * lambda
const Impulse impulseFriction1 = computeFriction1Impulse(
contactPoint.friction1Impulse, contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// --------- Friction 2 --------- //
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction2 = computeFriction2Impulse(
contactPoint.friction2Impulse, contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ Rolling resistance------ //
if (contactManifold.rollingResistanceFactor > 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);
}
}
}
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();
}
}
// 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) {
// 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);
// ------ 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);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// ------ 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);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ 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);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseTwistFriction, contactManifold);
// ------ 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);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseRollingResistance, contactManifold);
}
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();
}
}
*/
}
// Reset the total penetration impulse of friction constraints
@ -978,288 +679,13 @@ void ContactSolver::solveFrictionConstraints() {
}
}
// Solve the contacts
//void ContactSolver::solve() {
// PROFILE("ContactSolver::solve()");
// decimal deltaLambda;
// decimal lambdaTemp;
// // For each contact manifold
// for (uint c=0; c<mNbContactManifolds; c++) {
// ContactManifoldSolver& contactManifold = mContactConstraints[c];
// decimal sumPenetrationImpulse = 0.0;
// // Get the constrained velocities
// const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
// const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
// const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
// const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
// for (uint i=0; i<contactManifold.nbContacts; i++) {
// ContactPointSolver& contactPoint = contactManifold.contacts[i];
// // --------- Penetration --------- //
// // Compute J*v
// Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
// decimal deltaVDotN = deltaV.dot(contactPoint.normal);
// decimal Jv = deltaVDotN;
// // Compute the bias "b" of the constraint
// decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
// decimal biasPenetrationDepth = 0.0;
// if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
// max(0.0f, float(contactPoint.penetrationDepth - SLOP));
// decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// // Compute the Lagrange multiplier lambda
// if (mIsSplitImpulseActive) {
// deltaLambda = - (Jv + contactPoint.restitutionBias) *
// contactPoint.inversePenetrationMass;
// }
// else {
// deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
// }
// lambdaTemp = contactPoint.penetrationImpulse;
// contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
// deltaLambda, decimal(0.0));
// deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// // Compute the impulse P=J^T * lambda
// const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
// contactPoint);
// // Apply the impulse to the bodies of the constraint
// applyImpulse(impulsePenetration, contactManifold);
// sumPenetrationImpulse += contactPoint.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);
// decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
// contactPoint.inversePenetrationMass;
// decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
// contactPoint.penetrationSplitImpulse = std::max(
// contactPoint.penetrationSplitImpulse +
// deltaLambdaSplit, decimal(0.0));
// deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// // Compute the impulse P=J^T * lambda
// const Impulse splitImpulsePenetration = computePenetrationImpulse(
// deltaLambdaSplit, contactPoint);
// applySplitImpulse(splitImpulsePenetration, contactManifold);
// }
// // 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);
// }
// }
//}
// 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 ------ //
// // Compute J*v
// Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
// - v1 - w1.cross(contactManifold.r1Friction);
// decimal Jv = deltaV.dot(contactManifold.frictionVector1);
// // 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;
// // 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);
// // 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);
// }
// }
// }
//}
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
// Penetration constraints
for (uint i=0; i<mNbPenetrationConstraints; i++) {
mPenetrationConstraints[i].contactPoint->setPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse);
}
// Friction constraints
@ -1274,105 +700,17 @@ void ContactSolver::storeImpulses() {
}
/*
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
if (mPenetrationConstraints != nullptr) {
// TODO : Delete this
delete[] mPenetrationConstraints;
}
ContactManifoldSolver& manifold = mContactConstraints[c];
for (uint i=0; i<manifold.nbContacts; i++) {
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);
}
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);
if (mFrictionConstraints != nullptr) {
delete[] mFrictionConstraints;
}
*/
}
/*
// Apply an impulse to the two bodies of a constraint
void ContactSolver::applyImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold) {
PROFILE("ContactSolver::applyImpulse()");
// 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;
}
*/
// TODO : Delete this
// 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,
@ -1402,22 +740,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// friction vector and the contact normal
frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit();
}
// Clean up the constraint solver
void ContactSolver::cleanup() {
if (mContactConstraints != nullptr) {
delete[] mContactConstraints;
mContactConstraints = nullptr;
}
if (mPenetrationConstraints != nullptr) {
delete[] mPenetrationConstraints;
mPenetrationConstraints = nullptr;
}
if (mFrictionConstraints != nullptr) {
delete[] mFrictionConstraints;
mFrictionConstraints = nullptr;
}
}

249
src/engine/ContactSolver.h
View File

@ -31,6 +31,7 @@
#include "configuration.h"
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
#include "memory/SingleFrameAllocator.h"
#include "Island.h"
#include "Impulse.h"
#include <map>
@ -278,228 +279,6 @@ class ContactSolver {
bool hasAtLeastOneRestingContactPoint;
};
// Structure ContactPointSolver
/**
* Contact solver internal data structure that to store all the
* information relative to a contact point
*/
struct ContactPointSolver {
/// Index of body 1 in the constraint solver
uint indexBody1;
/// Index of body 2 in the constraint solver
uint indexBody2;
/// Inverse of the mass of body 1
decimal massInverseBody1;
/// Inverse of the mass of body 2
decimal massInverseBody2;
/// Inverse inertia tensor of body 1
Matrix3x3 inverseInertiaTensorBody1;
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
/// Point on body 1 where to apply the friction constraints
Vector3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints
Vector3 frictionPointBody2;
/// 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;
/// 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;
/// Inverse of the matrix K for the penenetration
//decimal inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction
decimal inverseFriction1Mass;
/// Inverse of the matrix K for the 2nd friction
decimal inverseFriction2Mass;
/// True if the contact was existing last time step
bool isRestingContact;
/// Pointer to the external contact
ContactPoint* externalContact;
};
// Structure ContactManifoldSolver
/**
* Contact solver internal data structure to store all the
* information relative to a contact manifold.
*/
struct ContactManifoldSolver {
/// Index of body 1 in the constraint solver
//uint indexBody1;
/// Index of body 2 in the constraint solver
//uint indexBody2;
/// Inverse of the mass of body 1
//decimal massInverseBody1;
// Inverse of the mass of body 2
//decimal massInverseBody2;
/// Inverse inertia tensor of body 1
//Matrix3x3 inverseInertiaTensorBody1;
/// 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
//Vector3 normal;
/// Point on body 1 where to apply the friction constraints
//Vector3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints
//Vector3 frictionPointBody2;
/// R1 vector for the friction constraints
//Vector3 r1Friction;
/// R2 vector for the friction constraints
//Vector3 r2Friction;
/// 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;
/// Matrix K for the first friction constraint
//decimal inverseFriction1Mass;
/// Matrix K for the second friction constraint
//decimal inverseFriction2Mass;
/// Matrix K for the twist friction constraint
//decimal inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint
//Matrix3x3 inverseRollingResistance;
/// First friction direction at contact manifold center
//Vector3 frictionVector1;
/// Second friction direction at contact manifold center
//Vector3 frictionVector2;
/// Old 1st friction direction at contact manifold center
//Vector3 oldFrictionVector1;
/// Old 2nd friction direction at contact manifold center
//Vector3 oldFrictionVector2;
/// First friction direction impulse at manifold center
//decimal friction1Impulse;
/// Second friction direction impulse at manifold center
//decimal friction2Impulse;
/// Twist friction impulse at contact manifold center
//decimal frictionTwistImpulse;
/// Rolling resistance impulse
//Vector3 rollingResistanceImpulse;
};
// -------------------- Constants --------------------- //
/// Beta value for the penetration depth position correction without split impulses
@ -519,12 +298,12 @@ class ContactSolver {
/// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities;
/// Reference to the single frame memory allocator
SingleFrameAllocator& mSingleFrameAllocator;
/// Current time step
decimal mTimeStep;
/// Contact constraints
ContactManifoldSolver* mContactConstraints;
PenetrationConstraint* mPenetrationConstraints;
FrictionConstraint* mFrictionConstraints;
@ -533,9 +312,6 @@ class ContactSolver {
uint mNbFrictionConstraints;
/// Number of contact constraints
uint mNbContactManifolds;
/// Array of linear velocities
Vector3* mLinearVelocities;
@ -557,8 +333,8 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Initialize the contact constraints before solving the system
void initializeContactConstraints();
/// Initialize the constraint solver for a given island
void initializeForIsland(Island* island);
/// Apply an impulse to the two bodies of a constraint
//void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
@ -608,13 +384,17 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
SingleFrameAllocator& singleFrameAllocator);
/// Destructor
~ContactSolver() = default;
/// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island);
/// Initialize the contact constraints
void init(Island** islands, uint nbIslands, decimal timeStep);
/// Solve the contact constraints of one iteration of the solve
void solve();
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@ -653,9 +433,6 @@ class ContactSolver {
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Clean up the constraint solver
void cleanup();
/// Return true if warmstarting is active
bool IsWarmStartingActive() const;
};

103
src/engine/DynamicsWorld.cpp
View File

@ -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),
@ -82,10 +83,10 @@ DynamicsWorld::~DynamicsWorld() {
mIslands[i]->~Island();
// Release the allocated memory for the island
mMemoryAllocator.release(mIslands[i], sizeof(Island));
mPoolAllocator.release(mIslands[i], sizeof(Island));
}
if (mNbIslandsCapacity > 0) {
mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
}
// Release the memory allocated for the bodies velocity arrays
@ -161,6 +162,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.
@ -364,27 +368,28 @@ void DynamicsWorld::solveContactsAndConstraints() {
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
// ---------- Solve velocity constraints for joints and contacts ---------- //
// Initialize the contact solver
mContactSolver.init(mIslands, mNbIslands, mTimeStep);
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
if (!isConstraintsToSolve && !isContactsToSolve) continue;
//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
if (mContactSolver.IsWarmStartingActive()) {
mContactSolver.warmStart();
}
}
// // Warm start the contact solver
// if (mContactSolver.IsWarmStartingActive()) {
// mContactSolver.warmStart();
// }
// }
// If there are constraints
if (isConstraintsToSolve) {
@ -392,32 +397,40 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Initialize the constraint solver
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
}
}
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
// Solve the constraints
if (isConstraintsToSolve) {
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Solve the constraints
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
if (isConstraintsToSolve) {
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
}
}
mContactSolver.solve();
// Solve the contacts
if (isContactsToSolve) {
// if (isContactsToSolve) {
mContactSolver.resetTotalPenetrationImpulse();
// mContactSolver.resetTotalPenetrationImpulse();
mContactSolver.solvePenetrationConstraints();
mContactSolver.solveFrictionConstraints();
}
// mContactSolver.solvePenetrationConstraints();
// mContactSolver.solveFrictionConstraints();
// }
}
// Cache the lambda values in order to use them in the next
// step and cleanup the contact solver
if (isContactsToSolve) {
mContactSolver.storeImpulses();
mContactSolver.cleanup();
}
}
// if (isContactsToSolve) {
// mContactSolver.storeImpulses();
// mContactSolver.cleanup();
// }
//}
mContactSolver.storeImpulses();
}
// Solve the position error correction of the constraints
@ -456,7 +469,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::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);
@ -497,7 +510,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
@ -515,7 +528,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<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(info);
@ -525,7 +538,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<const SliderJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) SliderJoint(info);
break;
@ -534,7 +547,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<const HingeJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) HingeJoint(info);
break;
@ -543,7 +556,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<const FixedJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) FixedJoint(info);
break;
@ -596,8 +609,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();
@ -605,7 +618,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
@ -614,13 +627,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;
@ -646,16 +659,16 @@ void DynamicsWorld::computeIslands() {
mIslands[i]->~Island();
// Release the allocated memory for the island
mMemoryAllocator.release(mIslands[i], sizeof(Island));
mPoolAllocator.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);
mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
}
mNbIslandsCapacity = nbBodies;
mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity);
mIslands = (Island**)mPoolAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity);
}
mNbIslands = 0;
@ -672,7 +685,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 = (RigidBody**)mPoolAllocator.allocate(nbBytesStack);
// For each rigid body of the world
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
@ -695,10 +708,10 @@ void DynamicsWorld::computeIslands() {
body->mIsAlreadyInIsland = true;
// Create the new island
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
void* allocatedMemoryIsland = mPoolAllocator.allocate(sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,
nbContactManifolds,
mJoints.size(), mMemoryAllocator);
mJoints.size(), mPoolAllocator);
// While there are still some bodies to visit in the stack
while (stackIndex > 0) {
@ -790,7 +803,7 @@ void DynamicsWorld::computeIslands() {
}
// Release the allocated memory for the stack of bodies to visit
mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack);
mPoolAllocator.release(stackBodiesToVisit, nbBytesStack);
}
// Put bodies to sleep if needed.

3
src/engine/DynamicsWorld.h
View File

@ -50,6 +50,9 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Attributes -------------------- //
/// Single frame Memory allocator
SingleFrameAllocator mSingleFrameAllocator;
/// Contact solver
ContactSolver mContactSolver;

2
src/engine/Island.cpp
View File

@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor
Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
MemoryAllocator& memoryAllocator)
PoolAllocator& memoryAllocator)
: mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {

6
src/engine/Island.h
View File

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_ISLAND_H
// Libraries
#include "memory/MemoryAllocator.h"
#include "memory/PoolAllocator.h"
#include "body/RigidBody.h"
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
@ -64,7 +64,7 @@ class Island {
uint mNbJoints;
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
PoolAllocator& mMemoryAllocator;
/// Number of bytes allocated for the bodies array
size_t mNbAllocatedBytesBodies;
@ -81,7 +81,7 @@ class Island {
/// Constructor
Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
MemoryAllocator& memoryAllocator);
PoolAllocator& memoryAllocator);
/// Destructor
~Island();

2
src/engine/OverlappingPair.cpp
View File

@ -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) {

2
src/engine/OverlappingPair.h
View File

@ -63,7 +63,7 @@ class OverlappingPair {
/// Constructor
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, MemoryAllocator& memoryAllocator);
int nbMaxContactManifolds, PoolAllocator& memoryAllocator);
/// Destructor
~OverlappingPair() = default;