C# Matrix.Decompose方法代码示例

        internal void Update(TransformComponent transformComponent, ref Matrix worldMatrix)
        {
            if (!Enabled || model == null)
                return;

            // Check if scaling is negative
            bool isScalingNegative = false;
            {
                Vector3 scale, translation;
                Matrix rotation;
                if (worldMatrix.Decompose(out scale, out rotation, out translation))
                    isScalingNegative = scale.X*scale.Y*scale.Z < 0.0f;
            }

            // Make sure skeleton is up to date
            CheckSkeleton();

            if (skeleton != null)
            {
                // Update model view hierarchy node matrices
                skeleton.NodeTransformations[0].LocalMatrix = worldMatrix;
                skeleton.NodeTransformations[0].IsScalingNegative = isScalingNegative;
                skeleton.UpdateMatrices();
            }

            // Update the bounding sphere / bounding box in world space
            var meshes = Model.Meshes;
            var modelBoundingSphere = BoundingSphere.Empty;
            var modelBoundingBox = BoundingBox.Empty;
            bool hasBoundingBox = false;
            Matrix world;
            foreach (var mesh in meshes)
            {
                var meshBoundingSphere = mesh.BoundingSphere;

                if (skeleton != null)
                    skeleton.GetWorldMatrix(mesh.NodeIndex, out world);
                else
                    world = worldMatrix;
                Vector3.TransformCoordinate(ref meshBoundingSphere.Center, ref world, out meshBoundingSphere.Center);
                BoundingSphere.Merge(ref modelBoundingSphere, ref meshBoundingSphere, out modelBoundingSphere);

                var boxExt = new BoundingBoxExt(mesh.BoundingBox);
                boxExt.Transform(world);
                var meshBox = (BoundingBox)boxExt;

                if (hasBoundingBox)
                {
                    BoundingBox.Merge(ref modelBoundingBox, ref meshBox, out modelBoundingBox);
                }
                else
                {
                    modelBoundingBox = meshBox;
                }

                hasBoundingBox = true;
            }

            // Update the bounds
            BoundingBox = modelBoundingBox;
            BoundingSphere = modelBoundingSphere;
        }

        //This is valid for Dynamic rigidbodies (called once at initialization)
        //and Kinematic rigidbodies, called every simulation tick (if body not sleeping) to let the physics engine know where the kinematic body is.
        private void RigidBodyGetWorldTransform(out Matrix physicsTransform)
        {
            Data.PhysicsComponent.Simulation.SimulationProfiler.Mark();
            Data.PhysicsComponent.Simulation.UpdatedRigidbodies++;

            if (BoneIndex == -1)
            {
                DerivePhysicsTransformation(out physicsTransform);
            }
            else
            {
                DeriveBonePhysicsTransformation(out physicsTransform);
            }

            if (DebugEntity == null) return;

            Vector3 scale, pos;
            Quaternion rot;
            physicsTransform.Decompose(out scale, out rot, out pos);
            DebugEntity.Transform.Position = pos;
            DebugEntity.Transform.Rotation = rot;
        }

        //public static void GetRotateMatrix(ref Matrix a  , ref Matrix b)
        //{
        //    b = Matrix.Identity;
        //    b.Right = a.Right;
        //    b.Up = a.Up;
        //    b.Backward = a.Backward;
        //}

        //public static Matrix GetRotateMatrix(ref Matrix a)
        //{
        //    Matrix b = Matrix.Identity;
        //    b.Right = a.Right;
        //    b.Up = a.Up;
        //    b.Backward = a.Backward;
        //    return b;
        //}

        public static void GetRotation(ref Matrix a, out Quaternion rot)
        {
            Vector3 pos;
            Vector3 scale;

            a.Decompose(out scale, out rot, out pos);
        }

	    public static void IntegrateTransform(ref Matrix curTrans,ref Vector3 linvel,ref Vector3 angvel,float timeStep,ref Matrix predictedTransform)
	    {
            predictedTransform = Matrix.Identity;
		    predictedTransform.Translation = (curTrans.Translation + linvel * timeStep);
    //	#define QUATERNION_DERIVATIVE
	    #if QUATERNION_DERIVATIVE
            Vector3 pos;
            Quaternion predictedOrn;
            Vector3 scale;

            curTrans.Decompose(ref scale, ref predictedOrn, ref pos);


		    predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
		    predictedOrn.Normalize();
        #else
            //Exponential map
		    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

		    Vector3 axis;
		    float	fAngle = angvel.Length(); 
		    //limit the angular motion
		    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
		    {
			    fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
		    }

		    if ( fAngle < 0.001f )
		    {
			    // use Taylor's expansions of sync function
			    axis   = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
		    }
		    else
		    {
			    // sync(fAngle) = sin(c*fAngle)/t
			    axis   = angvel*( (float)System.Math.Sin(0.5f*fAngle*timeStep)/fAngle );
		    }
		    Quaternion dorn = new Quaternion(axis.X,axis.Y,axis.Z,(float)System.Math.Cos( fAngle*timeStep*.5f) );
            Vector3 pos;
            Quaternion rot;
            Vector3 scale;

            curTrans.Decompose(out scale, out rot, out pos);
            Quaternion orn0 = rot;

		    Quaternion predictedOrn = dorn * orn0;
		    predictedOrn.Normalize();
	    #endif

            Matrix newMatrix = Matrix.CreateFromQuaternion(predictedOrn);
            CopyMatrixRotation(ref newMatrix, ref predictedTransform);
	    }