maths.cpp

// NOTE Compile without fast math flags.

/*
This is free and unencumbered software released into the public domain.

Anyone is free to copy, modify, publish, use, compile, sell, or
distribute this software, either in source code form or as a compiled
binary, for any purpose, commercial or non-commercial, and by any
means.

In jurisdictions that recognize copyright laws, the author or authors
of this software dedicate any and all copyright interest in the
software to the public domain. We make this dedication for the benefit
of the public at large and to the detriment of our heirs and
successors. We intend this dedication to be an overt act of
relinquishment in perpetuity of all present and future rights to this
software under copyright law.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.

For more information, please refer to http://unlicense.org/
*/

double Floor(double x);
float FloorFloat(float x);

double Sine(double x);
float SineFloat(float x);
double Cosine(double x);
float CosineFloat(float x);
double Tangent(double x);
float TangentFloat(float x);
double ArcSine(double x);
float ArcSineFloat(float x);
double ArcCosine(double x);
float ArcCosineFloat(float x);
double ArcTangent(double x);
float ArcTangentFloat(float x);
double ArcTangent2(double y, double x);
float ArcTangent2Float(float y, float x);
double Exponential2(double x);
float Exponential2Float(float x);
double Logarithm2(double x);
float Logarithm2Float(float x);
double Power(double x, double y);
float PowerFloat(float x, float y);
double Modulo(double x, double y);
float ModuloFloat(float x, float y);

# define PI_F           3.14159265358979323846f
# define PI_D           3.14159265358979323846

typedef union ConvertDoubleInteger {
	double d;
	uint64_t i;
} ConvertDoubleInteger;

typedef union ConvertFloatInteger {
	float f;
	uint64_t i;
} ConvertFloatInteger;

static double doubleToInteger = 1.0 / 2.22044604925031308085e-16;

double Floor(double x) {
	if (x == 0) return x;

	ConvertDoubleInteger convert = {x};
	uint64_t sign = convert.i & 0x8000000000000000;
	int exponent = (int) ((convert.i >> 52) & 0x7FF) - 0x3FF;

	if (exponent >= 52) {
		// There aren't any bits representing a fractional part.
		return x;
	} else if (exponent >= 0) {
		// Positive exponent.
		double y = sign ? (x - doubleToInteger + doubleToInteger - x) : (x + doubleToInteger - doubleToInteger - x);
		return y > 0 ? x + y - 1 : x + y;
	} else if (exponent < 0) {
		// Negative exponent.
		return sign ? -1.0 : 0.0;
	}

	return 0;
}

float FloorFloat(float x) {
	ConvertFloatInteger convert = {x};
	uint32_t sign = convert.i & 0x80000000;
	int exponent = (int) ((convert.i >> 23) & 0xFF) - 0x7F;

	if (exponent >= 23) {
		// There aren't any bits representing a fractional part.
	} else if (exponent >= 0) {
		// Positive exponent.
		uint32_t mask = 0x7FFFFF >> exponent;
		if (!(mask & convert.i)) return x; // Already an integer.
		if (sign) convert.i += mask;
		convert.i &= ~mask; // Mask out the fractional bits.
	} else if (exponent < 0) {
		// Negative exponent.
		return sign ? -1.0 : 0.0;
	}

	return convert.f;
}

#define D(x) (((ConvertDoubleInteger) { .i = (x) }).d)
#define F(x) (((ConvertFloatInteger) { .i = (x) }).f)

static
double Sine_(double x) {
	// Calculates sin(x) for x in [0, pi/4].

	double x2 = x * x;

	return x * (D(0x3FF0000000000000) +
				x2 * (D(0xBFC5555555555540) +
					  x2 * (D(0x3F8111111110ED80) +
							x2 * (D(0xBF2A01A019AE6000) +
								  x2 * (D(0x3EC71DE349280000) +
										x2 * (D(0xBE5AE5DC48000000) +
											  x2 * D(0x3DE5D68200000000)))))));
}

static
float SineFloat_(float x) {
	// Calculates sin(x) for x in [0, pi/4].

	float x2 = x * x;

	return x * (F(0x3F800000) +
				x2 * (F(0xBE2AAAA0) +
					  x2 * (F(0x3C0882C0) +
							x2 * F(0xB94C6000))));
}

