void setup()
	{
		mCam = new CameraPersp( getWindowWidth(), getWindowHeight(), 60.0f );
		mCam->lookAt(Vec3f(100,400,-400), Vec3f::zero());
		
		mSurface = 0;
		mTexture = 0;
		mCapture = new Capture( 320, 240 );
		mCapture->startCapture();
		mPaused = false;
		mDrawTextured = true;
		
		btVector3 worldAabbMin(-10000,-10000,-10000);
		btVector3 worldAabbMax(10000,10000,10000);
		int maxProxies = 1024;
		
		btAxisSweep3 * broadphase									= new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
		btDefaultCollisionConfiguration	* collisionConfiguration	= new btDefaultCollisionConfiguration();
		btCollisionDispatcher * dispatcher							= new btCollisionDispatcher(collisionConfiguration);
		btSequentialImpulseConstraintSolver * solver				= new btSequentialImpulseConstraintSolver;
		
		dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);
		dynamicsWorld->setGravity(btVector3(0,-10,0));
		
		btCollisionShape * groundShape	= new btStaticPlaneShape(btVector3(0,1,0),1);
		
		btDefaultMotionState * groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
		btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
		
		groundRigidBody = new btRigidBody(groundRigidBodyCI);
		dynamicsWorld->addRigidBody(groundRigidBody);
		
	}

    void init()
    {
        m_collisionConfiguration = new btDefaultCollisionConfiguration();
        m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
        m_overlappingPairCache = new btDbvtBroadphase();
        m_solver = new btSequentialImpulseConstraintSolver();
        m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_overlappingPairCache, m_solver, m_collisionConfiguration);

        m_dynamicsWorld->setGravity(btVector3(0, 0, 0));

        m_debugDraw = new BulletDebugDraw();
        m_dynamicsWorld->setDebugDrawer(m_debugDraw);
    }

BulletBoundsInstance::BulletBoundsInstance(btDiscreteDynamicsWorld& dynamicsWorld, float mass, const btTransform& instanceToWorldTransform, btCollisionShape* collisionShape, const btVector3& localInertia) :
	motionState_(instanceToWorldTransform),
	rigidBody_( btRigidBody::btRigidBodyConstructionInfo(mass, &motionState_, collisionShape, localInertia) )
{
	rigidBody_.setDamping(0.1, 0.75);

	//add the body to the dynamics world
	dynamicsWorld.addRigidBody(&rigidBody_);
}

void triMeshApp::initPhysics()
{
	// setup physics environment. for all basic rigid body physics this can be left as it is
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_broadphase = new btDbvtBroadphase();
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;
	
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
	
}

void winBodiesApp::initPhysics()
{
	// setup physics environment. for all basic rigid body physics this can be left as it is
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_broadphase = new btDbvtBroadphase();
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;
	
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
	
	
	// make a ground plane that cannot be moved
	btCollisionShape * groundShape	= new btStaticPlaneShape(btVector3(0,1,0),1);
	
	btDefaultMotionState * groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
	btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
	
	m_groundRigidBody = std::shared_ptr<btRigidBody>(new btRigidBody(groundRigidBodyCI));
	m_dynamicsWorld->addRigidBody(m_groundRigidBody.get());
	
	
}

     PhysicsObject(btDiscreteDynamicsWorld & world, const T & shape, 
             btScalar mass, btVector3 velocity,
             const btTransform & transform, 
             GameBall * ptrBall):
         _shape(shape),
         _motionState(transform),
         _rigidBody(mass, &_motionState, &_shape),
         _ptrBall(ptrBall)
 {
     // TODO angular velocity ?
     _rigidBody.setLinearVelocity(velocity);
     _rigidBody.setDamping(0.1, 0.1);
     _rigidBody.setRestitution(0.7);
     _rigidBody.setFriction(0.1);
     // TODO _rigidBody.setRollingFriction(0.01);
     world.addRigidBody(&_rigidBody);
 }

