Merge branch 'master' into stable

1 files changed, 365 insertions, 0 deletions

diff --git a/Source/Core/Common/MathUtil.cpp b/Source/Core/Common/MathUtil.cpp
new file mode 100644
index 0000000000..07fa5bf293
--- /dev/null
+++ b/Source/Core/Common/MathUtil.cpp
@@ -0,0 +1,365 @@
+// Copyright 2008 Dolphin Emulator Project
+// Licensed under GPLv2+
+// Refer to the license.txt file included.
+
+#include <cmath>
+#include <cstring>
+#include <limits>
+#include <numeric>
+
+#include "Common/CommonTypes.h"
+#include "Common/MathUtil.h"
+
+namespace MathUtil
+{
+
+u32 ClassifyDouble(double dvalue)
+{
+	// TODO: Optimize the below to be as fast as possible.
+	IntDouble value(dvalue);
+	u64 sign = value.i & DOUBLE_SIGN;
+	u64 exp  = value.i & DOUBLE_EXP;
+	if (exp > DOUBLE_ZERO && exp < DOUBLE_EXP)
+	{
+		// Nice normalized number.
+		return sign ? PPC_FPCLASS_NN : PPC_FPCLASS_PN;
+	}
+	else
+	{
+		u64 mantissa = value.i & DOUBLE_FRAC;
+		if (mantissa)
+		{
+			if (exp)
+			{
+				return PPC_FPCLASS_QNAN;
+			}
+			else
+			{
+				// Denormalized number.
+				return sign ? PPC_FPCLASS_ND : PPC_FPCLASS_PD;
+			}
+		}
+		else if (exp)
+		{
+			//Infinite
+			return sign ? PPC_FPCLASS_NINF : PPC_FPCLASS_PINF;
+		}
+		else
+		{
+			//Zero
+			return sign ? PPC_FPCLASS_NZ : PPC_FPCLASS_PZ;
+		}
+	}
+}
+
+u32 ClassifyFloat(float fvalue)
+{
+	// TODO: Optimize the below to be as fast as possible.
+	IntFloat value(fvalue);
+	u32 sign = value.i & FLOAT_SIGN;
+	u32 exp  = value.i & FLOAT_EXP;
+	if (exp > FLOAT_ZERO && exp < FLOAT_EXP)
+	{
+		// Nice normalized number.
+		return sign ? PPC_FPCLASS_NN : PPC_FPCLASS_PN;
+	}
+	else
+	{
+		u32 mantissa = value.i & FLOAT_FRAC;
+		if (mantissa)
+		{
+			if (exp)
+			{
+				return PPC_FPCLASS_QNAN; // Quiet NAN
+			}
+			else
+			{
+				// Denormalized number.
+				return sign ? PPC_FPCLASS_ND : PPC_FPCLASS_PD;
+			}
+		}
+		else if (exp)
+		{
+			// Infinite
+			return sign ? PPC_FPCLASS_NINF : PPC_FPCLASS_PINF;
+		}
+		else
+		{
+			//Zero
+			return sign ? PPC_FPCLASS_NZ : PPC_FPCLASS_PZ;
+		}
+	}
+}
+
+const int frsqrte_expected_base[] =
+{
+	0x3ffa000, 0x3c29000, 0x38aa000, 0x3572000,
+	0x3279000, 0x2fb7000, 0x2d26000, 0x2ac0000,
+	0x2881000, 0x2665000, 0x2468000, 0x2287000,
+	0x20c1000, 0x1f12000, 0x1d79000, 0x1bf4000,
+	0x1a7e800, 0x17cb800, 0x1552800, 0x130c000,
+	0x10f2000, 0x0eff000, 0x0d2e000, 0x0b7c000,
+	0x09e5000, 0x0867000, 0x06ff000, 0x05ab800,
+	0x046a000, 0x0339800, 0x0218800, 0x0105800,
+};
+const int frsqrte_expected_dec[] =
+{
+	0x7a4, 0x700, 0x670, 0x5f2,
+	0x584, 0x524, 0x4cc, 0x47e,
+	0x43a, 0x3fa, 0x3c2, 0x38e,
+	0x35e, 0x332, 0x30a, 0x2e6,
+	0x568, 0x4f3, 0x48d, 0x435,
+	0x3e7, 0x3a2, 0x365, 0x32e,
+	0x2fc, 0x2d0, 0x2a8, 0x283,
+	0x261, 0x243, 0x226, 0x20b,
+};
+
+double ApproximateReciprocalSquareRoot(double val)
+{
+	union
+	{
+		double valf;
+		s64 vali;
+	};
+	valf = val;
+	s64 mantissa = vali & ((1LL << 52) - 1);
+	s64 sign = vali & (1ULL << 63);
+	s64 exponent = vali & (0x7FFLL << 52);
+
+	// Special case 0
+	if (mantissa == 0 && exponent == 0)
+		return sign ? -std::numeric_limits<double>::infinity() :
+		std::numeric_limits<double>::infinity();
+	// Special case NaN-ish numbers
+	if (exponent == (0x7FFLL << 52))
+	{
+		if (mantissa == 0)
+		{
+			if (sign)
+				return std::numeric_limits<double>::quiet_NaN();
+
+			return 0.0;
+		}
+
+		return 0.0 + valf;
+	}
+
+	// Negative numbers return NaN
+	if (sign)
+		return std::numeric_limits<double>::quiet_NaN();
+
+	if (!exponent)
+	{
+		// "Normalize" denormal values
+		do
+		{
+			exponent -= 1LL << 52;
+			mantissa <<= 1;
+		} while (!(mantissa & (1LL << 52)));
+		mantissa &= (1LL << 52) - 1;
+		exponent += 1LL << 52;
+	}
+
+	bool odd_exponent = !(exponent & (1LL << 52));
+	exponent = ((0x3FFLL << 52) - ((exponent - (0x3FELL << 52)) / 2)) & (0x7FFLL << 52);
+
+	int i = (int)(mantissa >> 37);
+	vali = sign | exponent;
+	int index = i / 2048 + (odd_exponent ? 16 : 0);
+	vali |= (s64)(frsqrte_expected_base[index] - frsqrte_expected_dec[index] * (i % 2048)) << 26;
+	return valf;
+}
+
+const int fres_expected_base[] =
+{
+	0x7ff800, 0x783800, 0x70ea00, 0x6a0800,
+	0x638800, 0x5d6200, 0x579000, 0x520800,
+	0x4cc800, 0x47ca00, 0x430800, 0x3e8000,
+	0x3a2c00, 0x360800, 0x321400, 0x2e4a00,
+	0x2aa800, 0x272c00, 0x23d600, 0x209e00,
+	0x1d8800, 0x1a9000, 0x17ae00, 0x14f800,
+	0x124400, 0x0fbe00, 0x0d3800, 0x0ade00,
+	0x088400, 0x065000, 0x041c00, 0x020c00,
+};
+const int fres_expected_dec[] =
+{
+	0x3e1, 0x3a7, 0x371, 0x340,
+	0x313, 0x2ea, 0x2c4, 0x2a0,
+	0x27f, 0x261, 0x245, 0x22a,
+	0x212, 0x1fb, 0x1e5, 0x1d1,
+	0x1be, 0x1ac, 0x19b, 0x18b,
+	0x17c, 0x16e, 0x15b, 0x15b,
+	0x143, 0x143, 0x12d, 0x12d,
+	0x11a, 0x11a, 0x108, 0x106,
+};
+
+// Used by fres and ps_res.
+double ApproximateReciprocal(double val)
+{
+	union
+	{
+		double valf;
+		s64 vali;
+	};
+
+	valf = val;
+	s64 mantissa = vali & ((1LL << 52) - 1);
+	s64 sign = vali & (1ULL << 63);
+	s64 exponent = vali & (0x7FFLL << 52);
+
+	// Special case 0
+	if (mantissa == 0 && exponent == 0)
+		return sign ? -std::numeric_limits<double>::infinity() : std::numeric_limits<double>::infinity();
+
+	// Special case NaN-ish numbers
+	if (exponent == (0x7FFLL << 52))
+	{
+		if (mantissa == 0)
+			return sign ? -0.0 : 0.0;
+		return 0.0 + valf;
+	}
+
+	// Special case small inputs
+	if (exponent < (895LL << 52))
+		return sign ? -std::numeric_limits<float>::max() : std::numeric_limits<float>::max();
+
+	// Special case large inputs
+	if (exponent >= (1149LL << 52))
+		return sign ? -0.0f : 0.0f;
+
+	exponent = (0x7FDLL << 52) - exponent;
+
+	int i = (int)(mantissa >> 37);
+	vali = sign | exponent;
+	vali |= (s64)(fres_expected_base[i / 1024] - (fres_expected_dec[i / 1024] * (i % 1024) + 1) / 2) << 29;
+	return valf;
+}
+
+}  // namespace
+
+inline void MatrixMul(int n, const float *a, const float *b, float *result)
+{
+	for (int i = 0; i < n; ++i)
+	{
+		for (int j = 0; j < n; ++j)
+		{
+			float temp = 0;
+			for (int k = 0; k < n; ++k)
+			{
+				temp += a[i * n + k] * b[k * n + j];
+			}
+			result[i * n + j] = temp;
+		}
+	}
+}
+
+// Calculate sum of a float list
+float MathFloatVectorSum(const std::vector<float>& Vec)
+{
+	return std::accumulate(Vec.begin(), Vec.end(), 0.0f);
+}
+
+void Matrix33::LoadIdentity(Matrix33 &mtx)
+{
+	memset(mtx.data, 0, sizeof(mtx.data));
+	mtx.data[0] = 1.0f;
+	mtx.data[4] = 1.0f;
+	mtx.data[8] = 1.0f;
+}
+
+void Matrix33::RotateX(Matrix33 &mtx, float rad)
+{
+	float s = sin(rad);
+	float c = cos(rad);
+	memset(mtx.data, 0, sizeof(mtx.data));
+	mtx.data[0] = 1;
+	mtx.data[4] = c;
+	mtx.data[5] = -s;
+	mtx.data[7] = s;
+	mtx.data[8] = c;
+}
+void Matrix33::RotateY(Matrix33 &mtx, float rad)
+{
+	float s = sin(rad);
+	float c = cos(rad);
+	memset(mtx.data, 0, sizeof(mtx.data));
+	mtx.data[0] = c;
+	mtx.data[2] = s;
+	mtx.data[4] = 1;
+	mtx.data[6] = -s;
+	mtx.data[8] = c;
+}
+
+void Matrix33::Multiply(const Matrix33 &a, const Matrix33 &b, Matrix33 &result)
+{
+	MatrixMul(3, a.data, b.data, result.data);
+}
+
+void Matrix33::Multiply(const Matrix33 &a, const float vec[3], float result[3])
+{
+	for (int i = 0; i < 3; ++i)
+	{
+		result[i] = 0;
+
+		for (int k = 0; k < 3; ++k)
+		{
+			result[i] += a.data[i * 3 + k] * vec[k];
+		}
+	}
+}
+
+void Matrix44::LoadIdentity(Matrix44 &mtx)
+{
+	memset(mtx.data, 0, sizeof(mtx.data));
+	mtx.data[0] = 1.0f;
+	mtx.data[5] = 1.0f;
+	mtx.data[10] = 1.0f;
+	mtx.data[15] = 1.0f;
+}
+
+void Matrix44::LoadMatrix33(Matrix44 &mtx, const Matrix33 &m33)
+{
+	for (int i = 0; i < 3; ++i)
+	{
+		for (int j = 0; j < 3; ++j)
+		{
+			mtx.data[i * 4 + j] = m33.data[i * 3 + j];
+		}
+	}
+
+	for (int i = 0; i < 3; ++i)
+	{
+		mtx.data[i * 4 + 3] = 0;
+		mtx.data[i + 12] = 0;
+	}
+	mtx.data[15] = 1.0f;
+}
+
+void Matrix44::Set(Matrix44 &mtx, const float mtxArray[16])
+{
+	for (int i = 0; i < 16; ++i)
+	{
+		mtx.data[i] = mtxArray[i];
+	}
+}
+
+void Matrix44::Translate(Matrix44 &mtx, const float vec[3])
+{
+	LoadIdentity(mtx);
+	mtx.data[3] = vec[0];
+	mtx.data[7] = vec[1];
+	mtx.data[11] = vec[2];
+}
+
+void Matrix44::Shear(Matrix44 &mtx, const float a, const float b)
+{
+	LoadIdentity(mtx);
+	mtx.data[2] = a;
+	mtx.data[6] = b;
+}
+
+void Matrix44::Multiply(const Matrix44 &a, const Matrix44 &b, Matrix44 &result)
+{
+	MatrixMul(4, a.data, b.data, result.data);
+}
+