context: /*
      Sentience 3D Perception System: Grid viewer
      Copyright (C) 2000-2007 Bob Mottram
      fuzzgun@gmail.com
  
      This program is free software; you can redistribute it and/or modify
      it under the terms of the GNU General Public License as published by
      the Free Software Foundation; either version 2 of the License, or
      (at your option) any later version.
  
      This program is distributed in the hope that it will be useful,
      but WITHOUT ANY WARRANTY; without even the implied warranty of
      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      GNU General Public License for more details.
  
      You should have received a copy of the GNU General Public License along
      with this program; if not, write to the Free Software Foundation, Inc.,
      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
  
  
  using System;
  using System.IO;
  using System.Runtime.InteropServices;
  using System.Collections.Generic;
  using System.ComponentModel;
  using System.Data;
  using System.Drawing;
  using System.Text;
  using System.Windows.Forms;
  
  using Microsoft.DirectX;
  using Microsoft.DirectX.Direct3D;
  using Gosub;
  using sentience;
  using sentience.core;
  
  namespace DirectXSample
  {
  	public partial class frmGridViewer : Form
  	{
          float pan = 0, tilt = 0, viewing_distance = 220;
  
          String grid_type = "914";
          int current_grid_level = 0;
          occupancygrid currentGrid = null;
          String grid_filename;         //file name of the grid
          bool grid_loaded = true;      //whether the grid has been loaded
          int rendered = 0;
  		
          AutoMesh mCellMesh;
  		Color32	mBackgroundColor = Color.DarkBlue;
  
  	
  		public frmGridViewer()
  		{
  			InitializeComponent();
  		}
  
  
          /// <summary>
          /// view a grid from the given file
          /// </summary>
          /// <param name="d3d"></param>
          /// <param name="dx"></param>
          /// <param name="filename">grid filename</param>
          /// <param name="grid_level">the level of the grid within the grid to be viewed</param>
          /// <param name="min_evidence">minimum evidence threshold</param>
          public void viewOccupancyGrid(Gosub.Direct3d d3d, Microsoft.DirectX.Direct3D.Device dx,
                              String filename, int grid_level, int min_evidence)
          {
              if (!grid_loaded)
              {
                  currentGrid = new occupancygrid(64, 100);
                  switch(grid_type)
                  {
                      case "914":
                          {
                              currentGrid.Load(filename);
                              break;
                          }
                  }
                  grid_loaded = true;
              }
              if (currentGrid != null)
              {
                  if (grid_type=="914")
                      viewGrid(d3d, dx, currentGrid);
              }
          }
  
          public void viewGrid(Gosub.Direct3d d3d, Microsoft.DirectX.Direct3D.Device dx,
                              occupancygrid grid)
          {
              bool ground_plane_drawn = false;
              mCellMesh = new AutoMesh(d3d, Mesh.Box(dx, 1, 1, 1));
  
              //show the grid cells
              for (int z = grid.dimension-1; z >= 0 ; z--)
              {
                  int plane_hits = 0;
                  for (int x = 1; x < grid.dimension - 1; x++)
                  {
                      for (int y = 1; y < grid.dimension-1; y++)
                      {
                          if (grid.display_cell[x, y, z])
                          {
                              plane_hits++;
                              occupancyGridCell c = grid.cell[x, y, z];
                              int r = 0;
                              int g = 0;
                              int b = 255;
                              if (c != null)
                              {
                                  r = c.colour[0];
                                  g = c.colour[1];
                                  b = c.colour[2];
                              }
  
                              dx.Transform.World = Matrix.Translation(-grid.dimension / 2, -grid.dimension / 2, -grid.dimension / 2) //Center model
                                  //* Matrix.Scaling(1, 1, 1) // Make it bigger
                                  //* Matrix.RotationYawPitchRoll(0, 0, 0)
                                                   * Matrix.Translation(grid.dimension - 1 - x, y, z)
                                                   * Matrix.RotationYawPitchRoll(0, tilt, 0); // Then move it where you want
  
                              dx.Material = GraphicsUtility.InitMaterial(Color.FromArgb(r, g, b));
  
                              mCellMesh.M.DrawSubset(0);
                          }
  
  
                          //for (int z = 0; z < grid.dimension; z++)
                          {
                              if (z == grid.dimension / 2)
                              {
                                  if ((x == 1) || (x == grid.dimension - 2) || (y == 1) || (y == grid.dimension - 2))
                                  {
                                      dx.Transform.World = Matrix.Translation(-grid.dimension / 2, -grid.dimension / 2, -grid.dimension / 2) //Center model
                                                           * Matrix.Translation(x, y, z)
                                                           * Matrix.RotationYawPitchRoll(0, tilt, 0); // Then move it where you want
  
                                      dx.Material = GraphicsUtility.InitMaterial(Color.Green);
                                      mCellMesh.M.DrawSubset(0);
                                  }
                              }
                          }
                      }
  
                  }
  
                  if ((plane_hits > 30) && (!ground_plane_drawn))
                  {
                      ground_plane_drawn = true;
                      for (int x = 1; x < grid.dimension - 1; x++)
                      {
                          for (int y = 1; y < grid.dimension - 1; y++)
                          {
                              if (grid.empty[x, y])
                              {
                                  //occupancyGridCell c = grid.cell[x, y, z];
                                  int r = 0;
                                  int g = 0;
                                  int b = 255;
  
                                  dx.Transform.World = Matrix.Translation(-grid.dimension / 2, -grid.dimension / 2, -grid.dimension / 2) //Center model
                                      //* Matrix.Scaling(1, 1, 1) // Make it bigger
                                      //* Matrix.RotationYawPitchRoll(0, 0, 0)
                                                       * Matrix.Translation(grid.dimension - 1 - x, y, z)
                                                       * Matrix.RotationYawPitchRoll(0, tilt, 0); // Then move it where you want
  
                                  dx.Material = GraphicsUtility.InitMaterial(Color.FromArgb(r, g, b));
  
                                  mCellMesh.M.DrawSubset(0);
                              }
                          }
                      }
                  }
  
  
              }
  
          }
  
  
  		/// <summary>
  		/// Occurs once after DirectX has been initialized for the first time.  
  		/// Setup AutoMesh's, AutoVertexBuffer's, and AutoTexture's here.
  		/// </summary>
  		private void mD3d_DxLoaded(Gosub.Direct3d d3d, Microsoft.DirectX.Direct3D.Device dx)
  		{
  		}
  
  		// More accurate than Environment.TickCount for smoother motion
  		[System.Security.SuppressUnmanagedCodeSecurity]
  		[DllImport("winmm.dll")]
  		private static extern int timeGetTime();
  
  		
  		/// <summary>
  		/// Occurs when it is time to render 3d objects.  Place all 3d
  		/// drawing code in this event.
  		/// </summary>
  		private void mD3d_DxRender3d(Gosub.Direct3d d3d, Microsoft.DirectX.Direct3D.Device dx)
  		{
              //if (rendered < 5)
              {
                  // Setup the lights
                  dx.Lights[0].Enabled = true;
                  dx.Lights[0].Type = LightType.Directional;
                  dx.Lights[0].Direction = new Vector3(0, 0, 1);
                  dx.Lights[0].Diffuse = Color.White;
                  dx.Lights[0].Position = new Vector3(0, 0, 0);
                  dx.RenderState.Lighting = true;
  
                  // Set viewer		
                  dx.Transform.View = Matrix.LookAtLH(new Vector3(0, 0, -viewing_distance),
  
gt:                     new Vector3(0.0f, 0.0f, 0.0f), new Vector3(pan, 1.0f, tilt));