Augmented reality - XNA Models

#region Using Statements
using System;
using System.Windows;
using System.Windows.Navigation;
using Microsoft.Phone.Controls;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Devices;
using Microsoft.Devices.Sensors;
using Matrix = Microsoft.Xna.Framework.Matrix;
using Color = Microsoft.Xna.Framework.Color;
using System.Device.Location;
using Primitives3D;
#endregion Using Statements

namespace SlXnaGeoMotion
{
    public partial class GamePage : PhoneApplicationPage
    {
        #region Commonstuff
        ContentManager contentManager;
        GameTimer timer;
        SpriteBatch spriteBatch;
        #endregion Commonstuff

        #region Silverlight
        UIElementRenderer silverlightRenderer;
        #endregion Silverlight

        #region Motion
        Motion motion;
        #endregion Motion

        #region GeoCoordinateWatcher
        const double MeterInLatitude = 111290.91975341858;
        const double MeterInLongitude = 55729.5173743959;
        double PositionChangeX = 0;
        double PositionChangeZ = 0;
        GeoCoordinateWatcher geoWatcher;
        GeoPositionStatus currentState = GeoPositionStatus.Initializing;
        public GeoPosition<GeoCoordinate> InitialGeoCoordinate { get; set; }
        public GeoPosition<GeoCoordinate> CurrentGeoCoordinate { get; set; }
        #endregion GeoCoordinateWatcher

        #region PhotoCamera
        PhotoCamera photoCamera;
        #endregion PhotoCamera

        #region 3D
        TeapotPrimitive teapot;
        Matrix teapotWorld;
        Matrix projection;
        Matrix view;
        #region Axcis
        //CylinderPrimitive xAxcis;
        //CylinderPrimitive yAxcis;
        //CylinderPrimitive zAxcis;
        //Matrix xAxcisWorld;
        //Matrix yAxcisWorld;
        //Matrix zAxcisWorld;
        #endregion Axcis
        #endregion 3D

        public GamePage()
        {
            #region CommonStuff
            InitializeComponent();
            contentManager = (Application.Current as App).Content;
            #endregion CommonStuff

            #region Timer
            timer = new GameTimer();
            timer.UpdateInterval = TimeSpan.FromTicks(333333);
            timer.Update += OnUpdate;
            timer.Draw += OnDraw;
            #endregion Timer
        }

        protected override void OnNavigatedTo(NavigationEventArgs e)
        {
            #region CommonStuff
            SharedGraphicsDeviceManager.Current.GraphicsDevice.SetSharingMode(true);
            spriteBatch = new SpriteBatch(SharedGraphicsDeviceManager.Current.GraphicsDevice);
            #endregion CommonStuff

            #region Silverlight
            if (ActualHeight > 0 && ActualWidth > 0 && silverlightRenderer == null)
            {
                silverlightRenderer = new UIElementRenderer(this, (int)ActualWidth, (int)ActualHeight);
            }
            photoCamera = new PhotoCamera();
            videoBrush.SetSource(photoCamera);
            #endregion Silverlight

            #region Motion
            if (!Motion.IsSupported)
            {
                MessageBox.Show("The Motion API is not supported on this device.");
                return;
            }
            if (motion == null)
            {
                motion = new Motion();
                motion.TimeBetweenUpdates = timer.UpdateInterval;
                motion.Calibrate += new EventHandler<CalibrationEventArgs>(motion_Calibrate);
            }
            motion.Start();
            #endregion Motion

            #region GeoCoordinateWatcher
            // Request high precision location service and start the service
            geoWatcher = new GeoCoordinateWatcher(GeoPositionAccuracy.High);
            geoWatcher.StatusChanged += new EventHandler<GeoPositionStatusChangedEventArgs>(geoWatcher_StatusChanged);
            geoWatcher.PositionChanged += new EventHandler<GeoPositionChangedEventArgs<GeoCoordinate>>(geoWatcher_PositionChanged);
            geoWatcher.MovementThreshold = 0.0;
            geoWatcher.Start();
            #endregion GeoCoordinateWatcher

            #region 3D
            teapot = new TeapotPrimitive(SharedGraphicsDeviceManager.Current.GraphicsDevice);
            #region Axcis
            //xAxcis = new CylinderPrimitive(SharedGraphicsDeviceManager.Current.GraphicsDevice, 100, 0.1f, 32);
            //yAxcis = new CylinderPrimitive(SharedGraphicsDeviceManager.Current.GraphicsDevice, 100, 0.1f, 32);
            //zAxcis = new CylinderPrimitive(SharedGraphicsDeviceManager.Current.GraphicsDevice, 100, 0.1f, 32);
            #endregion Axcis
            #endregion 3D

            #region CommonStuff
            timer.Start();
            base.OnNavigatedTo(e);
            #endregion CommonStuff
        }

