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);
}