bullet_01

#include <btBulletDynamicsCommon.h>
#include <osgViewer/Viewer>
#include <map>
#include <osg/ShapeDrawable>
#include <osg/MatrixTransform>
#include <osgGA/StateSetManipulator>
#include <osgViewer/ViewerEventHandlers>
#pragma comment(lib, "osgd.lib")
#pragma comment(lib, "osgDBd.lib")
#pragma comment(lib, "osgGAd.lib")
#pragma comment(lib, "osgViewerd.lib")

class BulletInterface : public osg::Referenced
{
public:
    static BulletInterface * Instance();
    btDiscreteDynamicsWorld * GetScene() {return _scene;}

    void CreateWorld(const osg::Plane & plane, const osg::Vec3 & gravity);
    void CreateBox(int id, const osg::Vec3 & dim, double mass);
    void CreateSphere(int id, double radius, double mass);
    void SetVelocity(int id, const osg::Vec3 & pos);
    void SetMatrix(int id, const osg::Matrix & matrix);
    osg::Matrix GetMatrix(int id);

    void Simulate(double step);
protected:
    BulletInterface();
    virtual ~BulletInterface();

    typedef std::map<int , btRigidBody *>       ActorMap;
    ActorMap                                  _actors;
    btDiscreteDynamicsWorld                 * _scene;
    btDefaultCollisionConfiguration         * _configuration;
    btCollisionDispatcher                   * _dispatcher;
    btBroadphaseInterface                   * _overlappingPairCache;
    btSequentialImpulseConstraintSolver     * _solver;
};

BulletInterface * BulletInterface::Instance()
{
    static osg::ref_ptr<BulletInterface> s_registry = new BulletInterface;
    return s_registry.get();
}

void BulletInterface::CreateWorld( const osg::Plane & plane, const osg::Vec3 & gravity )
{
    _scene = new btDiscreteDynamicsWorld(_dispatcher, _overlappingPairCache, _solver, _configuration);
    _scene->setGravity(btVector3(gravity[0], gravity[1], gravity[2]));

    osg::Vec3 normal = plane.getNormal();
    btCollisionShape * groundShape = new btStaticPlaneShape(btVector3(normal[0], normal[1], normal[2]), plane[3]);

    btTransform groundTransform;
    groundTransform.setIdentity();

    btDefaultMotionState * motionState = new btDefaultMotionState(groundTransform);
    btRigidBody::btRigidBodyConstructionInfo rigidInfo(0.0, motionState, groundShape, btVector3(0.0, 0.0, 0.0));
    btRigidBody * body = new btRigidBody(rigidInfo);
    _scene->addRigidBody(body);
}

void BulletInterface::CreateBox( int id, const osg::Vec3 & dim, double mass )
{
    btCollisionShape * boxShape = new btBoxShape(btVector3(dim[0], dim[1], dim[2]));
    btTransform boxTransform;
    boxTransform.setIdentity();

    btVector3 localInertia(0.0, 0.0, 0.0);
    if (mass > 0.0)
    {
        boxShape->calculateLocalInertia(mass, localInertia);
    }

    btDefaultMotionState * motionState = new btDefaultMotionState(boxTransform);
    btRigidBody::btRigidBodyConstructionInfo rigidInfo(mass, motionState, boxShape, localInertia);
    btRigidBody * body = new btRigidBody(rigidInfo);
    _scene->addRigidBody(body);
    _actors[id] = body;
}

void BulletInterface::CreateSphere( int id, double radius, double mass )
{
    btCollisionShape * sphereShape = new btSphereShape(radius);

    btTransform sphereTransform;
    sphereTransform.setIdentity();
    
    btVector3 localInertia(0.0, 0.0, 0.0);
    if (mass > 0.0)
    {
        sphereShape->calculateLocalInertia(mass, localInertia);
    }
    btDefaultMotionState * motionState = new btDefaultMotionState(sphereTransform);
    btRigidBody::btRigidBodyConstructionInfo rigidInfo(mass, motionState, sphereShape, localInertia);
    btRigidBody * body = new btRigidBody(rigidInfo);
    _scene->addRigidBody(body);
    _actors[id] = body;
}

void BulletInterface::SetVelocity( int id, const osg::Vec3 & pos )
{
    btRigidBody * actor = _actors[id];
    if (actor)
    {
        actor->setLinearVelocity(btVector3(pos._v[0], pos._v[1], pos._v[2]));
    }
}

void BulletInterface::SetMatrix( int id, const osg::Matrix & matrix )
{
    btRigidBody * actor = _actors[id];
    if (actor)
    {
        btTransform trans;
        trans.setFromOpenGLMatrix(osg::Matrixf(matrix).ptr());
        actor->setWorldTransform(trans);
    }
}

osg::Matrix BulletInterface::GetMatrix( int id )
{
    btRigidBody * actor = _actors[id];
    if (actor)
    {
        btTransform trans = actor->getWorldTransform();
        osg::Matrixf matrix;
        trans.getOpenGLMatrix(matrix.ptr());
        return matrix;
    }
    return osg::Matrix();
}

void BulletInterface::Simulate( double step )
{
    _scene->stepSimulation(step, 10);
}

BulletInterface::BulletInterface()
    :_scene(NULL)
{
    _configuration = new btDefaultCollisionConfiguration;
    _dispatcher = new btCollisionDispatcher(_configuration);
    _overlappingPairCache = new btDbvtBroadphase;
    _solver = new btSequentialImpulseConstraintSolver;
}