        protected override void OnNavigatedFrom(NavigationEventArgs e)
        {
            #region CleanUp
            if (timer != null)
            {
                timer.Stop();
            }
            if (motion != null)
            {
                motion.Stop();
            }
            geoWatcher.Stop();
            if (photoCamera != null)
            {
                photoCamera.Dispose();
            }
            #endregion CleanUp

            #region CommonStuff
            SharedGraphicsDeviceManager.Current.GraphicsDevice.SetSharingMode(false);
            base.OnNavigatedFrom(e);
            #endregion CommonStuff
        }

        private void OnUpdate(object sender, GameTimerEventArgs e)
        {
            #region 3D
            float time = (float)e.TotalTime.TotalSeconds;
            //59.9511, 010.7654
            #endregion 3D
        }

        private void OnDraw(object sender, GameTimerEventArgs e)
        {
            #region CommonStuff
            SharedGraphicsDeviceManager.Current.GraphicsDevice.Clear(Color.CornflowerBlue);
            #endregion CommonStuff

            #region Silverlight
            spriteBatch.Begin();
            silverlightRenderer.Render();
            spriteBatch.Draw(silverlightRenderer.Texture, Vector2.Zero, Color.White);
            spriteBatch.End();
            SharedGraphicsDeviceManager.Current.GraphicsDevice.BlendState = BlendState.Opaque;
            SharedGraphicsDeviceManager.Current.GraphicsDevice.DepthStencilState = DepthStencilState.Default;
            SharedGraphicsDeviceManager.Current.GraphicsDevice.SamplerStates[0] = SamplerState.LinearWrap;
            #endregion Silverlight

            #region 3D
            view = Matrix.CreateRotationX(MathHelper.PiOver2); //Coordinates Xna 3d space with world space 
            view *= motion.CurrentValue.Attitude.RotationMatrix;
            view *= Matrix.CreateLookAt(Vector3.Zero, new Vector3(0.0f, 0.0f, -0.001f), Vector3.Up);

            projection = Matrix.CreatePerspectiveFieldOfView(1, SharedGraphicsDeviceManager.Current.GraphicsDevice.Viewport.AspectRatio, 0.1f, 100f);

            //Teapots
            teapotWorld = Matrix.CreateScale(3.0f)
                       * Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(0), MathHelper.ToRadians(0), MathHelper.ToRadians(0))
                       * Matrix.CreateTranslation(new Vector3((float)PositionChangeX + 0.0f, 0.0f, (float)(PositionChangeZ - 5.0f)));
            teapot.Draw(teapotWorld, view, projection, Color.Red);
            teapotWorld = Matrix.CreateScale(3.0f)
                       * Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(0), MathHelper.ToRadians(0), MathHelper.ToRadians(0))
                       * Matrix.CreateTranslation(new Vector3((float)PositionChangeX + 0.0f, 0.0f, (float)(PositionChangeZ + 5.0f)));
            teapot.Draw(teapotWorld, view, projection, Color.Blue);
            teapotWorld = Matrix.CreateScale(3.0f)
                       * Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(0), MathHelper.ToRadians(0), MathHelper.ToRadians(0))
                       * Matrix.CreateTranslation(new Vector3((float)PositionChangeX - 5.0f, 0.0f, (float)(PositionChangeZ)));
            teapot.Draw(teapotWorld, view, projection, Color.Green);
            teapotWorld = Matrix.CreateScale(3.0f)
                       * Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(0), MathHelper.ToRadians(0), MathHelper.ToRadians(0))
                       * Matrix.CreateTranslation(new Vector3((float)PositionChangeX + 5.0f, 0.0f, (float)(PositionChangeZ)));
            teapot.Draw(teapotWorld, view, projection, Color.Yellow);

            #region Axcis
            //xAxcisWorld = Matrix.CreateScale(1.0f)
            //           * Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(0), MathHelper.ToRadians(0), MathHelper.ToRadians(90))
            //           * Matrix.CreateTranslation(new Vector3((float)PositionChangeX, 0.0f, (float)PositionChangeZ));

            //yAxcisWorld = Matrix.CreateScale(1.0f)
            //           * Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(0), MathHelper.ToRadians(0), MathHelper.ToRadians(0))
            //           * Matrix.CreateTranslation(new Vector3((float)PositionChangeX, 0.0f, (float)PositionChangeZ));

            //zAxcisWorld = Matrix.CreateScale(1.0f)
            //           * Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(0), MathHelper.ToRadians(90), MathHelper.ToRadians(0))
            //           * Matrix.CreateTranslation(new Vector3((float)PositionChangeX, 0.0f, (float)PositionChangeZ));

            //xAxcis.Draw(xAxcisWorld, view, projection, Color.Red);
            //yAxcis.Draw(yAxcisWorld, view, projection, Color.Green);
            //zAxcis.Draw(zAxcisWorld, view, projection, Color.Blue);
            #endregion Axcis
            #endregion 3D

            #region HelpText
            //try
            //{
            //    Info.Text = "InitialLatitude: " + InitialGeoCoordinate.Location.Latitude + "\n"
            //              + "CurreLatitude: " + CurrentGeoCoordinate.Location.Latitude + "\n"
            //              + "PositionChangeX: " + PositionChangeX + "\n"
            //              + "InitialLongitude: " + InitialGeoCoordinate.Location.Longitude + "\n"
            //              + "CurreLongitude: " + CurrentGeoCoordinate.Location.Longitude + "\n"
            //              + "PositionChangeZ:  " + PositionChangeZ;
            //}
            //catch (Exception)
            //{
            //}
            #endregion HelpText
        }

        #region Motion
        void motion_Calibrate(object sender, CalibrationEventArgs e)
        {
            Dispatcher.BeginInvoke(() => MessageBox.Show("Please calibrate your device!"));
        }
        #endregion Motion

        #region GeoCoordinateWatcher
        void geoWatcher_StatusChanged(object sender, GeoPositionStatusChangedEventArgs e)
        {
            currentState = e.Status;

            switch (e.Status)
            {
                case GeoPositionStatus.Disabled:
                    if (geoWatcher.Permission == GeoPositionPermission.Denied)
                    {
                        string[] strings = { "ok" };
                        MessageBox.Show("Please turn on geo-location service in the settings tab.");
                    }
                    else
                        if (geoWatcher.Permission == GeoPositionPermission.Granted)
                        {
                            string[] strings = { "ok" };
                            MessageBox.Show("Your device doesn't support geo-location service.");
                        }
                    break;
                case GeoPositionStatus.Ready:
                    InitialGeoCoordinate = geoWatcher.Position; //Latitude: 59.949702262878418 Longitude: 10.763088226318359
                    break;
            }
        }

        void geoWatcher_PositionChanged(object sender, GeoPositionChangedEventArgs<GeoCoordinate> e)
        {
            if (currentState == GeoPositionStatus.Ready)
            {
                CurrentGeoCoordinate = e.Position;
                PositionChangeZ = (CurrentGeoCoordinate.Location.Latitude - InitialGeoCoordinate.Location.Latitude) * MeterInLatitude;
                PositionChangeX = (InitialGeoCoordinate.Location.Longitude - CurrentGeoCoordinate.Location.Longitude) * MeterInLongitude;

                //GeoCoordinate GeoCoordinate1 = new  GeoCoordinate(59.949702262878418, 10.763088226318359);
                //double meterInLatitude = GeoCoordinate1.GetDistanceTo(new  GeoCoordinate(60.949702262878418, 10.763088226318359));
                // 1 Latitude degree = 111290.91975341858 meter
                //double meterInLongitude = GeoCoordinate1.GetDistanceTo(new  GeoCoordinate(59.949702262878418, 11.763088226318359));
                // 1 Longitude degree = 55729.5173743959 meter
            }
        }
        #endregion GeoCoordinateWatcher
    }
}