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

Implement the islands computation

Browse Source
This commit is contained in:
Daniel Chappuis 2013-08-26 21:28:48 +02:00
parent 2e6f571b98
commit f1d29b5123
18 changed files with 836 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/body/Body.cpp
View File

@ -23,7 +23,7 @@
* *
********************************************************************************/
// Libraries
// Libraries
#include "Body.h"
#include "../collision/shapes/CollisionShape.h"
@ -31,7 +31,8 @@
using namespace reactphysics3d;
// Constructor
Body::Body(bodyindex id) : mID(id) {
Body::Body(bodyindex id)
: mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false) {
}

61
src/body/Body.h
View File

@ -47,6 +47,15 @@ class Body {
/// ID of the body
bodyindex mID;
/// True if the body has already been added in an island (for sleeping technique)
bool mIsAlreadyInIsland;
/// True if the body is allowed to go to sleep for better efficiency
bool mIsAllowedToSleep;
/// True if the body is sleeping (for sleeping technique)
bool mIsSleeping;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -68,6 +77,24 @@ class Body {
/// Return the id of the body
bodyindex getID() const;
/// Return true if the body has already been added in an island (for the sleeping technique)
bool isAlreadyInIsland() const;
/// Set the value of to know if the body has already been added into an island
void setIsAlreadyInIsland(bool isAlreadyInIsland);
/// Return whether or not the body is allowed to sleep
bool isAllowedToSleep() const;
/// Set whether or not the body is allowed to go to sleep
void setIsAllowedToSleep(bool isAllowedToSleep);
/// Return whether or not the body is sleeping
bool isSleeping() const;
/// Set the variable to know whether or not the body is sleeping
void setIsSleeping(bool isSleeping);
/// Smaller than operator
bool operator<(const Body& body2) const;
@ -79,6 +106,10 @@ class Body {
/// Not equal operator
bool operator!=(const Body& body2) const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
};
// Return the id of the body
@ -86,6 +117,36 @@ inline bodyindex Body::getID() const {
return mID;
}
// Return true if the body has already been added in an island (for the sleeping technique)
inline bool Body::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
// Set the value of to know if the body has already been added into an island
inline void Body::setIsAlreadyInIsland(bool isAlreadyInIsland) {
mIsAlreadyInIsland = isAlreadyInIsland;
}
// Return whether or not the body is allowed to sleep
inline bool Body::isAllowedToSleep() const {
return mIsAllowedToSleep;
}
// Set whether or not the body is allowed to go to sleep
inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
mIsAllowedToSleep = isAllowedToSleep;
}
// Return whether or not the body is sleeping
inline bool Body::isSleeping() const {
return mIsSleeping;
}
// Set the variable to know whether or not the body is sleeping
inline void Body::setIsSleeping(bool isSleeping) {
mIsSleeping = isSleeping;
}
// Smaller than operator
inline bool Body::operator<(const Body& body2) const {
return (mID < body2.mID);

24
src/body/CollisionBody.cpp
View File

@ -25,6 +25,7 @@
// Libraries
#include "CollisionBody.h"
#include "../engine/ContactManifold.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -33,7 +34,7 @@ using namespace reactphysics3d;
CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape,
bodyindex id)
: Body(id), mCollisionShape(collisionShape), mTransform(transform),
mIsActive(true), mHasMoved(false) {
mIsActive(true), mHasMoved(false), mContactManifoldsList(NULL) {
assert(collisionShape);
@ -50,5 +51,24 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisi
// Destructor
CollisionBody::~CollisionBody() {
assert(mContactManifoldsList == NULL);
}
// Reset the contact manifold lists
void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) {
// Delete the linked list of contact manifolds of that body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) {
ContactManifoldListElement* nextElement = currentElement->next;
// Delete the current element
currentElement->ContactManifoldListElement::~ContactManifoldListElement();
memoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
currentElement = nextElement;
}
mContactManifoldsList = NULL;
assert(mContactManifoldsList == NULL);
}

22
src/body/CollisionBody.h
View File

@ -33,11 +33,15 @@
#include "../mathematics/Transform.h"
#include "../collision/shapes/AABB.h"
#include "../collision/shapes/CollisionShape.h"
#include "../memory/MemoryAllocator.h"
#include "../configuration.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
// Class declarations
struct ContactManifoldListElement;
// Class CollisionBody
/**
* This class represents a body that is able to collide with others
@ -76,6 +80,9 @@ class CollisionBody : public Body {
/// True if the body has moved during the last frame
bool mHasMoved;
/// First element of the linked list of contact manifolds involving this body
ContactManifoldListElement* mContactManifoldsList;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -84,6 +91,9 @@ class CollisionBody : public Body {
/// Private assignment operator
CollisionBody& operator=(const CollisionBody& body);
/// Reset the contact manifold lists
void resetContactManifoldsList(MemoryAllocator& memoryAllocator);
public :
// -------------------- Methods -------------------- //
@ -144,6 +154,13 @@ class CollisionBody : public Body {
/// Update the Axis-Aligned Bounding Box coordinates
void updateAABB();
/// Return the first element of the linked list of contact manifolds involving this body
const ContactManifoldListElement* getContactManifoldsLists() const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
};
// Return true if the body has moved during the last frame
@ -243,6 +260,11 @@ inline void CollisionBody::updateAABB() {
mCollisionShape->updateAABB(mAabb, mTransform);
}
// Return the first element of the linked list of contact manifolds involving this body
inline const ContactManifoldListElement* CollisionBody::getContactManifoldsLists() const {
return mContactManifoldsList;
}
}
#endif

33
src/body/RigidBody.cpp
View File

@ -25,6 +25,7 @@
// Libraries
#include "RigidBody.h"
#include "constraint/Constraint.h"
#include "../collision/shapes/CollisionShape.h"
// We want to use the ReactPhysics3D namespace
@ -36,13 +37,41 @@ RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3&
: CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal),
mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
mMassInverse(decimal(1.0) / mass), mIsGravityEnabled(true),
mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)) {
mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(NULL) {
assert(collisionShape);
}
// Destructor
RigidBody::~RigidBody() {
assert(mJointsList == NULL);
}
// Remove a joint from the joints list
void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Constraint* joint) {
assert(joint != NULL);
assert(mJointsList != NULL);
// Remove the joint from the linked list of the joints of the first body
if (mJointsList->joint == joint) { // If the first element is the one to remove
JointListElement* elementToRemove = mJointsList;
mJointsList = elementToRemove->next;
elementToRemove->JointListElement::~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement));
}
else { // If the element to remove is not the first one in the list
JointListElement* currentElement = mJointsList;
while (currentElement->next != NULL) {
if (currentElement->next->joint == joint) {
JointListElement* elementToRemove = currentElement->next;
currentElement->next = elementToRemove->next;
elementToRemove->JointListElement::~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement));
break;
}
}
}
}

23
src/body/RigidBody.h
View File

@ -31,10 +31,15 @@
#include "CollisionBody.h"
#include "../engine/Material.h"
#include "../mathematics/mathematics.h"
#include "../memory/MemoryAllocator.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
// Class declarations
struct JointListElement;
class Constraint;
// Class RigidBody
/**
* This class represents a rigid body of the physics
@ -86,6 +91,9 @@ class RigidBody : public CollisionBody {
/// Angular velocity damping factor
decimal mAngularDamping;
/// First element of the linked list of joints involving this body
JointListElement* mJointsList;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -94,6 +102,9 @@ class RigidBody : public CollisionBody {
/// Private assignment operator
RigidBody& operator=(const RigidBody& body);
/// Remove a joint from the joints list
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Constraint* joint);
public :
// -------------------- Methods -------------------- //
@ -179,6 +190,13 @@ class RigidBody : public CollisionBody {
/// Set the angular damping factor
void setAngularDamping(decimal angularDamping);
/// Return the first element of the linked list of joints involving this body
const JointListElement* getJointsList() const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
};
// Method that return the mass of the body
@ -328,6 +346,11 @@ inline void RigidBody::setAngularDamping(decimal angularDamping) {
mAngularDamping = angularDamping;
}
// Return the first element of the linked list of joints involving this body
inline const JointListElement* RigidBody::getJointsList() const {
return mJointsList;
}
}
#endif

4
src/configuration.h
View File

@ -92,8 +92,8 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
/// Default bounciness factor for a rigid body
const decimal DEFAULT_BOUNCINESS = decimal(0.5);
/// True if the deactivation (sleeping) of inactive bodies is enabled
const bool DEACTIVATION_ENABLED = true;
/// True if the spleeping technique for inactive bodies is enabled
const bool SPLEEPING_ENABLED = true;
/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
const decimal OBJECT_MARGIN = decimal(0.04);

2
src/constraint/Constraint.cpp
View File

@ -33,7 +33,7 @@ Constraint::Constraint(const ConstraintInfo& constraintInfo)
:mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
mType(constraintInfo.type),
mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique),
mIsCollisionEnabled(constraintInfo.isCollisionEnabled){
mIsCollisionEnabled(constraintInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {
assert(mBody1 != NULL);
assert(mBody2 != NULL);

47
src/constraint/Constraint.h
View File

@ -39,6 +39,32 @@ enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOI
// Class declarations
struct ConstraintSolverData;
class Constraint;
// Structure JointListElement
/**
* This structure represents a single element of a linked list of joints
*/
struct JointListElement {
public:
// -------------------- Attributes -------------------- //
/// Pointer to the actual joint
Constraint* joint;
/// Next element of the list
JointListElement* next;
// -------------------- Methods -------------------- //
/// Constructor
JointListElement(Constraint* initJoint, JointListElement* initNext)
:joint(initJoint), next(initNext){
}
};
// Structure ConstraintInfo
/**
@ -87,9 +113,8 @@ struct ConstraintInfo {
// Class Constraint
/**
* This abstract class represents a constraint in the physics engine.
* A constraint can be a collision contact or a joint for
* instance. Each constraint can be made of several "mathematical
* constraints" needed to represent the main constraint.
* A constraint can be a collision contact point or a joint for
* instance.
*/
class Constraint {
@ -121,6 +146,9 @@ class Constraint {
/// True if the two bodies of the constraint are allowed to collide with each other
bool mIsCollisionEnabled;
/// True if the joint has already been added into an island
bool mIsAlreadyInIsland;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -154,6 +182,9 @@ class Constraint {
/// Return true if the collision between the two bodies of the constraint is enabled
bool isCollisionEnabled() const;
/// Return true if the joint has already been added into an island
bool isAlreadyInIsland() const;
/// Return the number of bytes used by the constraint
virtual size_t getSizeInBytes() const = 0;
@ -168,6 +199,11 @@ class Constraint {
/// Solve the position constraint
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class Island;
};
// Return the reference to the body 1
@ -195,6 +231,11 @@ inline bool Constraint::isCollisionEnabled() const {
return mIsCollisionEnabled;
}
// Return true if the joint has already been added into an island
inline bool Constraint::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
}
#endif

28
src/engine/ContactManifold.cpp
View File

@ -30,10 +30,10 @@
using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(Body* const body1, Body* const body2,
ContactManifold::ContactManifold(CollisionBody* body1, CollisionBody* body2,
MemoryAllocator& memoryAllocator)
: mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0),
mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0),
mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
}
@ -107,9 +107,12 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
// Update the world coordinates and penetration depth of the contact points in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1());
mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2());
mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
mContactPoints[i]->setWorldPointOnBody1(transform1 *
mContactPoints[i]->getLocalPointOnBody1());
mContactPoints[i]->setWorldPointOnBody2(transform2 *
mContactPoints[i]->getLocalPointOnBody2());
mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() -
mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
}
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
@ -172,7 +175,8 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) cons
/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
/// when we compute this area, we do not calculate it exactly but we
/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
/// this is only a guess that is faster to compute.
/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
/// by Erwin Coumans (http://wwww.bulletphysics.org).
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
@ -185,28 +189,32 @@ int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& ne
if (indexMaxPenetration != 0) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 1) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 2) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 3) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
}

