Simple application trying out ray picking in Bullet C++ Physics Engine

BulletPickingDemo.cpp

#include "btBulletDynamicsCommon.h"
#include <map>

#include <memory>

int main()
{
	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);

	btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();

	btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, overlappingPairCache, nullptr, collisionConfiguration);

	btAlignedObjectArray<btCollisionShape*> collision_shapes;

	std::map<const btRigidBody*, int> rigid_bodies;

	for (int x = 0; x < 11; ++x)
	{
		for (int z = 0; z < 11; ++z)
		{
			btCollisionShape* cube_shape = new btBoxShape(btVector3(btScalar(0.5), btScalar(0.5), btScalar(0.5)));

			collision_shapes.push_back(cube_shape);

			btTransform cube_transform;
			cube_transform.setIdentity();
			cube_transform.setOrigin(btVector3(x - 5, 0, z - 5));

			btScalar mass(0.);
			btVector3 local_inertia(0, 0, 0);

			btDefaultMotionState* my_motion_state = new btDefaultMotionState(cube_transform);
			btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, my_motion_state, cube_shape, local_inertia);
			btRigidBody* body = new btRigidBody(rbInfo);

			dynamicsWorld->addRigidBody(body);
			rigid_bodies[body] = (x * 11 + z);
		}
	}

	btVector3 ray_origin(0.0, 10.0, 0.0);
	// Confirm that can pick by sending a ray from the origin to the center of each box
	for (int x = 0; x < 11; ++x)
	{
		for (int z = 0; z < 11; ++z)
		{
			int cube_index = (x * 11 + z);

			btVector3 ray_end(x - 5, 0, z - 5);

			btCollisionWorld::ClosestRayResultCallback ray_callback(ray_origin, ray_end);

			dynamicsWorld->rayTest(ray_origin, ray_end, ray_callback);

			if (ray_callback.hasHit())
			{
				const btCollisionObject* collision_object = ray_callback.m_collisionObject;
				const btRigidBody* pBody = btRigidBody::upcast(ray_callback.m_collisionObject);

				auto result = rigid_bodies.find(pBody);
				if (result != end(rigid_bodies))
				{
					int index = result->second;
					printf("hit test result. cube index:%d, body index:%d\n", cube_index, index);
				}
				else
				{
					printf("could not find matching body. cube index: %d\n", cube_index);
				}
			}
			else
			{
				printf("no hit for cube index: %d\n", cube_index);
			}
		}
	}

	for (int i = dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
	{
		auto obj = dynamicsWorld->getCollisionObjectArray()[i];
		auto body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		dynamicsWorld->removeCollisionObject(obj);
		delete obj;
	}

	for (int j = 0; j < collision_shapes.size(); j++)
	{
		btCollisionShape* shape = collision_shapes[j];
		collision_shapes[j] = 0;
		delete shape;
	}

	delete dynamicsWorld;
	delete overlappingPairCache;
	delete dispatcher;
	delete collisionConfiguration;

	return 0;
}