Why some objects are traversed more than once?
Posted: Tue Nov 04, 2014 11:31 am
Why some objects are traversed more than once during stepSimulation?
And the positions also abruptly jump to some unexpected values? Why was that?
Could anyone explain what the bug in my program was?
Code: Select all
Lorry00001 -6.439426 -0.000001 -49.638145
Lorry00002 -65.447983 -0.000002 -6.281012
Lorry00003 -51.849895 -0.000002 -51.004433
CB00002 -29.331459 0.000008 -35.844658
CB00002 -29.331430 20.007578 -35.844646
CB00002 -29.330545 -0.000021 -35.921310
VNA00001 -6.544539 -0.000000 -25.457256
CB00001 -29.331461 0.000008 -35.844658
CB00001 -29.331448 20.007578 -35.844646
CB00001 -19.602650 -0.000021 -3.926569
Could anyone explain what the bug in my program was?
Code: Select all
#include "Cooperative_Pathfinding_Test.h"
#include "LinearMath\btDefaultMotionState.h"
#include <assert.h>
#include <conio.h>
struct YourContext {
ObjectBase *obj;
};
struct ContactSensorCallback : public btCollisionWorld::ContactResultCallback {
//! Constructor, pass whatever context you want to have available when processing contacts
/*! You may also want to set m_collisionFilterGroup and m_collisionFilterMask
* (supplied by the superclass) for needsCollision() */
ContactSensorCallback(btRigidBody& tgtBody , YourContext& context /*, ... */)
: btCollisionWorld::ContactResultCallback(), body(tgtBody), ctxt(context) {
}
btRigidBody& body; //!< The body the sensor is monitoring
YourContext& ctxt; //!< External information for contact processing
//! If you don't want to consider collisions where the bodies are joined by a constraint, override needsCollision:
/*! However, if you use a btCollisionObject for #body instead of a btRigidBody,
* then this is unnecessary—checkCollideWithOverride isn't available */
virtual bool needsCollision(btBroadphaseProxy* proxy) const {
// superclass will check m_collisionFilterGroup and m_collisionFilterMask
if(!btCollisionWorld::ContactResultCallback::needsCollision(proxy))
return false;
// if passed filters, may also want to avoid contacts between constraints
return body.checkCollideWithOverride(static_cast<btCollisionObject*>(proxy->m_clientObject));
}
//! Called with each contact for your own processing (e.g. test if contacts fall in within sensor parameters)
virtual btScalar addSingleResult(btManifoldPoint& cp,
const btCollisionObjectWrapper* colObj0,int partId0,int index0,
const btCollisionObjectWrapper* colObj1,int partId1,int index1)
{
btVector3 pt; // will be set to point of collision relative to body
if(colObj0->m_collisionObject==&body) {
pt = cp.getPositionWorldOnA();
cprintf ("%s %f %f %f\n", ctxt.obj->getName().c_str(), pt.getX(), pt.getY(), pt.getZ());
} else {
assert(colObj1->m_collisionObject==&body && "body does not match either collision object");
pt = cp.m_localPointB;
}
// do stuff with the collision point
return 0; // not actually sure if return value is used for anything...?
}
};
//-------------------------------------------------------------------------------------
Cooperative_Pathfinding_Test::Cooperative_Pathfinding_Test(void)
{
initPhysics();
}
//-------------------------------------------------------------------------------------
Cooperative_Pathfinding_Test::~Cooperative_Pathfinding_Test(void)
{
}
//-------------------------------------------------------------------------------------
void Cooperative_Pathfinding_Test::initPhysics() {
///-----includes_end-----
int i;
///-----initialization_start-----
///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration.
btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///-----initialization_end-----
///create a few basic rigid bodies
//btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),0);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,0,0));
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
m_Objects.push_back(0);
}
void Cooperative_Pathfinding_Test::AddNewAgent(ObjectBase* obj)
{
//create a dynamic rigidbody
//btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
AxisAlignedBox aabb = obj->aabb;
Ogre::Vector3 ogreSize = aabb.getSize();
btVector3 size(ogreSize.x, ogreSize.y, ogreSize.z);
btCollisionShape *colShape = new btBoxShape(size/2);
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
Ogre::Vector3 _position = obj->getPosition();
startTransform.setOrigin(btVector3(_position.x,_position.y,_position.z));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);
m_Objects.push_back(obj);
}
bool Cooperative_Pathfinding_Test::frameRenderingQueued(const Ogre::FrameEvent& evt) {
m_dynamicsWorld->stepSimulation(1.f/60.f,10);
for (int i = 1; i < m_dynamicsWorld->getNumCollisionObjects(); i++) {
YourContext foo;
foo.obj = m_Objects[i];
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
ContactSensorCallback callback(*body, foo);
m_dynamicsWorld->contactTest(body,callback);
}
return FrameListener::frameRenderingQueued(evt);
}