static
double ArcSine_(double x) {
	// Calculates arcsin(x) for x in [0, 0.5].

	double x2 = x * x;

	return x * (D(0x3FEFFFFFFFFFFFE6) +
				x2 * (D(0x3FC555555555FE00) +
					  x2 * (D(0x3FB333333292DF90) +
							x2 * (D(0x3FA6DB6DFD3693A0)
								  + x2 * (D(0x3F9F1C608DE51900) +
										  x2 * (D(0x3F96EA0659B9A080) +
												x2 * (D(0x3F91B4ABF1029100)
													  + x2 * (D(0x3F8DA8DAF31ECD00) +
															  x2 * (D(0x3F81C01FD5000C00) +
																	x2 * (D(0x3F94BDA038CF6B00)
																		  + x2 * (D(0xBF8E849CA75B1E00) +
																				  x2 * D(0x3FA146C2D37F2C60))))))))))));
}

static
float ArcSineFloat_(float x) {
	// Calculates arcsin(x) for x in [0, 0.5].

	float x2 = x * x;

	return x * (F(0x3F800004) +
				x2 * (F(0x3E2AA130) +
					  x2 * (F(0x3D9B2C28) +
							x2 * (F(0x3D1C1800) +
								  x2 * F(0x3D5A97C0)))));
}

static
double ArcTangent_(double x) {
	// Calculates arctan(x) for x in [0, 0.5].

	double x2 = x * x;

	return x * (D(0x3FEFFFFFFFFFFFF8) +
				x2 * (D(0xBFD5555555553B44) +
					  x2 * (D(0x3FC9999999803988) +
							x2 * (D(0xBFC249248C882E80) +
								  x2 * (D(0x3FBC71C5A4E4C220) +
										x2 * (D(0xBFB745B3B75243F0) +
											  x2 * (D(0x3FB3AFAE9A2939E0) +
													x2 * (D(0xBFB1030C4A4A1B90) +
														  x2 * (D(0x3FAD6F65C35579A0) +
																x2 * (D(0xBFA805BCFDAFEDC0) +
																	  x2 * (D(0x3F9FC6B5E115F2C0) +
																			x2 * D(0xBF87DCA5AB25BF80))))))))))));
}

static
float ArcTangentFloat_(float x) {
	// Calculates arctan(x) for x in [0, 0.5].

	float x2 = x * x;

	return x * (F(0x3F7FFFF8) +
				x2 * (F(0xBEAAA53C) +
					  x2 * (F(0x3E4BC990) +
							x2 * (F(0xBE084A60) +
								  x2 * F(0x3D8864B0)))));
}

static
double Cosine_(double x) {
	// Calculates cos(x) for x in [0, pi/4].

	double x2 = x * x;

	return (D(0x3FF0000000000000) +
			x2 * (D(0xBFDFFFFFFFFFFFA0) +
				  x2 * (D(0x3FA555555554F7C0) +
						x2 * (D(0xBF56C16C16475C00) +
							  x2 * (D(0x3EFA019F87490000) +
									x2 * (D(0xBE927DF66B000000) +
										  x2 * D(0x3E21B949E0000000)))))));
}

static
float CosineFloat_(float x) {
	// Calculates cos(x) for x in [0, pi/4].

	float x2 = x * x;

	return (F(0x3F800000) +
			x2 * (F(0xBEFFFFDA) +
				  x2 * (F(0x3D2A9F60) +
						x2 * F(0xBAB22C00))));
}

static
double Tangent_(double x) {
	// Calculates tan(x) for x in [0, pi/4].

	double x2 = x * x;

	return x * (D(0x3FEFFFFFFFFFFFE8) +
				x2 * (D(0x3FD5555555558000) +
					  x2 * (D(0x3FC1111110FACF90) +
							x2 * (D(0x3FABA1BA266BFD20) +
								  x2 * (D(0x3F9664F30E56E580) +
										x2 * (D(0x3F822703B08BDC00) +
											  x2 * (D(0x3F6D698D2E4A4C00) +
													x2 * (D(0x3F57FF4F23EA4400) +
														  x2 * (D(0x3F424F3BEC845800) +
																x2 * (D(0x3F34C78CA9F61000) +
																	  x2 * (D(0xBF042089F8510000) +
																			x2 * (D(0x3F29D7372D3A8000) +
																				  x2 * (D(0xBF19D1C5EF6F0000) +
																						x2 * (D(0x3F0980BDF11E8000)))))))))))))));
}

