dolphin/Source/Core/Common/MathUtil.cpp

// Copyright 2013 Dolphin Emulator Project
// Licensed under GPLv2
// Refer to the license.txt file included.

#include <cmath>
#include <cstring>
#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;
}

}  // 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::Multiply(const Matrix44 &a, const Matrix44 &b, Matrix44 &result)
{
	MatrixMul(4, a.data, b.data, result.data);
}