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

Correction of a bug with the contact caching

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@376 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-08-18 20:28:43 +00:00
parent 5277426ff8
commit 756c050890
3 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -23,6 +23,7 @@
#include "../body/RigidBody.h"
using namespace reactphysics3d;
using namespace std;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
@ -41,7 +42,7 @@ void ConstraintSolver::allocate() {
Constraint* constraint;
// For each constraint
std::vector<Constraint*>::iterator it;
vector<Constraint*>::iterator it;
for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) {
constraint = *it;
@ -57,8 +58,8 @@ void ConstraintSolver::allocate() {
constraintBodies.insert(constraint->getBody2());
// Fill in the body number maping
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
// Update the size of the jacobian matrix
nbConstraints += (1 + constraint->getNbAuxConstraints());
@ -174,7 +175,7 @@ void ConstraintSolver::fillInMatrices() {
Vector v(6);
Vector f(6);
uint b=0;
for (std::set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
body = *it;
uint bodyNumber = bodyNumberMapping.at(body);
@ -222,6 +223,7 @@ void ConstraintSolver::freeMemory() {
delete[] bodyMapping[i];
delete[] J_sp[i];
}
delete[] bodyMapping;
delete[] J_sp;
delete[] B_sp[0];
@ -299,7 +301,7 @@ void ConstraintSolver::updateContactCache() {
Contact* contact = dynamic_cast<Contact*>(activeConstraints.at(c));
if (contact) {
// Create a new ContactCachingInfo
ContactCachingInfo contactInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint));
ContactCachingInfo* contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint));
// Add it to the contact cache
contactCache.addContactCachingInfo(contactInfo);

18
sources/reactphysics3d/engine/ContactCache.cpp
View File

@ -40,17 +40,17 @@ void ContactCache::clear() {
}
// Add a new contact caching info in the cache
void ContactCache::addContactCachingInfo(const ContactCachingInfo& info) {
void ContactCache::addContactCachingInfo(ContactCachingInfo* info) {
// Check if there is already an entry for that pair of body in the cache
map<pair<Body*, Body*>, vector<ContactCachingInfo> >::iterator entry = cache.find(make_pair(info.body1, info.body2));
map<pair<Body*, Body*>, vector<ContactCachingInfo*> >::iterator entry = cache.find(make_pair(info->body1, info->body2));
if (entry != cache.end()) {
(*entry).second.push_back(info);
}
else {
// Add a new entry in the cache
vector<ContactCachingInfo> vec;
vector<ContactCachingInfo*> vec;
vec.push_back(info);
cache.insert(make_pair(make_pair(info.body1, info.body2), vec));
cache.insert(make_pair(make_pair(info->body1, info->body2), vec));
}
}
@ -59,9 +59,9 @@ void ContactCache::addContactCachingInfo(const ContactCachingInfo& info) {
// compatible (with approximatively the same position), this method returns NULL.
ContactCachingInfo* ContactCache::getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position) {
// Check if there is an entry for that pair of body in the cache
map<pair<Body*, Body*>, vector<ContactCachingInfo> >::iterator entry = cache.find(make_pair(body1, body2));
map<pair<Body*, Body*>, vector<ContactCachingInfo*> >::iterator entry = cache.find(make_pair(body1, body2));
if (entry != cache.end()) {
vector<ContactCachingInfo> vec = (*entry).second;
vector<ContactCachingInfo*> vec = (*entry).second;
assert((*entry).first.first == body1);
assert((*entry).first.second == body2);
@ -70,13 +70,13 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Body* body1, Body* body2
double posZ = position.getZ();
// Check among all the old contacts for this pair of body if there is an approximative match for the contact position
for(vector<ContactCachingInfo>::iterator it = vec.begin(); it != vec.end(); it++) {
Vector3D& contactPos = (*it).position;
for(vector<ContactCachingInfo*>::iterator it = vec.begin(); it != vec.end(); it++) {
Vector3D& contactPos = (*it)->position;
if (posX <= contactPos.getX() + POSITION_TOLERANCE && posX >= contactPos.getX() - POSITION_TOLERANCE &&
posY <= contactPos.getY() + POSITION_TOLERANCE && posY >= contactPos.getY() - POSITION_TOLERANCE &&
posZ <= contactPos.getZ() + POSITION_TOLERANCE && posZ >= contactPos.getZ() - POSITION_TOLERANCE) {
// Return the ContactCachingInfo
return &(*it);
return *it;
}
}
}

12
sources/reactphysics3d/engine/ContactCache.h
View File

@ -42,14 +42,14 @@ const double POSITION_TOLERANCE = 0.01 ; // Tolerance used to consider two
*/
class ContactCache {
private:
std::map<std::pair<Body*, Body*>, std::vector<ContactCachingInfo> > cache;
std::map<std::pair<Body*, Body*>, std::vector<ContactCachingInfo*> > cache;
public:
ContactCache(); // Constructor
~ContactCache(); // Destructor
void clear(); // Remove all the contact caching info of the cache
void addContactCachingInfo(const ContactCachingInfo& contactCachingInfo); // Add a new contact caching info in the cache
ContactCachingInfo* getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position); // Return the ContactCachingInfo (if exists) corresponding to the arguments
ContactCache(); // Constructor
~ContactCache(); // Destructor
void clear(); // Remove all the contact caching info of the cache
void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache
ContactCachingInfo* getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position); // Return the ContactCachingInfo (if exists) corresponding to the arguments
};
} // End of the ReactPhysics3D namespace