BulletInterface::~BulletInterface()
{
    if (_scene)
    {
        for (int i = _scene->getNumCollisionObjects() - 1; i >= 0; --i)
        {
            btCollisionObject * obj = _scene->getCollisionObjectArray()[i];
            btRigidBody * body = btRigidBody::upcast(obj);
            if (body && body->getMotionState())
            {
                delete body->getMotionState();
            }
            _scene->removeCollisionObject(obj);
            delete obj;
        }
        delete _scene;
    }
    delete _solver;
    delete _overlappingPairCache;
    delete _dispatcher;
    delete _configuration;
}

class SampleRigidUpdater : public osgGA::GUIEventHandler
{
public:
    SampleRigidUpdater(osg::Group * root)
        :_root(root)
    {

    }

    void AddGround(const osg::Vec3 & gravity)
    {
        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
        geode->addDrawable(new osg::ShapeDrawable(new osg::Box(osg::Vec3(0.0f, 0.0f, -0.5f), 100.0f, 100.0f, 1.0f)));

        osg::ref_ptr<osg::MatrixTransform> mt = new osg::MatrixTransform;
        mt->addChild(geode.get());
        _root->addChild(mt.get());

        BulletInterface::Instance()->CreateWorld(osg::Plane(0.0f, 0.0f, 1.0f, 0.0f), gravity);
    }

    void AddPhysicsBox(osg::Box * shape, const osg::Vec3 & pos, const osg::Vec3 & vel, double mass)
    {
        int id = _physicsNodes.size();
        BulletInterface::Instance()->CreateBox(id, shape->getHalfLengths(), mass);
        AddPhysicsData(id, shape, pos, vel, mass);
    }

    void AddPhysicsSphere(osg::Sphere * sphere, const osg::Vec3 & pos, const osg::Vec3 & vel, double mass)
    {
        int id = _physicsNodes.size();
        BulletInterface::Instance()->CreateSphere(id, sphere->getRadius(), mass);
        AddPhysicsData(id, sphere, pos, vel, mass);
    }

    bool handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa)
    {
        osgViewer::View * view = dynamic_cast<osgViewer::Viewer *>(&aa);
        if (!view || !_root)
        {
            return false;
        }
        switch (ea.getEventType())
        {
        case osgGA::GUIEventAdapter::KEYUP:
            {
                osg::Vec3 eye, center, up, dir;
                view->getCamera()->getViewMatrixAsLookAt(eye, center, up);
                dir = center - eye;
                dir.normalize();
                AddPhysicsSphere(new osg::Sphere(osg::Vec3(), 0.5f), eye, dir * 60.0f, 2.0);
            }
            break;
        case osgGA::GUIEventAdapter::FRAME:
            {
                BulletInterface::Instance()->Simulate(0.02);
                for (NodeMap::iterator iter = _physicsNodes.begin(); iter != _physicsNodes.end(); ++iter)
                {
                    osg::Matrix matrix = BulletInterface::Instance()->GetMatrix(iter->first);
                    iter->second->setMatrix(matrix);
                }
            }
            break;
        default:
            break;
        }
        return false;
    }
protected:
    void AddPhysicsData(int id, osg::Shape * shape, const osg::Vec3 & pos, const osg::Vec3 & vel, double mass)
    {
        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
        geode->addDrawable(new osg::ShapeDrawable(shape));
        osg::ref_ptr<osg::MatrixTransform> mt = new osg::MatrixTransform;
        mt->addChild(geode.get());
        _root->addChild(mt.get());
        BulletInterface::Instance()->SetMatrix(id, osg::Matrix::translate(pos));
        BulletInterface::Instance()->SetVelocity(id, vel);
        _physicsNodes[id] = mt;
    }

protected:
    typedef std::map<int, osg::observer_ptr<osg::MatrixTransform> > NodeMap;
    NodeMap                                                         _physicsNodes;
    osg::observer_ptr<osg::Group >                                  _root;
};
int main()
{
    int mode = 0;

    osg::ref_ptr<osg::Group> root = new osg::Group;
    osg::ref_ptr<osgGA::GUIEventHandler> updater;
    switch ( mode )
    {
    case 0:
        {
            SampleRigidUpdater* rigidUpdater = new SampleRigidUpdater( root.get() );
            rigidUpdater->AddGround( osg::Vec3(0.0f, 0.0f,-9.8f) );
            for ( unsigned int i=0; i<10; ++i )
            {
                for ( unsigned int j=0; j<10; ++j )
                {
                    rigidUpdater->AddPhysicsBox( new osg::Box(osg::Vec3(), 0.99f),
                        osg::Vec3((float)i, 0.0f, (float)j+0.5f), osg::Vec3(), 1.0f );
                }
            }
            updater = rigidUpdater;
        }
        break;
    case 1:
        break;
    default: break;
    }

    osgViewer::Viewer viewer;
    viewer.addEventHandler( new osgGA::StateSetManipulator(viewer.getCamera()->getOrCreateStateSet()) );
    viewer.addEventHandler( new osgViewer::StatsHandler );
    viewer.addEventHandler( new osgViewer::WindowSizeHandler );
    if ( updater.valid() )
        viewer.addEventHandler( updater.get() );
    viewer.setSceneData( root.get() );
    return viewer.run();
}