C# Matrix.Count方法代码示例

 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);
     }
 }

        protected Matrix<double> GetCentroid(Matrix<double>[] points)
        {
            var pointsMCv = new MCvPoint3D32f[points.Count()];
            for (int i = 0; i < points.Count(); ++i)
            {
                pointsMCv[i] = new MCvPoint3D32f((float)points[i][0, 0], (float)points[i][1, 0], (float)points[i][2, 0]);
            }

            return GetCentroid(pointsMCv);
        }

        protected double GetSVDRotation(Matrix<double>[] p1, Matrix<double>[] p2, out Matrix<double> rotation)
        {
            var centr1 = GetCentroid(p1);
            var centr2 = GetCentroid(p2);

            var q1 = new List<Matrix<double>>();
            var q2 = new List<Matrix<double>>();
            var H = new Matrix<double>(3, 3);

            for (int i = 0; i < Math.Min(p1.Count(), p2.Count()); ++i)
            {
                var q1d = new Matrix<double>(new double[,]
                {
                    {p1[i][0, 0] - centr1[0, 0]},
                    {p1[i][1, 0] - centr1[1, 0]},
                    {p1[i][2, 0] - centr1[2, 0]}
                });

                q1.Add(q1d);

                var q2d = new Matrix<double>(new double[,]
                {
                    {p2[i][0, 0] - centr2[0, 0]},
                    {p2[i][1, 0] - centr2[1, 0]},
                    {p2[i][2, 0] - centr2[2, 0]}
                });

                q2.Add(q2d);

                H = H.Add(q1d.Mul(q2d.Transpose()));
            }

            var U = new Matrix<double>(3, 3);
            var W = new Matrix<double>(3, 3);
            var V = new Matrix<double>(3, 3);

            CvInvoke.cvSVD(H, W, U, V, SVD_TYPE.CV_SVD_DEFAULT);

            var X = V.Mul(U.Transpose());

            var detX = CvInvoke.cvDet(X);

            rotation = X;
            return detX;
        }

        internal void UpdateRigidBodiesTransformsSynced(int transformsCount, Matrix worldMatrix, Matrix[] transforms)
        {           
            if (!m_inicialized || !IsActive) return;
            if (Ragdoll == null || !Ragdoll.IsAddedToWorld) return;

            Debug.Assert(transformsCount == transforms.Count(), "Wrong ragdoll transforms sync - transforms don't match!");
            Debug.Assert(transformsCount == Ragdoll.RigidBodies.Count, "The count of ragdoll transform matrices doesn't match the count of rigid bodies!");
            List<Vector3> linearVelocities = new List<Vector3>();
            List<Vector3> angularVelocities = new List<Vector3>();
            if (transformsCount == m_ragdollRigidBodiesAbsoluteTransforms.Count())
            {
                for (int i =0;i<transformsCount;++i)
                {
                    Debug.Assert(Ragdoll.RigidBodies.IsValidIndex(i), "Sync - Ragdoll rigid body index is invalid. Is the ragdoll model correctly built?");
                    Debug.Assert(transforms[i].IsValid(), "Sync - Ragdoll body absolute transform is invalid");
                    Debug.Assert(transforms[i] != Matrix.Zero, "Sync - Ragdoll body absolute transform is zero");
                    linearVelocities.Add(Ragdoll.RigidBodies[i].LinearVelocity);
                    angularVelocities.Add(Ragdoll.RigidBodies[i].AngularVelocity);
                    Ragdoll.SetRigidBodyLocalTransform(i, transforms[i]);
                }
            }

            MatrixD havokWorld = worldMatrix;
            havokWorld.Translation = m_character.Physics.WorldToCluster(worldMatrix.Translation);
            Ragdoll.SetWorldMatrix(havokWorld);
            Ragdoll.SetTransforms(havokWorld, false);

            foreach (var rigidBodyIndex in m_rigidBodiesToBonesIndices.Keys)
            {
                Ragdoll.RigidBodies[rigidBodyIndex].LinearVelocity = linearVelocities[rigidBodyIndex];
                Ragdoll.RigidBodies[rigidBodyIndex].AngularVelocity = angularVelocities[rigidBodyIndex];
            }            
        }