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