static
float TangentFloat_(float x) {
	// Calculates tan(x) for x in [0, pi/4].

	float x2 = x * x;

	return x * (F(0x3F800001) +
				x2 * (F(0x3EAAA9AA) +
					  x2 * (F(0x3E08ABA8) +
							x2 * (F(0x3D58EC90) +
								  x2 * (F(0x3CD24840) +
										x2 * (F(0x3AC3CA00) +
											  x2 * F(0x3C272F00)))))));
}

double Sine(double x) {
	bool negate = false;

	// x in -infty, infty

	if (x < 0) {
		x = -x;
		negate = true;
	}

	// x in 0, infty

	x -= 2 * PI_D * Floor(x / (2 * PI_D));

	// x in 0, 2*pi

	if (x < PI_D / 2) {
	} else if (x < PI_D) {
		x = PI_D - x;
	} else if (x < 3 * PI_D / 2) {
		x = x - PI_D;
		negate = !negate;
	} else {
		x = PI_D * 2 - x;
		negate = !negate;
	}

	// x in 0, pi/2

	double y = x < PI_D / 4 ? Sine_(x) : Cosine_(PI_D / 2 - x);
	return negate ? -y : y;
}

float SineFloat(float x) {
	bool negate = false;

	// x in -infty, infty

	if (x < 0) {
		x = -x;
		negate = true;
	}

	// x in 0, infty

	x -= 2 * PI_F * FloorFloat(x / (2 * PI_F));

	// x in 0, 2*pi

	if (x < PI_F / 2) {
	} else if (x < PI_F) {
		x = PI_F - x;
	} else if (x < 3 * PI_F / 2) {
		x = x - PI_F;
		negate = !negate;
	} else {
		x = PI_F * 2 - x;
		negate = !negate;
	}

	// x in 0, pi/2

	float y = x < PI_F / 4 ? SineFloat_(x) : CosineFloat_(PI_F / 2 - x);
	return negate ? -y : y;
}

double Cosine(double x) {
	bool negate = false;

	// x in -infty, infty

	if (x < 0) {
		x = -x;
	}

	// x in 0, infty

	x -= 2 * PI_D * Floor(x / (2 * PI_D));

	// x in 0, 2*pi

	if (x < PI_D / 2) {
	} else if (x < PI_D) {
		x = PI_D - x;
		negate = !negate;
	} else if (x < 3 * PI_D / 2) {
		x = x - PI_D;
		negate = !negate;
	} else {
		x = PI_D * 2 - x;
	}

	// x in 0, pi/2

	double y = x < PI_D / 4 ? Cosine_(x) : Sine_(PI_D / 2 - x);
	return negate ? -y : y;
}

float CosineFloat(float x) {
	bool negate = false;

	// x in -infty, infty

	if (x < 0) {
		x = -x;
	}

	// x in 0, infty

	x -= 2 * PI_F * FloorFloat(x / (2 * PI_F));

	// x in 0, 2*pi

	if (x < PI_F / 2) {
	} else if (x < PI_F) {
		x = PI_F - x;
		negate = !negate;
	} else if (x < 3 * PI_F / 2) {
		x = x - PI_F;
		negate = !negate;
	} else {
		x = PI_F * 2 - x;
	}

	// x in 0, pi/2

	float y = x < PI_F / 4 ? CosineFloat_(x) : SineFloat_(PI_F / 2 - x);
	return negate ? -y : y;
}

double Tangent(double x) {
	bool negate = false;

	// x in -infty, infty

	if (x < 0) {
		x = -x;
		negate = !negate;
	}

	// x in 0, infty

	x -= PI_D * Floor(x / PI_D);

	// x in 0, pi

	if (x > PI_D / 2) {
		x = PI_D - x;
		negate = !negate;
	}

	// x in 0, pi/2

	double y = x < PI_D / 4 ? Tangent_(x) : (1.0 / Tangent_(PI_D / 2 - x));
	return negate ? -y : y;
}

