///////////////////////////////////////////////////////////////
        // Create switch command for various sections, split code into separate objects / function of 3D drawing for each type
        /////////////////////////////////////////////////
        // Tutorial
        /// http://kindohm.com/technical/WPF3DTutorial.htm  ScreenSpaceLines3D
        /// <summary>
        /// ///////////////////////////////////////////////////////
        /// MAIN CONSTRUCTOR
        /// ///////////////////////////////////////////////////////
        /// </summary>
        //---------------------------------------------------------------------------------------------
        //---------------------------------------------------------------------------------------------
        //---------------------------------------------------------------------------------------------
        public Window2(bool bDebugging)
        {
            InitializeComponent();

              Model3DGroup gr = new Model3DGroup();
              //gr.Children.Add(new AmbientLight());

              GeometryModel3D SolidModel3D = new GeometryModel3D();
              MeshGeometry3D mesh = new MeshGeometry3D();
              mesh.Positions = new Point3DCollection();

              //ScreenSpaceLines3D line = new ScreenSpaceLines3D();
              //line.Color = Color.FromRgb(0,255,0);
              //line.Points.Add(mesh.Positions[0]);
              //line.Points.Add(mesh.Positions[1]);

              //Viewport3D view = new Viewport3D();
              //view.Children.Add(line);

              //gr.Children.Add(new AmbientLight());
              SolidModel3D.Geometry = mesh;
              SolidColorBrush br = new SolidColorBrush(Color.FromRgb(255, 0, 0));
              SolidModel3D.Material = new DiffuseMaterial(br);

              gr.Children.Add(SolidModel3D); // Add solid to model group

              _trackport.Model = (Model3D)gr; //CreateRectangle(p3, p2, p6, p7, Brushes.Red);

              _trackport.Trackball.TranslateScale = 1000;   //step for moving object (panning)

              _trackport.SetupScene();
        }

        public MainWindow()
        {
            this.InitializeComponent();
            //BuildSolid();   //导入图形入口

            // 在此点下面插入创建对象所需的代码。
            // 初始化
            // Geometry creation
            mGeometryAdd = new GeometryModel3D[100];
            txt3D = new GeometryModel3D[100];
            AddCnt = 0;
            AddCntTxt = 0;

               /*DiffuseMaterial tmp = new DiffuseMaterial();
            SolidColorBrush solid = new SolidColorBrush();
            solid.Opacity = 0.5;
            tmp.Brush = new SolidColorBrush(solid);
              mGeometry = new GeometryModel3D(test, tmp);*/

            mGeometry = new GeometryModel3D(test, new DiffuseMaterial(Brushes.YellowGreen));

            mGeometry.Transform = new Transform3DGroup();
            group.Children.Add(mGeometry);
            MapHave = 1;
            MapCnt++;

            draw = new DrawOperator();

            PathHave = 0;   //是否添加巡检路径
            DataHave = 0;   //是否添加巡检数据
        }

        public NodesVisual()
        {
            InitializeComponent();

            // Make IMU model here

            Model3DGroup items = new Model3DGroup();
            GeometryModel3D model = new GeometryModel3D();
            MeshGeometry3D geom = new MeshGeometry3D();
            DiffuseMaterial paint = new DiffuseMaterial(new SolidColorBrush(Colors.Red));

            double xPos, yPos, zPos;

            for(int z = 0; z < 2; z++)
                for (int y = 0; y < 2; y++)
                    for (int x = 0; x < 2; x++)
                    {
                        xPos = x * CUBE_SIZE - CUBE_SIZE / 2;
                        yPos = y * CUBE_SIZE - CUBE_SIZE / 2;
                        zPos = z * CUBE_SIZE - CUBE_SIZE / 2;

                        geom.Positions.Add(new Point3D(xPos, yPos, zPos));
                        Debug.Print("(" + x + "," + y + "," + z + ")");
                    }
            

            // TriangleIndices = "2 3 1  2 1 0  7 1 3  7 5 1  6 5 7  6 4 5  6 2 0  6 0 4  2 7 3  2 6 7  0 1 5  0 5 4" >
            geom.TriangleIndices.Add(2);    geom.TriangleIndices.Add(3);    geom.TriangleIndices.Add(1);
            geom.TriangleIndices.Add(2);    geom.TriangleIndices.Add(1);    geom.TriangleIndices.Add(0);
            geom.TriangleIndices.Add(7);    geom.TriangleIndices.Add(1);    geom.TriangleIndices.Add(3);

            geom.TriangleIndices.Add(7);    geom.TriangleIndices.Add(5);    geom.TriangleIndices.Add(1);
            geom.TriangleIndices.Add(6);    geom.TriangleIndices.Add(5);    geom.TriangleIndices.Add(7);
            geom.TriangleIndices.Add(6);    geom.TriangleIndices.Add(4);    geom.TriangleIndices.Add(5);

            geom.TriangleIndices.Add(6);    geom.TriangleIndices.Add(2);    geom.TriangleIndices.Add(0);
            geom.TriangleIndices.Add(6);    geom.TriangleIndices.Add(0);    geom.TriangleIndices.Add(4);
            geom.TriangleIndices.Add(2);    geom.TriangleIndices.Add(7);    geom.TriangleIndices.Add(3);

            geom.TriangleIndices.Add(2);    geom.TriangleIndices.Add(6);    geom.TriangleIndices.Add(7);
            geom.TriangleIndices.Add(0);    geom.TriangleIndices.Add(1);    geom.TriangleIndices.Add(5);
            geom.TriangleIndices.Add(0);    geom.TriangleIndices.Add(5);    geom.TriangleIndices.Add(4);

            model.Material = paint;
            model.Geometry = geom;
            items.Children.Add(model);

            imu = new ModelVisual3D();
            imu.Content = items;
            viewportIMU.Children.Add(imu);

            combobox_nodeSelect.Items.Clear();
            Nodes.UpdateAvailableSensors();

            foreach (string sensor in Nodes.COM_Ports)
                combobox_nodeSelect.Items.Add(sensor);

            if (!combobox_nodeSelect.Items.IsEmpty)
                combobox_nodeSelect.SelectedIndex = 0;
        }

 // Public parameterless constructor
 public ModelBase()
 {
     meshGeometry = new MeshGeometry3D();
     modelContent = new GeometryModel3D();
     Geometry = meshGeometry;
     Content = modelContent;
 }

        public virtual Model3DGroup GetModel()
        {
            MeshGeometry3D meshVehicle = new MeshGeometry3D();
            Point3D p1 = new Point3D(0, 0, 0);
            Point3D p2 = new Point3D(1, 0, 0);
            Point3D p3 = new Point3D(1, 0, 1);
            Point3D p4 = new Point3D(0, 0, 1);
            Point3D p5 = new Point3D(0, 1, 0);
            Point3D p6 = new Point3D(1, 1, 0);
            Point3D p7 = new Point3D(1, 1, 1);
            Point3D p8 = new Point3D(0, 1, 1);
            Helpers.Geometry.Geometry3DHelper.CubeModel(p1, p2, p3, p4, p5, p6, p7, p8, ref meshVehicle);

            GeometryModel3D geom = new GeometryModel3D(meshVehicle, new DiffuseMaterial(Brushes.OrangeRed));

            Model3DGroup group = new Model3DGroup();
            //geom.Transform = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 1, 0), angle));
            group.Children.Add(geom);

            Transform3DGroup trgr = new Transform3DGroup();
            trgr.Children.Add(new ScaleTransform3D(Size.Y, Size.X, Size.Z));
            trgr.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 3), -Angle)));
            trgr.Children.Add(new TranslateTransform3D(Position.Y, Position.X, 0));
            group.Transform = trgr;
            return group;
        }

 private void LoadMeshes(GeometryModel3D[] meshes, Scene scene, SystemMaterial[] materials)
 {
      for (int i = 0; i < scene.MeshCount; ++i)
     {
         meshes[i] = this.ConvertMesh(scene.Meshes[i], materials);
     }
 }

        //-------------------------------------------------------------------------------------
        /// <summary>
        /// CreateSatelliteModel
        /// </summary>
        //-------------------------------------------------------------------------------------
        public static GeometryModel3D CreateSatelliteModel(Model3DGroup group)
        {
            var model2 = new GeometryModel3D();
            var mesh2 = new MeshGeometry3D();

            mesh2.Positions.Add(new Point3D(0, 0, -.1));
            mesh2.Positions.Add(new Point3D(-.1, 0, .1));
            mesh2.Positions.Add(new Point3D(.1, 0, .1));
            mesh2.Positions.Add(new Point3D(0, .1, 0));
            mesh2.TriangleIndices.Add(0);
            mesh2.TriangleIndices.Add(1);
            mesh2.TriangleIndices.Add(2);

            mesh2.TriangleIndices.Add(0);
            mesh2.TriangleIndices.Add(3);
            mesh2.TriangleIndices.Add(1);

            mesh2.TriangleIndices.Add(3);
            mesh2.TriangleIndices.Add(2);
            mesh2.TriangleIndices.Add(1);

            mesh2.TriangleIndices.Add(1);
            mesh2.TriangleIndices.Add(1);
            mesh2.TriangleIndices.Add(3);

            model2.Geometry = mesh2;

            return model2;
        }

        public ParticleSystem(int maxCount, System.Windows.Media.Color color)
        {
            this.maxParticleCount = maxCount;

            this.particleList = new List<Particle>();

            this.particleModel = new GeometryModel3D();
            this.particleModel.Geometry = new MeshGeometry3D();

            Ellipse e = new Ellipse();
            e.Width = 32.0;
            e.Height = 32.0;
            RadialGradientBrush b = new RadialGradientBrush();
            b.GradientStops.Add(new GradientStop(System.Windows.Media.Color.FromArgb(0xFF, color.R, color.G, color.B), 0.25));
            b.GradientStops.Add(new GradientStop(System.Windows.Media.Color.FromArgb(0x00, color.R, color.G, color.B), 1.0));
            e.Fill = b;
            e.Measure(new System.Windows.Size(32, 32));
            e.Arrange(new Rect(0, 0, 32, 32));

            var brush = new VisualBrush(e);

            DiffuseMaterial material = new DiffuseMaterial(brush);

            this.particleModel.Material = material;

            this.rand = new Random(brush.GetHashCode());
        }

 /// <summary>
 /// Применить трансформацию к модели.
 /// </summary>
 /// <param name="model">Модель.</param>
 /// <param name="frameSection">Секция фрейма модели.</param>
 private void ApplyTransformation(GeometryModel3D model, Frame frameSection)
 {
     Matrix3D currentMatrix = this.GetMatrix(frameSection);
       while (frameSection.ParentFrame >= 0)
     currentMatrix = Matrix3D.Multiply(currentMatrix, this.GetMatrix(this.ModelGroupMetadata.FrameList.Structure.Frames[frameSection.ParentFrame]));
       model.Transform = new MatrixTransform3D(currentMatrix);
 }

        private void Expand(GeometryModel3D model, Transform3D transformation)
        {
            Transform3D ot;
            if (originalTransforms.ContainsKey(model))
                ot = originalTransforms[model];
            else
            {
                ot = model.Transform;
                originalTransforms.Add(model, ot);
            }

            Transform3D totalTransform = Transform3DHelper.CombineTransform(transformation, ot);

            var mesh = model.Geometry as MeshGeometry3D;
            if (mesh == null)
                return;
            var bounds = new Rect3D();
            foreach (int i in mesh.TriangleIndices)
                bounds.Union(totalTransform.Transform(mesh.Positions[i]));

            Point3D p = bounds.Location;
            Vector3D d = p - actualExpandOrigin;
            d *= Expansion;
            Point3D p2 = actualExpandOrigin + d;
            var t = new TranslateTransform3D(p2 - p);

            model.Transform = Transform3DHelper.CombineTransform(ot, t);
        }

        private void CreateBackFace(int width, int height, BitmapImage bitmapImage)
        {
            ;

            MeshGeometry3D mesh = new MeshGeometry3D();

            mesh.Positions.Add(new Point3D(-width, -height, -10));
            mesh.Positions.Add(new Point3D(width, -height, -10));
            mesh.Positions.Add(new Point3D(width, height, -10));
            mesh.Positions.Add(new Point3D(-width, height, -10));

            mesh.TriangleIndices.Add(0);
            mesh.TriangleIndices.Add(1);
            mesh.TriangleIndices.Add(3);
            mesh.TriangleIndices.Add(1);
            mesh.TriangleIndices.Add(2);
            mesh.TriangleIndices.Add(3);

            // These are the lines you need to allow an image to be painted onto the 3d model
            mesh.TextureCoordinates.Add(new Point(0, 0));
            mesh.TextureCoordinates.Add(new Point(1, 0));
            mesh.TextureCoordinates.Add(new Point(1, 1));
            mesh.TextureCoordinates.Add(new Point(0, 1));

            ImageBrush imageBrush = new ImageBrush(bitmapImage);

            GeometryModel3D geometry = new GeometryModel3D(mesh, new DiffuseMaterial(imageBrush));
            //GeometryModel3D geometry = new GeometryModel3D(mesh, new DiffuseMaterial(new SolidColorBrush(Colors.Red)));

            group.Children.Add(geometry);
        }

 public PlaneModel(GeometryModel3D plane, Color planeColor, Point3DCollection points)
 {
     this.plane = plane;
     this.planeColor = planeColor;
     this.points = points;
     this.crosses = 0;
 }

        private void CreateModel()
        {
            const double dt = 0.1;
            const int nSteps = 100;
            var mb = new MeshBuilder(true, true);
            for (double y0 = -5; y0 <= 5; y0 += 0.25)
            {
                var p0 = new Point(-3, y0);
                Point[] pts = Solve(Velocity, p0, dt, nSteps);
                var vel = new double[pts.Length];
                var diam = new double[pts.Length];
                int i = 0;
                var pts3d = new Point3D[pts.Length];
                double vmax = 0;
                foreach (Point pt in pts)
                {
                    pts3d[i] = new Point3D(pt.X, pt.Y, 0);
                    double v = Velocity(pt.X, pt.Y).Length;
                    if (v > vmax) vmax = v;
                    vel[i++] = v;
                }
                for (int j = 0; j < vel.Length; j++)
                    vel[j] /= vmax;
                for (int j = 0; j < vel.Length; j++)
                    diam[j] = 0.075;

                mb.AddTube(pts3d, vel, diam, 12, false);
            }
            StreamLinesModel = new GeometryModel3D();
            StreamLinesModel.Geometry = mb.ToMesh();
            StreamLinesModel.Material = Materials.Hue;
            StreamLinesModel.BackMaterial = Materials.Hue;
        }

        public GeometryModel3D ConvertOne( EngineViewModel engine )
        {
            var white = Color.FromRgb(255, 255, 255);
            var whiteBrush = new SolidColorBrush(white);
            var model = new GeometryModel3D(GetGeometry(), new DiffuseMaterial(whiteBrush));

            var transform = new Transform3DGroup();
            var scaleTransform = new ScaleTransform3D();
            var translateTransform = new TranslateTransform3D();

            transform.Children.Add(scaleTransform);
            transform.Children.Add(translateTransform);

            scaleTransform.ScaleX = 3;
            scaleTransform.ScaleY = 3;

            var powerBinding = new Binding("Power") {Mode = BindingMode.OneWay, Source = engine};
            BindingOperations.SetBinding(
                scaleTransform,
                ScaleTransform3D.ScaleZProperty,
                powerBinding);

            translateTransform.OffsetX = engine.OffsetX;
            translateTransform.OffsetY = engine.OffsetY;

            model.Transform = transform;

            return model;
        }

 // Public parameterless constructor
 public ModelVisualBase()
 {
     meshgeo = new MeshGeometry3D();
     geomodel = new GeometryModel3D();
     Geometry = meshgeo;
     Content = geomodel;
 }

        public Model3DGroup CreateTriangleModel(Point3D p0, Point3D p1, Point3D p2)
        {
            MeshGeometry3D mesh = new MeshGeometry3D();
            mesh.Positions.Add(p0);
            mesh.Positions.Add(p1);
            mesh.Positions.Add(p2);

            for (int i = 0; i < 3; i++)
            {
                mesh.TriangleIndices.Add(i);
            }

            Vector3D normal = CalculateNormal(p0, p1, p2);

            for (int i = 0; i < 3; i++)
            {
                mesh.Normals.Add(normal);
            }

            Material material = new DiffuseMaterial(new SolidColorBrush(Colors.DarkCyan));
            GeometryModel3D model = new GeometryModel3D(mesh, material);
            Model3DGroup group = new Model3DGroup();
            group.Children.Add(model);
            return group;
        }

        private void Initializer(Point3D location)
        {
            var geometryModel = new GeometryModel3D();

            var meshBuilder = new MeshBuilder();

            meshBuilder.AddBox(new Point3D(location.X, location.Y, location.Z + 3.5), 1.5, 1.5, 0.25);

            meshBuilder.AddBox(new Point3D(location.X, location.Y + 1, location.Z + 3.5), 0.25, 1.25, 0.25);
            meshBuilder.AddBox(new Point3D(location.X, location.Y - 1, location.Z + 3.5), 0.25, 1.25, 0.25);

            meshBuilder.AddBox(new Point3D(location.X + 1, location.Y , location.Z + 3.5), 1.25, 0.25, 0.25);
            meshBuilder.AddBox(new Point3D(location.X - 1, location.Y, location.Z + 3.5), 1.25, 0.25, 0.25);

            meshBuilder.AddBox(new Point3D(location.X, location.Y + 1.5, location.Z + 3), 0.25, 0.25, 0.75);
            meshBuilder.AddBox(new Point3D(location.X, location.Y - 1.5, location.Z + 3), 0.25, 0.25, 0.75);

            meshBuilder.AddBox(new Point3D(location.X + 1.5, location.Y, location.Z + 3), 0.25, 0.25, 0.75);
            meshBuilder.AddBox(new Point3D(location.X - 1.5, location.Y, location.Z + 3), 0.25, 0.25, 0.75);

            meshBuilder.AddBox(new Point3D(location.X, location.Y, location.Z + 4), 0.5, 0.5, 1);

            geometryModel.Geometry = meshBuilder.ToMesh();
            geometryModel.Material = Materials.Gold;

            Visual3DModel = geometryModel;
        }

        public MeshElement3D()
        {
            Content = new GeometryModel3D();
            CompositionTarget.Rendering += CompositionTarget_Rendering;

            InvalidateModel();
        }

        // Add all cubes to a ModelVisual3D, reuse geometry but create new visual for each cube - this is slow
        /*   GeometryModel3D AddGeometrySeparate(IEnumerable<Point3D> centers, double L)
           {
               var mv = new ModelVisual3D();

               var cubit = new CubeVisual3D { SideLength = L * 0.95, Fill = Brushes.Gold };
               var cuboidGeometry = cubit.Model.Geometry as MeshGeometry3D;
               var r = new Random();

               foreach (var center in centers)
               {
                   var tg = new Transform3DGroup();
                   tg.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(1, 0, 0), (r.NextDouble() - 0.5) * 10)));
                   tg.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 1, 0), (r.NextDouble() - 0.5) * 10)));
                   tg.Children.Add(new TranslateTransform3D(center.ToVector3D()));

                   var c = new ModelVisual3D
                               {
                                   Content =
                                       new GeometryModel3D
                                           {
                                               Geometry = cuboidGeometry,
                                               Material = cubit.Material,
                                               Transform = tg
                                           }
                               };
                   mv.Children.Add(c);
               }
              return mv;
           }*/

        // All cubes in one GeometryModel - much faster
        GeometryModel3D AddGeometry(IEnumerable<Point3D> centers, double L)
        {
            var w = new Stopwatch();
            w.Start();
            /*            var geometry = new MeshGeometry3D();
                        foreach (var center in centers)
                        {
                            MeshGeometryHelper.AddBox(geometry,center, L, L, L);
                        }
                        */

            var builder = new MeshBuilder();
            foreach (var center in centers)
            {
                builder.AddBox(center, L, L, L);
            }
            var geometry = builder.ToMesh();
            geometry.Freeze();

            Trace.WriteLine(Level + ": " + w.ElapsedMilliseconds + " ms");

            var mv = new GeometryModel3D
                             {
                                 Geometry = geometry,
                                 Material = MaterialHelper.CreateMaterial(Brushes.Gold)
                             };
            TriangleCount = geometry.TriangleIndices.Count / 3;

            return mv;
        }

        public PhotoAlbum()
        {
            InitializeComponent();
            dt.Interval = TimeSpan.FromSeconds(2);
            dt.Tick += new EventHandler(dt_Tick);
            for (int i = 0; i < 16; i++)
            {
                ImageBrush ib = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/images/albums/im" + i + ".jpg")));
                ib.Stretch = Stretch.Uniform;

                ModelVisual3D mv = new ModelVisual3D();
                Material mat = new DiffuseMaterial(ib);
                GeometryModel3D plane = new GeometryModel3D(planeFactory.Mesh, mat);
                mv.Content = plane;
                mvs[i] = mv;
                myViewPort3D.Children.Add(mv);
                Matrix3D trix = new Matrix3D();
                double x = ran.NextDouble() * 50 - 50;
                double y = ran.NextDouble() * 2 - 2;
                double z = -i * 10;
                p3s[i] = new Point3D(x, y, z);
                trix.Append(new TranslateTransform3D(x, y, z).Value);
                mv.Transform = new MatrixTransform3D(trix);
            }

            pa = new Point3DAnimation(p3s[0], TimeSpan.FromMilliseconds(300));
            pa.AccelerationRatio = 0.3;
            pa.DecelerationRatio = 0.3;
            pa.Completed += new EventHandler(pa_Completed);
            cam.BeginAnimation(PerspectiveCamera.PositionProperty, pa);
        }