internal void UpdateBoneTransformation(ref Matrix physicsTransform) { if (ColliderShape.LocalOffset != Vector3.Zero || ColliderShape.LocalRotation != Quaternion.Identity) { physicsTransform = Matrix.Multiply(ColliderShape.NegativeCenterMatrix, physicsTransform); } var rotation = Quaternion.RotationMatrix(physicsTransform); var translation = physicsTransform.TranslationVector; Vector3 scale; scale.X = (float)Math.Sqrt((BoneWorldMatrix.M11 * BoneWorldMatrix.M11) + (BoneWorldMatrix.M12 * BoneWorldMatrix.M12) + (BoneWorldMatrix.M13 * BoneWorldMatrix.M13)); scale.Y = (float)Math.Sqrt((BoneWorldMatrix.M21 * BoneWorldMatrix.M21) + (BoneWorldMatrix.M22 * BoneWorldMatrix.M22) + (BoneWorldMatrix.M23 * BoneWorldMatrix.M23)); scale.Z = (float)Math.Sqrt((BoneWorldMatrix.M31 * BoneWorldMatrix.M31) + (BoneWorldMatrix.M32 * BoneWorldMatrix.M32) + (BoneWorldMatrix.M33 * BoneWorldMatrix.M33)); TransformComponent.CreateMatrixTRS(ref translation, ref rotation, ref scale, out BoneWorldMatrixOut); }
/// <summary> /// Updades the graphics transformation from the given physics transformation /// </summary> /// <param name="physicsTransform"></param> internal void UpdateTransformationComponent(ref Matrix physicsTransform) { var entity = Entity; if (ColliderShape.LocalOffset != Vector3.Zero || ColliderShape.LocalRotation != Quaternion.Identity) { physicsTransform = Matrix.Multiply(ColliderShape.NegativeCenterMatrix, physicsTransform); } //extract from physics transformation matrix var physTranslation = physicsTransform.TranslationVector; var physRotation = Quaternion.RotationMatrix(physicsTransform); //we need to extract scale only.. Vector3 scale; scale.X = (float)Math.Sqrt((entity.Transform.WorldMatrix.M11 * entity.Transform.WorldMatrix.M11) + (entity.Transform.WorldMatrix.M12 * entity.Transform.WorldMatrix.M12) + (entity.Transform.WorldMatrix.M13 * entity.Transform.WorldMatrix.M13)); scale.Y = (float)Math.Sqrt((entity.Transform.WorldMatrix.M21 * entity.Transform.WorldMatrix.M21) + (entity.Transform.WorldMatrix.M22 * entity.Transform.WorldMatrix.M22) + (entity.Transform.WorldMatrix.M23 * entity.Transform.WorldMatrix.M23)); scale.Z = (float)Math.Sqrt((entity.Transform.WorldMatrix.M31 * entity.Transform.WorldMatrix.M31) + (entity.Transform.WorldMatrix.M32 * entity.Transform.WorldMatrix.M32) + (entity.Transform.WorldMatrix.M33 * entity.Transform.WorldMatrix.M33)); TransformComponent.CreateMatrixTRS(ref physTranslation, ref physRotation, ref scale, out entity.Transform.WorldMatrix); if (entity.Transform.Parent == null) { entity.Transform.LocalMatrix = entity.Transform.WorldMatrix; } else { //We are not root so we need to derive the local matrix as well var inverseParent = entity.Transform.Parent.WorldMatrix; inverseParent.Invert(); entity.Transform.LocalMatrix = Matrix.Multiply(entity.Transform.WorldMatrix, inverseParent); } Vector3 translation; Quaternion rotation; entity.Transform.LocalMatrix.Decompose(out scale, out rotation, out translation); entity.Transform.Position = translation; entity.Transform.Rotation = rotation; }