C# Matrix.GetOrientation方法代码示例

        public static void GetCubeParts(
            MyCubeBlockDefinition block,
            Vector3I inputPosition,
            Matrix rotation,
            float gridSize,
            List<string> outModels,
            List<MatrixD> outLocalMatrices,
            List<Vector3> outLocalNormals,
            List<Vector2> outPatternOffsets)
        {
            // CH:TODO: Is rotation argument really needed as a Matrix? It should suffice for it to be MyBlockOrientation

            outModels.Clear();
            outLocalMatrices.Clear();
            outLocalNormals.Clear();
            outPatternOffsets.Clear();

            if (block.CubeDefinition == null)
                return;

            Base6Directions.Direction forward = Base6Directions.GetDirection(Vector3I.Round(rotation.Forward));
            Base6Directions.Direction up = Base6Directions.GetDirection(Vector3I.Round(rotation.Up));
            MyCubeGridDefinitions.GetTopologyUniqueOrientation(block.CubeDefinition.CubeTopology, new MyBlockOrientation(forward, up)).GetMatrix(out rotation);

            MyTileDefinition[] tiles = MyCubeGridDefinitions.GetCubeTiles(block);

            int count = tiles.Length;
            int start = 0;
            int avoidZeroMirrorOffset = 32768;
            float epsilon = 0.01f;

            for (int i = 0; i < count; i++)
            {
                var entry = tiles[start + i];
                var localMatrix = (MatrixD)entry.LocalMatrix * rotation;
                var localNormal = Vector3.Transform(entry.Normal, rotation.GetOrientation());

                var position = inputPosition;
                if(block.CubeDefinition.CubeTopology == MyCubeTopology.Slope2Base)
                {
                    var addition = new Vector3I(Vector3.Sign(localNormal.MaxAbsComponent()));
                    position += addition;
                }
                

                string modelPath = block.CubeDefinition.Model[i];
                Vector2I patternSize = block.CubeDefinition.PatternSize[i];

                int scale = (int)MyModels.GetModelOnlyData(modelPath).PatternScale;

                patternSize = new Vector2I(patternSize.X * scale, patternSize.Y * scale);

                const float sinConst = 10;

                int u = 0;
                int v = 0;

                float yAxis = Vector3.Dot(Vector3.UnitY, localNormal);
                float xAxis = Vector3.Dot(Vector3.UnitX, localNormal);
                float zAxis = Vector3.Dot(Vector3.UnitZ, localNormal);
                if (MyUtils.IsZero(Math.Abs(yAxis) - 1, epsilon))
                {
                    int patternRow = (position.X + avoidZeroMirrorOffset) / patternSize.Y;
                    int offset = (MyMath.Mod(patternRow + (int)(patternRow * Math.Sin(patternRow * sinConst)), patternSize.X));
                    u = MyMath.Mod(position.Z + position.Y + offset + avoidZeroMirrorOffset, patternSize.X);
                    v = MyMath.Mod(position.X + avoidZeroMirrorOffset, patternSize.Y);
                    if (Math.Sign(yAxis) == 1)
                        v = (patternSize.Y - 1) - v;
                }
                else if (MyUtils.IsZero(Math.Abs(xAxis) - 1, epsilon))
                {
                    int patternRow = (position.Z + avoidZeroMirrorOffset) / patternSize.Y;
                    int offset = (MyMath.Mod(patternRow + (int)(patternRow * Math.Sin(patternRow * sinConst)), patternSize.X));
                    u = MyMath.Mod(position.X + position.Y + offset + avoidZeroMirrorOffset, patternSize.X);
                    v = MyMath.Mod(position.Z + avoidZeroMirrorOffset, patternSize.Y);
                    if (Math.Sign(xAxis) == 1)
                        v = (patternSize.Y - 1) - v;
                }
                else if (MyUtils.IsZero(Math.Abs(zAxis) - 1, epsilon))
                {
                    int patternRow = (position.Y + avoidZeroMirrorOffset) / patternSize.Y;
                    int offset = (MyMath.Mod(patternRow + (int)(patternRow * Math.Sin(patternRow * sinConst)), patternSize.X));
                    u = MyMath.Mod(position.X + offset + avoidZeroMirrorOffset, patternSize.X);
                    v = MyMath.Mod(position.Y + avoidZeroMirrorOffset, patternSize.Y);
                    if (Math.Sign(zAxis) == 1)
                        u = (patternSize.X - 1) - u;

                }
                else if (MyUtils.IsZero(xAxis, epsilon))
                {   //slope in YZ
                    u = MyMath.Mod(position.X + avoidZeroMirrorOffset, patternSize.X);
                    v = MyMath.Mod(position.Z + avoidZeroMirrorOffset, patternSize.Y);

                    if (Math.Sign(zAxis) == -1)
                    {
                        if (Math.Sign(yAxis) == 1)
                        {
                            //v = (patternSize.Y - 1) - v;
                            //u = (patternSize.X - 1) - u;
                        }
//.........这里部分代码省略.........

        public bool IsCameraPositionOk(Matrix worldMatrix)
        {
            IMyCameraController cameraController = MySession.Static.CameraController;
            if (cameraController == null)
                return true;

            MyEntity topControlledEntity = ((MyEntity)cameraController).GetTopMostParent();
            if (topControlledEntity.Closed) return false;

            var localAABBHr = topControlledEntity.PositionComp.LocalAABBHr;
            Vector3D center = Vector3D.Transform((Vector3D)localAABBHr.Center, worldMatrix);

            var safeOBB = new MyOrientedBoundingBoxD(center, localAABBHr.HalfExtents, Quaternion.CreateFromRotationMatrix(worldMatrix.GetOrientation()));
            //VRageRender.MyRenderProxy.DebugDrawOBB(safeOBB, Vector3.One, 1, false, false);
            //VRageRender.MyRenderProxy.DebugDrawAxis(topControlledEntity.WorldMatrix, 2, false);

            bool camPosIsOk = HandleIntersection(topControlledEntity, safeOBB, topControlledEntity is Sandbox.Game.Entities.Character.MyCharacter, true, m_target, m_targetOrientation.Forward);

            return camPosIsOk;
        }

 public void SendRagdollTransforms(Matrix world, Matrix[] localBodiesTransforms)
 {
     if (ResponsibleForUpdate(Sync.Clients.LocalClient))
     {
         Vector3 worldPosition = world.Translation;
         int transformsCount = localBodiesTransforms.Length;
         Quaternion worldOrientation = Quaternion.CreateFromRotationMatrix(world.GetOrientation());
         Vector3[] transformsPositions = new Vector3[transformsCount];
         Quaternion[] transformsOrientations = new Quaternion[transformsCount];
         for (int i = 0; i < localBodiesTransforms.Length; ++i)
         {
             transformsPositions[i] = localBodiesTransforms[i].Translation;
             transformsOrientations[i] = Quaternion.CreateFromRotationMatrix(localBodiesTransforms[i].GetOrientation());
         }
         MyMultiplayer.RaiseEvent(this, x => x.OnRagdollTransformsUpdate, transformsCount, transformsPositions, transformsOrientations, worldOrientation, worldPosition);
     }
 }

        public static bool SolveTwoJointsIkCCD(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
        {
            Vector3D rootPos, curEnd, targetVector, curVector, crossResult;
            double cosAngle, turnAngle;
            List<MyCharacterBone> bones = new List<MyCharacterBone>();
            bones.Add(firstBone);
            bones.Add(secondBone);
            bones.Add(endBone);

          
            int tries = 0;
            int maxTries = 50;
            float stopDistance = 0.00001f;
            float gain = 0.6f;

            curEnd = Vector3.Zero;

            do
            {
                foreach (MyCharacterBone bone in bones.Reverse<MyCharacterBone>())
                {
                    // first recalculate current final transformation
                    endBone.ComputeAbsoluteTransform();

                    // compute the position of the root
                    Matrix currentMatrix = bone.AbsoluteTransform;
                    rootPos = (Vector3D)currentMatrix.Translation;  // this is this bone root position
                    curEnd = (Vector3D)endBone.AbsoluteTransform.Translation;   // this is our current end of the final bone                  

                    // get the difference from desired and and current final position
                    double distance = Vector3D.DistanceSquared(curEnd, desiredEnd);

                    // see if i'm already close enough
                    if (distance > stopDistance)
                    {
                        // create the vector to the current effector posm this is the difference vector
                        curVector = curEnd - rootPos;
                        // create the desired effector position vector
                        targetVector = desiredEnd - rootPos;

                        // normalize the vectors (expensive, requires a sqrt)
                        curVector.Normalize();
                        targetVector.Normalize();

                        // the dot product gives me the cosine of the desired angle
                        cosAngle = curVector.Dot(targetVector);

                        // if the dot product returns 1.0, i don't need to rotate as it is 0 degrees
                        if (cosAngle < 1.0)
                        {
                            // use the cross product to check which way to rotate
                            crossResult = curVector.Cross(targetVector);
                            crossResult.Normalize();
                            turnAngle = System.Math.Acos(cosAngle);	// get the angle

                            // get the matrix needed to rotate to the desired position
                            Matrix rotation = Matrix.CreateFromAxisAngle((Vector3)crossResult, (float)turnAngle * gain);

                            // get the absolute matrix rotation ie - rotation including all the bones before
                            Matrix absoluteTransform = Matrix.Normalize(currentMatrix).GetOrientation() * rotation;

                            // compute just the local matrix for the bone - need to multiply with inversion ot its parent matrix and original bind transform      

                            Matrix parentMatrix = Matrix.Identity;
                            if (bone.Parent != null) parentMatrix = bone.Parent.AbsoluteTransform;
                            parentMatrix = Matrix.Normalize(parentMatrix); // may have different scale

                            Matrix localTransform = Matrix.Multiply(absoluteTransform, Matrix.Invert(bone.BindTransform * parentMatrix));

                            // now change the current matrix rotation                           
                            bone.Rotation = Quaternion.CreateFromRotationMatrix(localTransform);

                            // and recompute the transformation
                            bone.ComputeAbsoluteTransform();
                        }
                    }

                }

                // quit if i am close enough or been running long enough
            } while (tries++ < maxTries &&
                Vector3D.DistanceSquared(curEnd, desiredEnd) > stopDistance);

            // solve the last bone
            if (finalBone != null && finalTransform.IsValid())
            {
                //MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation)

                // get the related transformation to original binding posefirstBoneAbsoluteTransform
                MatrixD localTransformRelated;

                if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
                else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);

                //localTransformRelated = Matrix.Normalize(localTransformRelated);
                // from there get the rotation and translation
                finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
                if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation;
                finalBone.ComputeAbsoluteTransform();
            }
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                // cosine law c^2 = a^2 + b^2 - 2*a*b*cos(gamma)
                // gamma = acos ( - (c^2 - a^2 - b^2) / (2*a*b) )

                // alpha = angle between the first bone and originToDesiredEnd vector
                double cosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToDesiredEndLength * originToDesiredEndLength) /
                    (2 * firstBoneLength * originToDesiredEndLength);
                cosAlpha = MathHelper.Clamp(cosAlpha, -1, 1);
                finalAlpha = Math.Acos(cosAlpha);

                // beta = the angle between the first and second bone
                double cosBeta = -(originToDesiredEndLength * originToDesiredEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
                    (2 * firstBoneLength * secondBoneLength);
                cosBeta = MathHelper.Clamp(cosBeta, -1, 1);
                finalBeta = Math.Acos(cosBeta);
                // now get it to the root bone axis no
                finalBeta = Math.PI - finalBeta;
            }


            // get the current angles
            double cCosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToCurrentEndLength * originToCurrentEndLength) /
                (2 * firstBoneLength * originToCurrentEndLength);
            cCosAlpha = MathHelper.Clamp(cCosAlpha, -1, 1);
            double currentAlpha = Math.Acos(cCosAlpha);
            double cCosBeta = -(originToCurrentEndLength * originToCurrentEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
                (2 * firstBoneLength * secondBoneLength);
            cCosBeta = MathHelper.Clamp(cCosBeta, -1, 1);
            double currentBeta = Math.Acos(cCosBeta);
            currentBeta = Math.PI - currentBeta;

            Vector3 currentPlaneNormal = Vector3.Cross(firstBoneVector, originToCurrentEnd);
            currentPlaneNormal.Normalize();

            // we can now rotate the bones in current plane as if the desired end was on the currentEnd axis
            float alphaDif = (float)(finalAlpha - currentAlpha);
            float betaDif = (float)(finalBeta - currentBeta);
            Matrix firstBoneRotation = Matrix.CreateFromAxisAngle(-currentPlaneNormal, alphaDif);
            Matrix secondBoneRotation = Matrix.CreateFromAxisAngle(currentPlaneNormal, betaDif);


            // now get the angle between original and final position plane normal
            originToCurrentEnd.Normalize();
            originToDesiredEnd.Normalize();
            double dotProd = originToCurrentEnd.Dot(originToDesiredEnd);

            dotProd = MathHelper.Clamp(dotProd, -1, 1);
            double delta = Math.Acos(dotProd);
            Vector3 planeRotationAxis = Vector3.Cross(originToCurrentEnd, originToDesiredEnd);
            planeRotationAxis.Normalize();

            // find the rotation matrices for bones in the original plane
            Matrix planeRotation = Matrix.CreateFromAxisAngle(planeRotationAxis, (float)delta);

            // compute the final rotations
            firstBoneRotation = planeRotation * firstBoneRotation;
            secondBoneRotation = secondBoneRotation * firstBoneRotation;

            // draw the final positions if debug enabled
            if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
            {
                Vector3 rotatedFirst = Vector3.Transform(firstBoneVector, firstBoneRotation);
                Vector3 rotatedSecond = Vector3.Transform(secondBoneVector, secondBoneRotation);
                VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + rotatedFirst, WorldMatrix), Color.Purple, Color.Purple, false);
                VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + rotatedFirst, WorldMatrix), Vector3.Transform(origin + rotatedFirst + rotatedSecond, WorldMatrix), Color.White, Color.White, false);
            }

            // Now we compute the final absolute transforms for the bones
            Matrix firstBoneFinalAbsoluteTransform = firstBoneAbsoluteTransform * firstBoneRotation;
            Matrix firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
            Matrix localFirstBoneTransform = Matrix.Multiply(firstBoneFinalAbsoluteTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
            firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
            firstBone.ComputeAbsoluteTransform();

            Matrix secondBoneFinalAbsoluteTransform = secondBoneAbsoluteTransform * secondBoneRotation;
            Matrix secondBoneParentAbsoluteTransform = secondBone.Parent.AbsoluteTransform;
            Matrix localSecondBoneTransform = Matrix.Multiply(secondBoneFinalAbsoluteTransform, Matrix.Invert(secondBone.BindTransform * secondBoneParentAbsoluteTransform));

            secondBone.Rotation = Quaternion.CreateFromRotationMatrix(localSecondBoneTransform);
            secondBone.ComputeAbsoluteTransform();

            // solve the last bone 
            if (finalBone != null && finalTransform.IsValid() && isDesiredEndReachable)
            {
                //MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation)

                // get the related transformation to original binding pose
                MatrixD localTransformRelated;

                if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
                else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);

                //localTransformRelated = Matrix.Normalize(localTransformRelated);
                // from there get the rotation and translation
                finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
                if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation;
                finalBone.ComputeAbsoluteTransform();
            }

            return isDesiredEndReachable;
        }

 public void SendRagdollTransforms(Matrix world, Matrix[] localBodiesTransforms)
 {
     if (ResponsibleForUpdate(this))
     {
         var msg = new RagdollTransformsMsg();
         msg.CharacterEntityId = Entity.EntityId;
         msg.worldPosition = world.Translation;
         msg.TransformsCount = localBodiesTransforms.Count();
         msg.worldOrientation = Quaternion.CreateFromRotationMatrix(world.GetOrientation());
         msg.transformsPositions = new Vector3[msg.TransformsCount];
         msg.transformsOrientations = new Quaternion[msg.TransformsCount];
         for (int i = 0; i < localBodiesTransforms.Count(); ++i)
         {
             msg.transformsPositions[i] = localBodiesTransforms[i].Translation;
             msg.transformsOrientations[i] = Quaternion.CreateFromRotationMatrix(localBodiesTransforms[i].GetOrientation());
         }
         Sync.Layer.SendMessageToAll(ref msg);
     }
 }

