void vehiclePoseCallback(const nav_msgs::Odometry& odom) {
	if (!firstpass) {
		initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
		initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
		wMv_initial.setTrans(initialT);
		wMv_initial.setRotate(initialQ);
		firstpass=true;
	} 
}
OSG::NodeTransitPtr createScene(OSG::Window *win)
{
   OSG::NodeRecPtr rootN = OSG::makeNodeFor(OSG::Group::create());

   // Create ground:
   OSG::NodeUnrecPtr groundN = OSG::makePlane(25,25,1,1);
   OSG::Matrix m;
   OSG::Quaternion q;
   q.setValueAsAxisDeg(OSG::Vec3f(1,0,0), -90);
   m.setRotate(q);

   OSG::TransformUnrecPtr groundTransC = OSG::Transform::create();
   groundTransC->setMatrix(m);

   OSG::NodeUnrecPtr groundTransN = OSG::makeNodeFor(groundTransC);
   groundTransN->addChild(groundN);
   rootN->addChild(groundTransN);

   // Set ground material:
   OSG::SimpleMaterialUnrecPtr mat = OSG::SimpleMaterial::create();
   mat->setDiffuse(OSG::Color3f(0.8,0.8,0.8));
   dynamic_cast<OSG::Geometry*>(groundN->getCore())->setMaterial(mat);

//   // Create figure:
//   OSG::NodeUnrecPtr figure1N =
//         OSG::SceneFileHandler::the()->read("../Models/Figure.obj");

//   G.figure1TransC = OSG::Transform::create();
//   OSG::NodeUnrecPtr trans1N = OSG::makeNodeFor(G.figure1TransC);
//   trans1N->addChild(figure1N);
//   rootN->addChild(trans1N);

   OSG::NodeUnrecPtr figureModelA =
         OSG::SceneFileHandler::the()->read("../assets/Figure.obj");

   BoardGame::Figure* figureA = new BoardGame::Figure();
   figureA->setModel(figureModelA);
   rootN->addChild(figureA->getRoot());

   OSG::NodeUnrecPtr figureModelB =
         OSG::SceneFileHandler::the()->read("../assets/Figure.obj");

   BoardGame::Figure* figureB = new BoardGame::Figure();
   figureB->setModel(figureModelB);
   rootN->addChild(figureB->getRoot());

   G.selectedNode = figureModelA;

   return(OSG::NodeTransitPtr(rootN));
}
void ApplicationStartScreen::attachApplication(void)
{
    Inherited::attachApplication();

    //Camera Transformation Node
    Matrix CameraTransformMatrix;
    CameraTransformMatrix.setTranslate(0.0f,0.0f, 5.0f);
    TransformRefPtr CameraBeaconTransform = Transform::create();
    CameraBeaconTransform->setMatrix(CameraTransformMatrix);

    NodeRefPtr CameraBeaconNode = Node::create();
    CameraBeaconNode->setCore(CameraBeaconTransform);

    // Make Torus Node (creates Torus in background of scene)
    NodeRefPtr TorusGeometryNode = NULL;
    //BoostPath TorusKnotFile(MainApplication::the()->getSettings()->getDataDirectory() / std::string("Models") / std::string("TorusKnot.osb"));
    //SLOG << "Loading Torus Knot from: " << TorusKnotFile.string() << std::endl;
    //if(boost::filesystem::exists(TorusKnotFile))
    //{
        //TorusGeometryNode = SceneFileHandler::the()->read(TorusKnotFile.native_file_string().c_str());
    //}
    //if(TorusGeometryNode == NULL)
    //{
        //SWARNING << "Could not load Torus Knot from: "
                 //<< TorusKnotFile.string() << " because this file doesn't exist."  << std::endl;
        TorusGeometryNode = makeTorus(.5, 2, 64, 64);
    //}

    //Scene Transformation
    TransformRefPtr SceneTransformCore = Transform::create();

    NodeRefPtr SceneTransformNode = Node::create();
    SceneTransformNode->setCore(SceneTransformCore);
    SceneTransformNode->addChild(TorusGeometryNode);

    //Light
    NodeRefPtr LightBeaconNode = Node::create();
    LightBeaconNode->setCore(Transform::create());

    DirectionalLightRefPtr SceneLightCore = DirectionalLight::create();
    SceneLightCore->setDirection(1.0,0.0,0.0);
    SceneLightCore->setBeacon(LightBeaconNode);

    NodeRefPtr SceneLightNode = Node::create();
    SceneLightNode->setCore(SceneLightCore);
    SceneLightNode->addChild(SceneTransformNode);


    // Make Main Scene Node and add the Torus
    NodeRefPtr DefaultRootNode = OSG::Node::create();
    DefaultRootNode->setCore(OSG::Group::create());
    DefaultRootNode->addChild(CameraBeaconNode);
    DefaultRootNode->addChild(SceneLightNode);
    DefaultRootNode->addChild(LightBeaconNode);

    //Camera
    PerspectiveCameraRefPtr DefaultCamera = PerspectiveCamera::create();
    DefaultCamera->setBeacon(CameraBeaconNode);
    DefaultCamera->setFov   (osgDegree2Rad(60.f));
    DefaultCamera->setNear  (0.1f);
    DefaultCamera->setFar   (10000.f);

    //Background
    GradientBackgroundRefPtr DefaultBackground = GradientBackground::create();
    DefaultBackground->addLine(Color3f(0.0f,0.0f,0.0f), 0.0f);
    DefaultBackground->addLine(Color3f(0.0f,0.0f,0.5f), 1.0f);

    //Animation
    //KeyFrames
    KeyframeTransformationSequenceRefPtr TransformationKeyframes = KeyframeTransformationSequenceMatrix4f::create();
    OSG::Matrix TempMat;

    TransformationKeyframes->addKeyframe(TempMat,0.0f);
    TempMat.setRotate(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.5));
    TransformationKeyframes->addKeyframe(TempMat,4.0f);
    TempMat.setRotate(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.0));
    TransformationKeyframes->addKeyframe(TempMat,8.0f);
    TempMat.setRotate(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.5));
    TransformationKeyframes->addKeyframe(TempMat,12.0f);
    TempMat.setRotate(Quaternion(Vec3f(0.0f,1.0f,0.0f), 0.0f));
    TransformationKeyframes->addKeyframe(TempMat,16.0f);

    //Animator
    KeyframeAnimatorRefPtr TorusAnimator = OSG::KeyframeAnimator::create();
    TorusAnimator->setKeyframeSequence(TransformationKeyframes);

    //Animation
    _TorusAnimation = FieldAnimation::create();
    _TorusAnimation->setAnimator(TorusAnimator);
    _TorusAnimation->setInterpolationType(Animator::LINEAR_INTERPOLATION);
    _TorusAnimation->setCycling(-1);
    _TorusAnimation->setAnimatedField(SceneTransformCore, std::string("matrix"));
    _TorusAnimation->attachUpdateProducer(MainApplication::the()->getMainWindow()->editEventProducer());
    _TorusAnimation->start();

    //Foreground
    //ImageForegroundRefPtr LogoForeground = ImageForeground::create();
    //BoostPath LogoPath(MainApplication::the()->getSettings()->getDataDirectory() / "Images/Logo.png");
    //ImageRefPtr LoadedImage = ImageFileHandler::the().read(LogoPath.string().c_str());

    //	LogoForeground->addImage( LoadedImage, Pnt2f( 0,0 ) );

    ForegroundRefPtr UserInterfaceForeground = createInterface();
    _TheUIDrawingSurface->setEventProducer(MainApplication::the()->getMainWindow());

    if(MainApplication::the()->getMainWindow() != NULL &&
       MainApplication::the()->getMainWindow()->getMFPort()->size() == 0)
    {
        ViewportRefPtr DefaultViewport = Viewport::create();
        DefaultViewport->setCamera                  (DefaultCamera);
        DefaultViewport->setRoot                    (DefaultRootNode);
        DefaultViewport->setSize                    (0.0f,0.0f, 1.0f,1.0f);
        DefaultViewport->setBackground              (DefaultBackground);
        DefaultViewport->addForeground              (UserInterfaceForeground);

        MainApplication::the()->getMainWindow()->addPort(DefaultViewport);
    }

    MainApplication::the()->getMainWindow()->addKeyListener(&_StartScreenKeyListener);
    MainApplication::the()->getMainWindow()->addUpdateListener(&_ScreenUpdateListener);

}