ardupilot/Tools/ArdupilotMegaPlanner/Controls/OpenGLtest.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenTK;
using OpenTK.Graphics.OpenGL;
using System.Drawing.Imaging;
using System.Drawing;
using GMap.NET;
using GMap.NET.WindowsForms;

namespace ArdupilotMega.Controls
{
    public class OpenGLtest : GLControl
    {
        public static OpenGLtest instance;

        // terrain image
        Bitmap _terrain = new Bitmap(640,480);
        int texture = 0;

        float _angle = 0;
        double cameraX, cameraY, cameraZ;       // camera coordinates
        double lookX, lookY, lookZ;             // camera look-at coordinates

        double step = 1 / 1200.0;

        RectLatLng area = new RectLatLng(-35.04286,117.84262,0.1,0.1);

        double _alt = 0;
        public PointLatLngAlt LocationCenter { 
            get { 
                return new PointLatLngAlt(area.LocationMiddle.Lat, area.LocationMiddle.Lng,_alt,"");
            }
            set {

                if (area.LocationMiddle.Lat == value.Lat && area.LocationMiddle.Lng == value.Lng)
                    return;

                if (value.Lat == 0 && value.Lng == 0)
                    return;

                _alt = value.Alt;
                area = new RectLatLng(value.Lat + 0.15, value.Lng - 0.15, 0.3, 0.3);
               // Console.WriteLine(area.LocationMiddle + " " + value.ToString());
                this.Invalidate();
            } 
        }

        public Vector3 rpy = new Vector3();

        public OpenGLtest()
        {
            instance = this;

            InitializeComponent();

            GL.GenTextures(1, out texture);
        }

        void getImage()
        {
            MapType type = MapType.GoogleSatellite;
            PureProjection prj = null;
            int maxZoom;

            GMaps.Instance.AdjustProjection(type, ref prj, out maxZoom);
          int zoom = 11; // 12
            if (!area.IsEmpty)
            {
                try
                {
                    List<GPoint> tileArea = prj.GetAreaTileList(area, zoom, 0);
                    //string bigImage = zoom + "-" + type + "-vilnius.png";

                    //Console.WriteLine("Preparing: " + bigImage);
                    //Console.WriteLine("Zoom: " + zoom);
                    //Console.WriteLine("Type: " + type.ToString());
                    //Console.WriteLine("Area: " + area);

                    var types = GMaps.Instance.GetAllLayersOfType(type);

                    // current area
                    GPoint topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom);
                    GPoint rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom);
                    GPoint pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y);

                    int padding = 0;
                    {
                        using (Bitmap bmpDestination = new Bitmap(pxDelta.X + padding * 2, pxDelta.Y + padding * 2))
                        {
                            using (Graphics gfx = Graphics.FromImage(bmpDestination))
                            {
                                gfx.CompositingMode = System.Drawing.Drawing2D.CompositingMode.SourceOver;

                                // get tiles & combine into one
                                foreach (var p in tileArea)
                                {
                                   //Console.WriteLine("Downloading[" + p + "]: " + tileArea.IndexOf(p) + " of " + tileArea.Count);

                                    foreach (MapType tp in types)
                                    {
                                        Exception ex;
                                        WindowsFormsImage tile = GMaps.Instance.GetImageFrom(tp, p, zoom, out ex) as WindowsFormsImage;
                                        if (tile != null)
                                        {
                                            using (tile)
                                            {
                                                int x = p.X * prj.TileSize.Width - topLeftPx.X + padding;
                                                int y = p.Y * prj.TileSize.Width - topLeftPx.Y + padding;
                                                {
                                                    gfx.DrawImage(tile.Img, x, y, prj.TileSize.Width, prj.TileSize.Height);
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                            _terrain = new Bitmap(bmpDestination, 512, 512);


                            GL.BindTexture(TextureTarget.Texture2D, texture);

                            BitmapData data = _terrain.LockBits(new System.Drawing.Rectangle(0, 0, _terrain.Width, _terrain.Height),
                    ImageLockMode.ReadOnly, System.Drawing.Imaging.PixelFormat.Format32bppArgb);

                            //Console.WriteLine("w {0} h {1}",data.Width, data.Height);

                            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgba, data.Width, data.Height, 0,
                                OpenTK.Graphics.OpenGL.PixelFormat.Bgra, PixelType.UnsignedByte, data.Scan0);

                            _terrain.UnlockBits(data);

                            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear);
                            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear);


                        }
                    }
                }
                catch { }
            }
        }

        const float rad2deg = (float)(180 / Math.PI);
        const float deg2rad = (float)(1.0 / rad2deg);

        public Vector3 Normal(Vector3 a, Vector3 b, Vector3 c)
        {
                var dir = Vector3.Cross(b - a, c - a);
                var norm = Vector3.Normalize(dir);
                return norm;
        }


        protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
        {
            if (this.DesignMode)
                return;

            if (area.LocationMiddle.Lat == 0 && area.LocationMiddle.Lng == 0)
                return;

            _angle+=1f;

           // area.LocationTopLeft = new PointLatLng(area.LocationTopLeft.Lat + 0.0001,area.LocationTopLeft.Lng);

            //area.Size = new SizeLatLng(0.1, 0.1);

            try
            {
                base.OnPaint(e);

            }
            catch { return;  }

            double heightscale = (step / 90.0) * 4;

            float scale = 1.0f;

            float radians = (float)(Math.PI * (rpy.Z * -1) / 180.0f);

             //radians = 0;

            float mouseY = (float)(0.1 * scale);
 
      cameraX = area.LocationMiddle.Lng;     // multiplying by mouseY makes the
      cameraZ = area.LocationMiddle.Lat;    // camera get closer/farther away with mouseY
      cameraY = (LocationCenter.Alt < srtm.getAltitude(cameraZ, cameraX, 20)) ? (srtm.getAltitude(cameraZ, cameraX, 20)+ 0.2) * heightscale : LocationCenter.Alt * heightscale;// (srtm.getAltitude(lookZ, lookX, 20) + 100) * heighscale;


      lookX = area.LocationMiddle.Lng + Math.Sin(radians) * mouseY; ;
      lookY = cameraY;
      lookZ = area.LocationMiddle.Lat + Math.Cos(radians) * mouseY; ;


            MakeCurrent();


            GL.MatrixMode(MatrixMode.Projection);

            OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView(60 * deg2rad, 1f, 0.00001f, 5000.0f);
            GL.LoadMatrix(ref projection);

            Matrix4 modelview = Matrix4.LookAt((float)cameraX, (float)cameraY, (float)cameraZ, (float)lookX, (float)lookY, (float)lookZ, 0,1,0);
            GL.MatrixMode(MatrixMode.Modelview);

            // roll
            modelview = Matrix4.Mult(modelview,Matrix4.CreateRotationZ (rpy.X * deg2rad));
            // pitch
            modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationX(rpy.Y * -deg2rad));

            GL.LoadMatrix(ref modelview);

            GL.ClearColor(Color.Blue);

            GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);

            GL.LightModel(LightModelParameter.LightModelAmbient,new float[] {1f,1f,1f,1f});

            GL.Enable(EnableCap.Texture2D);
            GL.BindTexture(TextureTarget.Texture2D, texture);
            /*
            GL.Begin(BeginMode.LineStrip);

            GL.Color3(Color.White);
            GL.Vertex3(0, 0, 0);

            GL.Vertex3(area.Bottom, 0, area.Left);

            GL.Vertex3(lookX, lookY, lookZ);

            //GL.Vertex3(cameraX, cameraY, cameraZ);

            GL.End();
            */
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

            sw.Start();

            getImage();

            sw.Stop();

            Console.WriteLine("img " +sw.ElapsedMilliseconds);

            sw.Start();

           double increment = step * 1;

            double cleanup = area.Bottom % increment;
            double cleanup2 = area.Left % increment;

            for (double z = (area.Bottom - cleanup); z < area.Top - step; z += increment)
            {
                //Makes OpenGL draw a triangle at every three consecutive vertices
                GL.Begin(BeginMode.TriangleStrip);
                for (double x = (area.Left - cleanup2); x < area.Right - step; x += increment)
                {
                    int heightl = srtm.getAltitude(z, area.Right + area.Left - x, 20);

                 //   Console.WriteLine(x + " " + z);

                    GL.Color3(Color.White);


                   // int heightl = 0;

                    double scale2 = (Math.Abs(x - area.Left) / area.WidthLng);// / (float)_terrain.Width;

                    double scale3 = (Math.Abs(z - area.Bottom) / area.HeightLat);// / (float)_terrain.Height;

                    double imgx = 1 - scale2;
                    double imgy = 1 - scale3;
                   // GL.Color3(Color.Red);

                    //GL.Color3(_terrain.GetPixel(imgx, imgy));
                    GL.TexCoord2(imgx,imgy);
                    GL.Vertex3(x, heightl * heightscale, z); //  _terrain.GetPixel(x, z).R
            
                    try
                    {
                        heightl = srtm.getAltitude(z + increment, area.Right + area.Left - x, 20);

                        //scale2 = (Math.Abs(x - area.Left) / area.WidthLng) * (float)_terrain.Width;

                        scale3 = (Math.Abs(((z + increment) - area.Bottom)) / area.HeightLat);// / (float)_terrain.Height;

                        imgx = 1- scale2;
                        imgy = 1 - scale3;
                       // GL.Color3(Color.Green);
                       //GL.Color3(_terrain.GetPixel(imgx, imgy));
                        GL.TexCoord2(imgx,imgy);
                        GL.Vertex3(x, heightl * heightscale, z + increment);

                      //  Console.WriteLine(x + " " + (z + step));
                    }
                    catch { break; }
                    
                }
                GL.End();
            }

            GL.Enable(EnableCap.Blend);
            GL.DepthMask(false);
            GL.BlendFunc(BlendingFactorSrc.SrcAlpha, BlendingFactorDest.One);
            GL.DepthMask(true);
            GL.Disable(EnableCap.Blend);

            GL.Flush();


            sw.Stop();

            Console.WriteLine("GL  "+sw.ElapsedMilliseconds);

            this.SwapBuffers();

            //this.Invalidate();
        }

        private void InitializeComponent()
        {
            this.SuspendLayout();
            // 
            // OpenGLtest
            // 
            this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
            this.Name = "OpenGLtest";
            this.Load += new System.EventHandler(this.test_Load);
            this.Resize += new System.EventHandler(this.test_Resize);
            this.ResumeLayout(false);

        }

        private void test_Load(object sender, EventArgs e)
        {
            GL.Enable(EnableCap.DepthTest);
           // GL.Enable(EnableCap.Light0);
            GL.Enable(EnableCap.Lighting);
            GL.Enable(EnableCap.ColorMaterial);
            GL.Enable(EnableCap.Normalize);

            //GL.Enable(EnableCap.LineSmooth);
            //GL.Enable(EnableCap.PointSmooth);
            //GL.Enable(EnableCap.PolygonSmooth);
            GL.ShadeModel(ShadingModel.Smooth);
            GL.Enable(EnableCap.CullFace);
            GL.Enable(EnableCap.Texture2D);

        }

        private void test_Resize(object sender, EventArgs e)
        {
            MakeCurrent();

            GL.Viewport(0, 0, this.Width, this.Height);

            this.Invalidate();
        }
    }
}