float TangentFloat(float x) {
	bool negate = false;

	// x in -infty, infty

	if (x < 0) {
		x = -x;
		negate = !negate;
	}

	// x in 0, infty

	x -= PI_F * FloorFloat(x / PI_F);

	// x in 0, pi

	if (x > PI_F / 2) {
		x = PI_F - x;
		negate = !negate;
	}

	// x in 0, pi/2

	float y = x < PI_F / 4 ? TangentFloat_(x) : (1.0f / TangentFloat_(PI_F / 2 - x));
	return negate ? -y : y;
}

double ArcSine(double x) {
	bool negate = false;

	if (x < 0) {
		x = -x;
		negate = true;
	}

	double y;

	if (x < 0.5) {
		y = ArcSine_(x);
	} else {
		y = PI_D / 2 - 2 * ArcSine_(sqrt(0.5 - 0.5 * x));
	}

	return negate ? -y : y;
}

float ArcSineFloat(float x) {
	bool negate = false;

	if (x < 0) {
		x = -x;
		negate = true;
	}

	float y;

	if (x < 0.5f) {
		y = ArcSineFloat_(x);
	} else {
		y = PI_F / 2 - 2 * ArcSineFloat_(sqrtf(0.5f - 0.5f * x));
	}

	return negate ? -y : y;
}

double ArcCosine(double x) {
	return ArcSine(-x) + PI_D / 2;
}

float ArcCosineFloat(float x) {
	return ArcSineFloat(-x) + PI_F / 2;
}

double ArcTangent(double x) {
	bool negate = false;

	if (x < 0) {
		x = -x;
		negate = true;
	}

	bool reciprocalTaken = false;

	if (x > 1) {
		x = 1 / x;
		reciprocalTaken = true;
	}

	double y;

	if (x < 0.5) {
		y = ArcTangent_(x);
	} else {
		y = 0.463647609000806116 + ArcTangent_((2 * x - 1) / (2 + x));
	}

	if (reciprocalTaken) {
		y = PI_D / 2 - y;
	}

	return negate ? -y : y;
}

float ArcTangentFloat(float x) {
	bool negate = false;

	if (x < 0) {
		x = -x;
		negate = true;
	}

	bool reciprocalTaken = false;

	if (x > 1) {
		x = 1 / x;
		reciprocalTaken = true;
	}

	float y;

	if (x < 0.5f) {
		y = ArcTangentFloat_(x);
	} else {
		y = 0.463647609000806116f + ArcTangentFloat_((2 * x - 1) / (2 + x));
	}

	if (reciprocalTaken) {
		y = PI_F / 2 - y;
	}

	return negate ? -y : y;
}

double ArcTangent2(double y, double x) {
	if (x == 0) return y > 0 ? PI_D / 2 : -PI_D / 2;
	else if (x > 0) return ArcTangent(y / x);
	else if (y >= 0) return PI_D + ArcTangent(y / x);
	else return -PI_D + ArcTangent(y / x);
}

float ArcTangent2Float(float y, float x) {
	if (x == 0) return y > 0 ? PI_F / 2 : -PI_F / 2;
	else if (x > 0) return ArcTangentFloat(y / x);
	else if (y >= 0) return PI_F + ArcTangentFloat(y / x);
	else return -PI_F + ArcTangentFloat(y / x);
}

