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 OnBoundsChanged(Rect3D bounds)
		{
			Children.Clear();

			double x0 = bounds.X;
			double x1 = x0 + bounds.SizeX;
			double y0 = bounds.Y;
			double y1 = y0 + bounds.SizeY;
			double z0 = bounds.Z;
			double z1 = z0 + bounds.SizeZ;

			// bottom lines
			AddLine(x0, y0, z0, x1, y0, z0);
			AddLine(x1, y0, z0, x1, y1, z0);
			AddLine(x1, y1, z0, x0, y1, z0);
			AddLine(x0, y1, z0, x0, y0, z0);

			// top lines
			AddLine(x0, y0, z1, x1, y0, z1);
			AddLine(x1, y0, z1, x1, y1, z1);
			AddLine(x1, y1, z1, x0, y1, z1);
			AddLine(x0, y1, z1, x0, y0, z1);

			// vertical lines
			AddLine(x0, y0, z0, x0, y0, z1);
			AddLine(x1, y0, z0, x1, y0, z1);
			AddLine(x0, y1, z0, x0, y1, z1);
			AddLine(x1, y1, z0, x1, y1, z1);
		}

		public static Vector3D TransformToBounds(this Vector3D vector, Rect3D bounds)
		{
			vector.X = vector.X * bounds.SizeX + bounds.X;
			vector.Y = vector.Y * bounds.SizeY + bounds.Y;
			vector.Z = vector.Z * bounds.SizeZ + bounds.Z;

			return vector;
		}

		public static Point3D TransformTo01(this Point3D point, Rect3D bounds)
		{
			point.X = (point.X - bounds.X) / bounds.SizeX;
			point.Y = (point.Y - bounds.Y) / bounds.SizeY;
			point.Z = (point.Z - bounds.Z) / bounds.SizeZ;

			return point;
		}

		public static Point3D TransformToBounds(this Point3D point, Rect3D bounds)
		{
			point.X = point.X * bounds.SizeX + bounds.X;
			point.Y = point.Y * bounds.SizeY + bounds.Y;
			point.Z = point.Z * bounds.SizeZ + bounds.Z;

			return point;
		}

        public TestModelVisual3D()
        {
            _visualChild = new ModelVisual3D();
            Children.Add(_visualChild);

            _realRect3D = new Rect3D(-1, -0.75, -0.5, 2, 1.5, 1);

            TopBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/pz.bmp")));

            UpdateModel();
        }

    public MainWindow() {
      var args = System.Environment.GetCommandLineArgs();
        if (args != null)
            foreach (var arg in args)
            {
                if (arg == "/S") SCALABLE = true;
                if (arg == "/A") SYNCHRONOUS = false;
            };

      this.Title = (SCALABLE) ? "SCALABLE" : "LOCKBASED";

      this.Title += (SYNCHRONOUS) ? " SYNCHRONOUS" : " ASYNCHRONOUS";
      BoidModels = new ModelVisual3D[NumBoids];
      Timer = new DispatcherTimer();
      boids = new Boid[NumBoids];
      Space = new Rect3D(0.0, 0.0, 0.0, 600.0, 200.0, 800.0);
      CurrentData = new Data[NumBoids];
      InitializeComponent();
      
      InitJoin();

      // The color combinations to use for boids.  At least one combination is necessary,
      // but more can be added to get more variations.
      var colorCombinations = new Tuple<Color, Color>[]
            {
                Tuple.Create(Colors.SeaGreen, Colors.Silver),
                Tuple.Create(Colors.Pink, Colors.Purple),
                Tuple.Create(Colors.Yellow, Colors.Gold),
                Tuple.Create(Colors.Red, Colors.Tomato),
                Tuple.Create(Colors.Blue,Colors.BlueViolet),
                Tuple.Create(Colors.Green,Colors.LightGreen),
                Tuple.Create(Colors.Aqua,Colors.Aquamarine)
            };

      for (var i = 0; i < NumBoids; i++) {
        BoidModels[i] = new ModelVisual3D();
        var content = (System.Windows.Media.Media3D.GeometryModel3D)boidMain.Content.Clone();
        content.BackMaterial = new DiffuseMaterial(new SolidColorBrush(colorCombinations[i % colorCombinations.Length].Item2));
        content.Material = new DiffuseMaterial(new  SolidColorBrush(colorCombinations[i % colorCombinations.Length].Item1));
        BoidModels[i].Content = content;
        BoidModels[i].Transform = new TranslateTransform3D(CurrentData[i].position);
        viewport3D.Children.Add(BoidModels[i]);
      }

      
      System.Threading.ThreadPool.QueueUserWorkItem(_ => {
        while (true) {
         var d = Tick();
         Dispatcher.Invoke((Action<Data[]>) Animate,DispatcherPriority.Input, d);
        }
      });
    }

 public static Rect3D GetBounds(IDictionary dictionary)
 {
     var localToParent = GetLocalToParent(dictionary);
     var bounds = new Rect3D(localToParent.Transform(new Point3D(0, 0, 0)), new Size3D(0, 0, 0));
     foreach(var key in dictionary.Keys)
     {
         var childDictionary = dictionary[key] as IDictionary;
         if(childDictionary != null)
         {
             var childBounds = GetBounds(childDictionary);
             bounds.Union(localToParent.Transform(childBounds.Location));
         }
     }
     return bounds;
 }

        internal static IEnumerable<Point3D> TransformPoints(ref Rect3D bounds, Point3DCollection points, ref Vector3D dir)
        {
            if (dir == MathUtils.YAxis)
            {
                return points;
            }

            Vector3D rotAxis = Vector3D.CrossProduct(dir, MathUtils.YAxis);
            double rotAngle = Vector3D.AngleBetween(dir, MathUtils.YAxis);
            Quaternion q;

            if (rotAxis.X != 0 || rotAxis.Y != 0 || rotAxis.Z != 0)
            {
                Debug.Assert(rotAngle != 0);

                q = new Quaternion(rotAxis, rotAngle);
            }
            else
            {
                Debug.Assert(dir == -MathUtils.YAxis);

                q = new Quaternion(MathUtils.XAxis, rotAngle);
            }

            Vector3D center = new Vector3D(
                bounds.X + bounds.SizeX / 2,
                bounds.Y + bounds.SizeY / 2,
                bounds.Z + bounds.SizeZ / 2
                );

            Matrix3D t = Matrix3D.Identity;
            t.Translate(-center);
            t.Rotate(q);

            int count = points.Count;
            Point3D[] transformedPoints = new Point3D[count];

            for (int i = 0; i < count; i++)
            {
                transformedPoints[i] = t.Transform(points[i]);
            }

            // Finally, transform the bounds too
            bounds = MathUtils.TransformBounds(bounds, t);

            return transformedPoints;
        }

    private void Window_Loaded(object sender, EventArgs e) {
      int Xmargin = (int)Math.Round(Space.SizeX / 10.0);
      int Ymargin = (int)Math.Round(Space.SizeY / 10.0);
      int Zmargin = (int)Math.Round(Space.SizeZ / 10.0);
      Rect3D aviary = new Rect3D((double)Xmargin, (double)Ymargin, (double)Zmargin, Space.SizeX - (2 * Xmargin), Space.SizeY - (2 * Ymargin), Space.SizeZ - (2 * Zmargin));
      Random random = new Random();
      Vector3D place = new Vector3D(Space.SizeX / 2.0, Space.SizeY / 2.0, Space.SizeZ / 2.0);
      for (var i = 0; i < NumBoids; i++) {
        boids[i] = new Boid(aviary, place, NumBoids, i, this);
        boids[i].position = new Vector3D((double)random.Next(Xmargin, (int)Math.Round((double)(Space.SizeX - (2 * Xmargin)))), (double)random.Next(Ymargin, (int)Math.Round((double)(Space.SizeY - (2 * Ymargin)))), (double)random.Next(Zmargin, (int)Math.Round((double)(Space.SizeZ - (2 * Zmargin)))));
        boids[i].velocity = new Vector3D(0.0, 0.1, 0.0);
      }

      foreach (Boid boid in boids) {
        boid.Start();
      }
    }

        // Helper method for compiting the bounds of a set of points.  The given point
        // is added to the bounds of the given Rect3D.  The point/bounds are both passed 
        // by reference for perf.  Only the bounds may be modified.
        private static void AddPointToBounds(ref Point3D point, ref Rect3D bounds) 
        { 
            Debug.Assert(!bounds.IsEmpty,
                "Caller should construct the Rect3D from the first point before calling this method."); 

            if (point.X < bounds.X)
            {
                bounds.SizeX += (bounds.X - point.X); 
                bounds.X = point.X;
            } 
            else if (point.X > (bounds.X + bounds.SizeX)) 
            {
                bounds.SizeX = point.X - bounds.X; 
            }

            if (point.Y < bounds.Y)
            { 
                bounds.SizeY += (bounds.Y - point.Y);
                bounds.Y = point.Y; 
            } 
            else if (point.Y > (bounds.Y + bounds.SizeY))
            { 
                bounds.SizeY = point.Y - bounds.Y;
            }

            if (point.Z < bounds.Z) 
            {
                bounds.SizeZ += (bounds.Z - point.Z); 
                bounds.Z = point.Z; 
            }
            else if (point.Z > (bounds.Z + bounds.SizeZ)) 
            {
                bounds.SizeZ = point.Z - bounds.Z;
            }
 
#if NEVER
            // Because we do not store rectangles as TLRB (+ another dimension in 3D) 
            // we need to compute SizeX/Y/Z which involves subtraction and introduces 
            // cancelation so this assert isn't accurate.
            Debug.Assert(bounds.Contains(point), 
                "Error detect - bounds did not contain point on exit.");
#endif
        }

        /// <summary>
        /// 盤などのサイズを設定します。
        /// </summary>
        private void InitializeBounds(Rect3D banBounds, Rect3D komaboxBounds,
                                      Rect3D komadai0Bounds, Rect3D komadai1Bounds)
        {
            // 駒の表示サイズを設定
            CellSize = new Size(
                banBounds.SizeX / (Board.BoardSize + BanBorderRate * 2),
                banBounds.SizeY / (Board.BoardSize + BanBorderRate * 2));

            // 盤サイズの設定
            BanBounds = new Rect(
                banBounds.X + CellSize.Width * BanBorderRate,
                banBounds.Y + CellSize.Height * BanBorderRate,
                CellSize.Width * Board.BoardSize,
                CellSize.Height * Board.BoardSize);

            // index=0が駒箱の駒となります。
            this.capturedPieceBoxBounds[0] = WPFUtil.MakeRectXY(komaboxBounds);
            this.capturedPieceBoxBounds[1] = WPFUtil.MakeRectXY(komadai0Bounds);
            this.capturedPieceBoxBounds[2] = WPFUtil.MakeRectXY(komadai1Bounds);
        }

        public CubeViewModel()
        {
            ImageBox = new Rect3D(0, 0, 0, 1, 0.75, 0.5);

            XPBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/surfaces/px.bmp", UriKind.Absolute)));
            XNBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/surfaces/nx.bmp", UriKind.Absolute)));
            YPBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/surfaces/py.bmp", UriKind.Absolute)));
            YNBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/surfaces/ny.bmp", UriKind.Absolute)));
            ZPBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/surfaces/pz.bmp", UriKind.Absolute)));
            ZNBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/surfaces/nz.bmp", UriKind.Absolute)));

            ExpendedBrush = new ImageBrush(new BitmapImage(new Uri("pack://application:,,,/surfaces/exp.bmp", UriKind.Absolute)));

            SliderPosition = 0.4;

            SliderDirection = CooridinateDirection.ZN;

            CylindarMode = false;

            IntersectionMode = false;

            Radius = 0.3;
            Centre = new Point(0.3, 0.4);
        }

        public static PointCollection GeneratePlanarTextureCoordinates(MeshGeometry3D mesh, Rect3D bounds, Vector3D dir)
        {
            if (mesh == null)
                return null;

            //if (!bounds.Contains(mesh.Bounds))
            //    throw new ArgumentException("bounds must fully contain mesh.Bounds", "bounds");

            int count = mesh.Positions.Count;
            PointCollection texcoords = new PointCollection(count);
            IEnumerable<Point3D> positions = TransformPoints(ref bounds, mesh.Positions, ref dir);

            foreach (Point3D vertex in positions)
            {
                // The plane is looking along positive Y, so Z is really Y

                texcoords.Add(new Point(
                    GetPlanarCoordinate(vertex.X, bounds.X, bounds.SizeX),
                    GetPlanarCoordinate(vertex.Z, bounds.Z, bounds.SizeZ)
                    ));
            }

            return texcoords;
        }

        // CTAABB for an affine transforms
        internal static Rect3D ComputeTransformedAxisAlignedBoundingBoxAffine(/* IN */ ref Rect3D originalBox, /* IN */ ref Matrix3D matrix)
        {
            Debug.Assert(matrix.IsAffine); 

            // Based on Arvo's paper "Transforming Axis-Aligned Bounding Boxes" 
            // from the original Graphics Gems book. Specifically, this code 
            // is based on Figure 1 which is for a box stored as min and
            // max points. Our bounding boxes are stored as a min point and 
            // a diagonal so we'll convert when needed. Also, we have row
            // vectors.
            //
            // Mapping Arvo's variables to ours: 
            // A - the untransformed box (originalBox)
            // B - the transformed box (what we return at the end) 
            // M - the rotation + scale (matrix.Mji) 
            // T - the translation (matrix.Offset?)
            // 
            // for i = 1 ... 3
            //     Bmin_i = Bmax_i = T_i
            //         for j = 1 ... 3
            //             a = M_ij * Amin_j 
            //             b = M_ij * Amax_j
            //             Bmin_i += min(a, b) 
            //             Bmax_i += max(a, b) 
            //
            // Matrix3D doesn't have indexers because they're too slow so we'll 
            // have to unroll the loops. A complete unroll of both loops was
            // found to be the fastest.

            double oldMaxX = originalBox.X + originalBox.SizeX; 
            double oldMaxY = originalBox.Y + originalBox.SizeY;
            double oldMaxZ = originalBox.Z + originalBox.SizeZ; 
 
            // i = 1 (X)
            double newMinX = matrix.OffsetX; 
            double newMaxX = matrix.OffsetX;
            {
                // i = 1 (X), j = 1 (X)
                double a = matrix.M11 * originalBox.X; 
                double b = matrix.M11 * oldMaxX;
                if (b > a) 
                { 
                    newMinX += a;
                    newMaxX += b; 
                }
                else
                {
                    newMinX += b; 
                    newMaxX += a;
                } 
 
                // i = 1 (X), j = 2 (Y)
                a = matrix.M21 * originalBox.Y; 
                b = matrix.M21 * oldMaxY;
                if (b > a)
                {
                    newMinX += a; 
                    newMaxX += b;
                } 
                else 
                {
                    newMinX += b; 
                    newMaxX += a;
                }

                // i = 1 (X), j = 3 (Z) 
                a = matrix.M31 * originalBox.Z;
                b = matrix.M31 * oldMaxZ; 
                if (b > a) 
                {
                    newMinX += a; 
                    newMaxX += b;
                }
                else
                { 
                    newMinX += b;
                    newMaxX += a; 
                } 
            }
 
            // i = 2 (Y)
            double newMinY = matrix.OffsetY;
            double newMaxY = matrix.OffsetY;
            { 
                // i = 2 (Y), j = 1 (X)
                double a = matrix.M12 * originalBox.X; 
                double b = matrix.M12 * oldMaxX; 
                if (b > a)
                { 
                    newMinY += a;
                    newMaxY += b;
                }
                else 
                {
                    newMinY += b; 
                    newMaxY += a; 
                }
 
                // i = 2 (Y), j = 2 (Y)
//.........这里部分代码省略.........

        internal override RayHitTestParameters RayFromViewportPoint(Point p, Size viewSize, Rect3D boundingRect, out double distanceAdjustment)
        {
            //
            //  Compute rayParameters
            //
            
            // Find the point on the projection plane in post-projective space where
            // the viewport maps to a 2x2 square from (-1,1)-(1,-1).
            Point np = M3DUtil.GetNormalizedPoint(p, viewSize);

            // 

            
            // So (conceptually) the user clicked on the point (np.X,
            // np.Y, 0) in post-projection clipping space and the ray
            // extends in the direction (0, 0, 1) because our ray
            // after projection looks down the positive z axis.  We
            // need to convert this ray and direction back to world
            // space.

            Matrix3D worldToCamera = GetViewMatrix() * ProjectionMatrix;
            Matrix3D cameraToWorld = worldToCamera;

            if (!cameraToWorld.HasInverse)
            {
                // 




                // NTRAID#Longhorn-1180933-2004/07/30-danwo - Need to handle singular matrix cameras
                throw new NotSupportedException(SR.Get(SRID.HitTest_Singular));
            }
            
            cameraToWorld.Invert();

            Point4D origin4D = new Point4D(np.X,np.Y,0,1) * cameraToWorld;
            Point3D origin = new Point3D( origin4D.X/origin4D.W,
                                          origin4D.Y/origin4D.W,
                                          origin4D.Z/origin4D.W );

            // To transform the direction we use the Jacobian of
            // cameraToWorld at the point np.X,np.Y,0 that we just
            // transformed.
            //
            // The Jacobian of the homogeneous matrix M is a 3x3 matrix.
            //
            // Let x be the point we are computing the Jacobian at, and y be the
            // result of transforming x by M, i.e.
            // (wy w) = (x 1) M
            // Where (wy w) is the homogeneous point representing y with w as its homogeneous coordinate
            // And (x 1) is the homogeneous point representing x with 1 as its homogeneous coordinate
            //
            // Then the i,j component of the Jacobian (at x) is
            // (M_ij - M_i4 y_j) / w
            //
            // Since we're only concerned with the direction of the
            // transformed vector and not its magnitude, we can scale
            // this matrix by a POSITIVE factor.  The computation
            // below computes the Jacobian scaled by 1/w and then
            // after we normalize the final vector we flip it around
            // if w is negative.
            //
            // To transform a vector we just right multiply it by this Jacobian matrix.
            //
            // Compute the Jacobian at np.X,np.Y,0 ignoring the constant factor of w.
            // Here's the pattern
            //
            // double Jij = cameraToWorld.Mij - cameraToWorld.Mi4 * origin.j
            //
            // but we only need J31,J32,&J33 because we're only
            // transforming the vector 0,0,1

            double J31 = cameraToWorld.M31 - cameraToWorld.M34 * origin.X;
            double J32 = cameraToWorld.M32 - cameraToWorld.M34 * origin.Y;
            double J33 = cameraToWorld.M33 - cameraToWorld.M34 * origin.Z;

            // Then multiply that matrix by (0, 0, 1) which is
            // the direction of the ray in post-projection space.
            Vector3D direction = new Vector3D( J31, J32, J33 );
            direction.Normalize();

            // We multiplied by the Jacobian times W, so we need to
            // account for whether that flipped our result or not.
            if (origin4D.W < 0)
            {
                direction = -direction;
            }
            
            RayHitTestParameters rayParameters = new RayHitTestParameters(origin, direction);

            //
            //  Compute HitTestProjectionMatrix
            //

            // The viewportMatrix will take normalized clip space into
            // viewport coordinates, with an additional 2D translation
            // to put the ray at the origin.
            Matrix3D viewportMatrix = new Matrix3D();
            viewportMatrix.TranslatePrepend(new Vector3D(-p.X,viewSize.Height-p.Y,0));
//.........这里部分代码省略.........

        //------------------------------------------------------ 
        //
        //  Public Methods 
        // 
        //-----------------------------------------------------
 
        //------------------------------------------------------
        //
        //  Public Properties
        // 
        //------------------------------------------------------
 
        //----------------------------------------------------- 
        //
        //  Public Events 
        //
        //------------------------------------------------------

        //----------------------------------------------------- 
        //
        //  Internal Methods 
        // 
        //-----------------------------------------------------
 
        #region Internal Methods

        // Creates a ray by projecting the given point on the viewport into the scene.
        // Used for bridging 2D -> 3D hit testing. 
        //
        // The latter two parameters in this method are used to deal with the 
        // case where the camera's near plane is far away from the viewport 
        // contents. In these cases, we can sometimes construct a new, closer,
        // near plane and start the ray on that plane. To do this, we need an 
        // axis-aligned bounding box of the viewport's contents (boundingRect).
        // We also need to return the distance between the original an new near
        // planes (distanceAdjustment), so we can correct the hit-test
        // distances before handing them back to the user. For more 
        // information, see WindowsOS Bug #1329733.
        // 
        internal abstract RayHitTestParameters RayFromViewportPoint(Point point, Size viewSize, Rect3D boundingRect, out double distanceAdjustment); 

        /// <summary>
        /// Precompute pass.
        /// </summary>
        internal void PrecomputeRecursive(out Rect3D bboxSubgraph)
        {
            if (CheckFlagsAnd(VisualFlags.IsSubtreeDirtyForPrecompute))
            {
                //
                // Update the subgraph bounding box which includes the content bounds
                // and the bounds of our children.
                //
                
                _bboxSubgraph = GetContentBounds();

                for (int i = 0, count = Visual3DChildrenCount; i < count; i++)
                {
                    Visual3D child = GetVisual3DChild(i);

                    Rect3D bboxSubgraphChild;
                    child.PrecomputeRecursive(out bboxSubgraphChild);
                    _bboxSubgraph.Union(bboxSubgraphChild);
                }

                SetFlags(false, VisualFlags.IsSubtreeDirtyForPrecompute);
            }

            bboxSubgraph = M3DUtil.ComputeTransformedAxisAlignedBoundingBox(ref _bboxSubgraph, Transform);
       }

        internal void Debug_VerifyBoundsEqual(Rect3D bounds1, Rect3D bounds2, string errorString)
        {
            // The funny boolean logic below avoids asserts when the cached
            // bounds contain NaNs.  (NaN != NaN)
            bool boundsAreEqual =
                !(bounds1.X < bounds2.X || bounds1.X > bounds2.X) &&
                !(bounds1.Y < bounds2.Y || bounds1.Y > bounds2.Y) &&
                !(bounds1.Z < bounds2.Z || bounds1.Z > bounds2.Z) &&
                !(bounds1.SizeX < bounds2.SizeX || bounds1.SizeX > bounds2.SizeX) &&
                !(bounds1.SizeY < bounds2.SizeY || bounds1.SizeY > bounds2.SizeY) &&
                !(bounds1.SizeZ < bounds2.SizeZ || bounds1.SizeZ > bounds2.SizeZ);

            Debug.Assert(boundsAreEqual, errorString);
        }

 /// <summary>
 /// Zooms to the extents of the specified bounding box.
 /// </summary>
 /// <param name="viewport">The viewport.</param>
 /// <param name="bounds">The bounding rectangle.</param>
 /// <param name="animationTime">The animation time.</param>
 public static void ZoomExtents(this Viewport3DX viewport, Rect3D bounds, double animationTime = 0)
 {
     var diagonal = new Vector3D(bounds.SizeX, bounds.SizeY, bounds.SizeZ);
     var center = bounds.Location + (diagonal * 0.5);
     double radius = diagonal.Length * 0.5;
     ZoomExtents(viewport, center, radius, animationTime);
 }