*
This commit is contained in:
@@ -0,0 +1,125 @@
|
||||
#include "LifeMath.h"
|
||||
|
||||
namespace LifeMath
|
||||
{
|
||||
extern INLINE void Mat4x4Invert(const Matrix4x4* matrix, Matrix4x4* result)
|
||||
{
|
||||
float num5 = matrix->M11;
|
||||
float num4 = matrix->M12;
|
||||
float num3 = matrix->M13;
|
||||
float num2 = matrix->M14;
|
||||
float num9 = matrix->M21;
|
||||
float num8 = matrix->M22;
|
||||
float num7 = matrix->M23;
|
||||
float num6 = matrix->M24;
|
||||
float num17 = matrix->M31;
|
||||
float num16 = matrix->M32;
|
||||
float num15 = matrix->M33;
|
||||
float num14 = matrix->M34;
|
||||
float num13 = matrix->M41;
|
||||
float num12 = matrix->M42;
|
||||
float num11 = matrix->M43;
|
||||
float num10 = matrix->M44;
|
||||
float num23 = (num15 * num10) - (num14 * num11);
|
||||
float num22 = (num16 * num10) - (num14 * num12);
|
||||
float num21 = (num16 * num11) - (num15 * num12);
|
||||
float num20 = (num17 * num10) - (num14 * num13);
|
||||
float num19 = (num17 * num11) - (num15 * num13);
|
||||
float num18 = (num17 * num12) - (num16 * num13);
|
||||
float num39 = ((num8 * num23) - (num7 * num22)) + (num6 * num21);
|
||||
float num38 = -(((num9 * num23) - (num7 * num20)) + (num6 * num19));
|
||||
float num37 = ((num9 * num22) - (num8 * num20)) + (num6 * num18);
|
||||
float num36 = -(((num9 * num21) - (num8 * num19)) + (num7 * num18));
|
||||
float num = 1.0f / ((((num5 * num39) + (num4 * num38)) + (num3 * num37)) + (num2 * num36));
|
||||
result->M11 = num39 * num;
|
||||
result->M21 = num38 * num;
|
||||
result->M31 = num37 * num;
|
||||
result->M41 = num36 * num;
|
||||
result->M12 = -(((num4 * num23) - (num3 * num22)) + (num2 * num21)) * num;
|
||||
result->M22 = (((num5 * num23) - (num3 * num20)) + (num2 * num19)) * num;
|
||||
result->M32 = -(((num5 * num22) - (num4 * num20)) + (num2 * num18)) * num;
|
||||
result->M42 = (((num5 * num21) - (num4 * num19)) + (num3 * num18)) * num;
|
||||
float num35 = (num7 * num10) - (num6 * num11);
|
||||
float num34 = (num8 * num10) - (num6 * num12);
|
||||
float num33 = (num8 * num11) - (num7 * num12);
|
||||
float num32 = (num9 * num10) - (num6 * num13);
|
||||
float num31 = (num9 * num11) - (num7 * num13);
|
||||
float num30 = (num9 * num12) - (num8 * num13);
|
||||
result->M13 = (((num4 * num35) - (num3 * num34)) + (num2 * num33)) * num;
|
||||
result->M23 = -(((num5 * num35) - (num3 * num32)) + (num2 * num31)) * num;
|
||||
result->M33 = (((num5 * num34) - (num4 * num32)) + (num2 * num30)) * num;
|
||||
result->M43 = -(((num5 * num33) - (num4 * num31)) + (num3 * num30)) * num;
|
||||
float num29 = (num7 * num14) - (num6 * num15);
|
||||
float num28 = (num8 * num14) - (num6 * num16);
|
||||
float num27 = (num8 * num15) - (num7 * num16);
|
||||
float num26 = (num9 * num14) - (num6 * num17);
|
||||
float num25 = (num9 * num15) - (num7 * num17);
|
||||
float num24 = (num9 * num16) - (num8 * num17);
|
||||
result->M14 = -(((num4 * num29) - (num3 * num28)) + (num2 * num27)) * num;
|
||||
result->M24 = (((num5 * num29) - (num3 * num26)) + (num2 * num25)) * num;
|
||||
result->M34 = -(((num5 * num28) - (num4 * num26)) + (num2 * num24)) * num;
|
||||
result->M44 = (((num5 * num27) - (num4 * num25)) + (num3 * num24)) * num;
|
||||
}
|
||||
|
||||
extern INLINE void QuatRotationYawPitchRoll(float yaw, float pitch, float roll, Quaternion& result)
|
||||
{
|
||||
float num13 = roll * 0.5f;
|
||||
float num2 = sinf(num13);
|
||||
float num = cosf(num13);
|
||||
float num12 = pitch * 0.5f;
|
||||
float num11 = sinf(num12);
|
||||
float num10 = cosf(num12);
|
||||
float num9 = yaw * 0.5f;
|
||||
float num8 = sinf(num9);
|
||||
float num7 = cosf(num9);
|
||||
float num6 = num7 * num11;
|
||||
float num5 = num8 * num10;
|
||||
result.X = ((num2 * num5) + (num * num6));
|
||||
result.Y = ((num * num5) - (num2 * num6));
|
||||
float num4 = num7 * num10;
|
||||
float num3 = num8 * num11;
|
||||
result.Z = ((num2 * num4) - (num * num3));
|
||||
result.W = ((num2 * num3) + (num * num4));
|
||||
}
|
||||
|
||||
extern INLINE void QuatRotationMatrix(const Matrix4x4& matrix, Quaternion& dst)
|
||||
{
|
||||
float num8 = (matrix.M11 + matrix.M22) + matrix.M33;
|
||||
if (num8 > 0.0f)
|
||||
{
|
||||
float num = sqrtf(num8 + 1.0f);
|
||||
dst.W = num * 0.5f;
|
||||
num = 0.5f / num;
|
||||
dst.X = (matrix.M23 - matrix.M32) * num;
|
||||
dst.Y = (matrix.M31 - matrix.M13) * num;
|
||||
dst.Z = (matrix.M12 - matrix.M21) * num;
|
||||
}
|
||||
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
|
||||
{
|
||||
float num7 = sqrtf((((1.0f + matrix.M11) - matrix.M22) - matrix.M33));
|
||||
float num4 = 0.5f / num7;
|
||||
dst.X = 0.5f * num7;
|
||||
dst.Y = (matrix.M12 + matrix.M21) * num4;
|
||||
dst.Z = (matrix.M13 + matrix.M31) * num4;
|
||||
dst.W = (matrix.M23 - matrix.M32) * num4;
|
||||
}
|
||||
else if (matrix.M22 > matrix.M33)
|
||||
{
|
||||
float num6 = sqrtf((((1.0f + matrix.M22) - matrix.M11) - matrix.M33));
|
||||
float num3 = 0.5f / num6;
|
||||
dst.X = (matrix.M21 + matrix.M12) * num3;
|
||||
dst.Y = 0.5f * num6;
|
||||
dst.Z = (matrix.M32 + matrix.M23) * num3;
|
||||
dst.W = (matrix.M31 - matrix.M13) * num3;
|
||||
}
|
||||
else
|
||||
{
|
||||
float num5 = sqrtf((((1.0f + matrix.M33) - matrix.M11) - matrix.M22));
|
||||
float num2 = 0.5f / num5;
|
||||
dst.X = (matrix.M31 + matrix.M13) * num2;
|
||||
dst.Y = (matrix.M32 + matrix.M23) * num2;
|
||||
dst.Z = 0.5f * num5;
|
||||
dst.W = (matrix.M12 - matrix.M21) * num2;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user