2012年4月30日月曜日

Bullet 衝突回数のカウント

衝突回数をカウントして、剛体を削除するテスト bullet-2.79

main.cpp

#include <crtdbg.h>
#include <btBulletCollisionCommon.h>
#include <btBulletDynamicsCommon.h>

#ifdef _DEBUG
#pragma comment(lib, "BulletCollision_debug.lib")
#pragma comment(lib, "BulletDynamics_debug.lib")
#pragma comment(lib, "LinearMath_debug.lib")
#else
#pragma comment(lib, "BulletCollision.lib")
#pragma comment(lib, "BulletDynamics.lib")
#pragma comment(lib, "LinearMath.lib")
#endif

// ↓コンタクトのコールバック
extern ContactProcessedCallback gContactProcessedCallback;

struct TestData {
  int count; // 衝突回数
  TestData() : count(0) {}
};

class TestBullet {
  btDiscreteDynamicsWorld*        m_pWorld;
  btVector3 m_vWorldSize;

    btDefaultCollisionConfiguration     m_config;
    btCollisionDispatcher         m_dispatcher;
    btAxisSweep3              m_broadphase;
    btSequentialImpulseConstraintSolver   m_solver;
  btAlignedObjectArray<btCollisionShape*> m_collisionShapes;

  btRigidBody*  m_Body1;
  TestData    m_BodyData1;
public:
  TestBullet() :
    m_dispatcher(&m_config),
    m_vWorldSize(1000.0f, 1000.0f, 1000.0f),
    m_broadphase(m_vWorldSize * -0.5f, m_vWorldSize * 0.5f, 1024),
    m_pWorld(0), m_Body1(0)
  {}
  ~TestBullet();

  void Init();
  void Update();
  void DeleteBody(btRigidBody** pBody);
  static bool HandleContactProcess(btManifoldPoint& p, void* a, void* b);
};

TestBullet::~TestBullet() {
  for (int i = m_pWorld->getNumCollisionObjects() - 1; i >= 0 ; i--) {
    btCollisionObject* obj = m_pWorld->getCollisionObjectArray()[i];
    btRigidBody* body = btRigidBody::upcast(obj);
    DeleteBody(&body);
  }
  for (int j = 0; j < m_collisionShapes.size(); j++) {
    btCollisionShape* shape = m_collisionShapes[j];
    m_collisionShapes[j] = 0;
    delete shape;
  }
  delete m_pWorld;
}

void TestBullet::Init() {
  m_pWorld = new btDiscreteDynamicsWorld(&m_dispatcher, &m_broadphase, &m_solver, &m_config);
  m_pWorld->setGravity(btVector3(0.0f, -9.8f * 1.0f, 0.0f));
  m_pWorld->getSolverInfo().m_numIterations = 2;

  // 地面の形状
  btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
  m_collisionShapes.push_back(groundShape);

  btTransform groundTransform;
  groundTransform.setIdentity();
  groundTransform.setOrigin(btVector3(0.0f, -50.0f, 0.0f));

  // 地面の作成
  btScalar mass(0.0f);
  bool isDynamic = (mass != 0.f);

  btVector3 localInertia(0,0,0);
  if (isDynamic) groundShape->calculateLocalInertia(mass, localInertia);

  btDefaultMotionState* myMotionState = new btDefaultMotionState( groundTransform );
  btRigidBody::btRigidBodyConstructionInfo rbInfo0(mass, myMotionState, groundShape, localInertia);

  btRigidBody* pGroundBody = new btRigidBody(rbInfo0);
  m_pWorld->addRigidBody(pGroundBody);


  // 削除テスト用の剛体の作成
  btCollisionShape* colShape = new btSphereShape( 2.0f );
  m_collisionShapes.push_back(colShape);

  btTransform startTransform;
  startTransform.setIdentity();

  mass  = 100.0f;
  isDynamic = (mass != 0.f);
  if (isDynamic)  colShape->calculateLocalInertia(mass, localInertia);

  startTransform.setOrigin(btVector3(2, 5, 0));

  myMotionState = new btDefaultMotionState(startTransform);
  btRigidBody::btRigidBodyConstructionInfo rbInfo1(mass, myMotionState, colShape, localInertia);

  m_Body1 = new btRigidBody(rbInfo1);
  m_Body1->setUserPointer(&m_BodyData1);  // ユーザーデータをセット
  m_pWorld->addRigidBody(m_Body1);
}

void TestBullet::Update() {
  const btScalar dt = 1.0f / 30.0f;
  m_pWorld->stepSimulation(dt);

  btTransform trans;
  for (int i = m_pWorld->getNumCollisionObjects() - 1; i >= 0; i--) {
    btCollisionObject* obj = m_pWorld->getCollisionObjectArray()[i];
    btRigidBody* body = btRigidBody::upcast(obj);

    if(body && !body->isStaticObject()) {
      body->getMotionState()->getWorldTransform(trans);
      btVector3& pos = trans.getOrigin();

      printf("%f, %f, %f\n", pos.getX(), pos.getY(), pos.getZ());
    }
  }
  if(m_Body1 != NULL && m_BodyData1.count > 6) {
    DeleteBody(&m_Body1); // 削除テスト
  }
}

void TestBullet::DeleteBody(btRigidBody** ppBody) {
  btRigidBody* pBody = *ppBody;
  m_pWorld->removeRigidBody( pBody );
  btMotionState* pMotionState = pBody->getMotionState();
  if(pMotionState) {
    delete pMotionState;
  }
  delete pBody;
  *ppBody = NULL;
}

bool TestBullet::HandleContactProcess(btManifoldPoint& p, void* a, void* b) {
  btRigidBody* pBody0 = (btRigidBody*)a;
  btRigidBody* pBody1 = (btRigidBody*)b;

  TestData* pUserData0 = (TestData*)pBody0->getUserPointer();
  TestData* pUserData1 = (TestData*)pBody1->getUserPointer();

  // カウント
  if(pUserData0) pUserData0->count++;
  if(pUserData1) pUserData1->count++;
  return true;
}

int main() {
#if defined(DEBUG) | defined(_DEBUG)
    _CrtSetDbgFlag( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
#endif
  TestBullet test;
  test.Init();
  // 衝突のコールバック関数をセット
  gContactProcessedCallback = TestBullet::HandleContactProcess;

  for (int i = 0; i < 100; i++) {
    test.Update();
  }
  return 0;
}