void BulletWrapper::init()
{
  assert(!m_Broadphase && !m_CollisionConfiguration && !m_Dispatcher && !m_Solver && !m_DynamicsWorld);

  m_Broadphase = new btDbvtBroadphase();

  m_CollisionConfiguration = new btDefaultCollisionConfiguration();
  m_Dispatcher = new btCollisionDispatcher(m_CollisionConfiguration);

  m_Dispatcher->setNearCallback(MyNearCallback);

  m_Solver = new btSequentialImpulseConstraintSolver;

  m_DynamicsWorld = new btDiscreteDynamicsWorld(m_Dispatcher, m_Broadphase, m_Solver, m_CollisionConfiguration);

  //m_DynamicsWorld->setGravity(btVector3(0, -10, 0));
  m_DynamicsWorld->setGravity(btVector3(0, 0, 0));
}

    void createBulletSim(void) {
         // EN:: collision configuration contains default setup for memory, collision setup.
         // EN:: Advanced users can create their own configuration.

         // BR:: configuração de colisão contem configurações padrão da memória.
         // BR:: usuários avançados podem criar suas próprias configurações.
         collisionConfiguration = new btDefaultCollisionConfiguration();

         // EN:: use the default collision dispatcher. For parallel processing
         // EN:: you can use a diffent dispatcher (see Extras/BulletMultiThreaded)

         // BR:: use o dispatcher padrão. para processamento paralelo
         // BR:: você pode usar um dispatcher diferente. (ver Doc)
         dispatcher = new   btCollisionDispatcher(collisionConfiguration);

         // EN:: btDbvtBroadphase is a good general purpose broadphase.
         // EN:: You can also try out btAxis3Sweep.

         // BR:: btDbvtBroadphase é um bom broadphase de propósito geral.
         // BR:: Você pode tentar também btAxis3Sweep.
         overlappingPairCache = new btDbvtBroadphase();

         // EN:: the default constraint solver. For parallel processing
         // EN:: you can use a different solver (see Extras/BulletMultiThreaded)

         // BR:: usa a constraint solver padrão. Para processamento paralelo
         // BR:: você pode ver um solver diferente (ver Doc)
         solver = new btSequentialImpulseConstraintSolver;

         dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);
         dynamicsWorld->setGravity(btVector3(0,-10,0));

         // EN:: create a few basic rigid bodies
         // EN:: start with ground plane, 1500, 1500

         // BR:: cria alguns corpos rígidos básicos
         // BR:: inicializa com um chão plano.
         btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(1500.),btScalar(1.),btScalar(1500.)));

         collisionShapes.push_back(groundShape);

         btTransform groundTransform;
         groundTransform.setIdentity();
         groundTransform.setOrigin(btVector3(0,-2,0));

         {
            btScalar mass(0.);

            // EN:: rigidbody is dynamic if and only if mass is non zero, otherwise static
            // BR:: corpo rigido é dimâmico apenas se massa for diferente de 0.
            bool isDynamic = (mass != 0.f);

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

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

            // EN:: add the body to the dynamics world
            // BR:: adiciona o corpo às dinâmicas do mundo
            dynamicsWorld->addRigidBody(body);
         }


         {
            // EN:: create a dynamic rigidbody
            // BR:: cria um corpo rígido dinâmico
            btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
            collisionShapes.push_back(colShape);

            // EN:: Create Dynamic Objects
            // BR:: Cria objetos dinâmicos
            btTransform startTransform;
            startTransform.setIdentity();

            btScalar   mass(1.f);

            // EN:: rigidbody is dynamic if and only if mass is non zero, otherwise static
            // BR:: corpo rigido é dimâmico apenas se massa for diferente de 0.
            bool isDynamic = (mass != 0.f);

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

               startTransform.setOrigin(btVector3(0,250,0));
               // *** give it a slight twist so it bouncees more interesting
               startTransform.setRotation(btQuaternion(btVector3(1.0, 1.0, 0.0), 0.6));

               //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
               //btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
               MyMotionState* motionState = new MyMotionState(startTransform, boxNode);
               btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,colShape,localInertia);
               btRigidBody* body = new btRigidBody(rbInfo);

               dynamicsWorld->addRigidBody(body);
         }
    }

    void clean()
    {
        if (!m_dynamicsWorld)
            return;

        for (int i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
        {
            btCollisionObject *obj = m_dynamicsWorld->getCollisionObjectArray()[i];
            btRigidBody *body = btRigidBody::upcast(obj);
            if (body && body->getMotionState())
                delete body->getMotionState();
            if (body && body->getCollisionShape())
                delete body->getCollisionShape();

            m_dynamicsWorld->removeCollisionObject(obj);
            delete obj;
        }

        SAFE_DELETE(m_dynamicsWorld);
        SAFE_DELETE(m_solver);
        SAFE_DELETE(m_overlappingPairCache);
        SAFE_DELETE(m_dispatcher);
        SAFE_DELETE(m_collisionConfiguration);
        SAFE_DELETE(m_debugDraw);
    }

void triMeshApp::exitPhysics()
{
	//cleanup in the reverse order of creation/initialization
	
	if(m_convexRigidBody->getMotionState())
		delete m_convexRigidBody->getMotionState();
	m_dynamicsWorld->removeRigidBody(m_convexRigidBody);
	delete m_convexRigidBody;
	
	if(m_concaveRigidBody->getMotionState())
		delete m_concaveRigidBody->getMotionState();
	m_dynamicsWorld->removeRigidBody(m_concaveRigidBody);
	delete m_concaveRigidBody;
	
	if(m_hfRigidBody->getMotionState())
		delete m_hfRigidBody->getMotionState();
	m_dynamicsWorld->removeRigidBody(m_hfRigidBody);
	delete m_hfRigidBody;
	
	
	delete m_dynamicsWorld;
	
	delete m_solver;
	
	delete m_broadphase;
	
	delete m_dispatcher;
	
	delete m_collisionConfiguration;
}

 ~World(void) {
   for (auto &robot : robots)
     dynamics.removeRigidBody(&robot.body);
   for (auto &ball : balls)
     dynamics.removeRigidBody(&ball.body);
   dynamics.removeRigidBody(&field.ground_body);
   dynamics.removeRigidBody(&field.ceil_body);
   dynamics.removeRigidBody(&field.left_wall_body);
   dynamics.removeRigidBody(&field.right_wall_body);
   dynamics.removeRigidBody(&field.top_wall_body);
   dynamics.removeRigidBody(&field.bottom_wall_body);
   dynamics.removeRigidBody(&field.left_goal_body);
   dynamics.removeRigidBody(&field.right_goal_body);
 }

 World(const FieldGeometry *const field_geom)
     : field_geometry{field_geom}, field{field_geom} {
   dynamics.setGravity({0, 0, -9.80665});
   dynamics.addRigidBody(&field.ground_body);
   dynamics.addRigidBody(&field.ceil_body);
   dynamics.addRigidBody(&field.left_wall_body);
   dynamics.addRigidBody(&field.right_wall_body);
   dynamics.addRigidBody(&field.top_wall_body);
   dynamics.addRigidBody(&field.bottom_wall_body);
   dynamics.addRigidBody(&field.left_goal_body);
   dynamics.addRigidBody(&field.right_goal_body);
   balls.emplace_back();
   dynamics.addRigidBody(&balls.back().body);
   ball_set_vec(&balls.back(), {});
 }

	// Physics dll internal
	void RefreshColShape(IPhysicsInterface* colProvider){
		if (!colProvider)
		{
			Logger::Log(FB_ERROR_LOG_ARG, "No colProvider");			
			return;
		}
		mWorld->removeRigidBody(mSelf);
		mAddedToWorld = false;
		auto prevColShape = mSelf->getCollisionShape();
		if (prevColShape){
			Physics::GetInstance().Release(prevColShape);
		}

		btCollisionShape* colShape = 0;
		float mass = 1.0f;
		auto& physics = Physics::GetInstance();
		if (mGroupedRigidBody){
			if (colProvider->GetNumColShapes(mGroupIdx) == 0)
				return;

			colShape = physics.CreateColShapeForGroup(colProvider, mGroupIdx);
			mass = colProvider->GetMassForGroup(mGroupIdx);
		}
		else{
			if (colProvider->GetNumColShapes() == 0)
				return;

			colShape = physics.CreateColShape(colProvider);
			mass = colProvider->GetMass();
		}
		assert(colShape);
		mSelf->setCollisionShape(colShape);
		if (colShape)
			physics.AddRef(colShape);

		if (mass > 0 && colShape)
		{
			btVector3 inertia;
			colShape->calculateLocalInertia(mass, inertia);			
			mSelf->setMassProps(mass, inertia);
		}
		else{
			SetMass(0.f);
		}
		assert(!mAddedToWorld);
		mWorld->addRigidBody(mSelf, colProvider->GetCollisionGroup(), colProvider->GetCollisionMask());
		mAddedToWorld = true;
	}

void triMeshApp::update()
{
	
	m_dynamicsWorld->stepSimulation(1.0f, 10);
	
	double now = getElapsedSeconds();
	double dt = now - lastTime;
	lastTime = now;
	
	bool objectsErased = false;
	
	for(int i = 0; i < mObjects.size(); i++)
	{
		mObjects[i]->update(dt);
		
		if(mObjects[i]->lifetime > MAX_LIFETIME)
		{
			mObjects.erase(mObjects.begin() + i);
			objectsErased = true;
		}
	}
	
	if(objectsErased)
		activateAllObjects(m_dynamicsWorld);
	
	yrot += dt * 36.0;
	if(yrot >=360.0) yrot = 0.0;
	
}

      // frame listener
   bool frameStarted(const FrameEvent &evt)
    {
      mKeyboard->capture();

        // update physics simulation
      dynamicsWorld->stepSimulation(evt.timeSinceLastFrame,50);
        return mContinue;
    }

	void update()
	{
		if( mCapture->checkNewFrame() ) {
			delete mTexture;
			mTexture = new gl::Texture( mCapture->getSurface() );
		}
		
		if( ! mPaused )
			dynamicsWorld->stepSimulation(1.0f, 10);
	}

	void UnregisterFromWorld(){
		if (mWorld){
			auto num = mSelf->getNumConstraintRefs();
			for (int i = 0; i < num; ++i){
				auto constraint = mSelf->getConstraintRef(i);
				assert(constraint);
				constraint->setEnabled(false);
			}
			mWorld->removeRigidBody(mSelf);
			mAddedToWorld = false;
		}		
	}

	void UnregisterFromWorld(){
		if (mWorld){
			auto num = mSelf->getNumConstraintRefs();
			for (int i = 0; i < num; ++i){
				auto constraint = mSelf->getConstraintRef(i);
				assert(constraint);
				constraint->setEnabled(false);
			}
			mWorld->removeRigidBody(mSelf);
			mAddedToWorld = false;
		}
		else
			Logger::Log(FB_ERROR_LOG_ARG, "No colprovier exists!");			
	}

void BulletExample::drawEvent() {
    GL::defaultFramebuffer.clear(GL::FramebufferClear::Color|GL::FramebufferClear::Depth);

    /* Housekeeping: remove any objects which are far away from the origin */
    for(Object3D* obj = _scene.children().first(); obj; )
    {
        Object3D* next = obj->nextSibling();
        if(obj->transformation().translation().dot() > 100*100)
            delete obj;

        obj = next;
    }

    /* Step bullet simulation */
    _bWorld.stepSimulation(_timeline.previousFrameDuration(), 5);

    /* Draw the cubes */
    if(_drawCubes) _camera->draw(_drawables);

    /* Debug draw. If drawing on top of cubes, avoid flickering by setting
       depth function to <= instead of just <. */
    if(_drawDebug) {
        if(_drawCubes)
            GL::Renderer::setDepthFunction(GL::Renderer::DepthFunction::LessOrEqual);

        _debugDraw.setTransformationProjectionMatrix(
            _camera->projectionMatrix()*_camera->cameraMatrix());
        _bWorld.debugDrawWorld();

        if(_drawCubes)
            GL::Renderer::setDepthFunction(GL::Renderer::DepthFunction::Less);
    }

    swapBuffers();
    _timeline.nextFrame();
    redraw();
}

void BulletWrapper::removeAllBodies()
{
  for (unsigned int i = 0; i < m_BodyDatas.size(); i++)
  {
    btRigidBody* body = m_BodyDatas[i].m_Body;

    m_DynamicsWorld->removeRigidBody(body);
    delete body->getMotionState();
    delete body;

    // Shape is automatically released via the shared pointer
  }

  m_BodyDatas.clear();
}