//.........这里部分代码省略.........
                        double distanceSq = Vector3D.DistanceSquared(curEnd, desiredEnd);
                        
                        //{
                        //    Color c = Color.FromNonPremultiplied(new Vector4(4 * (float) (distanceSq),
                        //        1 - 4 * (float) (distanceSq), 0, 1));
                        //    VRageRender.MyRenderProxy.DebugDrawLine3D(
                        //        Vector3D.Transform(lastEnd, worldMatrix),
                        //        Vector3D.Transform(curEnd, worldMatrix), c, c, false);
                        //}

                        // see if i'm already close enough
                        if (distanceSq > stopDistanceSq)
                        {
                            // create the vector to the current effector posm this is the difference vector
                            curVector = curEnd - rootPos;
                            // create the desired effector position vector
                            var targetVector = desiredEnd - rootPos;

                            // normalize the vectors (expensive, requires a sqrt)
                            // MZ: we don't need to do that
                            // curVector.Normalize();
                            // targetVector.Normalize();

                            double curVectorLenSq = curVector.LengthSquared();
                            double targetVectorLenSq = targetVector.LengthSquared();

                            // the dot product gives me the cosine of the desired angle
                            // cosAngle = curVector.Dot(targetVector);

                            double dotCurTarget = curVector.Dot(targetVector);

                            // if the dot product returns 1.0, i don't need to rotate as it is 0 degrees
                            // MZ: yes, but when does this happen to be exactly 1???
                            // if (cosAngle < 1.0)
                            if (dotCurTarget < 0 || dotCurTarget * dotCurTarget < curVectorLenSq * targetVectorLenSq * (1 - MyMathConstants.EPSILON))
                            {
                                // use the cross product to check which way to rotate
                                //var rotationAxis = curVector.Cross(targetVector);
                                //rotationAxis.Normalize();
                                //turnAngle = System.Math.Acos(cosAngle); // get the angle

                                // get the matrix needed to rotate to the desired position
                                //Matrix rotation = Matrix.CreateFromAxisAngle((Vector3) rotationAxis,
                                //    (float) turnAngle * gain);

                                // get the absolute matrix rotation ie - rotation including all the bones before
                                Matrix rotation;
                                float weight = 1 / (initialDistSqInv * (float)distanceSq + 1);
                                Vector3 weightedTarget = Vector3.Lerp(curVector, targetVector, weight);
                                Matrix.CreateRotationFromTwoVectors(ref curVector, ref weightedTarget, out rotation);
                                Matrix absoluteTransform = Matrix.Normalize(currentMatrix).GetOrientation() * rotation;

                                // MZ: faster
                                
                                // compute just the local matrix for the bone - need to multiply with inversion ot its parent matrix and original bind transform      

                                Matrix parentMatrix = Matrix.Identity;
                                if (bone.Parent != null) parentMatrix = bone.Parent.AbsoluteTransform;
                                parentMatrix = Matrix.Normalize(parentMatrix); // may have different scale

                                Matrix localTransform = Matrix.Multiply(absoluteTransform,
                                    Matrix.Invert(bone.BindTransform * parentMatrix));

                                // now change the current matrix rotation                           
                                bone.Rotation = Quaternion.CreateFromRotationMatrix(localTransform);

                                // and recompute the transformation
                                bone.ComputeAbsoluteTransform();
                            }
                        }

                    }

                    // quit if i am close enough or been running long enough
                } while (tries++ < maxTries &&
                         Vector3D.DistanceSquared(curEnd, desiredEnd) > stopDistanceSq);
            }

            // solve the last bone
            if (finalTransform.IsValid())
            {
                // get the related transformation to original binding posefirstBoneAbsoluteTransform
                MatrixD localTransformRelated;

                if (allowFinalBoneTranslation) 
                    localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
                else
                    localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);

                // localTransformRelated = Matrix.Normalize(localTransformRelated);
                // from there get the rotation and translation
                finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
                if (allowFinalBoneTranslation) 
                    finalBone.Translation = (Vector3)localTransformRelated.Translation;

                finalBone.ComputeAbsoluteTransform();
            }

            return true;//Vector3D.DistanceSquared(curEnd, desiredEnd) <= stopDistanceSq;
        }

        /// <summary>
        /// Calculates the force necessary to rotate the grid.
        /// </summary>
        /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
        /// <param name="Direction">The direction to face the localMatrix in.</param>
        /// <param name="angularVelocity">The local angular velocity of the controlling block.</param>
        private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity)
        {
            CheckGrid();

            myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR);

            angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());

            //myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()");
            float gyroForce = myGyro.TotalGyroForce();

            const ulong UpdateFrequency = ShipController_Autopilot.UpdateFrequency;
            if (rotateForceRatio != Vector3.Zero)
                if (Globals.UpdateCount - updated_prevAngleVel == UpdateFrequency) // needs to be == because displacment is not divided by frequency
                {
                    Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce);

                    //myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");

                    myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
                }
                else
                    myLogger.debugLog("prevAngleVel is old: " + (Globals.UpdateCount - updated_prevAngleVel), "CalcRotate()", Logger.severity.DEBUG);

            localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
            Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);

            localMatrix = localMatrix.GetOrientation();
            inverted = inverted.GetOrientation();

            //myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()");
            //myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()");
            //myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()");
            //myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()");

            Vector3 localDirect = Direction.ToLocal();
            Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);

            rotBlockDirect.Normalize();

            float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

            Vector3 rotaRight = localMatrix.Right;
            Vector3 rotaUp = localMatrix.Up;

            Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
            Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

            Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

            if (UpDirect != null)
            {
                Vector3 upLocal = UpDirect.ToLocal();
                Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
                float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);

                Vector3 rotaBackward = localMatrix.Backward;
                Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

                myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (-roll * NFR_backward), "CalcRotate()");

                displacement += roll * NFR_backward;
            }

            NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();

            if (NavSet.Settings_Current.CollisionAvoidance)
            {
                myPathfinder.TestRotate(displacement);
                if (!myPathfinder.CanRotate)
                {
                    Logger.debugNotify("Cannot Rotate", 50);
                    myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
                    return;
                }
            }

            //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");

            if (myGyro.torqueAccelRatio == 0)
            {
                // do a test
                myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
                rotateForceRatio = new Vector3(0, 1f, 0);
                return;
            }

            Vector3 targetVelocity = MaxAngleVelocity(displacement);

            // Adjust for moving target by measuring changes in displacement. Part of the change in displacement is attributable to ship rotation.
            const float dispToVel = (float)Globals.UpdatesPerSecond / (float)ShipController_Autopilot.UpdateFrequency;

            Vector3 addVelocity = angularVelocity - (prevAngleDisp - displacement) * dispToVel;
            if (addVelocity.LengthSquared() < 0.1f)
