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