int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
  {
    Eigen::VectorXf epsilon(1);
    epsilon(0) = 1e-5;

    Eigen::VectorXf fvec1(1);
    operator()(x + epsilon, fvec1);
    Eigen::VectorXf fvec2(1);
    operator()(x - epsilon, fvec2);
    fjac = (fvec1 - fvec2)/2.0f;
    return 0;
  }
PlayState::PlayState(Game *game) {
#ifdef CLIENT


	m_camera = new Camera();
	m_camera->setSize(game->getWindow()->getWidth(), game->getWindow()->getHeight());
	m_position = glm::vec3(0, 0, 1.0f);
#endif

	m_world = new World();

	m_entityManager = new EntityManager();

	EntityFactory *entityFactory = game->getEntityFactory();
	{


		ComponentSprite *componentSprite = new ComponentSprite();

		ComponentB2Physics *componentB2Physics = new ComponentB2Physics();

		ComponentUser *componentUser = new ComponentUser();

		componentSprite->texture = game->getTextureHandler()->getTexture(0);

		Entity *entityTemplate = new Entity(0);
		entityTemplate->addComponent(componentSprite);
		entityTemplate->addComponent(componentB2Physics);
		entityTemplate->addComponent(componentUser);


		entityFactory->registerEntity("player", entityTemplate);
	}
	// Create player entity
	m_entityManager->addEntity(entityFactory->createEntity("player"), game, m_world);

	Entity *entity = entityFactory->createEntity("player");
	m_entityManager->addEntity(entity, game, m_world);
	ComponentB2Physics *physics = entity->getComponent<ComponentB2Physics>();
	physics->spawnPosition = glm::vec2(13 * 16, 37 * 16);
	physics->spawnGravity = glm::vec2(0, 0);
	physics->spawnAcceleration = glm::vec2(0, 0);
	physics->spawnVelocity = glm::vec2(0.1f, 0);

	for (int x = 8; x < 16; ++x) {
		for (int y = 8; y < 16; ++y) {
			Entity *entity = entityFactory->createEntity("player");

			ComponentB2Physics *physics = entity->getComponent<ComponentB2Physics>();
			physics->spawnPosition = glm::vec2(x * 16.f, y * 16.f);
			physics->spawnVelocity = glm::vec2(+6.4f - 0.8f*x, +6.4 - 0.8f*y + 0.5f*x);
			physics->spawnAngularVelocity = x*64.f;
			physics->spawnAngle = (float)x / 16.f*3.14;
			physics->spawnSize = fvec2(y*0.0625f, y*0.0625f);


			ComponentSprite *sprite = entity->getComponent<ComponentSprite>();
			//sprite->angle = (float)x / 16.f*3.14;
			sprite->scale = fvec2(y*0.0625f,y*0.0625f);

			m_entityManager->addEntity(entity, game, m_world);
		}
	}


	// Add systems (entity logic)
	m_entityManager->addSystem(new SystemPhysics());
	m_entityManager->addSystem(new SystemPhysicsSpriteLink());

	// Add renderSystems (entity renderers)
	m_entityManager->addRenderSystem((IRenderSystem*)(new RenderSystemSprite()));

	// Add key bindings
	{
		InputManager *inputManager = game->getInputManager();

		inputManager->bindKey(std::string("ZoomIn"), GLFW_KEY_SPACE);
		inputManager->bindKey(std::string("ZoomOut"), GLFW_KEY_LEFT_SHIFT);
	}
}
namespace vl
{
  typedef enum { CC_FlatCap, CC_RoundedCap, CC_NoCap } ECapsuleCap;

  //! Creates a box
  VLGRAPHICS_EXPORT ref<Geometry> makeBox( const vec3& origin, real xside=1, real yside=1, real zside=1, bool tex_coords=true );

  //! Creates a box
  VLGRAPHICS_EXPORT ref<Geometry> makeBox( const vec3& min, const vec3& max, bool tex_coords=true );

  //! Creates a box
  VLGRAPHICS_EXPORT ref<Geometry> makeBox( const AABB& aabb, bool tex_coords=true );

  //! Creates a cone
  VLGRAPHICS_EXPORT ref<Geometry> makeCone( const vec3& origin, real diameter=1, real height=1, int phi=20, bool bottom=true );

  //! Creates a pyramid
  VLGRAPHICS_EXPORT ref<Geometry> makePyramid( const vec3& origin, real side=1, real height=1 );

  //! Creates an icosahedron
  VLGRAPHICS_EXPORT ref<Geometry> makeIcosahedron( const vec3& origin, real diameter );

  //! Creates a sphere by iteratively subdividing an icosahedron.
  VLGRAPHICS_EXPORT ref<Geometry> makeIcosphere( const vec3& pos, real diameter=1, int detail=2, bool remove_doubles = true );

  //! Creates a uv sphere
  VLGRAPHICS_EXPORT ref<Geometry> makeUVSphere( const vec3& origin, real diameter=1, int phi=20, int theta=20 );

  //! Creates a cylinder
  VLGRAPHICS_EXPORT ref<Geometry> makeCylinder( const vec3& origin, real diameter=1, real height=1, int phi=20, int theta=2, bool top=true, bool bottom=true );

  //! Creates torus. This function generates also appropriate normals.
  VLGRAPHICS_EXPORT ref<Geometry> makeTorus( const vec3& origin, real diameter=1, real thickness=0.2, int phi=10, int theta=10, float tex_coords = 0.0f );

  //! Creates a 3d capsule with rounded, flat or no caps
  VLGRAPHICS_EXPORT ref<Geometry> makeCapsule(float radius, float height, int segments, ECapsuleCap top_cap, ECapsuleCap bottom_cap, const fvec4& top_col, const fvec4& bottom_col);

  //! Creates a classic Newell's teapot
  VLGRAPHICS_EXPORT ref<Geometry> makeTeapot( const vec3& origin, real diameter=1, int detail=8);

  //! Creates a 2D grid
  VLGRAPHICS_EXPORT ref<Geometry> makeGrid( const vec3& origin, real xside, real zside, int x, int z, bool gen_texcoords = false, fvec2 uv0=fvec2(0,0), fvec2 uv1=fvec2(1,1));

  //! Creates a set of points
  VLGRAPHICS_EXPORT ref<Geometry> makePoints( const std::vector< vec3 >& pos, const fvec4& color = white);

  //! Creates a 2D circle
  VLGRAPHICS_EXPORT ref<Geometry> makeCircle( vec3 origin, real radius, int slices = 60 );
}