//.........这里部分代码省略.........

        /// <summary>
        /// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect.
        /// </summary>
        /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
        /// <param name="Direction">The direction to face the localMatrix in.</param>
        private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity)
        {
            m_logger.debugLog(Direction == null, "Direction == null", Logger.severity.ERROR);

            m_gyro.Update();
            float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor);
            if (minimumMoment <= 0f)
            {
                // == 0f, not calculated yet. < 0f, we have math failure
                StopRotate();
                m_logger.debugLog(minimumMoment < 0f, "minimumMoment < 0f", Logger.severity.FATAL);
                return;
            }

            localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
            Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);

            localMatrix = localMatrix.GetOrientation();
            inverted = inverted.GetOrientation();

            Vector3 localDirect = Direction.ToLocalNormalized();
            Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);

            float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

            Vector3 rotaRight = localMatrix.Right;
            Vector3 rotaUp = localMatrix.Up;

            Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
            Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

            Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

            if (UpDirect != null)
            {
                Vector3 upLocal = UpDirect.ToLocalNormalized();
                Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
                upRotBlock.Z = 0f;
                upRotBlock.Normalize();
                float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f));

                Vector3 rotaBackward = localMatrix.Backward;
                Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

                //m_logger.debugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()");

                displacement += roll * NFR_backward;
            }

            m_lastMoveAttempt = Globals.UpdateCount;
            Pathfinder.TestRotate(displacement);

            switch (Pathfinder.m_rotateState)
            {
                case Autopilot.Pathfinder.Pathfinder.PathState.Not_Running:
                    m_logger.debugLog("Pathfinder not run yet: " + Pathfinder.m_rotateState);
                    m_lastMove = Globals.UpdateCount;
                    return;
                case Autopilot.Pathfinder.Pathfinder.PathState.No_Obstruction:
                    break;
                default:
                    m_logger.debugLog("Pathfinder not allowing rotation: " + Pathfinder.m_rotateState);
                    return;
            }

            float distanceAngle = displacement.Length();
            if (distanceAngle < m_bestAngle || float.IsNaN(m_navSet.Settings_Current.DistanceAngle))
            {
                m_bestAngle = distanceAngle;
                m_lastMove = Globals.UpdateCount;
            }
            m_navSet.Settings_Task_NavWay.DistanceAngle = distanceAngle;

            //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()");

            m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null);

            // adjustment to face a moving entity
            if (targetEntity != null)
            {
                Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - LinearVelocity;
                float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());

                //myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()");

                float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down);
                float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right);
                float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
                float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);

                m_logger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw));

                m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw;
            }
            //m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()");
