//Bullet callback updates Unity transform
public override void SetWorldTransform(ref Matrix m)
{
UnityEngine.Vector3 position = BulletExtensionMethods.ExtractTranslationFromMatrix(ref m);
UnityEngine.Quaternion rotation = BulletExtensionMethods.GetUnityRotationFromMatrix(ref m);
_transform.SetPositionAndRotation(position, rotation);
}
/// <summary>
/// Directly extract a Unity Quaternion from Bullet Matrix
/// </summary>
/// <param name="matrix"></param>
/// <param name="result"></param>
public static void GetUnityRotationFromMatrix(ref Matrix matrix, out UnityEngine.Quaternion result)
{
var num1 = matrix.M11 + matrix.M22 + matrix.M33;
if (num1 > 0.0f)
{
var num2 = Mathf.Sqrt(num1 + 1.0f);
result.w = num2 * 0.5f;
var num3 = 0.5f / num2;
result.x = (matrix.M23 - matrix.M32) * num3;
result.y = (matrix.M31 - matrix.M13) * num3;
result.z = (matrix.M12 - matrix.M21) * num3;
}
else if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
{
var num2 = Mathf.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33);
var num3 = 0.5f / num2;
result.x = 0.5f * num2;
result.y = (matrix.M12 + matrix.M21) * num3;
result.z = (matrix.M13 + matrix.M31) * num3;
result.w = (matrix.M23 - matrix.M32) * num3;
}
else if (matrix.M22 > matrix.M33)
{
var num2 = Mathf.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33);
var num3 = 0.5f / num2;
result.x = (matrix.M21 + matrix.M12) * num3;
result.y = 0.5f * num2;
result.z = (matrix.M32 + matrix.M23) * num3;
result.w = (matrix.M31 - matrix.M13) * num3;
}
else
{
var num2 = Mathf.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22);
var num3 = 0.5f / num2;
result.x = (matrix.M31 + matrix.M13) * num3;
result.y = (matrix.M32 + matrix.M23) * num3;
result.z = 0.5f * num2;
result.w = (matrix.M12 - matrix.M21) * num3;
}
}
/// <summary>
/// Extract position, rotation and scale from TRS matrix.
/// </summary>
/// <param name="matrix">Transform matrix. This parameter is passed by reference
/// to improve performance; no changes will be made to it.</param>
/// <param name="localPosition">Output position.</param>
/// <param name="localRotation">Output rotation.</param>
/// <param name="localScale">Output scale.</param>
public static void DecomposeMatrix(ref Matrix4x4 matrix, out UnityEngine.Vector3 localPosition, out UnityEngine.Quaternion localRotation, out UnityEngine.Vector3 localScale)
{
localPosition = ExtractTranslationFromMatrix(ref matrix);
localRotation = ExtractRotationFromMatrix(ref matrix);
localScale = ExtractScaleFromMatrix(ref matrix);
}
/// <summary>
/// Convert a Unity Quaternion to BulletSharp
/// </summary>
/// <param name="q">Quaternion to be converted</param>
/// <returns></returns>
public static Quaternion ToBullet(this UnityEngine.Quaternion q)
{
return(new Quaternion(q.x, q.y, q.z, q.w));
}