75
src/engine/ContactManifold.h
View File

@ -28,7 +28,7 @@
// Libraries
#include <vector>
#include "../body/Body.h"
#include "../body/CollisionBody.h"
#include "../constraint/ContactPoint.h"
#include "../memory/MemoryAllocator.h"
@ -38,6 +38,35 @@ namespace reactphysics3d {
// Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class declarations
class ContactManifold;
// Structure ContactManifoldListElement
/**
* This structure represents a single element of a linked list of contact manifolds
*/
struct ContactManifoldListElement {
public:
// -------------------- Attributes -------------------- //
/// Pointer to the actual contact manifold
ContactManifold* contactManifold;
/// Next element of the list
ContactManifoldListElement* next;
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldListElement(ContactManifold* initContactManifold,
ContactManifoldListElement* initNext)
:contactManifold(initContactManifold), next(initNext) {
}
};
// Class ContactManifold
/**
* This class represents the set of contact points between two bodies.
@ -59,11 +88,11 @@ class ContactManifold {
// -------------------- Attributes -------------------- //
/// Pointer to the first body
Body* const mBody1;
/// Pointer to the first body of the contact
CollisionBody* mBody1;
/// Pointer to the second body
Body* const mBody2;
/// Pointer to the second body of the contact
CollisionBody* mBody2;
/// Contact points in the manifold
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
@ -86,6 +115,9 @@ class ContactManifold {
/// Twist friction constraint accumulated impulse
decimal mFrictionTwistImpulse;
/// True if the contact manifold has already been added into an island
bool mIsAlreadyInIsland;
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
@ -97,6 +129,12 @@ class ContactManifold {
/// Private assignment operator
ContactManifold& operator=(const ContactManifold& contactManifold);
/// Return a pointer to the first body of the contact manifold
CollisionBody* getBody1() const;
/// Return a pointer to the second body of the contact manifold
CollisionBody* getBody2() const;
/// Return the index of maximum area
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
@ -109,15 +147,16 @@ class ContactManifold {
/// Remove a contact point from the manifold
void removeContactPoint(uint index);
/// Return true if two vectors are approximatively equal
bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifold(Body* const mBody1, Body* const mBody2, MemoryAllocator& memoryAllocator);
ContactManifold(CollisionBody* body1, CollisionBody* body2,
MemoryAllocator& memoryAllocator);
/// Destructor
~ContactManifold();
@ -166,8 +205,23 @@ class ContactManifold {
/// Return a contact point of the manifold
ContactPoint* getContactPoint(uint index) const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class Island;
};
// Return a pointer to the first body of the contact manifold
inline CollisionBody* ContactManifold::getBody1() const {
return mBody1;
}
// Return a pointer to the second body of the contact manifold
inline CollisionBody* ContactManifold::getBody2() const {
return mBody2;
}
// Return the number of contact points in the manifold
inline uint ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
@ -229,6 +283,11 @@ inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
return mContactPoints[index];
}
// Return true if the contact manifold has already been added into an island
inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
}
#endif

267
src/engine/DynamicsWorld.cpp
View File

@ -44,7 +44,8 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsDeactivationActive(DEACTIVATION_ENABLED) {
mIsSleepingEnabled(SPLEEPING_ENABLED), mNbIslands(0), mNbIslandsCapacity(0),
mIslands(NULL) {
}
@ -59,6 +60,19 @@ DynamicsWorld::~DynamicsWorld() {
mMemoryAllocator.release((*it).second, sizeof(OverlappingPair));
}
// Release the memory allocated for the islands
for (uint i=0; i<mNbIslands; i++) {
// Call the island destructor
mIslands[i]->Island::~Island();
// Release the allocated memory for the island
mMemoryAllocator.release(mIslands[i], sizeof(Island));
}
if (mNbIslandsCapacity > 0) {
mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
}
// Free the allocated memory for the constrained velocities
cleanupConstrainedVelocitiesArray();
@ -94,6 +108,9 @@ void DynamicsWorld::update() {
// Remove all contact manifolds
mContactManifolds.clear();
// Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies();
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
@ -106,6 +123,9 @@ void DynamicsWorld::update() {
// Update the timer
mTimer.nextStep();
// Compute the islands (separate groups of bodies with constraints between each others)
computeIslands();
// Solve the contacts and constraints
solveContactsAndConstraints();
@ -448,7 +468,8 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove the collision shape from the world
removeCollisionShape(rigidBody->getCollisionShape());
// Destroy all the joints that contains the rigid body to be destroyed
// Destroy all the joints in which the rigid body to be destroyed is involved
// TODO : Iterate on the mJointList of the rigid body instead over all the joints of the world
bodyindex idToRemove = rigidBody->getID();
for (std::set<Constraint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) {
@ -456,6 +477,9 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
}
}
// Reset the contact manifold list of the body
rigidBody->resetContactManifoldsList(mMemoryAllocator);
// Call the destructor of the rigid body
rigidBody->RigidBody::~RigidBody();
@ -529,6 +553,9 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
// Add the joint into the world
mJoints.insert(newJoint);
// Add the joint into the joint list of the bodies involved in the joint
addJointToBody(newJoint);
// Return the pointer to the created joint
return newJoint;
}
@ -548,14 +575,235 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
// Remove the joint from the world
mJoints.erase(joint);
// Get the size in bytes of the joint
size_t nbBytes = joint->getSizeInBytes();
// Remove the joint from the joint list of the bodies involved in the joint
joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint);
joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint);
// Call the destructor of the joint
joint->Constraint::~Constraint();
// Release the allocated memory
mMemoryAllocator.release(joint, nbBytes);
mMemoryAllocator.release(joint, joint->getSizeInBytes());
}
// Add the joint to the list of joints of the two bodies involved in the joint
void DynamicsWorld::addJointToBody(Constraint* joint) {
assert(joint != NULL);
// Add the joint at the beginning of the linked list of joints of the first body
void* allocatedMemory1 = mMemoryAllocator.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));
JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint,
joint->mBody2->mJointsList);
joint->mBody2->mJointsList = jointListElement2;
}
// Add a contact manifold to the linked list of contact manifolds of the two bodies involed
// in the corresponding contact
void DynamicsWorld::addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody* body1, CollisionBody* body2) {
assert(contactManifold != NULL);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the joint at the beginning of the linked list of joints of the second body
void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
}
// Reset all the contact manifolds linked list of each body
void DynamicsWorld::resetContactManifoldListsOfBodies() {
// For each rigid body of the world
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// Reset the contact manifold list of the body
(*it)->resetContactManifoldsList(mMemoryAllocator);
}
}
// Compute the islands of awake bodies.
/// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
/// between each other. This method computes the islands at each time step as follows: For each
/// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body
/// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to
/// find all the bodies that are connected with it (the bodies that share joints or contacts with
/// it). Then, we create an island with this group of connected bodies.
void DynamicsWorld::computeIslands() {
PROFILE("DynamicsWorld::computeIslands()");
uint nbBodies = mRigidBodies.size();
// Clear all the islands
for (uint i=0; i<mNbIslands; i++) {
// Call the island destructor
mIslands[i]->Island::~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);
}
mNbIslands = 0;
// Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
}
for (std::vector<ContactManifold*>::iterator it = mContactManifolds.begin();
it != mContactManifolds.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
}
for (std::set<Constraint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
}
// 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);
uint idIsland = 0; // TODO : REMOVE THIS
// For each rigid body of the world
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
RigidBody* body = *it;
// If the body has already been added to an island, we go to the next body
if (body->isAlreadyInIsland()) continue;
// If the body is not moving, we go to the next body
// TODO : When we will use STATIC bodies, we will need to take care of this case here
if (!body->getIsMotionEnabled()) continue;
// If the body is sleeping, we go to the next body
if (body->isSleeping()) continue;
// Reset the stack of bodies to visit
uint stackIndex = 0;
stackBodiesToVisit[stackIndex] = body;
stackIndex++;
body->mIsAlreadyInIsland = true;
// Create the new island
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(idIsland, nbBodies,mContactManifolds.size(),
mJoints.size(), mMemoryAllocator);
idIsland++;
// While there are still some bodies to visit in the stack
while (stackIndex > 0) {
// Get the next body to visit from the stack
stackIndex--;
RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex];
// Awake the body if it is slepping
bodyToVisit->setIsSleeping(false);
// Add the body into the island
mIslands[mNbIslands]->addBody(bodyToVisit);
// If the current body is not moving, we do not want to perform the DFS
// search across that body
if (!bodyToVisit->getIsMotionEnabled()) continue;
// For each contact manifold in which the current body is involded
ContactManifoldListElement* contactElement;
for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != NULL;
contactElement = contactElement->next) {
ContactManifold* contactManifold = contactElement->contactManifold;
// Check if the current contact manifold has already been added into an island
if (contactManifold->isAlreadyInIsland()) continue;
// Add the contact manifold into the island
mIslands[mNbIslands]->addContactManifold(contactManifold);
contactManifold->mIsAlreadyInIsland = true;
// Get the other body of the contact manifold
RigidBody* body1 = dynamic_cast<RigidBody*>(contactManifold->getBody1());
RigidBody* body2 = dynamic_cast<RigidBody*>(contactManifold->getBody2());
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island
if (otherBody->isAlreadyInIsland()) continue;
// Insert the other body into the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++;
otherBody->mIsAlreadyInIsland = true;
}
// For each joint in which the current body is involved
JointListElement* jointElement;
for (jointElement = bodyToVisit->mJointsList; jointElement != NULL;
jointElement = jointElement->next) {
Constraint* joint = jointElement->joint;
// Check if the current joint has already been added into an island
if (joint->isAlreadyInIsland()) continue;
// Add the joint into the island
mIslands[mNbIslands]->addJoint(joint);
joint->mIsAlreadyInIsland = true;
// Get the other body of the contact manifold
RigidBody* body1 = dynamic_cast<RigidBody*>(joint->getBody1());
RigidBody* body2 = dynamic_cast<RigidBody*>(joint->getBody2());
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island
if (otherBody->isAlreadyInIsland()) continue;
// Insert the other body into the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++;
otherBody->mIsAlreadyInIsland = true;
}
}
// Reset the isAlreadyIsland variable of the static bodies so that they
// can also be included in the other islands
for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
if (!mIslands[mNbIslands]->mBodies[i]->getIsMotionEnabled()) {
mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
}
}
mNbIslands++;
}
// Release the allocated memory for the stack of bodies to visit
mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack);
}
// Notify the world about a new broad-phase overlapping pair
@ -565,8 +813,8 @@ void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair)
bodyindexpair indexPair = addedPair->getBodiesIndexPair();
// Add the pair into the set of overlapping pairs (if not there yet)
OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair))) OverlappingPair(
addedPair->body1, addedPair->body2, mMemoryAllocator);
OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(addedPair->body1, addedPair->body2, mMemoryAllocator);
assert(newPair != NULL);
std::pair<map<bodyindexpair, OverlappingPair*>::iterator, bool> check =
mOverlappingPairs.insert(make_pair(indexPair, newPair));
@ -604,4 +852,9 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
// Add the contact manifold to the world
mContactManifolds.push_back(overlappingPair->getContactManifold());
// Add the contact manifold into the list of contact manifolds
// of the two bodies involved in the contact
addContactManifoldToBody(overlappingPair->getContactManifold(), overlappingPair->mBody1,
overlappingPair->mBody2);
}

35
src/engine/DynamicsWorld.h
View File

@ -33,6 +33,7 @@
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
#include "Island.h"
#include "../configuration.h"
/// Namespace ReactPhysics3D
@ -65,13 +66,14 @@ class DynamicsWorld : public CollisionWorld {
/// Number of iterations for the position solver of the Sequential Impulses technique
uint mNbPositionSolverIterations;
/// True if the deactivation (sleeping) of inactive bodies is enabled
bool mIsDeactivationActive;
/// True if the spleeping technique for inactive bodies is enabled
bool mIsSleepingEnabled;
/// All the rigid bodies of the physics world
std::set<RigidBody*> mRigidBodies;
/// All the contact constraints
// TODO : Remove this variable (we will use the ones in the island now)
std::vector<ContactManifold*> mContactManifolds;
/// All the joints of the world
@ -100,6 +102,15 @@ class DynamicsWorld : public CollisionWorld {
/// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
/// 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;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -136,6 +147,9 @@ class DynamicsWorld : public CollisionWorld {
/// Reset the boolean movement variable of each body
void resetBodiesMovementVariable();
/// Compute the islands of awake bodies.
void computeIslands();
/// Update the overlapping pair
virtual void updateOverlappingPair(const BroadPhasePair* pair);
@ -198,6 +212,17 @@ public :
/// Destroy a joint
void destroyJoint(Constraint* joint);
/// Add the joint to the list of joints of the two bodies involved in the joint
void addJointToBody(Constraint* joint);
//// Add a contact manifold to the linked list of contact manifolds of the two bodies involed
//// in the corresponding contact.
void addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody *body1, CollisionBody *body2);
/// Reset all the contact manifolds linked list of each body
void resetContactManifoldListsOfBodies();
/// Return the gravity vector of the world
Vector3 getGravity() const;
@ -227,6 +252,12 @@ public :
/// Return a reference to the contact manifolds of the world
const std::vector<ContactManifold*>& getContactManifolds() const;
// TODO : REMOVE THIS
Island** getIslands() { return mIslands;}
// TODO : REMOVE THIS
uint getNbIslands() const {return mNbIslands;}
};
// Start the physics simulation

54
src/engine/Island.cpp Normal file
View File

@ -0,0 +1,54 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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 "Island.h"
using namespace reactphysics3d;
// Constructor
Island::Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
MemoryAllocator& memoryAllocator)
: mID(id), mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {
// 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(Constraint*) * nbMaxJoints;
mJoints = (Constraint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints);
}
// Destructor
Island::~Island() {
// Release the memory of the arrays
mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies);
mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds);
mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints);
}

186
src/engine/Island.h Normal file
View File

@ -0,0 +1,186 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_ISLAND_H
#define REACTPHYSICS3D_ISLAND_H
// Libraries
#include "../memory/MemoryAllocator.h"
#include "../body/RigidBody.h"
#include "../constraint/Constraint.h"
#include "ContactManifold.h"
namespace reactphysics3d {
// Class Island
/**
* An island represent an isolated group of awake bodies that are connected with each other by
* some contraints (contacts or joints).
*/
class Island {
private:
// -------------------- Attributes -------------------- //
// TODO : REMOVE THIS
uint mID;
/// Array with all the bodies of the island
RigidBody** mBodies;
/// Array with all the contact manifolds between bodies of the island
ContactManifold** mContactManifolds;
/// Array with all the joints between bodies of the island
Constraint** mJoints;
/// Current number of bodies in the island
uint mNbBodies;
/// Current number of contact manifold in the island
uint mNbContactManifolds;
/// 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;
// -------------------- Methods -------------------- //
/// Private assignment operator
Island& operator=(const Island& island);
/// Private copy-constructor
Island(const Island& island);
public:
// -------------------- Methods -------------------- //
/// Constructor
Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
MemoryAllocator& memoryAllocator);
/// Destructor
~Island();
/// Add a body into the island
void addBody(RigidBody* body);
/// Add a contact manifold into the island
void addContactManifold(ContactManifold* contactManifold);
/// Add a joint into the island
void addJoint(Constraint* joint);
/// Return the number of bodies in the island
uint getNbBodies() const;
/// Return the number of contact manifolds in the island
uint getNbContactManifolds() const;
/// Return the number of joints in the island
uint getNbJoints() const;
/// Return a pointer to the array of bodies
RigidBody** getBodies();
/// Return a pointer to the array of contact manifolds
ContactManifold** getContactManifold();
/// Return a pointer to the array of joints
Constraint** getJoints();
// TODO : REMOVE THIS
uint getID() const {return mID;}
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
};
// Add a body into the island
inline void Island::addBody(RigidBody* body) {
assert(!body->isSleeping());
mBodies[mNbBodies] = body;
mNbBodies++;
}
// Add a contact manifold into the island
inline void Island::addContactManifold(ContactManifold* contactManifold) {
mContactManifolds[mNbContactManifolds] = contactManifold;
mNbContactManifolds++;
}
// Add a joint into the island
inline void Island::addJoint(Constraint* joint) {
mJoints[mNbJoints] = joint;
mNbJoints++;
}
// Return the number of bodies in the island
inline uint Island::getNbBodies() const {
return mNbBodies;
}
// Return the number of contact manifolds in the island
inline uint Island::getNbContactManifolds() const {
return mNbContactManifolds;
}
// Return the number of joints in the island
inline uint Island::getNbJoints() const {
return mNbJoints;
}
// Return a pointer to the array of bodies
inline RigidBody** Island::getBodies() {
return mBodies;
}
// Return a pointer to the array of contact manifolds
inline ContactManifold** Island::getContactManifold() {
return mContactManifolds;
}
// Return a pointer to the array of joints
inline Constraint** Island::getJoints() {
return mJoints;
}
}
#endif

8
src/engine/OverlappingPair.h
View File

@ -47,10 +47,10 @@ class OverlappingPair {
// -------------------- Attributes -------------------- //
/// Pointer to the first body of the contact
CollisionBody* const mBody1;
CollisionBody* mBody1;
/// Pointer to the second body of the contact
CollisionBody* const mBody2;
CollisionBody* mBody2;
/// Persistent contact manifold
ContactManifold mContactManifold;
@ -100,6 +100,10 @@ class OverlappingPair {
/// Return the contact manifold
ContactManifold* getContactManifold();
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
};
// Return the pointer to first body

4
src/memory/MemoryAllocator.cpp
View File

@ -148,10 +148,10 @@ void* MemoryAllocator::allocate(size_t size) {
assert(nbUnits * unitSize <= BLOCK_SIZE);
for (size_t i=0; i < nbUnits - 1; i++) {
MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i);
MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i + 1));
MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1));
unit->nextUnit = nextUnit;
}
MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (nbUnits - 1));
MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1));
lastUnit->nextUnit = NULL;
// Add the new allocated block into the list of free memory units in the heap