//.........这里部分代码省略.........

		/// <summary>
		/// Calculates the force necessary to rotate the grid.
		/// </summary>
		/// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
		/// <param name="Direction">The direction to face the localMatrix in.</param>
		/// <param name="angularVelocity">The local angular velocity of the controlling block.</param>
		private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity, IMyEntity targetEntity)
		{
			myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR);

			angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());

			//myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()");
			float gyroForce = myGyro.TotalGyroForce();

			float secondsSinceLast = (float)(DateTime.UtcNow - updated_prevAngleVel).TotalSeconds;
			updated_prevAngleVel = DateTime.UtcNow;

			if (rotateForceRatio != Vector3.Zero)
			{
				if (secondsSinceLast <= MaxUpdateSeconds)
				{
					Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce * secondsSinceLast);

					//myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");

					myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
				}
				else
					myLogger.debugLog("prevAngleVel is old: " + secondsSinceLast, "CalcRotate()", Logger.severity.DEBUG);
			}

			localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
			Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);

			localMatrix = localMatrix.GetOrientation();
			inverted = inverted.GetOrientation();

			//myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()");
			//myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()");
			//myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()");
			//myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()");

			Vector3 localDirect = Direction.ToLocal();
			Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);

			rotBlockDirect.Normalize();

			float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

			Vector3 rotaRight = localMatrix.Right;
			Vector3 rotaUp = localMatrix.Up;

			Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
			Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

			Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

			if (UpDirect != null)
			{
				Vector3 upLocal = UpDirect.ToLocal();
				Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
				float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);

				Vector3 rotaBackward = localMatrix.Backward;
				Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

				myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (roll * NFR_backward), "CalcRotate()");

				displacement += roll * NFR_backward;
			}

			NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();

			if (NavSet.Settings_Current.CollisionAvoidance)
			{
				myPathfinder.TestRotate(displacement);
				if (!myPathfinder.CanRotate)
				{
					// if cannot rotate and not calculating move, move away from obstruction
					if (myPathfinder.RotateObstruction != null && Globals.UpdateCount >= m_notCalcMove)
					{
						Vector3 position  = Block.CubeBlock.GetPosition();
						Vector3 away = position - myPathfinder.RotateObstruction.GetCentre();
						away.Normalize();
						myLogger.debugLog("Cannot rotate and not calculating move, creating GOLIS to move away from obstruction", "CalcRotate()", Logger.severity.INFO);
						new GOLIS(this, NavSet, position + away * (10f + NavSet.Settings_Current.DestinationRadius), true);
					}
					Logger.debugNotify("Cannot Rotate", 50);
					myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
					return;
				}
			}

			//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");

			if (myGyro.torqueAccelRatio == 0)
			{
				// do a test
				myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
//.........这里部分代码省略.........