double Exponential2(double x) {
	double a = Floor(x * 8);
	int64_t ai = (int64_t)a;

	if (ai < -1024) {
		return 0;
	}

	double b = x - a / 8;

	double y = (D(0x3FF0000000000000) +
				b * (D(0x3FE62E42FEFA3A00) +
					 b * (D(0x3FCEBFBDFF829140) +
						  b * (D(0x3FAC6B08D73C4A40) +
							   b * (D(0x3F83B2AB53873280) +
									b * (D(0x3F55D88F363C6C00) +
										 b * (D(0x3F242C003E4A2000) +
											  b * D(0x3EF0B291F6C00000)))))));

	const double m[8] = {
		D(0x3FF0000000000000),
		D(0x3FF172B83C7D517B),
		D(0x3FF306FE0A31B715),
		D(0x3FF4BFDAD5362A27),
		D(0x3FF6A09E667F3BCD),
		D(0x3FF8ACE5422AA0DB),
		D(0x3FFAE89F995AD3AD),
		D(0x3FFD5818DCFBA487),
	};

	y *= m[ai & 7];

	ConvertDoubleInteger c;
	c.d = y;
	c.i += (uint64_t)((ai >> 3) << 52);
	return c.d;
}

float Exponential2Float(float x) {
	float a = FloorFloat(x);
	int32_t ai = (int32_t)a;

	if (ai < -128) {
		return 0;
	}

	float b = x - a;

	float y = (F(0x3F7FFFFE) +
			   b * (F(0x3F31729A) +
					b * (F(0x3E75E700) +
						 b * (F(0x3D64D520) +
							  b * (F(0x3C128280) +
								   b * F(0x3AF89400)))));

	ConvertFloatInteger c;
	c.f = y;
	c.i += (uint32_t)(ai << 23);
	return c.f;
}

double Logarithm2(double x) {
	ConvertDoubleInteger c;
	c.d = x;
	int64_t e = ((c.i >> 52) & 2047) - 0x3FF;
	c.i = (c.i & (uint64_t)~(0x7FFLL << 52)) + (uint64_t)(0x3FFLL << 52);
	x = c.d;

	double a;

	if (x < 1.125) {
		a = 0;
	} else if (x < 1.250) {
		x *= 1.125 / 1.250;
		a = D(0xBFC374D65D9E608E);
	} else if (x < 1.375) {
		x *= 1.125 / 1.375;
		a = D(0xBFD28746C334FECB);
	} else if (x < 1.500) {
		x *= 1.125 / 1.500;
		a = D(0xBFDA8FF971810A5E);
	} else if (x < 1.625) {
		x *= 1.125 / 1.625;
		a = D(0xBFE0F9F9FFC8932A);
	} else if (x < 1.750) {
		x *= 1.125 / 1.750;
		a = D(0xBFE465D36ED11B11);
	} else if (x < 1.875) {
		x *= 1.125 / 1.875;
		a = D(0xBFE79538DEA712F5);
	} else {
		x *= 1.125 / 2.000;
		a = D(0xBFEA8FF971810A5E);
	}

	double y = (D(0xC00FF8445026AD97) +
				x * (D(0x40287A7A02D9353F) +
					 x * (D(0xC03711C58D55CEE2) +
						  x * (D(0x4040E8263C321A26) +
							   x * (D(0xC041EB22EA691BB3) +
									x * (D(0x403B00FB376D1F10) +
										 x * (D(0xC02C416ABE857241) +
											  x * (D(0x40138BA7FAA3523A) +
												   x * (D(0xBFF019731AF80316) +
														x * D(0x3FB7F1CD3852C200)))))))));

	return y - a + (double)e;
}

float Logarithm2Float(float x) {
	ConvertFloatInteger c;
	c.f = x;
	int32_t e = ((c.i >> 23) & 255) - 0x7F;
	c.i = (c.i & (uint64_t)~(0xFF << 23)) + (0x7F << 23);
	x = c.f;

	double y = (double)(F(0xC05B5154) +
						x * (F(0x410297C6) +
							 x * (F(0xC1205CEB)
								  + x * (F(0x4114DF63) +
										 x * (F(0xC0C0DBBB) +
											  x * (F(0x402942C6)
												   + x * (F(0xBF3FF98A) +
														  x * (F(0x3DFE1050) +
															   x * F(0xBC151480)))))))));

	return (float)(y + e);
}

double Power(double x, double y) {
	return Exponential2(y * Logarithm2(x));
}

float PowerFloat(float x, float y) {
	return Exponential2Float(y * Logarithm2Float(x));
}

double Modulo(double x, double y) {
	return x - y * Floor(x / y);
}

float ModuloFloat(float x, float y) {
	return x - y * FloorFloat(x / y);
}