Merge branch 'sleeping' into develop
This commit is contained in:
Daniel Chappuis
commit
1e64a93971
|
|
@ -23,7 +23,7 @@
|
|||
* *
|
||||
********************************************************************************/
|
||||
|
||||
// Libraries
|
||||
// Libraries
|
||||
#include "Body.h"
|
||||
#include "../collision/shapes/CollisionShape.h"
|
||||
|
||||
|
|
@ -31,7 +31,9 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
Body::Body(bodyindex id) : mID(id) {
|
||||
Body::Body(bodyindex id)
|
||||
: mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false),
|
||||
mSleepTime(0) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -47,6 +47,18 @@ 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;
|
||||
|
||||
/// Elapsed time since the body velocity was bellow the sleep velocity
|
||||
decimal mSleepTime;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
|
|
@ -68,6 +80,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
|
||||
virtual void setIsSleeping(bool isSleeping);
|
||||
|
||||
/// Smaller than operator
|
||||
bool operator<(const Body& body2) const;
|
||||
|
||||
|
|
@ -79,6 +109,10 @@ class Body {
|
|||
|
||||
/// Not equal operator
|
||||
bool operator!=(const Body& body2) const;
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class DynamicsWorld;
|
||||
};
|
||||
|
||||
// Return the id of the body
|
||||
|
|
@ -86,6 +120,48 @@ 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;
|
||||
|
||||
if (!mIsAllowedToSleep) setIsSleeping(false);
|
||||
}
|
||||
|
||||
// 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) {
|
||||
|
||||
if (isSleeping) {
|
||||
mSleepTime = decimal(0.0);
|
||||
}
|
||||
else {
|
||||
if (mIsSleeping) {
|
||||
mSleepTime = decimal(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
mIsSleeping = isSleeping;
|
||||
}
|
||||
|
||||
// Smaller than operator
|
||||
inline bool Body::operator<(const Body& body2) const {
|
||||
return (mID < body2.mID);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,42 @@ 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;
|
||||
}
|
||||
currentElement = currentElement->next;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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,16 @@ 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;
|
||||
|
||||
/// Set the variable to know whether or not the body is sleeping
|
||||
virtual void setIsSleeping(bool isSleeping);
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class DynamicsWorld;
|
||||
};
|
||||
|
||||
// Method that return the mass of the body
|
||||
|
|
@ -328,6 +349,24 @@ 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;
|
||||
}
|
||||
|
||||
// Set the variable to know whether or not the body is sleeping
|
||||
inline void RigidBody::setIsSleeping(bool isSleeping) {
|
||||
|
||||
if (isSleeping) {
|
||||
mLinearVelocity.setToZero();
|
||||
mAngularVelocity.setToZero();
|
||||
mExternalForce.setToZero();
|
||||
mExternalTorque.setToZero();
|
||||
}
|
||||
|
||||
Body::setIsSleeping(isSleeping);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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 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);
|
||||
|
|
@ -110,6 +110,17 @@ const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15;
|
|||
/// Number of iterations when solving the position constraints of the Sequential Impulse technique
|
||||
const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
|
||||
|
||||
/// Time (in seconds) that a body must stay still to be considered sleeping
|
||||
const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
|
||||
|
||||
/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
|
||||
/// might enter sleeping mode.
|
||||
const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02);
|
||||
|
||||
/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
|
||||
/// might enter sleeping mode
|
||||
const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
|
|||
|
||||
// Compute the inverse of the initial orientation difference between the two bodies
|
||||
mInitOrientationDifferenceInv = transform2.getOrientation() *
|
||||
transform1.getOrientation().getInverse();
|
||||
transform1.getOrientation().getInverse();
|
||||
mInitOrientationDifferenceInv.normalize();
|
||||
mInitOrientationDifferenceInv.inverse();
|
||||
}
|
||||
|
|
@ -732,7 +732,9 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
|
|||
mIsMotorEnabled = isMotorEnabled;
|
||||
mImpulseMotor = 0.0;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
|
||||
// Set the minimum angle limit
|
||||
|
|
@ -770,7 +772,9 @@ void HingeJoint::resetLimits() {
|
|||
mImpulseLowerLimit = 0.0;
|
||||
mImpulseUpperLimit = 0.0;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
|
||||
// Set the motor speed
|
||||
|
|
@ -780,7 +784,9 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) {
|
|||
|
||||
mMotorSpeed = motorSpeed;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -792,7 +798,9 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
|
|||
assert(mMaxMotorTorque >= 0.0);
|
||||
mMaxMotorTorque = maxMotorTorque;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -768,7 +768,9 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
|
|||
mIsMotorEnabled = isMotorEnabled;
|
||||
mImpulseMotor = 0.0;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
|
||||
// Return the current translation value of the joint
|
||||
|
|
@ -830,7 +832,9 @@ void SliderJoint::resetLimits() {
|
|||
mImpulseLowerLimit = 0.0;
|
||||
mImpulseUpperLimit = 0.0;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
|
||||
// Set the motor speed
|
||||
|
|
@ -840,7 +844,9 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) {
|
|||
|
||||
mMotorSpeed = motorSpeed;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -852,6 +858,8 @@ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) {
|
|||
assert(mMaxMotorForce >= 0.0);
|
||||
mMaxMotorForce = maxMotorForce;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
// Wake up the two bodies of the joint
|
||||
mBody1->setIsSleeping(false);
|
||||
mBody2->setIsSleeping(false);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,18 +30,14 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
|
||||
std::vector<Vector3>& linearVelocities,
|
||||
std::vector<Vector3>& angularVelocities,
|
||||
std::vector<Vector3>& positions,
|
||||
ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
|
||||
std::vector<Quaternion>& orientations,
|
||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
||||
: mJoints(joints), mLinearVelocities(linearVelocities),
|
||||
mAngularVelocities(angularVelocities), mPositions(positions),
|
||||
: mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
|
||||
mOrientations(orientations),
|
||||
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
||||
mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities,
|
||||
angularVelocities, positions, orientations, mapBodyToVelocityIndex){
|
||||
mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
|
||||
mapBodyToVelocityIndex){
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -50,10 +46,16 @@ ConstraintSolver::~ConstraintSolver() {
|
|||
|
||||
}
|
||||
|
||||
// Initialize the constraint solver
|
||||
void ConstraintSolver::initialize(decimal dt) {
|
||||
// Initialize the constraint solver for a given island
|
||||
void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
|
||||
|
||||
PROFILE("ConstraintSolver::initialize()");
|
||||
PROFILE("ConstraintSolver::initializeForIsland()");
|
||||
|
||||
assert(mLinearVelocities != NULL);
|
||||
assert(mAngularVelocities != NULL);
|
||||
assert(island != NULL);
|
||||
assert(island->getNbBodies() > 0);
|
||||
assert(island->getNbJoints() > 0);
|
||||
|
||||
// Set the current time step
|
||||
mTimeStep = dt;
|
||||
|
|
@ -62,54 +64,50 @@ void ConstraintSolver::initialize(decimal dt) {
|
|||
mConstraintSolverData.timeStep = mTimeStep;
|
||||
mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
|
||||
|
||||
// For each joint
|
||||
std::set<Constraint*>::iterator it;
|
||||
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
|
||||
|
||||
Constraint* joint = (*it);
|
||||
|
||||
// Get the rigid bodies of the joint
|
||||
RigidBody* body1 = joint->getBody1();
|
||||
RigidBody* body2 = joint->getBody2();
|
||||
|
||||
// Add the bodies to the set of constrained bodies
|
||||
mConstraintBodies.insert(body1);
|
||||
mConstraintBodies.insert(body2);
|
||||
// For each joint of the island
|
||||
Constraint** joints = island->getJoints();
|
||||
for (uint i=0; i<island->getNbJoints(); i++) {
|
||||
|
||||
// Initialize the constraint before solving it
|
||||
joint->initBeforeSolve(mConstraintSolverData);
|
||||
joints[i]->initBeforeSolve(mConstraintSolverData);
|
||||
|
||||
// Warm-start the constraint if warm-starting is enabled
|
||||
if (mIsWarmStartingActive) {
|
||||
joint->warmstart(mConstraintSolverData);
|
||||
joints[i]->warmstart(mConstraintSolverData);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the velocity constraints
|
||||
void ConstraintSolver::solveVelocityConstraints() {
|
||||
void ConstraintSolver::solveVelocityConstraints(Island* island) {
|
||||
|
||||
PROFILE("ConstraintSolver::solveVelocityConstraints()");
|
||||
|
||||
// For each joint
|
||||
std::set<Constraint*>::iterator it;
|
||||
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
|
||||
assert(island != NULL);
|
||||
assert(island->getNbJoints() > 0);
|
||||
|
||||
// For each joint of the island
|
||||
Constraint** joints = island->getJoints();
|
||||
for (uint i=0; i<island->getNbJoints(); i++) {
|
||||
|
||||
// Solve the constraint
|
||||
(*it)->solveVelocityConstraint(mConstraintSolverData);
|
||||
joints[i]->solveVelocityConstraint(mConstraintSolverData);
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the position constraints
|
||||
void ConstraintSolver::solvePositionConstraints() {
|
||||
void ConstraintSolver::solvePositionConstraints(Island* island) {
|
||||
|
||||
PROFILE("ConstraintSolver::solvePositionConstraints()");
|
||||
|
||||
// For each joint
|
||||
std::set<Constraint*>::iterator it;
|
||||
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
|
||||
assert(island != NULL);
|
||||
assert(island->getNbJoints() > 0);
|
||||
|
||||
// For each joint of the island
|
||||
Constraint** joints = island->getJoints();
|
||||
for (uint i=0; i < island->getNbJoints(); i++) {
|
||||
|
||||
// Solve the constraint
|
||||
(*it)->solvePositionConstraint(mConstraintSolverData);
|
||||
joints[i]->solvePositionConstraint(mConstraintSolverData);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@
|
|||
#include "../configuration.h"
|
||||
#include "mathematics/mathematics.h"
|
||||
#include "../constraint/Constraint.h"
|
||||
#include "Island.h"
|
||||
#include <map>
|
||||
#include <set>
|
||||
|
||||
|
|
@ -47,11 +48,11 @@ struct ConstraintSolverData {
|
|||
/// Current time step of the simulation
|
||||
decimal timeStep;
|
||||
|
||||
/// Reference to the bodies linear velocities
|
||||
std::vector<Vector3>& linearVelocities;
|
||||
/// Array with the bodies linear velocities
|
||||
Vector3* linearVelocities;
|
||||
|
||||
/// Reference to the bodies angular velocities
|
||||
std::vector<Vector3>& angularVelocities;
|
||||
/// Array with the bodies angular velocities
|
||||
Vector3* angularVelocities;
|
||||
|
||||
/// Reference to the bodies positions
|
||||
std::vector<Vector3>& positions;
|
||||
|
|
@ -67,13 +68,11 @@ struct ConstraintSolverData {
|
|||
bool isWarmStartingActive;
|
||||
|
||||
/// Constructor
|
||||
ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
|
||||
std::vector<Vector3>& refAngularVelocities,
|
||||
std::vector<Vector3>& refPositions,
|
||||
ConstraintSolverData(std::vector<Vector3>& refPositions,
|
||||
std::vector<Quaternion>& refOrientations,
|
||||
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
|
||||
:linearVelocities(refLinearVelocities),
|
||||
angularVelocities(refAngularVelocities),
|
||||
:linearVelocities(NULL),
|
||||
angularVelocities(NULL),
|
||||
positions(refPositions), orientations(refOrientations),
|
||||
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
|
||||
|
||||
|
|
@ -156,19 +155,13 @@ class ConstraintSolver {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Reference to all the joints of the world
|
||||
std::set<Constraint*>& mJoints;
|
||||
|
||||
/// Constrained bodies
|
||||
std::set<RigidBody*> mConstraintBodies;
|
||||
|
||||
/// Reference to the array of constrained linear velocities (state of the linear velocities
|
||||
/// Array of constrained linear velocities (state of the linear velocities
|
||||
/// after solving the constraints)
|
||||
std::vector<Vector3>& mLinearVelocities;
|
||||
Vector3* mLinearVelocities;
|
||||
|
||||
/// Reference to the array of constrained angular velocities (state of the angular velocities
|
||||
/// Array of constrained angular velocities (state of the angular velocities
|
||||
/// after solving the constraints)
|
||||
std::vector<Vector3>& mAngularVelocities;
|
||||
Vector3* mAngularVelocities;
|
||||
|
||||
/// Reference to the array of bodies positions (for position error correction)
|
||||
std::vector<Vector3>& mPositions;
|
||||
|
|
@ -194,24 +187,20 @@ class ConstraintSolver {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ConstraintSolver(std::set<Constraint*>& joints,
|
||||
std::vector<Vector3>& linearVelocities,
|
||||
std::vector<Vector3>& angularVelocities,
|
||||
std::vector<Vector3>& positions,
|
||||
std::vector<Quaternion>& orientations,
|
||||
ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
|
||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
|
||||
|
||||
/// Destructor
|
||||
~ConstraintSolver();
|
||||
|
||||
/// Initialize the constraint solver
|
||||
void initialize(decimal dt);
|
||||
/// Initialize the constraint solver for a given island
|
||||
void initializeForIsland(decimal dt, Island* island);
|
||||
|
||||
/// Solve the constraints
|
||||
void solveVelocityConstraints();
|
||||
void solveVelocityConstraints(Island* island);
|
||||
|
||||
/// Solve the position constraints
|
||||
void solvePositionConstraints();
|
||||
void solvePositionConstraints(Island* island);
|
||||
|
||||
/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
|
||||
bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
|
||||
|
|
@ -219,13 +208,20 @@ class ConstraintSolver {
|
|||
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
|
||||
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
|
||||
|
||||
/// Return true if the body is in at least one constraint
|
||||
bool isConstrainedBody(RigidBody* body) const;
|
||||
/// Set the constrained velocities arrays
|
||||
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
|
||||
Vector3* constrainedAngularVelocities);
|
||||
};
|
||||
|
||||
// Return true if the body is in at least one constraint
|
||||
inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
|
||||
return mConstraintBodies.count(body) == 1;
|
||||
// Set the constrained velocities arrays
|
||||
inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
|
||||
Vector3* constrainedAngularVelocities) {
|
||||
assert(constrainedLinearVelocities != NULL);
|
||||
assert(constrainedAngularVelocities != NULL);
|
||||
mLinearVelocities = constrainedLinearVelocities;
|
||||
mAngularVelocities = constrainedAngularVelocities;
|
||||
mConstraintSolverData.linearVelocities = mLinearVelocities;
|
||||
mConstraintSolverData.angularVelocities = mAngularVelocities;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -36,18 +36,12 @@ using namespace std;
|
|||
// Constants initialization
|
||||
const decimal ContactSolver::BETA = decimal(0.2);
|
||||
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
|
||||
const decimal ContactSolver::SLOP = decimal(0.01);
|
||||
const decimal ContactSolver::SLOP= decimal(0.01);
|
||||
|
||||
// Constructor
|
||||
ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
|
||||
std::vector<Vector3>& constrainedLinearVelocities,
|
||||
std::vector<Vector3>& constrainedAngularVelocities,
|
||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
||||
:mContactManifolds(contactManifolds),
|
||||
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
|
||||
mContactConstraints(NULL),
|
||||
mLinearVelocities(constrainedLinearVelocities),
|
||||
mAngularVelocities(constrainedAngularVelocities),
|
||||
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
||||
:mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
|
||||
mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL),
|
||||
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
||||
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
|
||||
mIsSolveFrictionAtContactManifoldCenterActive(true) {
|
||||
|
|
@ -59,26 +53,32 @@ ContactSolver::~ContactSolver() {
|
|||
|
||||
}
|
||||
|
||||
// Initialize the constraint solver
|
||||
void ContactSolver::initialize(decimal dt) {
|
||||
// Initialize the constraint solver for a given island
|
||||
void ContactSolver::initializeForIsland(decimal dt, Island* island) {
|
||||
|
||||
PROFILE("ContactSolver::initialize()");
|
||||
PROFILE("ContactSolver::initializeForIsland()");
|
||||
|
||||
assert(island != NULL);
|
||||
assert(island->getNbBodies() > 0);
|
||||
assert(island->getNbContactManifolds() > 0);
|
||||
assert(mSplitLinearVelocities != NULL);
|
||||
assert(mSplitAngularVelocities != NULL);
|
||||
|
||||
// Set the current time step
|
||||
mTimeStep = dt;
|
||||
|
||||
// TODO : Use better memory allocation here
|
||||
mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
|
||||
mNbContactManifolds = island->getNbContactManifolds();
|
||||
|
||||
mNbContactManifolds = 0;
|
||||
mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
|
||||
assert(mContactConstraints != NULL);
|
||||
|
||||
// For each contact manifold of the world
|
||||
vector<ContactManifold*>::iterator it;
|
||||
for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) {
|
||||
// For each contact manifold of the island
|
||||
ContactManifold** contactManifolds = island->getContactManifold();
|
||||
for (uint i=0; i<mNbContactManifolds; i++) {
|
||||
|
||||
ContactManifold* externalManifold = *it;
|
||||
ContactManifold* externalManifold = contactManifolds[i];
|
||||
|
||||
ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds];
|
||||
ContactManifoldSolver& internalManifold = mContactConstraints[i];
|
||||
|
||||
assert(externalManifold->getNbContactPoints() > 0);
|
||||
|
||||
|
|
@ -86,10 +86,6 @@ void ContactSolver::initialize(decimal dt) {
|
|||
RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1();
|
||||
RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2();
|
||||
|
||||
// Add the two bodies of the constraint in the constraintBodies list
|
||||
mConstraintBodies.insert(body1);
|
||||
mConstraintBodies.insert(body2);
|
||||
|
||||
// Get the position of the two bodies
|
||||
Vector3 x1 = body1->getTransform().getPosition();
|
||||
Vector3 x2 = body2->getTransform().getPosition();
|
||||
|
|
@ -173,46 +169,12 @@ void ContactSolver::initialize(decimal dt) {
|
|||
internalManifold.frictionTwistImpulse = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
mNbContactManifolds++;
|
||||
}
|
||||
|
||||
// Allocated memory for split impulse velocities
|
||||
// TODO : Use better memory allocation here
|
||||
mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
|
||||
mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
|
||||
assert(mSplitLinearVelocities != NULL);
|
||||
assert(mSplitAngularVelocities != NULL);
|
||||
|
||||
assert(mConstraintBodies.size() > 0);
|
||||
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
|
||||
assert(mLinearVelocities.size() >= mConstraintBodies.size());
|
||||
assert(mAngularVelocities.size() >= mConstraintBodies.size());
|
||||
|
||||
// Initialize the split impulse velocities
|
||||
initializeSplitImpulseVelocities();
|
||||
|
||||
// Fill-in all the matrices needed to solve the LCP problem
|
||||
initializeContactConstraints();
|
||||
}
|
||||
|
||||
// Initialize the split impulse velocities
|
||||
void ContactSolver::initializeSplitImpulseVelocities() {
|
||||
|
||||
// For each current body that is implied in some constraint
|
||||
set<RigidBody*>::iterator it;
|
||||
for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
|
||||
RigidBody* rigidBody = *it;
|
||||
assert(rigidBody);
|
||||
|
||||
uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
|
||||
|
||||
// Initialize the split impulse velocities to zero
|
||||
mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
|
||||
mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize the contact constraints before solving the system
|
||||
void ContactSolver::initializeContactConstraints() {
|
||||
|
||||
|
|
@ -304,10 +266,10 @@ void ContactSolver::initializeContactConstraints() {
|
|||
// 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 under a given threshold), we don't add a restitution velocity bias
|
||||
// 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) {
|
||||
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
|
||||
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
|
||||
}
|
||||
|
||||
|
|
@ -884,18 +846,8 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
|||
// Clean up the constraint solver
|
||||
void ContactSolver::cleanup() {
|
||||
|
||||
mConstraintBodies.clear();
|
||||
|
||||
if (mContactConstraints != NULL) {
|
||||
delete[] mContactConstraints;
|
||||
mContactConstraints = NULL;
|
||||
}
|
||||
if (mSplitLinearVelocities != NULL) {
|
||||
delete[] mSplitLinearVelocities;
|
||||
mSplitLinearVelocities = NULL;
|
||||
}
|
||||
if (mSplitAngularVelocities != NULL) {
|
||||
delete[] mSplitAngularVelocities;
|
||||
mSplitAngularVelocities = NULL;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@
|
|||
#include "../configuration.h"
|
||||
#include "../constraint/Constraint.h"
|
||||
#include "ContactManifold.h"
|
||||
#include "Island.h"
|
||||
#include "Impulse.h"
|
||||
#include <map>
|
||||
#include <set>
|
||||
|
|
@ -311,9 +312,6 @@ class ContactSolver {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Reference to all the contact manifold of the world
|
||||
std::vector<ContactManifold*>& mContactManifolds;
|
||||
|
||||
/// Split linear velocities for the position contact solver (split impulse)
|
||||
Vector3* mSplitLinearVelocities;
|
||||
|
||||
|
|
@ -329,14 +327,11 @@ class ContactSolver {
|
|||
/// Number of contact constraints
|
||||
uint mNbContactManifolds;
|
||||
|
||||
/// Constrained bodies
|
||||
std::set<RigidBody*> mConstraintBodies;
|
||||
/// Array of linear velocities
|
||||
Vector3* mLinearVelocities;
|
||||
|
||||
/// Reference to the array of linear velocities
|
||||
std::vector<Vector3>& mLinearVelocities;
|
||||
|
||||
/// Reference to the array of angular velocities
|
||||
std::vector<Vector3>& mAngularVelocities;
|
||||
/// Array of angular velocities
|
||||
Vector3* mAngularVelocities;
|
||||
|
||||
/// Reference to the map of rigid body to their index in the constrained velocities array
|
||||
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
|
||||
|
|
@ -353,9 +348,6 @@ class ContactSolver {
|
|||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Initialize the split impulse velocities
|
||||
void initializeSplitImpulseVelocities();
|
||||
|
||||
/// Initialize the contact constraints before solving the system
|
||||
void initializeContactConstraints();
|
||||
|
||||
|
|
@ -403,16 +395,21 @@ class ContactSolver {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ContactSolver(std::vector<ContactManifold*>& contactManifolds,
|
||||
std::vector<Vector3>& constrainedLinearVelocities,
|
||||
std::vector<Vector3>& constrainedAngularVelocities,
|
||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
|
||||
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
|
||||
|
||||
/// Destructor
|
||||
virtual ~ContactSolver();
|
||||
|
||||
/// Initialize the constraint solver
|
||||
void initialize(decimal dt);
|
||||
/// Initialize the constraint solver for a given island
|
||||
void initializeForIsland(decimal dt, Island* island);
|
||||
|
||||
/// Set the split velocities arrays
|
||||
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
|
||||
Vector3* splitAngularVelocities);
|
||||
|
||||
/// Set the constrained velocities arrays
|
||||
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
|
||||
Vector3* constrainedAngularVelocities);
|
||||
|
||||
/// Warm start the solver.
|
||||
void warmStart();
|
||||
|
|
@ -424,24 +421,6 @@ class ContactSolver {
|
|||
/// Solve the contacts
|
||||
void solve();
|
||||
|
||||
/// Return true if the body is in at least one constraint
|
||||
bool isConstrainedBody(RigidBody* body) const;
|
||||
|
||||
/// Return the constrained linear velocity of a body after solving the constraints
|
||||
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
|
||||
|
||||
/// Return the split linear velocity
|
||||
Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
|
||||
|
||||
/// Return the constrained angular velocity of a body after solving the constraints
|
||||
Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
|
||||
|
||||
/// Return the split angular velocity
|
||||
Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
|
||||
|
||||
/// Clean up the constraint solver
|
||||
void cleanup();
|
||||
|
||||
/// Return true if the split impulses position correction technique is used for contacts
|
||||
bool isSplitImpulseActive() const;
|
||||
|
||||
|
|
@ -451,25 +430,27 @@ class ContactSolver {
|
|||
/// Activate or deactivate the solving of friction constraints at the center of
|
||||
/// the contact manifold instead of solving them at each contact point
|
||||
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
|
||||
|
||||
/// Clean up the constraint solver
|
||||
void cleanup();
|
||||
};
|
||||
|
||||
// Return true if the body is in at least one constraint
|
||||
inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
|
||||
return mConstraintBodies.count(body) == 1;
|
||||
// Set the split velocities arrays
|
||||
inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
|
||||
Vector3* splitAngularVelocities) {
|
||||
assert(splitLinearVelocities != NULL);
|
||||
assert(splitAngularVelocities != NULL);
|
||||
mSplitLinearVelocities = splitLinearVelocities;
|
||||
mSplitAngularVelocities = splitAngularVelocities;
|
||||
}
|
||||
|
||||
// Return the split linear velocity
|
||||
inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
|
||||
return mSplitLinearVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Return the split angular velocity
|
||||
inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
|
||||
return mSplitAngularVelocities[indexBody];
|
||||
// Set the constrained velocities arrays
|
||||
inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
|
||||
Vector3* constrainedAngularVelocities) {
|
||||
assert(constrainedLinearVelocities != NULL);
|
||||
assert(constrainedAngularVelocities != NULL);
|
||||
mLinearVelocities = constrainedLinearVelocities;
|
||||
mAngularVelocities = constrainedAngularVelocities;
|
||||
}
|
||||
|
||||
// Return true if the split impulses position correction technique is used for contacts
|
||||
|
|
|
|||
|
|
@ -36,15 +36,19 @@ using namespace std;
|
|||
|
||||
// Constructor
|
||||
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
||||
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
|
||||
mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
|
||||
mMapBodyToConstrainedVelocityIndex),
|
||||
mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
|
||||
mConstrainedPositions, mConstrainedOrientations,
|
||||
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
|
||||
mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
|
||||
mContactSolver(mMapBodyToConstrainedVelocityIndex),
|
||||
mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
|
||||
mMapBodyToConstrainedVelocityIndex),
|
||||
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
|
||||
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
|
||||
mIsDeactivationActive(DEACTIVATION_ENABLED) {
|
||||
mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
|
||||
mSplitAngularVelocities(NULL), mNbIslands(0), mNbIslandsCapacity(0),
|
||||
mIslands(NULL), mNbBodiesCapacity(0),
|
||||
mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
|
||||
mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
|
||||
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -59,8 +63,26 @@ DynamicsWorld::~DynamicsWorld() {
|
|||
mMemoryAllocator.release((*it).second, sizeof(OverlappingPair));
|
||||
}
|
||||
|
||||
// Free the allocated memory for the constrained velocities
|
||||
cleanupConstrainedVelocitiesArray();
|
||||
// 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);
|
||||
}
|
||||
|
||||
// Release the memory allocated for the bodies velocity arrays
|
||||
if (mNbBodiesCapacity > 0) {
|
||||
delete[] mSplitLinearVelocities;
|
||||
delete[] mSplitAngularVelocities;
|
||||
delete[] mConstrainedLinearVelocities;
|
||||
delete[] mConstrainedAngularVelocities;
|
||||
}
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
|
|
@ -93,10 +115,16 @@ 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();
|
||||
|
||||
// Compute the islands (separate groups of bodies with constraints between each others)
|
||||
computeIslands();
|
||||
|
||||
// Integrate the velocities
|
||||
integrateRigidBodiesVelocities();
|
||||
|
||||
|
|
@ -115,14 +143,10 @@ void DynamicsWorld::update() {
|
|||
// Solve the position correction for constraints
|
||||
solvePositionCorrection();
|
||||
|
||||
if (mIsSleepingEnabled) updateSleepingBodies();
|
||||
|
||||
// Update the AABBs of the bodies
|
||||
updateRigidBodiesAABB();
|
||||
|
||||
// Cleanup of the contact solver
|
||||
mContactSolver.cleanup();
|
||||
|
||||
// Cleanup the constrained velocities
|
||||
cleanupConstrainedVelocitiesArray();
|
||||
}
|
||||
|
||||
// Compute and set the interpolation factor to all the bodies
|
||||
|
|
@ -138,45 +162,47 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
|||
|
||||
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
||||
|
||||
// For each rigid body of the world
|
||||
set<RigidBody*>::iterator it;
|
||||
for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
||||
// For each island of the world
|
||||
for (uint i=0; i < mNbIslands; i++) {
|
||||
|
||||
RigidBody* rigidBody = *it;
|
||||
assert(rigidBody != NULL);
|
||||
RigidBody** bodies = mIslands[i]->getBodies();
|
||||
|
||||
// If the body is allowed to move
|
||||
if (rigidBody->getIsMotionEnabled()) {
|
||||
// For each body of the island
|
||||
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
|
||||
|
||||
// Get the constrained velocity
|
||||
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
|
||||
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
||||
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
|
||||
// If the body is allowed to move
|
||||
if (bodies[b]->getIsMotionEnabled()) {
|
||||
|
||||
// Update the linear and angular velocity of the body
|
||||
rigidBody->setLinearVelocity(newLinVelocity);
|
||||
rigidBody->setAngularVelocity(newAngVelocity);
|
||||
// Get the constrained velocity
|
||||
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
|
||||
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
||||
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
|
||||
|
||||
// Add the split impulse velocity from Contact Solver (only used to update the position)
|
||||
if (mContactSolver.isConstrainedBody(rigidBody) &&
|
||||
mContactSolver.isSplitImpulseActive()) {
|
||||
// Update the linear and angular velocity of the body
|
||||
bodies[b]->setLinearVelocity(newLinVelocity);
|
||||
bodies[b]->setAngularVelocity(newAngVelocity);
|
||||
|
||||
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
|
||||
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
|
||||
// Add the split impulse velocity from Contact Solver (only used
|
||||
// to update the position)
|
||||
if (mContactSolver.isSplitImpulseActive()) {
|
||||
|
||||
newLinVelocity += mSplitLinearVelocities[indexArray];
|
||||
newAngVelocity += mSplitAngularVelocities[indexArray];
|
||||
}
|
||||
|
||||
// Get current position and orientation of the body
|
||||
const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
|
||||
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
|
||||
|
||||
// Compute the new position of the body
|
||||
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
||||
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
|
||||
currentOrientation * decimal(0.5) * dt;
|
||||
|
||||
// Update the Transform of the body
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
bodies[b]->setTransform(newTransform);
|
||||
}
|
||||
|
||||
// Get current position and orientation of the body
|
||||
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
|
||||
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
|
||||
|
||||
// Compute the new position of the body
|
||||
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
||||
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
|
||||
currentOrientation * decimal(0.5) * dt;
|
||||
|
||||
// Update the Transform of the body
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
rigidBody->setTransform(newTransform);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -188,7 +214,7 @@ void DynamicsWorld::updateRigidBodiesAABB() {
|
|||
|
||||
// For each rigid body of the world
|
||||
set<RigidBody*>::iterator it;
|
||||
for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
|
||||
// If the body has moved
|
||||
if ((*it)->getHasMoved()) {
|
||||
|
|
@ -210,12 +236,49 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
|||
|
||||
// Set the factor to all bodies
|
||||
set<RigidBody*>::iterator it;
|
||||
for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
|
||||
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
|
||||
assert(rigidBody);
|
||||
(*it)->setInterpolationFactor(factor);
|
||||
}
|
||||
}
|
||||
|
||||
rigidBody->setInterpolationFactor(factor);
|
||||
// Initialize the bodies velocities arrays for the next simulation step.
|
||||
void DynamicsWorld::initVelocityArrays() {
|
||||
|
||||
// Allocate memory for the bodies velocity arrays
|
||||
uint nbBodies = mRigidBodies.size();
|
||||
if (mNbBodiesCapacity != nbBodies && nbBodies > 0) {
|
||||
if (mNbBodiesCapacity > 0) {
|
||||
delete[] mSplitLinearVelocities;
|
||||
delete[] mSplitAngularVelocities;
|
||||
}
|
||||
mNbBodiesCapacity = nbBodies;
|
||||
mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
|
||||
mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
|
||||
mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
|
||||
mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
|
||||
assert(mSplitLinearVelocities != NULL);
|
||||
assert(mSplitAngularVelocities != NULL);
|
||||
assert(mConstrainedLinearVelocities != NULL);
|
||||
assert(mConstrainedAngularVelocities != NULL);
|
||||
}
|
||||
|
||||
// Reset the velocities arrays
|
||||
for (uint i=0; i<mNbBodiesCapacity; i++) {
|
||||
mSplitLinearVelocities[i].setToZero();
|
||||
mSplitAngularVelocities[i].setToZero();
|
||||
}
|
||||
|
||||
// Initialize the map of body indexes in the velocity arrays
|
||||
mMapBodyToConstrainedVelocityIndex.clear();
|
||||
std::set<RigidBody*>::const_iterator it;
|
||||
uint indexBody = 0;
|
||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
|
||||
// Add the body into the map
|
||||
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*,
|
||||
uint>(*it, indexBody));
|
||||
indexBody++;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -228,66 +291,74 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
|
|||
|
||||
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
|
||||
|
||||
// TODO : Use better memory allocation here
|
||||
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||
// Initialize the bodies velocity arrays
|
||||
initVelocityArrays();
|
||||
|
||||
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
||||
|
||||
// Fill in the mapping of rigid body to their index in the constrained
|
||||
// velocities arrays
|
||||
uint i = 0;
|
||||
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
RigidBody* rigidBody = *it;
|
||||
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
|
||||
// For each island of the world
|
||||
for (uint i=0; i < mNbIslands; i++) {
|
||||
|
||||
// If the body is allowed to move
|
||||
if (rigidBody->getIsMotionEnabled()) {
|
||||
RigidBody** bodies = mIslands[i]->getBodies();
|
||||
|
||||
// Integrate the external force to get the new velocity of the body
|
||||
mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() +
|
||||
dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() +
|
||||
dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||
// For each body of the island
|
||||
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
|
||||
|
||||
// If the gravity has to be applied to this rigid body
|
||||
if (rigidBody->isGravityEnabled() && mIsGravityOn) {
|
||||
// Insert the body into the map of constrained velocities
|
||||
uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
|
||||
|
||||
// Integrate the gravity force
|
||||
mConstrainedLinearVelocities[i] += dt * rigidBody->getMassInverse() *
|
||||
rigidBody->getMass() * mGravity;
|
||||
assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
|
||||
assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
|
||||
|
||||
// If the body is allowed to move
|
||||
if (bodies[b]->getIsMotionEnabled()) {
|
||||
|
||||
// Integrate the external force to get the new velocity of the body
|
||||
mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
|
||||
dt * bodies[b]->getMassInverse() * bodies[b]->getExternalForce();
|
||||
mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
|
||||
dt * bodies[b]->getInertiaTensorInverseWorld() *
|
||||
bodies[b]->getExternalTorque();
|
||||
|
||||
// If the gravity has to be applied to this rigid body
|
||||
if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
|
||||
|
||||
// Integrate the gravity force
|
||||
mConstrainedLinearVelocities[indexBody] += dt * bodies[b]->getMassInverse() *
|
||||
bodies[b]->getMass() * mGravity;
|
||||
}
|
||||
|
||||
// Apply the velocity damping
|
||||
// Damping force : F_c = -c' * v (c=damping factor)
|
||||
// Equation : m * dv/dt = -c' * v
|
||||
// => dv/dt = -c * v (with c=c'/m)
|
||||
// => dv/dt + c * v = 0
|
||||
// Solution : v(t) = v0 * e^(-c * t)
|
||||
// => v(t + dt) = v0 * e^(-c(t + dt))
|
||||
// = v0 * e^(-ct) * e^(-c * dt)
|
||||
// = v(t) * e^(-c * dt)
|
||||
// => v2 = v1 * e^(-c * dt)
|
||||
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
|
||||
// => e^(-x) ~ 1 - x
|
||||
// => v2 = v1 * (1 - c * dt)
|
||||
decimal linDampingFactor = bodies[b]->getLinearDamping();
|
||||
decimal angDampingFactor = bodies[b]->getAngularDamping();
|
||||
decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor,
|
||||
decimal(0.0), decimal(1.0));
|
||||
decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor,
|
||||
decimal(0.0), decimal(1.0));
|
||||
mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0),
|
||||
decimal(1.0));
|
||||
mConstrainedAngularVelocities[indexBody] *= clamp(angularDamping, decimal(0.0),
|
||||
decimal(1.0));
|
||||
|
||||
// Update the old Transform of the body
|
||||
bodies[b]->updateOldTransform();
|
||||
}
|
||||
|
||||
// Apply the velocity damping
|
||||
// Damping force : F_c = -c' * v (c=damping factor)
|
||||
// Equation : m * dv/dt = -c' * v
|
||||
// => dv/dt = -c * v (with c=c'/m)
|
||||
// => dv/dt + c * v = 0
|
||||
// Solution : v(t) = v0 * e^(-c * t)
|
||||
// => v(t + dt) = v0 * e^(-c(t + dt))
|
||||
// = v0 * e^(-ct) * e^(-c * dt)
|
||||
// = v(t) * e^(-c * dt)
|
||||
// => v2 = v1 * e^(-c * dt)
|
||||
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
|
||||
// => e^(-x) ~ 1 - x
|
||||
// => v2 = v1 * (1 - c * dt)
|
||||
decimal linDampingFactor = rigidBody->getLinearDamping();
|
||||
decimal angDampingFactor = rigidBody->getAngularDamping();
|
||||
decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor,
|
||||
decimal(0.0), decimal(1.0));
|
||||
decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor,
|
||||
decimal(0.0), decimal(1.0));
|
||||
mConstrainedLinearVelocities[i] *= clamp(linearDamping, decimal(0.0), decimal(1.0));
|
||||
mConstrainedAngularVelocities[i] *= clamp(angularDamping, decimal(0.0), decimal(1.0));
|
||||
|
||||
// Update the old Transform of the body
|
||||
rigidBody->updateOldTransform();
|
||||
indexBody++;
|
||||
}
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
|
||||
}
|
||||
|
||||
// Solve the contacts and constraints
|
||||
|
|
@ -298,42 +369,59 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
|||
// Get the current time step
|
||||
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
||||
|
||||
// Check if there are contacts and constraints to solve
|
||||
bool isConstraintsToSolve = !mJoints.empty();
|
||||
bool isContactsToSolve = !mContactManifolds.empty();
|
||||
if (!isConstraintsToSolve && !isContactsToSolve) return;
|
||||
// Set the velocities arrays
|
||||
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
|
||||
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
|
||||
mConstrainedAngularVelocities);
|
||||
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
|
||||
mConstrainedAngularVelocities);
|
||||
|
||||
// ---------- Solve velocity constraints for joints and contacts ---------- //
|
||||
|
||||
// If there are contacts
|
||||
if (isContactsToSolve) {
|
||||
// For each island of the world
|
||||
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
|
||||
|
||||
// Initialize the solver
|
||||
mContactSolver.initialize(dt);
|
||||
// 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;
|
||||
|
||||
// Warm start the contact solver
|
||||
mContactSolver.warmStart();
|
||||
// If there are contacts in the current island
|
||||
if (isContactsToSolve) {
|
||||
|
||||
// Initialize the solver
|
||||
mContactSolver.initializeForIsland(dt, mIslands[islandIndex]);
|
||||
|
||||
// Warm start the contact solver
|
||||
mContactSolver.warmStart();
|
||||
}
|
||||
|
||||
// If there are constraints
|
||||
if (isConstraintsToSolve) {
|
||||
|
||||
// Initialize the constraint solver
|
||||
mConstraintSolver.initializeForIsland(dt, 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]);
|
||||
}
|
||||
|
||||
// Solve the contacts
|
||||
if (isContactsToSolve) mContactSolver.solve();
|
||||
}
|
||||
|
||||
// Cache the lambda values in order to use them in the next
|
||||
// step and cleanup the contact solver
|
||||
if (isContactsToSolve) {
|
||||
mContactSolver.storeImpulses();
|
||||
mContactSolver.cleanup();
|
||||
}
|
||||
}
|
||||
|
||||
// If there are constraints
|
||||
if (isConstraintsToSolve) {
|
||||
|
||||
// Initialize the constraint solver
|
||||
mConstraintSolver.initialize(dt);
|
||||
}
|
||||
|
||||
// For each iteration of the velocity solver
|
||||
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
|
||||
|
||||
// Solve the constraints
|
||||
if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
|
||||
|
||||
// Solve the contacts
|
||||
if (isContactsToSolve) mContactSolver.solve();
|
||||
}
|
||||
|
||||
// Cache the lambda values in order to use them in the next step
|
||||
if (isContactsToSolve) mContactSolver.storeImpulses();
|
||||
}
|
||||
|
||||
// Solve the position error correction of the constraints
|
||||
|
|
@ -349,37 +437,36 @@ void DynamicsWorld::solvePositionCorrection() {
|
|||
// TODO : Use better memory allocation here
|
||||
mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
|
||||
mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
|
||||
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
|
||||
// If it is a constrained bodies (by a joint)
|
||||
if (mConstraintSolver.isConstrainedBody(*it)) {
|
||||
// For each island of the world
|
||||
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
|
||||
|
||||
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
|
||||
// For each body of the island
|
||||
RigidBody** bodies = mIslands[islandIndex]->getBodies();
|
||||
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
|
||||
|
||||
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
|
||||
|
||||
// Get the position/orientation of the rigid body
|
||||
const Transform& transform = (*it)->getTransform();
|
||||
const Transform& transform = bodies[b]->getTransform();
|
||||
mConstrainedPositions[index] = transform.getPosition();
|
||||
mConstrainedOrientations[index]= transform.getOrientation();
|
||||
}
|
||||
}
|
||||
|
||||
// ---------- Solve the position error correction for the constraints ---------- //
|
||||
// ---------- Solve the position error correction for the constraints ---------- //
|
||||
|
||||
// For each iteration of the position (error correction) solver
|
||||
for (uint i=0; i<mNbPositionSolverIterations; i++) {
|
||||
// For each iteration of the position (error correction) solver
|
||||
for (uint i=0; i<mNbPositionSolverIterations; i++) {
|
||||
|
||||
// Solve the position constraints
|
||||
mConstraintSolver.solvePositionConstraints();
|
||||
}
|
||||
// Solve the position constraints
|
||||
mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
|
||||
}
|
||||
|
||||
// ---------- Update the position/orientation of the rigid bodies ---------- //
|
||||
// ---------- Update the position/orientation of the rigid bodies ---------- //
|
||||
|
||||
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
|
||||
|
||||
// If it is a constrained bodies (by a joint)
|
||||
if (mConstraintSolver.isConstrainedBody(*it)) {
|
||||
|
||||
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
|
||||
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
|
||||
|
||||
// Get the new position/orientation of the body
|
||||
const Vector3& newPosition = mConstrainedPositions[index];
|
||||
|
|
@ -387,22 +474,11 @@ void DynamicsWorld::solvePositionCorrection() {
|
|||
|
||||
// Update the Transform of the body
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
(*it)->setTransform(newTransform);
|
||||
bodies[b]->setTransform(newTransform);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Cleanup the constrained velocities array at each step
|
||||
void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
|
||||
|
||||
// Clear the constrained velocites
|
||||
mConstrainedLinearVelocities.clear();
|
||||
mConstrainedAngularVelocities.clear();
|
||||
|
||||
// Clear the rigid body to velocities array index mapping
|
||||
mMapBodyToConstrainedVelocityIndex.clear();
|
||||
}
|
||||
|
||||
// Create a rigid body into the physics world
|
||||
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
|
||||
const Matrix3x3& inertiaTensorLocal,
|
||||
|
|
@ -448,7 +524,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 +533,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 +609,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;
|
||||
}
|
||||
|
|
@ -545,10 +628,17 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
|
|||
mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2());
|
||||
}
|
||||
|
||||
// Wake up the two bodies of the joint
|
||||
joint->getBody1()->setIsSleeping(false);
|
||||
joint->getBody2()->setIsSleeping(false);
|
||||
|
||||
// Remove the joint from the world
|
||||
mJoints.erase(joint);
|
||||
|
||||
// Get the size in bytes of the 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);
|
||||
|
||||
size_t nbBytes = joint->getSizeInBytes();
|
||||
|
||||
// Call the destructor of the joint
|
||||
|
|
@ -558,6 +648,281 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
|
|||
mMemoryAllocator.release(joint, nbBytes);
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// Put bodies to sleep if needed.
|
||||
/// For each island, if all the bodies have been almost still for a long enough period of
|
||||
/// time, we put all the bodies of the island to sleep.
|
||||
void DynamicsWorld::updateSleepingBodies() {
|
||||
|
||||
PROFILE("DynamicsWorld::updateSleepingBodies()");
|
||||
|
||||
const decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
||||
const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
|
||||
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
|
||||
|
||||
// For each island of the world
|
||||
for (uint i=0; i<mNbIslands; i++) {
|
||||
|
||||
decimal minSleepTime = DECIMAL_LARGEST;
|
||||
|
||||
// For each body of the island
|
||||
RigidBody** bodies = mIslands[i]->getBodies();
|
||||
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
|
||||
|
||||
// Skip static bodies
|
||||
if (!bodies[b]->getIsMotionEnabled()) continue;
|
||||
|
||||
// If the body is velocity is large enough to stay awake
|
||||
if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||
|
||||
bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare ||
|
||||
!bodies[b]->isAllowedToSleep()) {
|
||||
|
||||
// Reset the sleep time of the body
|
||||
bodies[b]->mSleepTime = decimal(0.0);
|
||||
minSleepTime = decimal(0.0);
|
||||
}
|
||||
else { // If the body velocity is bellow the sleeping velocity threshold
|
||||
|
||||
// Increase the sleep time
|
||||
bodies[b]->mSleepTime += dt;
|
||||
if (bodies[b]->mSleepTime < minSleepTime) {
|
||||
minSleepTime = bodies[b]->mSleepTime;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If the velocity of all the bodies of the island is under the
|
||||
// sleeping velocity threshold for a period of time larger than
|
||||
// the time required to become a sleeping body
|
||||
if (minSleepTime >= mTimeBeforeSleep) {
|
||||
|
||||
// Put all the bodies of the island to sleep
|
||||
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
|
||||
bodies[b]->setIsSleeping(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Notify the world about a new broad-phase overlapping pair
|
||||
void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
|
||||
|
||||
|
|
@ -565,8 +930,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 +969,25 @@ 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);
|
||||
}
|
||||
|
||||
// Enable/Disable the sleeping technique
|
||||
void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
|
||||
mIsSleepingEnabled = isSleepingEnabled;
|
||||
|
||||
if (!mIsSleepingEnabled) {
|
||||
|
||||
// For each body of the world
|
||||
std::set<RigidBody*>::iterator it;
|
||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
|
||||
// Wake up the rigid body
|
||||
(*it)->setIsSleeping(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -81,15 +83,21 @@ class DynamicsWorld : public CollisionWorld {
|
|||
Vector3 mGravity;
|
||||
|
||||
/// True if the gravity force is on
|
||||
bool mIsGravityOn;
|
||||
bool mIsGravityEnabled;
|
||||
|
||||
/// Array of constrained linear velocities (state of the linear velocities
|
||||
/// after solving the constraints)
|
||||
std::vector<Vector3> mConstrainedLinearVelocities;
|
||||
Vector3* mConstrainedLinearVelocities;
|
||||
|
||||
/// Array of constrained angular velocities (state of the angular velocities
|
||||
/// after solving the constraints)
|
||||
std::vector<Vector3> mConstrainedAngularVelocities;
|
||||
Vector3* mConstrainedAngularVelocities;
|
||||
|
||||
/// Split linear velocities for the position contact solver (split impulse)
|
||||
Vector3* mSplitLinearVelocities;
|
||||
|
||||
/// Split angular velocities for the position contact solver (split impulse)
|
||||
Vector3* mSplitAngularVelocities;
|
||||
|
||||
/// Array of constrained rigid bodies position (for position error correction)
|
||||
std::vector<Vector3> mConstrainedPositions;
|
||||
|
|
@ -100,6 +108,28 @@ 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;
|
||||
|
||||
/// Current allocated capacity for the bodies
|
||||
uint mNbBodiesCapacity;
|
||||
|
||||
/// Sleep linear velocity threshold
|
||||
decimal mSleepLinearVelocity;
|
||||
|
||||
/// Sleep angular velocity threshold
|
||||
decimal mSleepAngularVelocity;
|
||||
|
||||
/// Time (in seconds) before a body is put to sleep if its velocity
|
||||
/// becomes smaller than the sleep velocity.
|
||||
decimal mTimeBeforeSleep;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
|
|
@ -121,6 +151,9 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Compute and set the interpolation factor to all bodies
|
||||
void setInterpolationFactorToAllBodies();
|
||||
|
||||
/// Initialize the bodies velocities arrays for the next simulation step.
|
||||
void initVelocityArrays();
|
||||
|
||||
/// Integrate the velocities of rigid bodies.
|
||||
void integrateRigidBodiesVelocities();
|
||||
|
||||
|
|
@ -136,6 +169,12 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Reset the boolean movement variable of each body
|
||||
void resetBodiesMovementVariable();
|
||||
|
||||
/// Compute the islands of awake bodies.
|
||||
void computeIslands();
|
||||
|
||||
/// Put bodies to sleep if needed.
|
||||
void updateSleepingBodies();
|
||||
|
||||
/// Update the overlapping pair
|
||||
virtual void updateOverlappingPair(const BroadPhasePair* pair);
|
||||
|
||||
|
|
@ -198,14 +237,25 @@ 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;
|
||||
|
||||
/// Return if the gravity is on
|
||||
bool getIsGravityOn() const;
|
||||
bool isGravityEnabled() const;
|
||||
|
||||
/// Set the isGravityOn attribute
|
||||
void setIsGratityOn(bool isGravityOn);
|
||||
/// Enable/Disable the gravity
|
||||
void setIsGratityEnabled(bool isGravityEnabled);
|
||||
|
||||
/// Return the number of rigid bodies in the world
|
||||
uint getNbRigidBodies() const;
|
||||
|
|
@ -227,6 +277,36 @@ public :
|
|||
|
||||
/// Return a reference to the contact manifolds of the world
|
||||
const std::vector<ContactManifold*>& getContactManifolds() const;
|
||||
|
||||
/// Return true if the sleeping technique is enabled
|
||||
bool isSleepingEnabled() const;
|
||||
|
||||
/// Enable/Disable the sleeping technique
|
||||
void enableSleeping(bool isSleepingEnabled);
|
||||
|
||||
/// Return the current sleep linear velocity
|
||||
decimal getSleepLinearVelocity() const;
|
||||
|
||||
/// Set the sleep linear velocity.
|
||||
void setSleepLinearVelocity(decimal sleepLinearVelocity);
|
||||
|
||||
/// Return the current sleep angular velocity
|
||||
decimal getSleepAngularVelocity() const;
|
||||
|
||||
/// Set the sleep angular velocity.
|
||||
void setSleepAngularVelocity(decimal sleepAngularVelocity);
|
||||
|
||||
/// Return the time a body is required to stay still before sleeping
|
||||
decimal getTimeBeforeSleep() const;
|
||||
|
||||
/// Set the time a body is required to stay still before sleeping
|
||||
void setTimeBeforeSleep(decimal timeBeforeSleep);
|
||||
|
||||
// TODO : REMOVE THIS
|
||||
Island** getIslands() { return mIslands;}
|
||||
|
||||
// TODO : REMOVE THIS
|
||||
uint getNbIslands() const {return mNbIslands;}
|
||||
};
|
||||
|
||||
// Start the physics simulation
|
||||
|
|
@ -306,14 +386,14 @@ inline Vector3 DynamicsWorld::getGravity() const {
|
|||
return mGravity;
|
||||
}
|
||||
|
||||
// Return if the gravity is on
|
||||
inline bool DynamicsWorld::getIsGravityOn() const {
|
||||
return mIsGravityOn;
|
||||
// Return if the gravity is enaled
|
||||
inline bool DynamicsWorld::isGravityEnabled() const {
|
||||
return mIsGravityEnabled;
|
||||
}
|
||||
|
||||
// Set the isGravityOn attribute
|
||||
inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
|
||||
mIsGravityOn = isGravityOn;
|
||||
// Enable/Disable the gravity
|
||||
inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) {
|
||||
mIsGravityEnabled = isGravityEnabled;
|
||||
}
|
||||
|
||||
// Return the number of rigid bodies in the world
|
||||
|
|
@ -351,6 +431,51 @@ inline long double DynamicsWorld::getPhysicsTime() const {
|
|||
return mTimer.getPhysicsTime();
|
||||
}
|
||||
|
||||
// Return true if the sleeping technique is enabled
|
||||
inline bool DynamicsWorld::isSleepingEnabled() const {
|
||||
return mIsSleepingEnabled;
|
||||
}
|
||||
|
||||
// Return the current sleep linear velocity
|
||||
inline decimal DynamicsWorld::getSleepLinearVelocity() const {
|
||||
return mSleepLinearVelocity;
|
||||
}
|
||||
|
||||
// Set the sleep linear velocity.
|
||||
/// When the velocity of a body becomes smaller than the sleep linear/angular
|
||||
/// velocity for a given amount of time, the body starts sleeping and does not need
|
||||
/// to be simulated anymore.
|
||||
inline void DynamicsWorld::setSleepLinearVelocity(decimal sleepLinearVelocity) {
|
||||
assert(sleepLinearVelocity >= decimal(0.0));
|
||||
mSleepLinearVelocity = sleepLinearVelocity;
|
||||
}
|
||||
|
||||
// Return the current sleep angular velocity
|
||||
inline decimal DynamicsWorld::getSleepAngularVelocity() const {
|
||||
return mSleepAngularVelocity;
|
||||
}
|
||||
|
||||
// Set the sleep angular velocity.
|
||||
/// When the velocity of a body becomes smaller than the sleep linear/angular
|
||||
/// velocity for a given amount of time, the body starts sleeping and does not need
|
||||
/// to be simulated anymore.
|
||||
inline void DynamicsWorld::setSleepAngularVelocity(decimal sleepAngularVelocity) {
|
||||
assert(sleepAngularVelocity >= decimal(0.0));
|
||||
mSleepAngularVelocity = sleepAngularVelocity;
|
||||
}
|
||||
|
||||
// Return the time a body is required to stay still before sleeping
|
||||
inline decimal DynamicsWorld::getTimeBeforeSleep() const {
|
||||
return mTimeBeforeSleep;
|
||||
}
|
||||
|
||||
|
||||
// Set the time a body is required to stay still before sleeping
|
||||
inline void DynamicsWorld::setTimeBeforeSleep(decimal timeBeforeSleep) {
|
||||
assert(timeBeforeSleep >= decimal(0.0));
|
||||
mTimeBeforeSleep = timeBeforeSleep;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
54
src/engine/Island.cpp
Normal file
54
src/engine/Island.cpp
Normal 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
186
src/engine/Island.h
Normal 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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user