diff --git a/src/configuration.h b/src/configuration.h index a5be511f..fd26fb72 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -79,8 +79,8 @@ const reactphysics3d::decimal OBJECT_MARGIN = 0.04; // Friction coefficient const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4; -// Distance threshold for two contact points for a valid persistent contact -const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; +// Distance threshold for two contact points for a valid persistent contact (in meters) +const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03; // Maximum number of bodies const int NB_MAX_BODIES = 100000; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index cd98bb64..97c95107 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -34,9 +34,7 @@ using namespace std; // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) - :world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS), - nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION), - isErrorCorrectionActive(false) { + :world(world), nbConstraints(0), nbIterations(10) { } @@ -47,43 +45,56 @@ ConstraintSolver::~ConstraintSolver() { // Initialize the constraint solver before each solving void ConstraintSolver::initialize() { - Constraint* constraint; nbConstraints = 0; - nbConstraintsError = 0; + + // TOOD : Use better allocation here + mContactConstraints = new ContactConstraint[world->getNbContactConstraints()]; + + uint nbContactConstraints = 0; // For each constraint - vector::iterator it; - for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { - constraint = *it; + vector::iterator it; + for (it = world->getContactConstraintsBeginIterator(); it != world->getContactConstraintsEndIterator(); ++it) { + Contact* contact = *it; // If the constraint is active - if (constraint->isActive()) { - activeConstraints.push_back(constraint); + if (contact->isActive()) { + + RigidBody* body1 = contact->getBody1(); + RigidBody* body2 = contact->getBody2(); + + activeConstraints.push_back(contact); // Add the two bodies of the constraint in the constraintBodies list - constraintBodies.insert(constraint->getBody1()); - constraintBodies.insert(constraint->getBody2()); + mConstraintBodies.insert(body1); + mConstraintBodies.insert(body2); // Fill in the body number maping - bodyNumberMapping.insert(pair(constraint->getBody1(), bodyNumberMapping.size())); - bodyNumberMapping.insert(pair(constraint->getBody2(), bodyNumberMapping.size())); + mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size())); + mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size())); // Update the size of the jacobian matrix - nbConstraints += constraint->getNbConstraints(); + nbConstraints += contact->getNbConstraints(); - // Update the size of the jacobian matrix for error correction projection - if (constraint->getType() == CONTACT) { - nbConstraintsError++; - } + ContactConstraint constraint = mContactConstraints[nbContactConstraints]; + constraint.indexBody1 = mMapBodyToIndex[body1]; + constraint.indexBody2 = mMapBodyToIndex[body2]; + constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + constraint.isBody1Moving = body1->getIsMotionEnabled(); + constraint.isBody2Moving = body2->getIsMotionEnabled(); + constraint.massInverseBody1 = body1->getMassInverse(); + constraint.massInverseBody2 = body2->getMassInverse(); + + nbContactConstraints++; } } // Compute the number of bodies that are part of some active constraint - nbBodies = bodyNumberMapping.size(); + nbBodies = mMapBodyToIndex.size(); assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS); - assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS); assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES); } @@ -95,7 +106,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) { // For each active constraint int noConstraint = 0; - int noConstraintError = 0; for (int c=0; cgetType() == CONTACT) { Contact* contact = dynamic_cast(constraint); - decimal penetrationDepth = contact->getPenetrationDepth(); - // If error correction with projection is active - if (isErrorCorrectionActive) { - - // Fill in the error correction projection parameters - lowerBoundsError[noConstraintError] = lowerBounds[noConstraint]; - upperBoundsError[noConstraintError] = upperBounds[noConstraint]; - for (int i=0; i<12; i++) { - J_spError[noConstraintError][i] = J_sp[noConstraint][i]; - } - bodyMappingError[noConstraintError][0] = constraint->getBody1(); - bodyMappingError[noConstraintError][1] = constraint->getBody2(); - penetrationDepths[noConstraintError] = contact->getPenetrationDepth(); - - // If the penetration depth is small - if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { - // Use the Baumgarte error correction term for contacts instead of - // first order world projection - errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); - } - - noConstraintError++; - } - else { // If error correction with projection is not active - // Add the Baumgarte error correction term for contacts - errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); + // Add the Baumgarte error correction term for contacts + decimal slop = 0.005; + if (contact->getPenetrationDepth() > slop) { + errorValues[noConstraint] += 0.2 * oneOverDt * std::max(double(contact->getPenetrationDepth() - slop), 0.0); } } @@ -160,11 +148,11 @@ void ConstraintSolver::fillInMatrices(decimal dt) { // For each current body that is implied in some constraint RigidBody* rigidBody; - Body* body; + RigidBody* body; uint b=0; - for (set::iterator it = constraintBodies.begin(); it != constraintBodies.end(); ++it, b++) { + for (set::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it, b++) { body = *it; - uint bodyNumber = bodyNumberMapping[body]; + uint bodyNumber = mMapBodyToIndex[body]; // TODO : Use polymorphism and remove this downcasting rigidBody = dynamic_cast(body); @@ -189,16 +177,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) { Vconstraint[bodyIndexArray + 4] = 0.0; Vconstraint[bodyIndexArray + 5] = 0.0; - // Compute the vector Vconstraint with final constraint velocities - if (isErrorCorrectionActive) { - VconstraintError[bodyIndexArray] = 0.0; - VconstraintError[bodyIndexArray + 1] = 0.0; - VconstraintError[bodyIndexArray + 2] = 0.0; - VconstraintError[bodyIndexArray + 3] = 0.0; - VconstraintError[bodyIndexArray + 4] = 0.0; - VconstraintError[bodyIndexArray + 5] = 0.0; - } - // Compute the vector with forces and torques values Vector3 externalForce = rigidBody->getExternalForce(); Vector3 externalTorque = rigidBody->getExternalTorque(); @@ -232,8 +210,8 @@ void ConstraintSolver::computeVectorB(decimal dt) { b[c] = errorValues[c] * oneOverDT; // Substract 1.0/dt*J*V to the vector b - indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; - indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; + indexBody1 = mMapBodyToIndex[bodyMapping[c][0]]; + indexBody2 = mMapBodyToIndex[bodyMapping[c][1]]; decimal multiplication = 0.0; int body1ArrayIndex = 6 * indexBody1; int body2ArrayIndex = 6 * indexBody2; @@ -268,22 +246,6 @@ void ConstraintSolver::computeVectorB(decimal dt) { } } -// Compute the vector b for error correction projection -void ConstraintSolver::computeVectorBError(decimal dt) { - decimal oneOverDT = 1.0 / dt; - - for (uint c = 0; c= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { - bError[c] = penetrationDepths[c] * oneOverDT; - } - else { - bError[c] = 0.0; - } - } -} - // Compute the matrix B_sp void ConstraintSolver::computeMatrixB_sp() { uint indexConstraintArray, indexBody1, indexBody2; @@ -292,8 +254,8 @@ void ConstraintSolver::computeMatrixB_sp() { for (uint c = 0; c activeConstraints; // Current active constraints in the physics world bool isErrorCorrectionActive; // True if error correction (with world order) is active - uint nbIterationsLCP; // Number of iterations of the LCP solver + uint nbIterations; // Number of iterations of the LCP solver uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction uint nbConstraints; // Total number of constraints (with the auxiliary constraints) uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints) uint nbBodies; // Current number of bodies in the physics world - std::set constraintBodies; // Bodies that are implied in some constraint - std::map bodyNumberMapping; // Map a body pointer with its index number - Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference + RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains // the pointer to the body that correspond to the 1x6 J_ij matrix in the // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector - Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint) // a 1x6 matrix @@ -112,89 +154,68 @@ class ConstraintSolver { decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques // Each cell contains a 6x1 vector with external force and torque. + + // Contact constraints + ContactConstraint* mContactConstraints; + + // Constrained bodies + std::set mConstraintBodies; + + // Map body to index + std::map mMapBodyToIndex; + + void initialize(); // Initialize the constraint solver before each solving void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem void computeVectorB(decimal dt); // Compute the vector b - void computeVectorBError(decimal dt); // Compute the vector b for error correction projection void computeMatrixB_sp(); // Compute the matrix B_sp - void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection void computeVectorVconstraint(decimal dt); // Compute the vector V2 - void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector void computeVectorA(); // Compute the vector a used in the solve() method - void computeVectorAError(); // Same as computeVectorA() but for error correction projection void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm - void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection public: ConstraintSolver(DynamicsWorld* world); // Constructor virtual ~ConstraintSolver(); // Destructor void solve(decimal dt); // Solve the current LCP problem - bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint - Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem - Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem - Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction - Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction + bool isConstrainedBody(RigidBody* body) const; // Return true if the body is in at least one constraint + Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); // Return the constrained linear velocity of a body after solving the LCP problem + Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); // Return the constrained angular velocity of a body after solving the LCP problem void cleanup(); // Cleanup of the constraint solver void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver - void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value }; // Return true if the body is in at least one constraint -inline bool ConstraintSolver::isConstrainedBody(Body* body) const { - if(constraintBodies.find(body) != constraintBodies.end()) { - return true; - } - return false; +inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { + return mConstraintBodies.count(body) == 1; } // Return the constrained linear velocity of a body after solving the LCP problem -inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { +inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; + uint indexBodyArray = 6 * mMapBodyToIndex[body]; return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]); } // Return the constrained angular velocity of a body after solving the LCP problem -inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { +inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; + uint indexBodyArray = 6 * mMapBodyToIndex[body]; return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]); } -// Return the constrained linear velocity of a body after solving the LCP problem for error correction -inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) { - //assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]); -} - -// Return the constrained angular velocity of a body after solving the LCP problem for error correction -inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) { - //assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]); -} - // Cleanup of the constraint solver inline void ConstraintSolver::cleanup() { - bodyNumberMapping.clear(); - constraintBodies.clear(); + mMapBodyToIndex.clear(); + mConstraintBodies.clear(); activeConstraints.clear(); } // Set the number of iterations of the LCP solver inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { - nbIterationsLCP = nbIterations; + nbIterations = nbIterations; } - -// Set the isErrorCorrectionActive value -inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { - this->isErrorCorrectionActive = isErrorCorrectionActive; -} - - // Solve the current LCP problem inline void ConstraintSolver::solve(decimal dt) { @@ -206,30 +227,18 @@ inline void ConstraintSolver::solve(decimal dt) { // Compute the vector b computeVectorB(dt); - if (isErrorCorrectionActive) { - computeVectorBError(dt); - } // Compute the matrix B computeMatrixB_sp(); - if (isErrorCorrectionActive) { - computeMatrixB_spErrorCorrection(); - } // Solve the LCP problem (computation of lambda) solveLCP(); - if (isErrorCorrectionActive) { - solveLCPErrorCorrection(); - } // Cache the lambda values in order to use them in the next step cacheLambda(); // Compute the vector Vconstraint computeVectorVconstraint(dt); - if (isErrorCorrectionActive) { - computeVectorVconstraintError(dt); - } } } // End of ReactPhysics3D namespace diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 1c54d39b..35a5359f 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -65,7 +65,8 @@ void DynamicsWorld::update() { existCollision = false; - removeAllContactConstraints(); + // Remove all contact constraints + mContactConstraints.clear(); // Compute the collision detection if (mCollisionDetection.computeCollisionDetection()) { @@ -107,8 +108,6 @@ void DynamicsWorld::updateAllBodiesMotion() { decimal dt = mTimer.getTimeStep(); Vector3 newLinearVelocity; Vector3 newAngularVelocity; - Vector3 linearVelocityErrorCorrection; - Vector3 angularVelocityErrorCorrection; // For each body of thephysics world for (set::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { @@ -120,17 +119,12 @@ void DynamicsWorld::updateAllBodiesMotion() { if (rigidBody->getIsMotionEnabled()) { newLinearVelocity.setAllValues(0.0, 0.0, 0.0); newAngularVelocity.setAllValues(0.0, 0.0, 0.0); - linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); - angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); // If it's a constrained body if (mConstraintSolver.isConstrainedBody(*it)) { // Get the constrained linear and angular velocities from the constraint solver newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it); newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it); - - linearVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody); - angularVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody); } // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the @@ -143,8 +137,7 @@ void DynamicsWorld::updateAllBodiesMotion() { newAngularVelocity += rigidBody->getAngularVelocity(); // Update the position and the orientation of the body according to the new velocity - updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity, - linearVelocityErrorCorrection, angularVelocityErrorCorrection); + updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); // Update the AABB of the rigid body rigidBody->updateAABB(); @@ -155,8 +148,9 @@ void DynamicsWorld::updateAllBodiesMotion() { // Update the position and orientation of a body // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // orientation of the body -void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity, - const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) { +void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, + const Vector3& newLinVelocity, + const Vector3& newAngVelocity) { decimal dt = mTimer.getTimeStep(); assert(rigidBody); @@ -171,13 +165,9 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con // Get current position and orientation of the body const Vector3& currentPosition = rigidBody->getTransform().getPosition(); const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); - - // Error correction projection - Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection; - Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt; - - Vector3 newPosition = correctedPosition + newLinVelocity * dt; - Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt; + + Vector3 newPosition = currentPosition + newLinVelocity * dt; + Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; Transform newTransform(newPosition, newOrientation.getUnit()); rigidBody->setTransform(newTransform); @@ -262,26 +252,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mMemoryPoolRigidBodies.freeObject(rigidBody); } -// Remove all collision contacts constraints -// TODO : This method should be in the collision detection class -void DynamicsWorld::removeAllContactConstraints() { - // For all constraints - for (vector::iterator it = mConstraints.begin(); it != mConstraints.end(); ) { - - // Try a downcasting - Contact* contact = dynamic_cast(*it); - - // If the constraint is a contact - if (contact) { - // Remove it from the constraints of the physics world - it = mConstraints.erase(it); - } - else { - ++it; - } - } -} - // Remove all constraints in the physics world void DynamicsWorld::removeAllConstraints() { mConstraints.clear(); @@ -335,6 +305,6 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const // Add all the contacts in the contact cache of the two bodies // to the set of constraints in the physics world for (uint i=0; igetNbContacts(); i++) { - addConstraint(overlappingPair->getContact(i)); + mContactConstraints.push_back(overlappingPair->getContact(i)); } } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index e31d6fef..84e8122e 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld { // All the rigid bodies of the physics world std::set mRigidBodies; - // List that contains all the current constraints + // All the contact constraints + std::vector mContactConstraints; + + // All the constraints (except contact constraints) std::vector mConstraints; // Gravity vector of the world @@ -93,9 +96,7 @@ class DynamicsWorld : public CollisionWorld { // Update the position and orientation of a body void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, - const Vector3& newAngVelocity, - const Vector3& linearVelocityErrorCorrection, - const Vector3& angularVelocityErrorCorrection); + const Vector3& newAngVelocity); // Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); @@ -166,18 +167,24 @@ public : // Remove a constraint void removeConstraint(Constraint* constraint); - // Remove all collision contacts constraints - void removeAllContactConstraints(); - // Remove all constraints and delete them (free their memory) void removeAllConstraints(); + // Return the number of contact constraints in the world + uint getNbContactConstraints() const; + // Return a start iterator on the constraint list std::vector::iterator getConstraintsBeginIterator(); // Return a end iterator on the constraint list std::vector::iterator getConstraintsEndIterator(); + // Return a start iterator on the contact constraint list + std::vector::iterator getContactConstraintsBeginIterator(); + + // Return a end iterator on the contact constraint list + std::vector::iterator getContactConstraintsEndIterator(); + // Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -202,11 +209,6 @@ inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) { mConstraintSolver.setNbLCPIterations(nbIterations); } -// Set the isErrorCorrectionActive value -inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { - mConstraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive); -} - // Reset the boolean movement variable of each body inline void DynamicsWorld::resetBodiesMovementVariable() { @@ -275,6 +277,11 @@ inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() return mRigidBodies.end(); } +// Return the number of contact constraints in the world +inline uint DynamicsWorld::getNbContactConstraints() const { + return mContactConstraints.size(); +} + // Return a start iterator on the constraint list inline std::vector::iterator DynamicsWorld::getConstraintsBeginIterator() { return mConstraints.begin(); @@ -285,6 +292,16 @@ inline std::vector::iterator DynamicsWorld::getConstraintsEndIterat return mConstraints.end(); } +// Return a start iterator on the contact constraint list +inline std::vector::iterator DynamicsWorld::getContactConstraintsBeginIterator() { + return mContactConstraints.begin(); +} + +// Return a end iterator on the contact constraint list +inline std::vector::iterator DynamicsWorld::getContactConstraintsEndIterator() { + return mContactConstraints.end(); +} + } #endif diff --git a/src/engine/PersistentContactCache.cpp b/src/engine/PersistentContactCache.cpp index 36bdbc45..49b7490d 100644 --- a/src/engine/PersistentContactCache.cpp +++ b/src/engine/PersistentContactCache.cpp @@ -41,19 +41,22 @@ PersistentContactCache::~PersistentContactCache() { // Add a contact in the cache void PersistentContactCache::addContact(Contact* contact) { - - int indexNewContact = mNbContacts; // For contact already in the cache for (uint i=0; igetLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) { - // Delete the new contact - contact->Contact::~Contact(); - mMemoryPoolContacts.freeObject(contact); - - return; + // already in the cache. + decimal distance = (mContacts[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare(); + if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { + + // Delete the new contact + contact->Contact::~Contact(); + mMemoryPoolContacts.freeObject(contact); + //removeContact(i); + + return; + //break; } } @@ -62,11 +65,10 @@ void PersistentContactCache::addContact(Contact* contact) { int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); removeContact(indexToRemove); - indexNewContact = indexToRemove; } // Add the new contact in the cache - mContacts[indexNewContact] = contact; + mContacts[mNbContacts] = contact; mNbContacts++; } @@ -104,21 +106,28 @@ void PersistentContactCache::update(const Transform& transform1, const Transform mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal())); } + const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * + PERSISTENT_CONTACT_DIST_THRESHOLD; + // Remove the contacts that don't represent very well the persistent contact for (int i=mNbContacts-1; i>=0; i--) { assert(i>= 0 && i < mNbContacts); + + // Compute the distance between contact points in the normal direction + decimal distanceNormal = -mContacts[i]->getPenetrationDepth(); - // Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore) - if (mContacts[i]->getPenetrationDepth() <= 0.0) { + // If the contacts points are too far from each other in the normal direction + if (distanceNormal > squarePersistentContactThreshold) { removeContact(i); } else { - // Compute the distance of the two contact points in the place orthogonal to the contact normal - Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth(); + // Compute the distance of the two contact points in the plane orthogonal to the contact normal + Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() + + mContacts[i]->getNormal() * distanceNormal; Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1; // If the orthogonal distance is larger than the valid distance threshold, we remove the contact - if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) { + if (projDifference.lengthSquare() > squarePersistentContactThreshold) { removeContact(i); } } @@ -224,4 +233,4 @@ void PersistentContactCache::clear() { mMemoryPoolContacts.freeObject(mContacts[i]); } mNbContacts = 0; -} \ No newline at end of file +}