C# Matrix.IsValid方法代码示例

//.........这里部分代码省略.........
                // 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 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();
            }
//.........这里部分代码省略.........

        private void ActivateRagdoll(Matrix worldMatrix)
        {
            if (MyFakes.ENABLE_RAGDOLL_DEBUG)
            {
                Debug.WriteLine("MyPhysicsBody.ActivateRagdoll");
                MyLog.Default.WriteLine("MyPhysicsBody.ActivateRagdoll");
            }

            if (Ragdoll == null)
            {
                Debug.Fail("Can not switch to Ragdoll mode, ragdoll is null!");
                return;
            }
            if (HavokWorld == null)
            {
                Debug.Fail("Can not swtich to Ragdoll mode, HavokWorld is null!");
                return;
            }
            if (IsRagdollModeActive)
            {
                Debug.Fail("Can not switch to ragdoll mode, ragdoll is still active!");
                return;
            }
            //Matrix world = Entity.WorldMatrix;
            //world.Translation = WorldToCluster(world.Translation);
            Debug.Assert(worldMatrix.IsValid() && worldMatrix != Matrix.Zero, "Ragdoll world matrix is invalid!");
                        
            Ragdoll.SetWorldMatrix(worldMatrix);            
                        
            // Because after cluster's reorder, the bodies can collide!            
            HavokWorld.AddRagdoll(Ragdoll);
            DisableRagdollBodiesCollisions();

            if (MyFakes.ENABLE_RAGDOLL_DEBUG)
            {
                Debug.WriteLine("MyPhysicsBody.ActivateRagdoll - FINISHED");
                MyLog.Default.WriteLine("MyPhysicsBody.ActivateRagdoll - FINISHED");
            }
        }

//.........这里部分代码省略.........
                        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>
        /// Called when [world position changed].
        /// </summary>
        /// <param name="source">The source object that caused this event.</param>
        public override void OnWorldPositionChanged(object source)
        {
            if (IsInWorld == false)
                return;

            Debug.Assert(this != source, "Recursion!");
            //Debug.Assert(Entity.Parent == null || RigidBody.IsFixedOrKeyframed);

            Vector3 velocity = Vector3.Zero;
            IMyEntity parentEntity = Entity.GetTopMostParent();
            if (parentEntity.Physics != null)
            {
                velocity = parentEntity.Physics.LinearVelocity;
                //TODO:this should be optional, all our child bodies are kinematic and dependent on parent atm
                if (Entity != parentEntity)
                    LinearVelocity = parentEntity.Physics.GetVelocityAtPoint(Entity.PositionComp.GetPosition());
            }
            if(!IsWelded)
                MyPhysics.MoveObject(ClusterObjectID, parentEntity.WorldAABB, velocity);

            Matrix bodyMatrix;
            GetRigidBodyMatrix(out bodyMatrix);
            if (bodyMatrix.EqualsFast(ref m_bodyMatrix))
                return;

            m_bodyMatrix = bodyMatrix;

            if (RigidBody != null)
            {
                RigidBody.SetWorldMatrix(m_bodyMatrix);
            }

            if (RigidBody2 != null)
            {
                RigidBody2.SetWorldMatrix(m_bodyMatrix);
            }

            if (CharacterProxy != null)
            {
                CharacterProxy.Position = m_bodyMatrix.Translation;
                CharacterProxy.Forward = m_bodyMatrix.Forward;
                CharacterProxy.Up = m_bodyMatrix.Up;
                CharacterProxy.Speed = 0;

                //if (CharacterProxy.ImmediateSetWorldTransform)
                {
                    CharacterProxy.SetRigidBodyTransform(m_bodyMatrix);
                }
            }

            // TODO: This is disabled due to world synchronization, Ragdoll if set to some position from server doesn't simulate properly
            // Ragdoll updates it's position also in AfterUpdate on MyCharacter, so now this is not needed, but should be working.
            //if (Ragdoll != null && IsRagdollModeActive && m_ragdollDeadMode && !Sync.IsServer && MyFakes.ENABLE_RAGDOLL_CLIENT_SYNC)
            //{
            //    //Ragdoll.SetToKeyframed();
            //    //Ragdoll.SwitchToLayer(MyPhysics.CollisionLayers.RagdollCollisionLayer);
            //    Ragdoll.SetWorldMatrix(rigidBodyMatrix,true);
            //}

            if (Ragdoll != null && IsRagdollModeActive && source is MyCockpit)
            {
                Debug.Assert(m_bodyMatrix.IsValid() && m_bodyMatrix != Matrix.Zero, "Ragdoll world matrix is invalid!");
                Ragdoll.ResetToRigPose();
                Ragdoll.SetWorldMatrix(m_bodyMatrix);
                Ragdoll.ResetVelocities();
            }
        }