C# RotateTransform3D.Transform方法代码示例

        private Vector3D GetDirectionModelCoords()
        {
            var worldCoords = base.GetWorldLocation();

            // Calculate the difference between the world orientation and model orientation
            Quaternion toModelQuat = worldCoords.Item2.ToUnit() * this.Orientation.ToUnit();

            RotateTransform3D toModelTransform = new RotateTransform3D(new QuaternionRotation3D(toModelQuat));

            Vector3D directionWorld = _homePoint - worldCoords.Item1;

            // Rotate into model coords
            return toModelTransform.Transform(directionWorld);
        }

 /// <summary>
 ///     Rotate the rectangle by a quaternion
 /// </summary>
 /// <param name="rot">The quaternion to rotate by</param>
 /// <returns>The current rectangle instance</returns>
 public Rectangle3D Rotate(Quaternion rot)
 {
     foreach (var k in new List<string>(Corners.Keys)) {
         var q = new QuaternionRotation3D(new System.Windows.Media.Media3D.Quaternion(rot.X, rot.Y, rot.Z, rot.W));
         var r = new RotateTransform3D(q, ToPoint3D(Center));
         Corners[k] = ToVector3(r.Transform(ToPoint3D(Corners[k])));
     }
     Position = Corners["000"];
     GenerateEdges();
     GenerateFaces();
     return this;
 }

        public void RenderMesh(WLD wld,IEnumerable<Mesh> meshes, TrackAnimationBuilder animation = null, int textureNum = 0, int face = 0)
        {
            if (meshes == null) return;
            Model3DGroup group = Model as Model3DGroup;

            group.Children.Clear();
            Dictionary<BitmapImage, List<EQEmu.Files.WLD.Polygon>> polysbyTex = new Dictionary<BitmapImage, List<EQEmu.Files.WLD.Polygon>>();
            List<EQEmu.Files.WLD.Polygon> untexturedPolys = new List<EQEmu.Files.WLD.Polygon>();
            foreach (var mesh in meshes)
            {
                foreach (var p in mesh.Polygons)
                {
                    if (p.BitmapInfo != null)
                    {
                        if (polysbyTex.ContainsKey(p.BitmapInfo.Image))
                        {
                            polysbyTex[p.BitmapInfo.Image].Add(p);
                        }
                        else
                        {
                            polysbyTex[p.BitmapInfo.Image] = new List<EQEmu.Files.WLD.Polygon>();
                            polysbyTex[p.BitmapInfo.Image].Add(p);
                        }
                    }
                    else
                    {
                        untexturedPolys.Add(p);
                    }
                }
            }

            Material mat = null;
            foreach (var polytex in polysbyTex)
            {
                MeshBuilder builder = new MeshBuilder();
                if (mat == null)
                {
                    if (polytex.Value.ElementAt(0).BitmapInfo != null)
                    {
                        //mat = HelixToolkit.Wpf.MaterialHelper.CreateImageMaterial(polytex.Value.ElementAt(0).Image, 100.0);
                        BitmapImage img = polytex.Value.ElementAt(0).BitmapInfo.Image;
                        if (textureNum > 0 || face > 0)
                        {
                            string baseTexture = polytex.Value.ElementAt(0).BitmapInfo.Name;
                            var textureStr = String.Format("{0:d2}", textureNum);
                            var faceStr = String.Format("{0:d1}", face);

                            var index = baseTexture.IndexOf("00");
                            if (index < 0) index = baseTexture.IndexOf("01");

                            if (index > 0)
                            {
                                var faceAndTexture = baseTexture.Substring(0, index) + textureStr + faceStr + baseTexture.Substring(index + textureStr.Length + faceStr.Length);
                                var textureOnly = baseTexture.Substring(0, index) + textureStr + baseTexture.Substring(index + textureStr.Length);
                                var faceOnly = baseTexture.Substring(0, index + textureStr.Length) + faceStr + baseTexture.Substring(index + textureStr.Length + faceStr.Length);

                                if (wld.ImageMapping.ContainsKey(faceAndTexture))
                                {
                                    img = wld.ImageMapping[faceAndTexture].Image;
                                }
                                else if (wld.ImageMapping.ContainsKey(textureOnly))
                                {
                                    img = wld.ImageMapping[textureOnly].Image;
                                }
                                else if (wld.ImageMapping.ContainsKey(faceOnly))
                                {
                                    img = wld.ImageMapping[faceOnly].Image;
                                }
                            }
                        }

                        var brush = new System.Windows.Media.ImageBrush(img);
                        brush.ViewportUnits = System.Windows.Media.BrushMappingMode.Absolute;
                        //brush.TileMode
                        brush.TileMode = System.Windows.Media.TileMode.Tile;
                        //brush.Stretch = System.Windows.Media.Stretch.Fill;
                        mat = HelixToolkit.Wpf.MaterialHelper.CreateMaterial(brush);
                    }
                    else
                    {
                        mat = Materials.LightGray;
                    }
                }
                foreach (var poly in polytex.Value)
                {
                    Point3D p1 = new Point3D(poly.V1.X, poly.V1.Y, poly.V1.Z);
                    Point3D p2 = new Point3D(poly.V2.X, poly.V2.Y, poly.V2.Z);
                    Point3D p3 = new Point3D(poly.V3.X, poly.V3.Y, poly.V3.Z);

                    if (animation != null)
                    {
                        if (animation.SkeletonPieceTransforms.ContainsKey(poly.V1.BodyPiece))
                        {
                            var atrans = animation.SkeletonPieceTransforms[poly.V1.BodyPiece];
                            p1 = atrans.Transform(p1);
                        }

                        if (animation.SkeletonPieceTransforms.ContainsKey(poly.V2.BodyPiece))
                        {
                            var atrans = animation.SkeletonPieceTransforms[poly.V2.BodyPiece];
//.........这里部分代码省略.........

        /// <summary>
        /// Do the tessellation and return the <see cref="MeshGeometry3D" />.
        /// </summary>
        /// <returns>
        /// A triangular mesh geometry.
        /// </returns>
        protected override MeshGeometry3D Tessellate()
        {
            this.lengthDirection = this.LengthDirection;
            this.lengthDirection.Normalize();

            // #136, chrkon, 2015-03-26
            // if NormalVector and LenghtDirection are not perpendicular then overwrite LengthDirection
            if (Vector3D.DotProduct(this.Normal, this.LengthDirection) != 0.0)
            {
                this.lengthDirection = this.Normal.FindAnyPerpendicular();
                this.lengthDirection.Normalize();
            }

            // create WidthDirection by rotating lengthDirection vector 90° around normal vector
            var rotate = new RotateTransform3D(new AxisAngleRotation3D(this.Normal, 90.0));
            this.widthDirection = rotate.Transform(this.lengthDirection);
            this.widthDirection.Normalize();
            // #136

            var mesh = new MeshBuilder(true, false);
            double minX = -this.Width / 2;
            double minY = -this.Length / 2;
            double maxX = this.Width / 2;
            double maxY = this.Length / 2;

            double x = minX;
            double eps = this.MinorDistance / 10;
            while (x <= maxX + eps)
            {
                double t = this.Thickness;
                if (IsMultipleOf(x, this.MajorDistance))
                {
                    t *= 2;
                }

                this.AddLineX(mesh, x, minY, maxY, t);
                x += this.MinorDistance;
            }

            double y = minY;
            while (y <= maxY + eps)
            {
                double t = this.Thickness;
                if (IsMultipleOf(y, this.MajorDistance))
                {
                    t *= 2;
                }

                this.AddLineY(mesh, y, minX, maxX, t);
                y += this.MinorDistance;
            }

            var m = mesh.ToMesh();
            m.Freeze();
            return m;
        }

        /// <summary>
        /// Rotates around the camera up and right axes.
        /// </summary>
        /// <param name="p1">
        /// The previous mouse position.
        /// </param>
        /// <param name="p2">
        /// The current mouse position.
        /// </param>
        /// <param name="rotateAround">
        /// The point to rotate around.
        /// </param>
        private void RotateAroundUpAndRight(Point p1, Point p2, Point3D rotateAround)
        {
            var dp = p2 - p1;

            // Rotate around the camera up direction
            var delta1 = new Quaternion(this.CameraUpDirection, -dp.X * this.RotationSensitivity);

            // Rotate around the camera right direction
            var delta2 = new Quaternion(
                Vector3D.CrossProduct(this.CameraUpDirection, this.CameraLookDirection), dp.Y * this.RotationSensitivity);

            var delta = delta1 * delta2;
            var rotate = new RotateTransform3D(new QuaternionRotation3D(delta));
            var relativeTarget = rotateAround - this.CameraTarget;
            var relativePosition = rotateAround - this.CameraPosition;

            var newRelativeTarget = rotate.Transform(relativeTarget);
            var newRelativePosition = rotate.Transform(relativePosition);
            var newUpDirection = rotate.Transform(this.CameraUpDirection);

            var newTarget = rotateAround - newRelativeTarget;
            var newPosition = rotateAround - newRelativePosition;

            this.CameraLookDirection = newTarget - newPosition;
            if (CameraMode == CameraMode.Inspect)
            {
                this.CameraPosition = newPosition;
            }
            this.CameraUpDirection = newUpDirection;
        }

        /// <summary>
        /// Takes the average velocity of the requested cells
        /// </summary>
        /// <remarks>
        /// This was written to assist with IFluidField.GetForce.  I decided to not use the interface to give the caller
        /// the option of model or world coords.  Also, there is more flexibility if multiple fields need to be stitched
        /// together.
        /// 
        /// TODO: When this class implements viscosity, also implement IFluidField directly (and assume world coords)
        /// </remarks>
        public Vector3D[] GetFlowAtLocations(Point3D[] points, double radius)
        {
            //TODO: look at radius

            double fullSize = this.SizeWorld;
            double halfSize = fullSize / 2d;
            double cellSize = fullSize / _size;

            // This is to transform the point from world to model coords
            Transform3DGroup transformPoint = new Transform3DGroup();
            transformPoint.Children.Add(new RotateTransform3D(new QuaternionRotation3D(this.RotationWorld.ToReverse())));
            transformPoint.Children.Add(new TranslateTransform3D(this.PositionWorld.ToVector() * -1d));
            transformPoint.Children.Add(new TranslateTransform3D(new Vector3D(halfSize, halfSize, halfSize)));       // The point passed in is from -half to +half, but the ints are 0 to size

            // This is to transform the return velocity back to world coords
            RotateTransform3D transformVect = new RotateTransform3D(new QuaternionRotation3D(this.RotationWorld));

            Vector3D[] retVal = new Vector3D[points.Length];

            for (int cntr = 0; cntr < points.Length; cntr++)
            {
                // Convert into int positions
                Point3D transformed = transformPoint.Transform(points[cntr]);

                int x = Convert.ToInt32(Math.Round(transformed.X / cellSize));
                int y = Convert.ToInt32(Math.Round(transformed.Y / cellSize));
                int z = Convert.ToInt32(Math.Round(transformed.Z / cellSize));

                int index = Get1DIndex(x, y, z);
                if (index < 0 || index >= _size1D)
                {
                    // The point is outside this field
                    retVal[cntr] = new Vector3D(0, 0, 0);
                }

                double negate = 1d;
                if (_blocked[index])
                {
                    // Blocked cells have the opposite velocity, so that they push back.  But when reporting, it needs to be flipped back
                    // so that it's more like the force felt at that point
                    negate = -1d;
                }

                retVal[cntr] = transformVect.Transform(new Vector3D(_velocityX[index] * negate, _velocityY[index] * negate, _velocityZ[index] * negate));
            }

            // Exit Function
            return retVal;
        }

        public override void StartDrag(RayHitTestParameters ray)
        {
            if (_circleCenter == null || _circleOrientation == null)
            {
                throw new InvalidOperationException("CircleCenter and CircleOrientation need to be set before calling this method");
            }

            _rotateTransform = new RotateTransform3D(new QuaternionRotation3D(_circleOrientation.Value));
            _circlePlane = new Triangle(_circleCenter.Value, _circleCenter.Value + _rotateTransform.Transform(_intialAxisX), _circleCenter.Value + _rotateTransform.Transform(_intialAxisY));

            // Compare the ray with the drag ring to see where on the circle they clicked
            Point3D[] nearestCirclePoints, nearestLinePoints;
            if (Math3D.GetClosestPoints_Circle_Line(out nearestCirclePoints, out nearestLinePoints, _circlePlane, _circleCenter.Value, _radius, ray.Origin, ray.Direction, Math3D.RayCastReturn.ClosestToRay))
            {
                _mouseDownAngle = GetAngle(nearestCirclePoints[0]);
                _mouseDownAngle = new Quaternion(_mouseDownAngle.Axis, _mouseDownAngle.Angle * -1d).ToUnit();		// taking angle times -1 so that the drag just has to do a simple multiply
            }
            else
            {
                // The click ray is perpendicular to the circle plane, and they are clicking through the direct center of the circle (this should never happen)
                _mouseDownAngle = Quaternion.Identity;
            }
        }

        public void ViewChanged(DoubleVector lookDirection)
        {
            if (_marker2D != null)
            {
                _viewport.Children.Remove(_marker2D);
            }

            if (_marker2D == null)
            {
                _marker2D = new ScreenSpaceLines3D();
                _marker2D.Color = UtilityWPF.ColorFromHex("60A0A0A0");
                _marker2D.Thickness = 4d;
            }

            _marker2D.Clear();

            RotateTransform3D transform = new RotateTransform3D(new QuaternionRotation3D(Math3D.GetRotation(new DoubleVector(0, 0, 1, 0, -1, 0), lookDirection)));

            #region Marker 2D

            double orthDist = (_sizeMult * _field.Size * 1.1d) / 2d;
            double cornerDist = (_sizeMult * _field.Size * .2d) / 2d;

            // TopLeft
            Point3D corner = transform.Transform(new Point3D(-orthDist, -orthDist, -orthDist));
            Vector3D direction = transform.Transform(new Vector3D(cornerDist, 0, 0));
            _marker2D.AddLine(corner, corner + direction);
            direction = transform.Transform(new Vector3D(0, cornerDist, 0));
            _marker2D.AddLine(corner, corner + direction);

            //TopRight
            corner = transform.Transform(new Point3D(orthDist, -orthDist, -orthDist));
            direction = transform.Transform(new Vector3D(-cornerDist, 0, 0));
            _marker2D.AddLine(corner, corner + direction);
            direction = transform.Transform(new Vector3D(0, cornerDist, 0));
            _marker2D.AddLine(corner, corner + direction);

            //BottomRight
            corner = transform.Transform(new Point3D(orthDist, orthDist, -orthDist));
            direction = transform.Transform(new Vector3D(-cornerDist, 0, 0));
            _marker2D.AddLine(corner, corner + direction);
            direction = transform.Transform(new Vector3D(0, -cornerDist, 0));
            _marker2D.AddLine(corner, corner + direction);

            //BottomLeft
            corner = transform.Transform(new Point3D(-orthDist, orthDist, -orthDist));
            direction = transform.Transform(new Vector3D(cornerDist, 0, 0));
            _marker2D.AddLine(corner, corner + direction);
            direction = transform.Transform(new Vector3D(0, -cornerDist, 0));
            _marker2D.AddLine(corner, corner + direction);

            _viewport.Children.Add(_marker2D);

            #endregion

            // Camera
            _camera.Position = transform.Transform(new Point3D(0, 0, (_sizeMult * _field.Size * -4d) / 2d));
            _camera.LookDirection = lookDirection.Standard;
            _camera.UpDirection = lookDirection.Orth;

            // Remember it
            _lookDirection = lookDirection;
        }

        public static void Read(this MeshGeometry3D m3D, string shapeData, XbimMatrix3D? transform = null)
        {
            transform = null;
            RotateTransform3D qrd = new RotateTransform3D();
            Matrix3D? matrix3D = null;
            if (transform.HasValue)
            {
                XbimQuaternion xq = transform.Value.GetRotationQuaternion();
                qrd.Rotation = new QuaternionRotation3D(new Quaternion(xq.X, xq.Y, xq.Z, xq.W));
                matrix3D = transform.Value.ToMatrix3D();
            }
            
            using (StringReader sr = new StringReader(shapeData))
            {
                int version = 1;
                List<Point3D> vertexList = new List<Point3D>(512); //holds the actual unique positions of the vertices in this data set in the mesh
                List<Vector3D> normalList = new List<Vector3D>(512); //holds the actual unique normals of the vertices in this data set in the mesh

                List<Point3D> positions = new List<Point3D>(1024); //holds the actual positions of the vertices in this data set in the mesh
                List<Vector3D> normals = new List<Vector3D>(1024); //holds the actual normals of the vertices in this data set in the mesh
                List<int> triangleIndices = new List<int>(2048);
                String line;
                // Read and display lines from the data until the end of
                // the data is reached.

                while ((line = sr.ReadLine()) != null)
                {

                    string[] tokens = line.Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
                    if (tokens.Length > 0) //we need a command
                    {
                        string command = tokens[0].Trim().ToUpper();
                        switch (command)
                        {
                            case "P":
                                vertexList = new List<Point3D>(512);
                                normalList = new List<Vector3D>(512);
                                if (tokens.Length > 0)
                                    version = Int32.Parse(tokens[1]);
                                break;
                            case "V": //process vertices
                                for (int i = 1; i < tokens.Length; i++)
                                {
                                    string[] xyz = tokens[i].Split(',');
                                    Point3D p = new Point3D(Convert.ToDouble(xyz[0], CultureInfo.InvariantCulture),
                                                                      Convert.ToDouble(xyz[1], CultureInfo.InvariantCulture),
                                                                      Convert.ToDouble(xyz[2], CultureInfo.InvariantCulture));
                                    if (matrix3D.HasValue)
                                        p = matrix3D.Value.Transform(p);
                                    vertexList.Add(p);
                                }
                                break;
                            case "N": //processes normals
                                for (int i = 1; i < tokens.Length; i++)
                                {
                                    string[] xyz = tokens[i].Split(',');
                                    Vector3D v = new Vector3D(Convert.ToDouble(xyz[0], CultureInfo.InvariantCulture),
                                                                       Convert.ToDouble(xyz[1], CultureInfo.InvariantCulture),
                                                                       Convert.ToDouble(xyz[2], CultureInfo.InvariantCulture));
                                    normalList.Add(v);
                                }
                                break;
                            case "T": //process triangulated meshes
                                Vector3D currentNormal = new Vector3D(0,0,0);
                                //each time we start a new mesh face we have to duplicate the vertices to ensure that we get correct shading of planar and non planar faces
                                var writtenVertices = new Dictionary<int, int>();

                                for (int i = 1; i < tokens.Length; i++)
                                {
                                    string[] indices = tokens[i].Split(new[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
                                    if (indices.Length != 3) throw new Exception("Invalid triangle definition");
                                    for (int t = 0; t < 3; t++)
                                    {
                                        string[] indexNormalPair = indices[t].Split(new[] { '/' }, StringSplitOptions.RemoveEmptyEntries);

                                        if (indexNormalPair.Length > 1) //we have a normal defined
                                        {
                                            if (version == 1)
                                            {
                                                string normalStr = indexNormalPair[1].Trim();
                                                switch (normalStr)
                                                {
                                                    case "F": //Front
                                                        currentNormal = new Vector3D(0, -1, 0);
                                                        break;
                                                    case "B": //Back
                                                        currentNormal = new Vector3D(0, 1, 0);
                                                        break;
                                                    case "L": //Left
                                                        currentNormal = new Vector3D(-1, 0, 0);
                                                        break;
                                                    case "R": //Right
                                                        currentNormal = new Vector3D(1, 0, 0);
                                                        break;
                                                    case "U": //Up
                                                        currentNormal = new Vector3D(0, 0, 1);
                                                        break;
                                                    case "D": //Down
                                                        currentNormal = new Vector3D(0, 0, -1);
                                                        break;
//.........这里部分代码省略.........

        // Process keys for moving the camera position.
        private void KeyDownHandlerForCamera(object sender, KeyEventArgs e)
        {
            double currentDistance, newDistance;
            Vector3D rotationAxis;
            RotateTransform3D rotationTransform;
            PerspectiveCamera camera = (PerspectiveCamera)myViewport3D.Camera;

            switch (e.Key)
            {
                case Key.Up:
                    rotationAxis = Vector3D.CrossProduct((Vector3D)camera.Position, camera.UpDirection);
                    rotationTransform = new RotateTransform3D(new AxisAngleRotation3D(rotationAxis, stepAngle));

                    camera.Position = rotationTransform.Transform(camera.Position);
                    camera.UpDirection = rotationTransform.Transform(camera.UpDirection);

                    e.Handled = true;
                    break;

                case Key.Down:
                    rotationAxis = Vector3D.CrossProduct((Vector3D)camera.Position, camera.UpDirection);
                    rotationTransform = new RotateTransform3D(new AxisAngleRotation3D(rotationAxis, -stepAngle));

                    camera.Position = rotationTransform.Transform(camera.Position);
                    camera.UpDirection = rotationTransform.Transform(camera.UpDirection);

                    e.Handled = true;
                    break;

                case Key.Left:
                    rotationAxis = camera.UpDirection;
                    rotationTransform = new RotateTransform3D(new AxisAngleRotation3D(rotationAxis, -stepAngle));

                    camera.Position = rotationTransform.Transform(camera.Position);
                    camera.UpDirection = rotationTransform.Transform(camera.UpDirection);

                    e.Handled = true;
                    break;

                case Key.Right:
                    rotationAxis = camera.UpDirection;
                    rotationTransform = new RotateTransform3D(new AxisAngleRotation3D(rotationAxis, stepAngle));

                    camera.Position = rotationTransform.Transform(camera.Position);
                    camera.UpDirection = rotationTransform.Transform(camera.UpDirection);

                    e.Handled = true;
                    break;

                case Key.OemPlus:
                case Key.Add:
                    currentDistance = ((Vector3D)camera.Position).Length;
                    newDistance = currentDistance - stepDistance;

                    if (newDistance > minDistance)
                    {
                        double scale = newDistance / currentDistance;
                        camera.Position *= new ScaleTransform3D(new Vector3D(scale, scale, scale)).Value;
                    }

                    e.Handled = true;
                    break;

                case Key.OemMinus:
                case Key.Subtract:
                    currentDistance = ((Vector3D)camera.Position).Length;
                    newDistance = currentDistance + stepDistance;

                    if (newDistance < maxDistance)
                    {
                        double scale = newDistance / currentDistance;
                        camera.Position *= new ScaleTransform3D(new Vector3D(scale, scale, scale)).Value;
                    }

                    e.Handled = true;
                    break;

                case Key.OemComma:
                    rotationAxis = (Vector3D)camera.Position;
                    rotationTransform = new RotateTransform3D(new AxisAngleRotation3D(rotationAxis, stepAngle));

                    camera.UpDirection = rotationTransform.Transform(camera.UpDirection);

                    e.Handled = true;
                    break;

                case Key.OemPeriod:
                    rotationAxis = (Vector3D)camera.Position;
                    rotationTransform = new RotateTransform3D(new AxisAngleRotation3D(rotationAxis, -stepAngle));

                    camera.UpDirection = rotationTransform.Transform(camera.UpDirection);

                    e.Handled = true;
                    break;
            }
        }

        private void compute(Joint shoulderCenter, Joint spine, Joint shoulder, Joint elbow, Joint wrist)
        {
            Vector3D center = Util.vectorFromJoint(shoulderCenter);
            Vector3D spineVec = Util.vectorFromJoint(spine);
            Vector3D centerToSpine = spineVec - center;
            centerToSpine.Normalize();

            Vector3D shoulderVec = Util.vectorFromJoint(shoulder);
            Vector3D centerToShoulder = shoulderVec - center;
            centerToShoulder.Normalize();

            Vector3D chestPlaneNormal = Vector3D.CrossProduct(centerToShoulder, -centerToSpine);

            Vector3D elbowVec = Util.vectorFromJoint(elbow);
            Vector3D shoulderToElbow = elbowVec - shoulderVec;
            shoulderToElbow.Normalize();

            Vector3D naoPitchPlaneNormal = Vector3D.CrossProduct(shoulderToElbow, centerToShoulder);

            Vector3D pitchVector = Util.projectToPlane(naoPitchPlaneNormal, shoulderToElbow);

            Vector3D oldPitchVector = Util.projectToPlane(chestPlaneNormal, shoulderToElbow);
            double oldShoulderPitch = Vector3D.AngleBetween(oldPitchVector, centerToSpine);
            if (Vector3D.DotProduct(oldPitchVector, centerToShoulder) < 0)
            {
                Console.WriteLine("Inverting shoulder pitch");
                oldShoulderPitch = -oldShoulderPitch;
            }
            Vector3D naoYawPlaneNormal = centerToShoulder;

            Vector3D yawVector = Util.projectToPlane(naoYawPlaneNormal, shoulderToElbow);

            Vector3D wristVec = Util.vectorFromJoint(wrist);
            Vector3D elbowToWrist = wristVec - elbowVec;

            Vector3D rollVector = Util.projectToPlane(shoulderToElbow, elbowToWrist);

            double shoulderYaw = 0;
            if (yawVector.Length > 0.5)
            {
                if (_leftSide)
                {
                    shoulderYaw = Vector3D.AngleBetween(yawVector, chestPlaneNormal);
                }
                else
                {
                    shoulderYaw = Vector3D.AngleBetween(yawVector, -chestPlaneNormal);
                }

                if (Vector3D.DotProduct(yawVector, centerToSpine) < 0)
                {
                    Console.WriteLine("Inverting shoulder yaw");
                    shoulderYaw = 2 * Math.PI - shoulderYaw;
                }
                prevYaw = Yaw;
                Yaw = Util.degToRad(shoulderYaw);
            }
            else
            {
                Console.WriteLine("yaw too small, ignoring");
            }

            AxisAngleRotation3D YawRotation = new AxisAngleRotation3D(
                centerToShoulder, shoulderYaw);
            Transform3D tr = new RotateTransform3D(YawRotation);
            Vector3D downInPitchFrame = tr.Transform(centerToSpine);

            double shoulderPitch = Vector3D.AngleBetween(pitchVector, downInPitchFrame);
            /*
            if (_leftSide)
            {
                shoulderPitch = Vector3D.AngleBetween(pitchVector, downInPitchFrame);
            }
            else
            {
                shoulderPitch = Vector3D.AngleBetween(pitchVector, -downInPitchFrame);
            }*/
            // AngleBetween returns between [0,180] so we need to add the sign appropriately
            if (Vector3D.DotProduct(pitchVector, centerToShoulder) < 0)
            {
                Console.WriteLine("Inverting shoulder pitch");
                shoulderPitch = -shoulderPitch;
            }

            //TODO this is very wrong
            AxisAngleRotation3D aar = new AxisAngleRotation3D(
                chestPlaneNormal, 90 - oldShoulderPitch);
            Transform3D t = new RotateTransform3D(aar);
            Vector3D upInShoulderFrame = t.Transform(-centerToSpine);

            double abt = Vector3D.AngleBetween(upInShoulderFrame, shoulderToElbow);
            Console.WriteLine("Abt:" + abt + " shoulderPitch" + shoulderPitch);

            double shoulderRoll = Vector3D.AngleBetween(rollVector, upInShoulderFrame);

            Pitch = Util.degToRad(shoulderPitch);

            Roll = Util.degToRad(shoulderRoll);
        }

        /// <summary>
        /// Render the frame
        /// </summary>
        protected void RenderScene()
        {
            if (needsResizing)
            {
                needsResizing = false;
                renderTargetView.Dispose();
                SwapChainDescription sd = swapChain.Description;
                swapChain.ResizeBuffers(sd.BufferCount, (uint)directControl.ClientSize.Width, (uint)directControl.ClientSize.Height, sd.BufferDescription.Format, sd.Flags);
                SetViews();
                // Update the projection matrix
                projectionMatrix = Microsoft.WindowsAPICodePack.DirectX.DirectXUtilities.Camera.MatrixPerspectiveFovLH((float)Math.PI * 0.25f, ((float)directControl.ClientSize.Width / (float)directControl.ClientSize.Height), 0.5f, 100.0f);
                projectionVariable.Matrix = projectionMatrix;
            }
            Matrix4x4F worldMatrix;

            t = (Environment.TickCount - dwTimeStart) / 50.0f;

            //WPF transforms used here use degrees as opposed to D3DX which uses radians in the native tutorial
            //360 degrees == 2 * Math.PI
            //world matrix rotates the first cube by t degrees
            RotateTransform3D rt1 = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 1, 0), t));
            worldMatrix = rt1.Value.ToMatrix4x4F();

            //Setup our lighting parameters
            Vector4F[] vLightDirs =
            {
                new Vector4F( -0.577f, 0.577f, -0.577f, 1.0f ),
                new Vector4F( 0.0f, 0.0f, -1.0f, 1.0f )
            };
            Vector4F[] vLightColors = 
            {
                new Vector4F ( 0.5f, 0.5f, 0.5f, 1.0f ),
                new Vector4F ( 0.5f, 0.0f, 0.0f, 1.0f )
            };

            //rotate the second light around the origin
            //create a rotation matrix
            RotateTransform3D rt2 = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 2, 0), -t));
            //rotate vLightDirs[1] vector using the rotation matrix
            Vector3D vDir = new Vector3D(vLightDirs[1].X, vLightDirs[1].Y, vLightDirs[1].Z);
            vDir = rt2.Transform(vDir);
            vLightDirs[1].X = (float)vDir.X;
            vLightDirs[1].Y = (float)vDir.Y;
            vLightDirs[1].Z = (float)vDir.Z;

            // Clear the backbuffer
            device.ClearRenderTargetView(renderTargetView, backColor);

            // Clear the depth buffer to 1.0 (max depth)
            device.ClearDepthStencilView(depthStencilView, ClearFlag.Depth, 1.0f, (byte)0);

            //
            // Update variables that change once per frame
            //
            worldVariable.Matrix = worldMatrix;
            lightDirVariable.SetFloatVectorArray(vLightDirs);
            lightColorVariable.SetFloatVectorArray(vLightColors);

            //
            // Render the cube
            //
            TechniqueDescription techDesc = technique.Description;
            for (uint p = 0; p < techDesc.Passes; ++p)
            {
                technique.GetPassByIndex(p).Apply();
                device.DrawIndexed(36, 0, 0);
            }

            //
            // Render each light
            //
            TechniqueDescription techLightDesc = techniqueLight.Description;
            for (int m = 0; m < 2; m++)
            {
                Vector3F vLightPos = new Vector3F(vLightDirs[m].X * 5, vLightDirs[m].Y * 5, vLightDirs[m].Z * 5);
                Transform3DGroup tg = new Transform3DGroup();
                tg.Children.Add(new ScaleTransform3D(0.2, 0.2, 0.2));
                tg.Children.Add(new TranslateTransform3D(vLightPos.X, vLightPos.Y, vLightPos.Z));
                worldVariable.Matrix = tg.Value.ToMatrix4x4F();
                outputColorVariable.FloatVector = new Vector4F (vLightColors[m].X, vLightColors[m].Y, vLightColors[m].Z, vLightColors[m].W);

                for (uint p = 0; p < techLightDesc.Passes; ++p)
                {
                    techniqueLight.GetPassByIndex(p).Apply();
                    device.DrawIndexed(36, 0, 0);
                }
            }

            swapChain.Present(0, PresentFlag.Default);
        } 

//.........这里部分代码省略.........
                        // 假如其中有一条边的一个顶点等于新折线，而另一个顶点等于旧折线中的点，折更新旧点到新点
                    }
                    continue;
                }

                // 计算现在的折线位置
                Edge currentFoldingLineForThisFace = CloverTreeHelper.GetEdgeCrossedFace(face, foldingLine);
                if (currentFoldingLineForThisFace == null)
                    continue;

                // 通过该面的原始面查询折线并更新折线位置和纹理坐标
                foreach (Edge e in face.Parent.Edges)
                {
                    // 判断折线的两个顶点，更新坐标并计算纹理
                    // 顶点1
                    if (CloverMath.IsPointInTwoPoints(foldingLineForThisFace.Vertex1.GetPoint3D(), e.Vertex1.GetPoint3D(), e.Vertex2.GetPoint3D()) &&
                        !foldingLineForThisFace.Vertex1.Moved)
                    {
                        cloverController.FoldingSystem.CalculateTexcoord(foldingLineForThisFace.Vertex1, e);
                        if (CloverMath.IsPointInTwoPoints(currentFoldingLineForThisFace.Vertex1.GetPoint3D(), e.Vertex1.GetPoint3D(), e.Vertex2.GetPoint3D()))
                        {
                            foldingLineOnFace.Vertex1.SetPoint3D(currentFoldingLineForThisFace.Vertex1.GetPoint3D());
                            foldingLineOnFace.Vertex1.Moved = true;
                        }
                        else
                        {
                            foldingLineOnFace.Vertex1.SetPoint3D(currentFoldingLineForThisFace.Vertex2.GetPoint3D());
                            foldingLineOnFace.Vertex1.Moved = true;
                        }
                    }

                    // 顶点2
                    if (CloverMath.IsPointInTwoPoints(foldingLineForThisFace.Vertex2.GetPoint3D(), e.Vertex1.GetPoint3D(), e.Vertex2.GetPoint3D()) &&
                        !foldingLineForThisFace.Vertex2.Moved)
                    {
                        cloverController.FoldingSystem.CalculateTexcoord(foldingLineForThisFace.Vertex2, e);
                        if (CloverMath.IsPointInTwoPoints(currentFoldingLineForThisFace.Vertex1.GetPoint3D(), e.Vertex1.GetPoint3D(), e.Vertex2.GetPoint3D()))
                        {
                            foldingLineOnFace.Vertex2.SetPoint3D(currentFoldingLineForThisFace.Vertex1.GetPoint3D());
                            foldingLineOnFace.Vertex2.Moved = true;
                        }
                        else
                        {
                            foldingLineOnFace.Vertex2.SetPoint3D(currentFoldingLineForThisFace.Vertex2.GetPoint3D());
                            foldingLineOnFace.Vertex2.Moved = true;
                        }
                    }
                }
            }

            // 求得旋转量,并创建旋转矩阵
            Vector3D axis = Vector3D.CrossProduct(vectorFromCurrentFoldingLineToProjVertex, vectorFromLastFoldingLineToOriginVertex);
            axis.Normalize();
            axis.Negate();
            double angle = Vector3D.AngleBetween(vectorFromCurrentFoldingLineToProjVertex, vectorFromLastFoldingLineToOriginVertex);
            AxisAngleRotation3D asixRotation = new AxisAngleRotation3D(axis, angle);
            RotateTransform3D rotateTransform = new RotateTransform3D(asixRotation);
            rotateTransform.CenterX = projectionPoint.X;
            rotateTransform.CenterY = projectionPoint.Y;
            rotateTransform.CenterZ = projectionPoint.Z;

            // 创建平移矩阵
            Vector3D vectorFromProjToOrigin = projectionPoint - lastProjectionPoint;
            TranslateTransform3D translateTransform = new TranslateTransform3D(vectorFromProjToOrigin);

            // 移动所有没有移动过的顶点
            foreach (Face face in lastTimeMovedFaces)
            {
                foreach (Vertex v in face.Vertices)
                {
                    if (!v.Moved)
                    {
                        v.SetPoint3D(translateTransform.Transform(v.GetPoint3D()));
                        v.SetPoint3D(rotateTransform.Transform(v.GetPoint3D()));
                        v.Moved = true;
                    }
                }
            }

            // 最终更改所有顶点移动属性复位
            foreach (Face face in lastTimeMovedFaces)
            {
                foreach (Vertex v in face.Vertices)
                {
                    v.Moved = false;
                }
            }

            // 更新渲染层
            // 也许UpdateAll曾今给了你们许多欢乐的时光，但现在它应该退场了 ---kid
            //cloverController.RenderController.UpdateAll();
            //foreach (Face face in lastTimeMovedFaces)
            //{
            //    cloverController.RenderController.Update(face);
            //}
            // 好吧。。你们还是继续使用UpdateAll好了。。 ---kid
            cloverController.RenderController.UpdateAll();

            return true;
        }

        /// <summary>
        /// 旋转一个面表中除去折痕的所有点 
        /// </summary>
        /// <param name="beRotatedFaceList">待旋转的面表</param>
        /// <param name="foldingLine">折线</param>
        /// <param name="angle">角度</param>
        public List<Vertex> RotateFaces(List<Face> beRotatedFaceList, Edge foldingLine, double angle)
        {
            ShadowSystem shadowSystem = CloverController.GetInstance().ShadowSystem;
            VertexLayer vertexLayer = CloverController.GetInstance().VertexLayer;
            RenderController render = CloverController.GetInstance().RenderController;
            FaceGroupLookupTable table = CloverController.GetInstance().FaceGroupLookupTable;

            List<Vertex> movedVertexList = new List<Vertex>();
            try
            {
                shadowSystem.CheckUndoTree();

                angle *= sign;

                Dictionary<int, bool> movedVertexDict = new Dictionary<int, bool>();
                foreach (Face f in beRotatedFaceList)
                {
                    //foreach (Edge e in f.Edges)
                    //{
                    foreach (Vertex v in f.Vertices)
                        movedVertexDict[v.Index] = false;
                    //}
                }

                // 根据鼠标位移修正所有移动面中不属于折线顶点的其他顶点
                foreach (Face f in beRotatedFaceList)
                {
                    foreach (Edge e in f.Edges)
                    {
                        if (!CloverTreeHelper.IsVertexInEdge(e.Vertex1, foldingLine) && !movedVertexDict[e.Vertex1.Index])
                        {
                            CloneAndUpdateVertex(e.Vertex1);

                            e.Vertex1 = vertexLayer.GetVertex(e.Vertex1.Index);

                            Vector3D axis = new Vector3D();
                            axis.X = foldingLine.Vertex1.X - foldingLine.Vertex2.X;
                            axis.Y = foldingLine.Vertex1.Y - foldingLine.Vertex2.Y;
                            axis.Z = foldingLine.Vertex1.Z - foldingLine.Vertex2.Z;
                            axis.Normalize();

                            AxisAngleRotation3D rotation = new AxisAngleRotation3D(axis, angle);
                            RotateTransform3D rotateTransform = new RotateTransform3D(rotation);
                            rotateTransform.CenterX = (foldingLine.Vertex1.X + foldingLine.Vertex2.X) / 2;
                            rotateTransform.CenterY = (foldingLine.Vertex1.Y + foldingLine.Vertex2.Y) / 2;
                            rotateTransform.CenterZ = (foldingLine.Vertex1.Z + foldingLine.Vertex2.Z) / 2;
                            e.Vertex1.SetPoint3D(rotateTransform.Transform(e.Vertex1.GetPoint3D()));

                            movedVertexDict[e.Vertex1.Index] = true;
                            movedVertexList.Add(e.Vertex1);
                        }

                        if (!CloverTreeHelper.IsVertexInEdge(e.Vertex2, foldingLine) && !movedVertexDict[e.Vertex2.Index])
                        {
                            CloneAndUpdateVertex(e.Vertex2);

                            e.Vertex2 = vertexLayer.GetVertex(e.Vertex2.Index);

                            Vector3D axis = new Vector3D();
                            axis.X = foldingLine.Vertex1.X - foldingLine.Vertex2.X;
                            axis.Y = foldingLine.Vertex1.Y - foldingLine.Vertex2.Y;
                            axis.Z = foldingLine.Vertex1.Z - foldingLine.Vertex2.Z;
                            axis.Normalize();

                            AxisAngleRotation3D rotation = new AxisAngleRotation3D(axis, angle);
                            RotateTransform3D rotateTransform = new RotateTransform3D(rotation);
                            rotateTransform.CenterX = (foldingLine.Vertex1.X + foldingLine.Vertex2.X) / 2;
                            rotateTransform.CenterY = (foldingLine.Vertex1.Y + foldingLine.Vertex2.Y) / 2;
                            rotateTransform.CenterZ = (foldingLine.Vertex1.Z + foldingLine.Vertex2.Z) / 2;

                            e.Vertex2.SetPoint3D(rotateTransform.Transform(e.Vertex2.GetPoint3D()));

                            movedVertexDict[e.Vertex2.Index] = true;
                            movedVertexList.Add(e.Vertex2);
                        }
                    }
                }

                // 因为顶点克隆过了，所以所有的面的边都要更新到引用最新的顶点
                foreach (Face f in CloverController.GetInstance().FaceLayer.Leaves)
                {
                    CloverTreeHelper.UpdateFaceVerticesToLastedVersion(f);
                }

                // 必须先更新group后更新render
                //table.UpdateLookupTable();
                // 你们在滥用UpdateAll…… ---kid
                //render.UpdateAll();
                foreach (Face face in beRotatedFaceList)
                {
                    render.Update(face);
                }

            }
//.........这里部分代码省略.........

        public static MeshGeometry3D GetCone_AlongX(int numSegments, double radius, double height)
        {
            // This is a copy of GetCylinder_AlongX
            //TODO: This is so close to GetMultiRingedTube, the only difference is the multi ring tube has "hard" faces, and this has "soft" faces (this one shares points and normals, so the lighting is smoother)

            if (numSegments < 3)
            {
                throw new ArgumentException("numSegments must be at least 3: " + numSegments.ToString(), "numSegments");
            }

            MeshGeometry3D retVal = new MeshGeometry3D();

            #region Initial calculations

            Transform3D transform = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 1, 0), 90d));

            double halfHeight = height / 2d;

            Point[] points = Math2D.GetCircle_Cached(numSegments);

            double rotateAngleForPerp = Vector3D.AngleBetween(new Vector3D(1, 0, 0), new Vector3D(radius, 0, height));		// the 2nd vector is perpendicular to line formed from the edge of the cone to the tip

            #endregion

            #region Side

            for (int cntr = 0; cntr < numSegments; cntr++)
            {
                retVal.Positions.Add(transform.Transform(new Point3D(points[cntr].X * radius, points[cntr].Y * radius, -halfHeight)));
                retVal.Positions.Add(transform.Transform(new Point3D(0, 0, halfHeight)));		// creating a unique point for each face so that the lighting will be correct

                // The normal points straight out for a cylinder, but for a cone, it needs to be perpendicular to the slope of the cone
                Vector3D normal = new Vector3D(points[cntr].X, points[cntr].Y, 0d);
                Vector3D rotateAxis = new Vector3D(-points[cntr].Y, points[cntr].X, 0d);
                normal = normal.GetRotatedVector(rotateAxis, rotateAngleForPerp);

                retVal.Normals.Add(transform.Transform(normal));

                // This one just points straight up
                retVal.Normals.Add(transform.Transform(new Vector3D(0, 0, 1d)));
            }

            for (int cntr = 0; cntr < numSegments - 1; cntr++)
            {
                // 0,2,3
                retVal.TriangleIndices.Add((cntr * 2) + 0);
                retVal.TriangleIndices.Add((cntr * 2) + 2);
                retVal.TriangleIndices.Add((cntr * 2) + 3);

                // The cylinder has two triangles per face, but the cone only has one
                //// 0,3,1
                //retVal.TriangleIndices.Add((cntr * 2) + 0);
                //retVal.TriangleIndices.Add((cntr * 2) + 3);
                //retVal.TriangleIndices.Add((cntr * 2) + 1);
            }

            // Connecting the last 2 points to the first 2
            // last,0,1
            int offset = (numSegments - 1) * 2;
            retVal.TriangleIndices.Add(offset + 0);
            retVal.TriangleIndices.Add(0);
            retVal.TriangleIndices.Add(1);

            //// last,1,last+1
            //retVal.TriangleIndices.Add(offset + 0);
            //retVal.TriangleIndices.Add(1);
            //retVal.TriangleIndices.Add(offset + 1);

            #endregion

            // Caps
            int pointOffset = retVal.Positions.Count;

            //NOTE: The normals are backward from what you'd think

            GetCylinder_AlongXSprtEndCap(ref pointOffset, retVal, points, new Vector3D(0, 0, 1), radius, radius, -halfHeight, transform);
            //GetCylinder_AlongXSprtEndCap(ref pointOffset, retVal, points, new Vector3D(0, 0, -1), radius, halfHeight, transform);

            // Exit Function
            //retVal.Freeze();
            return retVal;
        }