From f5d536ed4bc0a720b17bbd213b2b71d611a73d81 Mon Sep 17 00:00:00 2001
From: asmodai <asmodai@dedicated-server.ru>
Date: Fri, 5 Feb 2016 16:59:11 +0300
Subject: [PATCH 1/4] Added sse mathfun implementation of sin, cos, sincos
 Written parallel sse version of AngleVectors, AngleVectorsTranspose

---
 rehlds/engine/mathlib.cpp          | 132 ++++++++-
 rehlds/engine/sse_mathfun.cpp      | 447 +++++++++++++++++++++++++++++
 rehlds/engine/sse_mathfun.h        | 120 ++++++++
 rehlds/msvc/ReHLDS.vcxproj         |   2 +
 rehlds/msvc/ReHLDS.vcxproj.filters |   6 +
 rehlds/rehlds/precompiled.h        |   1 +
 6 files changed, 694 insertions(+), 14 deletions(-)
 create mode 100644 rehlds/engine/sse_mathfun.cpp
 create mode 100644 rehlds/engine/sse_mathfun.h

diff --git a/rehlds/engine/mathlib.cpp b/rehlds/engine/mathlib.cpp
index ee38a97..e667f46 100644
--- a/rehlds/engine/mathlib.cpp
+++ b/rehlds/engine/mathlib.cpp
@@ -28,6 +28,9 @@
 
 #include "precompiled.h"
 
+// Intrisics guide: https://software.intel.com/sites/landingpage/IntrinsicsGuide/
+// Shufps calculator: http://wurstcaptures.untergrund.net/assembler_tricks.html
+
 vec3_t vec3_origin;
 //int nanmask;
 //short int new_cw;
@@ -36,6 +39,7 @@ vec3_t vec3_origin;
 
 // aligned vec4_t
 typedef ALIGN16 vec4_t avec4_t;
+typedef ALIGN16 int aivec4_t[4];
 
 // conversion multiplier
 const avec4_t deg2rad =
@@ -46,8 +50,24 @@ const avec4_t deg2rad =
 	M_PI / 180.f
 };
 
+const aivec4_t negmask[4] =
+{
+	0x80000000,
+	0x80000000,
+	0x80000000,
+	0x80000000
+};
+
+const aivec4_t negmask_1001 =
+{
+	0x80000000,
+	0,
+	0,
+	0x80000000
+};
+
 // save 4d xmm to 3d vector. we can't optimize many simple vector3 functions because saving back to 3d is slow.
-void xmm2vec(vec_t *v, const __m128 m)
+static inline void xmm2vec(vec_t *v, const __m128 m)
 {
 	_mm_store_ss(v, m);
 	_mm_storel_pi((__m64*)(v + 1), _mm_shuffle_ps(m, m, _MM_SHUFFLE(3, 2, 2, 1)));
@@ -145,6 +165,53 @@ void EXT_FUNC AngleVectors_ext(const vec_t *angles, vec_t *forward, vec_t *right
 	AngleVectors(angles, forward, right, up);
 }
 
+#ifdef REHLDS_FIXES
+// parallel SSE version
+void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
+{
+#ifndef SWDS
+	g_engdstAddrs.pfnAngleVectors(&angles, &forward, &right, &up);
+#endif // SWDS
+
+	__m128 s, c;
+	sincos_ps(_mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)), &s, &c);
+
+	__m128 m1 = _mm_shuffle_ps(c, s, 0x90); // [cp][cp][sy][sr]
+	__m128 m2 = _mm_shuffle_ps(c, c, 0x09); // [cy][cr][cp][cp]
+	__m128 cp_mults = _mm_mul_ps(m1, m2); // [cp * cy][cp * cr][cp * sy][cp * sr];
+
+	m1 = _mm_shuffle_ps(c, s, 0x15); // [cy][cy][sy][sp]
+	m2 = _mm_shuffle_ps(s, c, 0xA0); // [sp][sp][cr][cr]
+	m1 = _mm_shuffle_ps(m1, m1, 0xC8); // [cy][sy][cy][sp]
+
+	__m128 m3 = _mm_shuffle_ps(s, s, 0x4A); // [sr][sr][sp][sy];
+	m3 = _mm_mul_ps(m3, _mm_mul_ps(m1, m2)); // [sp*cy*sr][sp*sy*sr][cr*cy*sp][cr*sp*sy]
+
+	m2 = _mm_shuffle_ps(s, c, 0x65); // [sy][sy][cr][cy]
+	m1 = _mm_shuffle_ps(c, s, 0xA6); // [cr][cy][sr][sr]
+	m2 = _mm_shuffle_ps(m2, m2, 0xD8); // [sy][cr][sy][cy]
+	m1 = _mm_xor_ps(m1, _mm_load_ps((float *)&negmask_1001)); // [-cr][cy][sr][-sr]
+	m1 = _mm_mul_ps(m1, m2); // [-cr*sy][cy*cr][sr*sy][-sr*cy]
+
+	m3 = _mm_add_ps(m3, m1);
+
+	if (forward)
+	{
+		_mm_storel_pi((__m64 *)forward, _mm_shuffle_ps(cp_mults, cp_mults, 0x08));
+		forward[2] = -s.m128_f32[PITCH];
+	}
+	if (right)
+	{
+		__m128 r = _mm_shuffle_ps(m3, cp_mults, 0xF4); // [m3(0)][m3(1)][cp(3)][cp(3)]
+		xmm2vec(right, _mm_xor_ps(r, _mm_load_ps((float *)&negmask)));
+	}
+	if (up)
+	{
+		_mm_storel_pi((__m64 *)up, _mm_shuffle_ps(m3, m3, 0x0E));
+		up[2] = cp_mults.m128_f32[1];
+	}
+}
+#else // REHLDS_FIXES
 /* <47067> ../engine/mathlib.c:267 */
 void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 {
@@ -154,18 +221,6 @@ void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 	g_engdstAddrs.pfnAngleVectors(&angles, &forward, &right, &up);
 #endif // SWDS
 
-#ifdef REHLDS_FIXES
-	// convert to radians
-	avec4_t rad_angles;
-	_mm_store_ps(rad_angles, _mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)));
-
-	sy = sin(rad_angles[YAW]);
-	cy = cos(rad_angles[YAW]);
-	sp = sin(rad_angles[PITCH]);
-	cp = cos(rad_angles[PITCH]);
-	sr = sin(rad_angles[ROLL]);
-	cr = cos(rad_angles[ROLL]);
-#else
 	float angle;
 	angle = (float)(angles[YAW] * (M_PI * 2 / 360));
 	sy = sin(angle);
@@ -176,7 +231,6 @@ void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 	angle = (float)(angles[ROLL] * (M_PI * 2 / 360));
 	sr = sin(angle);
 	cr = cos(angle);
-#endif
 
 	if (forward)
 	{
@@ -197,7 +251,56 @@ void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 		up[2] = cr*cp;
 	}
 }
+#endif // REHLDS_FIXES
 
+#ifdef REHLDS_FIXES
+// parallel SSE version
+void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
+{
+#ifndef SWDS
+	g_engdstAddrs.pfnAngleVectors(&angles, &forward, &right, &up);
+#endif // SWDS
+
+	__m128 s, c;
+	sincos_ps(_mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)), &s, &c);
+
+	__m128 m1 = _mm_shuffle_ps(c, s, 0x90); // [cp][cp][sy][sr]
+	__m128 m2 = _mm_shuffle_ps(c, c, 0x09); // [cy][cr][cp][cp]
+	__m128 cp_mults = _mm_mul_ps(m1, m2); // [cp * cy][cp * cr][cp * sy][cp * sr];
+
+	m1 = _mm_shuffle_ps(s, s, 0x50); // [sp][sp][sy][sy]
+	m2 = _mm_shuffle_ps(c, s, 0x05); // [cy][cy][sp][sp]
+
+	__m128 m3 = _mm_shuffle_ps(s, c, 0xAA); // [sr][sr][cr][cr]
+	m1 = _mm_mul_ps(m1, m2);
+	m3 = _mm_shuffle_ps(m3, m3, 0xD8); // [sr][cr][sr][cr]
+	m3 = _mm_mul_ps(m3, m1); // [sp*cy*sr][sp*cy*cr][sy*sp*sr][sy*sp*cr]
+
+	m2 = _mm_shuffle_ps(c, s, 0xA6); // [cr][cy][sr][sr]
+	m1 = _mm_shuffle_ps(s, c, 0x65); // [sy][sy][cr][cy]
+	m2 = _mm_shuffle_ps(m2, m2, 0xD8); // [cr][sr][cy][sr]
+	m1 = _mm_xor_ps(m1, _mm_load_ps((float *)&negmask_1001)); // [-cr][cy][sr][-sr]
+	m1 = _mm_mul_ps(m1, m2); // [-cr*sy][sr*sy][cy*cr][-sr*cy]
+
+	m3 = _mm_add_ps(m3, m1);
+
+	if (forward)
+	{
+		forward[0] = cp_mults.m128_f32[0];
+		_mm_storel_pi((__m64*)(forward + 1), m3); // (sr*sp*cy + cr*-sy);
+	}
+	if (right)
+	{
+		right[0] = cp_mults.m128_f32[2];
+		_mm_storel_pi((__m64*)(right + 1), _mm_shuffle_ps(m3, m3, 0x0E));
+	}
+	if (up)
+	{
+		up[0] = -s.m128_f32[PITCH];
+		_mm_storel_pi((__m64 *)&up[1], _mm_shuffle_ps(cp_mults, cp_mults, 0x07));
+	}
+}
+#else // REHLDS_FIXES
 /* <4712e> ../engine/mathlib.c:304 */
 void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 {
@@ -246,6 +349,7 @@ void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, ve
 		up[2] = cr*cp;
 	}
 }
+#endif
 
 /* <471e9> ../engine/mathlib.c:340 */
 void AngleMatrix(const vec_t *angles, float(*matrix)[4])
diff --git a/rehlds/engine/sse_mathfun.cpp b/rehlds/engine/sse_mathfun.cpp
new file mode 100644
index 0000000..724cb26
--- /dev/null
+++ b/rehlds/engine/sse_mathfun.cpp
@@ -0,0 +1,447 @@
+/* SIMD (SSE1+MMX or SSE2) implementation of sin, cos, exp and log
+
+Inspired by Intel Approximate Math library, and based on the
+corresponding algorithms of the cephes math library
+
+The default is to use the SSE1 version. If you define USE_SSE2 the
+the SSE2 intrinsics will be used in place of the MMX intrinsics. Do
+not expect any significant performance improvement with SSE2.
+*/
+
+/* Copyright (C) 2007  Julien Pommier
+
+This software is provided 'as-is', without any express or implied
+warranty.  In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not
+claim that you wrote the original software. If you use this software
+in a product, an acknowledgment in the product documentation would be
+appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+(this is the zlib license)
+*/
+
+#include "precompiled.h"
+
+/* natural logarithm computed for 4 simultaneous float
+return NaN for x <= 0
+*/
+v4sf log_ps(v4sf x) {
+	v4si emm0;
+
+	v4sf one = *(v4sf*)_ps_1;
+
+	v4sf invalid_mask = _mm_cmple_ps(x, _mm_setzero_ps());
+
+	x = _mm_max_ps(x, *(v4sf*)_ps_min_norm_pos);  /* cut off denormalized stuff */
+
+	emm0 = _mm_srli_epi32(_mm_castps_si128(x), 23);
+
+	/* keep only the fractional part */
+	x = _mm_and_ps(x, *(v4sf*)_ps_inv_mant_mask);
+	x = _mm_or_ps(x, *(v4sf*)_ps_0p5);
+
+	emm0 = _mm_sub_epi32(emm0, *(v4si*)_pi32_0x7f);
+	v4sf e = _mm_cvtepi32_ps(emm0);
+
+	e = _mm_add_ps(e, one);
+
+	/* part2:
+	if( x < SQRTHF ) {
+	e -= 1;
+	x = x + x - 1.0;
+	} else { x = x - 1.0; }
+	*/
+	v4sf mask = _mm_cmplt_ps(x, *(v4sf*)_ps_cephes_SQRTHF);
+	v4sf tmp = _mm_and_ps(x, mask);
+	x = _mm_sub_ps(x, one);
+	e = _mm_sub_ps(e, _mm_and_ps(one, mask));
+	x = _mm_add_ps(x, tmp);
+
+
+	v4sf z = _mm_mul_ps(x, x);
+
+	v4sf y = *(v4sf*)_ps_cephes_log_p0;
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p1);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p2);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p3);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p4);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p5);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p6);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p7);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p8);
+	y = _mm_mul_ps(y, x);
+
+	y = _mm_mul_ps(y, z);
+
+
+	tmp = _mm_mul_ps(e, *(v4sf*)_ps_cephes_log_q1);
+	y = _mm_add_ps(y, tmp);
+
+
+	tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+	y = _mm_sub_ps(y, tmp);
+
+	tmp = _mm_mul_ps(e, *(v4sf*)_ps_cephes_log_q2);
+	x = _mm_add_ps(x, y);
+	x = _mm_add_ps(x, tmp);
+	x = _mm_or_ps(x, invalid_mask); // negative arg will be NAN
+	return x;
+}
+
+v4sf exp_ps(v4sf x) {
+	v4sf tmp = _mm_setzero_ps(), fx;
+	v4si emm0;
+	v4sf one = *(v4sf*)_ps_1;
+
+	x = _mm_min_ps(x, *(v4sf*)_ps_exp_hi);
+	x = _mm_max_ps(x, *(v4sf*)_ps_exp_lo);
+
+	/* express exp(x) as exp(g + n*log(2)) */
+	fx = _mm_mul_ps(x, *(v4sf*)_ps_cephes_LOG2EF);
+	fx = _mm_add_ps(fx, *(v4sf*)_ps_0p5);
+
+	/* how to perform a floorf with SSE: just below */
+	emm0 = _mm_cvttps_epi32(fx);
+	tmp = _mm_cvtepi32_ps(emm0);
+
+	/* if greater, substract 1 */
+	v4sf mask = _mm_cmpgt_ps(tmp, fx);
+	mask = _mm_and_ps(mask, one);
+	fx = _mm_sub_ps(tmp, mask);
+
+	tmp = _mm_mul_ps(fx, *(v4sf*)_ps_cephes_exp_C1);
+	v4sf z = _mm_mul_ps(fx, *(v4sf*)_ps_cephes_exp_C2);
+	x = _mm_sub_ps(x, tmp);
+	x = _mm_sub_ps(x, z);
+
+	z = _mm_mul_ps(x, x);
+
+	v4sf y = *(v4sf*)_ps_cephes_exp_p0;
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p1);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p2);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p3);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p4);
+	y = _mm_mul_ps(y, x);
+	y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p5);
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, x);
+	y = _mm_add_ps(y, one);
+
+	/* build 2^n */
+	emm0 = _mm_cvttps_epi32(fx);
+	emm0 = _mm_add_epi32(emm0, *(v4si*)_pi32_0x7f);
+	emm0 = _mm_slli_epi32(emm0, 23);
+	v4sf pow2n = _mm_castsi128_ps(emm0);
+
+	y = _mm_mul_ps(y, pow2n);
+	return y;
+}
+
+/* evaluation of 4 sines at onces, using only SSE1+MMX intrinsics so
+it runs also on old athlons XPs and the pentium III of your grand
+mother.
+
+The code is the exact rewriting of the cephes sinf function.
+Precision is excellent as long as x < 8192 (I did not bother to
+take into account the special handling they have for greater values
+-- it does not return garbage for arguments over 8192, though, but
+the extra precision is missing).
+
+Note that it is such that sinf((float)M_PI) = 8.74e-8, which is the
+surprising but correct result.
+
+Performance is also surprisingly good, 1.33 times faster than the
+macos vsinf SSE2 function, and 1.5 times faster than the
+__vrs4_sinf of amd's ACML (which is only available in 64 bits). Not
+too bad for an SSE1 function (with no special tuning) !
+However the latter libraries probably have a much better handling of NaN,
+Inf, denormalized and other special arguments..
+
+On my core 1 duo, the execution of this function takes approximately 95 cycles.
+
+From what I have observed on the experiments with Intel AMath lib, switching to an
+SSE2 version would improve the perf by only 10%.
+
+Since it is based on SSE intrinsics, it has to be compiled at -O2 to
+deliver full speed.
+*/
+v4sf sin_ps(v4sf x) { // any x
+	v4sf xmm1, xmm2 = _mm_setzero_ps(), xmm3, sign_bit, y;
+
+	v4si emm0, emm2;
+
+	sign_bit = x;
+	/* take the absolute value */
+	x = _mm_and_ps(x, *(v4sf*)_ps_inv_sign_mask);
+	/* extract the sign bit (upper one) */
+	sign_bit = _mm_and_ps(sign_bit, *(v4sf*)_ps_sign_mask);
+
+	/* scale by 4/Pi */
+	y = _mm_mul_ps(x, *(v4sf*)_ps_cephes_FOPI);
+
+	/* store the integer part of y in mm0 */
+	emm2 = _mm_cvttps_epi32(y);
+	/* j=(j+1) & (~1) (see the cephes sources) */
+	emm2 = _mm_add_epi32(emm2, *(v4si*)_pi32_1);
+	emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_inv1);
+	y = _mm_cvtepi32_ps(emm2);
+
+	/* get the swap sign flag */
+	emm0 = _mm_and_si128(emm2, *(v4si*)_pi32_4);
+	emm0 = _mm_slli_epi32(emm0, 29);
+	/* get the polynom selection mask
+	there is one polynom for 0 <= x <= Pi/4
+	and another one for Pi/4<x<=Pi/2
+
+	Both branches will be computed.
+	*/
+	emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_2);
+	emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
+
+	v4sf swap_sign_bit = _mm_castsi128_ps(emm0);
+	v4sf poly_mask = _mm_castsi128_ps(emm2);
+	sign_bit = _mm_xor_ps(sign_bit, swap_sign_bit);
+
+	/* The magic pass: "Extended precision modular arithmetic"
+	x = ((x - y * DP1) - y * DP2) - y * DP3; */
+	xmm1 = *(v4sf*)_ps_minus_cephes_DP1;
+	xmm2 = *(v4sf*)_ps_minus_cephes_DP2;
+	xmm3 = *(v4sf*)_ps_minus_cephes_DP3;
+	xmm1 = _mm_mul_ps(y, xmm1);
+	xmm2 = _mm_mul_ps(y, xmm2);
+	xmm3 = _mm_mul_ps(y, xmm3);
+	x = _mm_add_ps(x, xmm1);
+	x = _mm_add_ps(x, xmm2);
+	x = _mm_add_ps(x, xmm3);
+
+	/* Evaluate the first polynom  (0 <= x <= Pi/4) */
+	y = *(v4sf*)_ps_coscof_p0;
+	v4sf z = _mm_mul_ps(x, x);
+
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p1);
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p2);
+	y = _mm_mul_ps(y, z);
+	y = _mm_mul_ps(y, z);
+	v4sf tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+	y = _mm_sub_ps(y, tmp);
+	y = _mm_add_ps(y, *(v4sf*)_ps_1);
+
+	/* Evaluate the second polynom  (Pi/4 <= x <= 0) */
+
+	v4sf y2 = *(v4sf*)_ps_sincof_p0;
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p1);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p2);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_mul_ps(y2, x);
+	y2 = _mm_add_ps(y2, x);
+
+	/* select the correct result from the two polynoms */
+	xmm3 = poly_mask;
+	y2 = _mm_and_ps(xmm3, y2); //, xmm3);
+	y = _mm_andnot_ps(xmm3, y);
+	y = _mm_add_ps(y, y2);
+	/* update the sign */
+	y = _mm_xor_ps(y, sign_bit);
+	return y;
+}
+
+/* almost the same as sin_ps */
+v4sf cos_ps(v4sf x) { // any x
+	v4sf xmm1, xmm2 = _mm_setzero_ps(), xmm3, y;
+
+	v4si emm0, emm2;
+
+	/* take the absolute value */
+	x = _mm_and_ps(x, *(v4sf*)_ps_inv_sign_mask);
+
+	/* scale by 4/Pi */
+	y = _mm_mul_ps(x, *(v4sf*)_ps_cephes_FOPI);
+
+	/* store the integer part of y in mm0 */
+	emm2 = _mm_cvttps_epi32(y);
+	/* j=(j+1) & (~1) (see the cephes sources) */
+	emm2 = _mm_add_epi32(emm2, *(v4si*)_pi32_1);
+	emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_inv1);
+	y = _mm_cvtepi32_ps(emm2);
+
+	emm2 = _mm_sub_epi32(emm2, *(v4si*)_pi32_2);
+
+	/* get the swap sign flag */
+	emm0 = _mm_andnot_si128(emm2, *(v4si*)_pi32_4);
+	emm0 = _mm_slli_epi32(emm0, 29);
+	/* get the polynom selection mask */
+	emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_2);
+	emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
+
+	v4sf sign_bit = _mm_castsi128_ps(emm0);
+	v4sf poly_mask = _mm_castsi128_ps(emm2);
+
+	/* The magic pass: "Extended precision modular arithmetic"
+	x = ((x - y * DP1) - y * DP2) - y * DP3; */
+	xmm1 = *(v4sf*)_ps_minus_cephes_DP1;
+	xmm2 = *(v4sf*)_ps_minus_cephes_DP2;
+	xmm3 = *(v4sf*)_ps_minus_cephes_DP3;
+	xmm1 = _mm_mul_ps(y, xmm1);
+	xmm2 = _mm_mul_ps(y, xmm2);
+	xmm3 = _mm_mul_ps(y, xmm3);
+	x = _mm_add_ps(x, xmm1);
+	x = _mm_add_ps(x, xmm2);
+	x = _mm_add_ps(x, xmm3);
+
+	/* Evaluate the first polynom  (0 <= x <= Pi/4) */
+	y = *(v4sf*)_ps_coscof_p0;
+	v4sf z = _mm_mul_ps(x, x);
+
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p1);
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p2);
+	y = _mm_mul_ps(y, z);
+	y = _mm_mul_ps(y, z);
+	v4sf tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+	y = _mm_sub_ps(y, tmp);
+	y = _mm_add_ps(y, *(v4sf*)_ps_1);
+
+	/* Evaluate the second polynom  (Pi/4 <= x <= 0) */
+
+	v4sf y2 = *(v4sf*)_ps_sincof_p0;
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p1);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p2);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_mul_ps(y2, x);
+	y2 = _mm_add_ps(y2, x);
+
+	/* select the correct result from the two polynoms */
+	xmm3 = poly_mask;
+	y2 = _mm_and_ps(xmm3, y2); //, xmm3);
+	y = _mm_andnot_ps(xmm3, y);
+	y = _mm_add_ps(y, y2);
+	/* update the sign */
+	y = _mm_xor_ps(y, sign_bit);
+
+	return y;
+}
+
+
+
+/* since sin_ps and cos_ps are almost identical, sincos_ps could replace both of them..
+it is almost as fast, and gives you a free cosine with your sine */
+void sincos_ps(v4sf x, v4sf *s, v4sf *c) {
+	v4sf xmm1, xmm2, xmm3 = _mm_setzero_ps(), sign_bit_sin, y;
+	v4si emm0, emm2, emm4;
+
+	sign_bit_sin = x;
+	/* take the absolute value */
+	x = _mm_and_ps(x, *(v4sf*)_ps_inv_sign_mask);
+	/* extract the sign bit (upper one) */
+	sign_bit_sin = _mm_and_ps(sign_bit_sin, *(v4sf*)_ps_sign_mask);
+
+	/* scale by 4/Pi */
+	y = _mm_mul_ps(x, *(v4sf*)_ps_cephes_FOPI);
+
+	/* store the integer part of y in emm2 */
+	emm2 = _mm_cvttps_epi32(y);
+
+	/* j=(j+1) & (~1) (see the cephes sources) */
+	emm2 = _mm_add_epi32(emm2, *(v4si*)_pi32_1);
+	emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_inv1);
+	y = _mm_cvtepi32_ps(emm2);
+
+	emm4 = emm2;
+
+	/* get the swap sign flag for the sine */
+	emm0 = _mm_and_si128(emm2, *(v4si*)_pi32_4);
+	emm0 = _mm_slli_epi32(emm0, 29);
+	v4sf swap_sign_bit_sin = _mm_castsi128_ps(emm0);
+
+	/* get the polynom selection mask for the sine*/
+	emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_2);
+	emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
+	v4sf poly_mask = _mm_castsi128_ps(emm2);
+
+	/* The magic pass: "Extended precision modular arithmetic"
+	x = ((x - y * DP1) - y * DP2) - y * DP3; */
+	xmm1 = *(v4sf*)_ps_minus_cephes_DP1;
+	xmm2 = *(v4sf*)_ps_minus_cephes_DP2;
+	xmm3 = *(v4sf*)_ps_minus_cephes_DP3;
+	xmm1 = _mm_mul_ps(y, xmm1);
+	xmm2 = _mm_mul_ps(y, xmm2);
+	xmm3 = _mm_mul_ps(y, xmm3);
+	x = _mm_add_ps(x, xmm1);
+	x = _mm_add_ps(x, xmm2);
+	x = _mm_add_ps(x, xmm3);
+
+	emm4 = _mm_sub_epi32(emm4, *(v4si*)_pi32_2);
+	emm4 = _mm_andnot_si128(emm4, *(v4si*)_pi32_4);
+	emm4 = _mm_slli_epi32(emm4, 29);
+	v4sf sign_bit_cos = _mm_castsi128_ps(emm4);
+
+	sign_bit_sin = _mm_xor_ps(sign_bit_sin, swap_sign_bit_sin);
+
+
+	/* Evaluate the first polynom  (0 <= x <= Pi/4) */
+	v4sf z = _mm_mul_ps(x, x);
+	y = *(v4sf*)_ps_coscof_p0;
+
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p1);
+	y = _mm_mul_ps(y, z);
+	y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p2);
+	y = _mm_mul_ps(y, z);
+	y = _mm_mul_ps(y, z);
+	v4sf tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+	y = _mm_sub_ps(y, tmp);
+	y = _mm_add_ps(y, *(v4sf*)_ps_1);
+
+	/* Evaluate the second polynom  (Pi/4 <= x <= 0) */
+
+	v4sf y2 = *(v4sf*)_ps_sincof_p0;
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p1);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p2);
+	y2 = _mm_mul_ps(y2, z);
+	y2 = _mm_mul_ps(y2, x);
+	y2 = _mm_add_ps(y2, x);
+
+	/* select the correct result from the two polynoms */
+	xmm3 = poly_mask;
+	v4sf ysin2 = _mm_and_ps(xmm3, y2);
+	v4sf ysin1 = _mm_andnot_ps(xmm3, y);
+	y2 = _mm_sub_ps(y2, ysin2);
+	y = _mm_sub_ps(y, ysin1);
+
+	xmm1 = _mm_add_ps(ysin1, ysin2);
+	xmm2 = _mm_add_ps(y, y2);
+
+	/* update the sign */
+	*s = _mm_xor_ps(xmm1, sign_bit_sin);
+	*c = _mm_xor_ps(xmm2, sign_bit_cos);
+}
diff --git a/rehlds/engine/sse_mathfun.h b/rehlds/engine/sse_mathfun.h
new file mode 100644
index 0000000..24fd11c
--- /dev/null
+++ b/rehlds/engine/sse_mathfun.h
@@ -0,0 +1,120 @@
+/* SIMD (SSE1+MMX or SSE2) implementation of sin, cos, exp and log
+
+Inspired by Intel Approximate Math library, and based on the
+corresponding algorithms of the cephes math library
+
+The default is to use the SSE1 version. If you define USE_SSE2 the
+the SSE2 intrinsics will be used in place of the MMX intrinsics. Do
+not expect any significant performance improvement with SSE2.
+*/
+
+/* Copyright (C) 2007  Julien Pommier
+
+This software is provided 'as-is', without any express or implied
+warranty.  In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not
+claim that you wrote the original software. If you use this software
+in a product, an acknowledgment in the product documentation would be
+appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+(this is the zlib license)
+*/
+#pragma once
+
+#include <xmmintrin.h>
+
+/* yes I know, the top of this file is quite ugly */
+
+#ifdef _MSC_VER /* visual c++ */
+# define ALIGN16_BEG __declspec(align(16))
+# define ALIGN16_END 
+#else /* gcc or icc */
+# define ALIGN16_BEG
+# define ALIGN16_END __attribute__((aligned(16)))
+#endif
+
+/* __m128 is ugly to write */
+typedef __m128 v4sf;  // vector of 4 float (sse1)
+
+#include <emmintrin.h>
+typedef __m128i v4si; // vector of 4 int (sse2)
+
+
+/* declare some SSE constants -- why can't I figure a better way to do that? */
+#define _PS_CONST(Name, Val)                                            \
+  static const ALIGN16_BEG float _ps_##Name[4] ALIGN16_END = { Val, Val, Val, Val }
+#define _PI32_CONST(Name, Val)                                            \
+  static const ALIGN16_BEG int _pi32_##Name[4] ALIGN16_END = { Val, Val, Val, Val }
+#define _PS_CONST_TYPE(Name, Type, Val)                                 \
+  static const ALIGN16_BEG Type _ps_##Name[4] ALIGN16_END = { Val, Val, Val, Val }
+
+_PS_CONST(1, 1.0f);
+_PS_CONST(0p5, 0.5f);
+/* the smallest non denormalized float number */
+_PS_CONST_TYPE(min_norm_pos, int, 0x00800000);
+_PS_CONST_TYPE(mant_mask, int, 0x7f800000);
+_PS_CONST_TYPE(inv_mant_mask, int, ~0x7f800000);
+
+_PS_CONST_TYPE(sign_mask, int, (int)0x80000000);
+_PS_CONST_TYPE(inv_sign_mask, int, ~0x80000000);
+
+_PI32_CONST(1, 1);
+_PI32_CONST(inv1, ~1);
+_PI32_CONST(2, 2);
+_PI32_CONST(4, 4);
+_PI32_CONST(0x7f, 0x7f);
+
+_PS_CONST(cephes_SQRTHF, 0.707106781186547524f);
+_PS_CONST(cephes_log_p0, 7.0376836292E-2f);
+_PS_CONST(cephes_log_p1, -1.1514610310E-1f);
+_PS_CONST(cephes_log_p2, 1.1676998740E-1f);
+_PS_CONST(cephes_log_p3, -1.2420140846E-1f);
+_PS_CONST(cephes_log_p4, +1.4249322787E-1f);
+_PS_CONST(cephes_log_p5, -1.6668057665E-1f);
+_PS_CONST(cephes_log_p6, +2.0000714765E-1f);
+_PS_CONST(cephes_log_p7, -2.4999993993E-1f);
+_PS_CONST(cephes_log_p8, +3.3333331174E-1f);
+_PS_CONST(cephes_log_q1, -2.12194440e-4f);
+_PS_CONST(cephes_log_q2, 0.693359375f);
+
+
+
+_PS_CONST(exp_hi, 88.3762626647949f);
+_PS_CONST(exp_lo, -88.3762626647949f);
+
+_PS_CONST(cephes_LOG2EF, 1.44269504088896341f);
+_PS_CONST(cephes_exp_C1, 0.693359375f);
+_PS_CONST(cephes_exp_C2, -2.12194440e-4f);
+
+_PS_CONST(cephes_exp_p0, 1.9875691500E-4f);
+_PS_CONST(cephes_exp_p1, 1.3981999507E-3f);
+_PS_CONST(cephes_exp_p2, 8.3334519073E-3f);
+_PS_CONST(cephes_exp_p3, 4.1665795894E-2f);
+_PS_CONST(cephes_exp_p4, 1.6666665459E-1f);
+_PS_CONST(cephes_exp_p5, 5.0000001201E-1f);
+
+_PS_CONST(minus_cephes_DP1, -0.78515625f);
+_PS_CONST(minus_cephes_DP2, -2.4187564849853515625e-4f);
+_PS_CONST(minus_cephes_DP3, -3.77489497744594108e-8f);
+_PS_CONST(sincof_p0, -1.9515295891E-4f);
+_PS_CONST(sincof_p1, 8.3321608736E-3f);
+_PS_CONST(sincof_p2, -1.6666654611E-1f);
+_PS_CONST(coscof_p0, 2.443315711809948E-005f);
+_PS_CONST(coscof_p1, -1.388731625493765E-003f);
+_PS_CONST(coscof_p2, 4.166664568298827E-002f);
+_PS_CONST(cephes_FOPI, 1.27323954473516f); // 4 / M_PI
+
+extern v4sf log_ps(v4sf x);
+extern v4sf exp_ps(v4sf x);
+extern v4sf sin_ps(v4sf x);
+extern v4sf cos_ps(v4sf x);
+extern void sincos_ps(v4sf x, v4sf *s, v4sf *c);
diff --git a/rehlds/msvc/ReHLDS.vcxproj b/rehlds/msvc/ReHLDS.vcxproj
index f39bb63..6bf6df6 100644
--- a/rehlds/msvc/ReHLDS.vcxproj
+++ b/rehlds/msvc/ReHLDS.vcxproj
@@ -77,6 +77,7 @@
     <ClCompile Include="..\engine\public_amalgamation.cpp" />
     <ClCompile Include="..\engine\r_studio.cpp" />
     <ClCompile Include="..\engine\snd_null.cpp" />
+    <ClCompile Include="..\engine\sse_mathfun.cpp" />
     <ClCompile Include="..\engine\sv_log.cpp" />
     <ClCompile Include="..\engine\sv_main.cpp" />
     <ClCompile Include="..\engine\sv_move.cpp" />
@@ -440,6 +441,7 @@
     <ClInclude Include="..\engine\server.h" />
     <ClInclude Include="..\engine\server_static.h" />
     <ClInclude Include="..\engine\sound.h" />
+    <ClInclude Include="..\engine\sse_mathfun.h" />
     <ClInclude Include="..\engine\studio_rehlds.h" />
     <ClInclude Include="..\engine\sv_log.h" />
     <ClInclude Include="..\engine\sv_move.h" />
diff --git a/rehlds/msvc/ReHLDS.vcxproj.filters b/rehlds/msvc/ReHLDS.vcxproj.filters
index 6991b59..a26fe7d 100644
--- a/rehlds/msvc/ReHLDS.vcxproj.filters
+++ b/rehlds/msvc/ReHLDS.vcxproj.filters
@@ -346,6 +346,9 @@
     <ClCompile Include="..\rehlds\rehlds_security.cpp">
       <Filter>rehlds</Filter>
     </ClCompile>
+    <ClCompile Include="..\engine\sse_mathfun.cpp">
+      <Filter>engine</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="..\hookers\memory.h">
@@ -1068,6 +1071,9 @@
     <ClInclude Include="..\common\qlimits.h">
       <Filter>common</Filter>
     </ClInclude>
+    <ClInclude Include="..\engine\sse_mathfun.h">
+      <Filter>engine</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <None Include="..\linux\appversion.sh">
diff --git a/rehlds/rehlds/precompiled.h b/rehlds/rehlds/precompiled.h
index 89439d8..645f9e7 100644
--- a/rehlds/rehlds/precompiled.h
+++ b/rehlds/rehlds/precompiled.h
@@ -6,6 +6,7 @@
 
 #include "archtypes.h"
 #include "asmlib.h"
+#include "sse_mathfun.h"
 #include "mathlib.h"
 
 #include "sys_shared.h"

From 6c01cf7817702c41d2a29a0d2c8713d556906b42 Mon Sep 17 00:00:00 2001
From: asmodai <asmodai@dedicated-server.ru>
Date: Sat, 6 Feb 2016 02:43:01 +0300
Subject: [PATCH 2/4] Optimized all other mathlib functions

---
 rehlds/engine/mathlib.cpp | 259 +++++++++++++++++++++++++++++---------
 1 file changed, 202 insertions(+), 57 deletions(-)

diff --git a/rehlds/engine/mathlib.cpp b/rehlds/engine/mathlib.cpp
index e667f46..10e09f3 100644
--- a/rehlds/engine/mathlib.cpp
+++ b/rehlds/engine/mathlib.cpp
@@ -66,11 +66,19 @@ const aivec4_t negmask_1001 =
 	0x80000000
 };
 
+const aivec4_t negmask_0010 =
+{
+	0,
+	0,
+	0x80000000,
+	0
+};
+
 // save 4d xmm to 3d vector. we can't optimize many simple vector3 functions because saving back to 3d is slow.
 static inline void xmm2vec(vec_t *v, const __m128 m)
 {
-	_mm_store_ss(v, m);
-	_mm_storel_pi((__m64*)(v + 1), _mm_shuffle_ps(m, m, _MM_SHUFFLE(3, 2, 2, 1)));
+	_mm_storel_pi((__m64*)v, m);
+	_mm_store_ss(v + 2, _mm_shuffle_ps(m, m, 0x02));
 }
 
 /* <46ebf> ../engine/mathlib.c:14 */
@@ -85,6 +93,80 @@ void BOPS_Error(void)
 	Sys_Error("BoxOnPlaneSide:  Bad signbits");
 }
 
+#ifdef REHLDS_FIXES
+int BoxOnPlaneSide(vec_t *emins, vec_t *emaxs, mplane_t *p)
+{
+	float	dist1, dist2;
+	int		sides = 0;
+
+	__m128 emin = _mm_loadu_ps(emins);
+	__m128 emax = _mm_loadu_ps(emaxs);;
+	avec4_t d1, d2;
+
+	// general case
+	switch (p->signbits)
+	{
+	case 0:
+		_mm_store_ps(d1, emax);
+		_mm_store_ps(d2, emin);
+		break;
+	case 1:
+		_mm_store_ps(d1, emax);
+		_mm_store_ps(d2, emin);
+		d1[0] = emins[0];
+		d2[0] = emaxs[0];
+		break;
+	case 2:
+		_mm_store_ps(d1, emax);
+		_mm_store_ps(d2, emin);
+		d1[1] = emins[1];
+		d2[1] = emaxs[1];
+		break;
+	case 3:
+		_mm_store_ps(d1, emax);
+		_mm_store_ps(d2, emin);
+		d1[2] = emins[2];
+		d2[2] = emaxs[2];
+		break;
+	case 4:
+		_mm_store_ps(d1, emin);
+		_mm_store_ps(d2, emax);
+		d1[2] = emaxs[2];
+		d2[2] = emins[2];
+		break;
+	case 5:
+		_mm_store_ps(d1, emin);
+		_mm_store_ps(d2, emax);
+		d1[1] = emaxs[1];
+		d2[1] = emins[1];
+		break;
+	case 6:
+		_mm_store_ps(d1, emin);
+		_mm_store_ps(d2, emax);
+		d1[0] = emaxs[0];
+		d2[0] = emins[0];
+		break;
+	case 7:
+		_mm_store_ps(d1, emin);
+		_mm_store_ps(d2, emax);
+		break;
+	default:
+		BOPS_Error();
+		dist1 = dist2 = 0.0;
+		break;
+	}
+
+	dist1 = _DotProduct(p->normal, d1);
+	dist2 = _DotProduct(p->normal, d2);
+
+	if (dist1 >= p->dist)
+		sides = 1;
+	if (dist2 < p->dist)
+		sides |= 2;
+
+	return sides;
+}
+#else
 /* <46f05> ../engine/mathlib.c:48 */
 int BoxOnPlaneSide(vec_t *emins, vec_t *emaxs, mplane_t *p)
 {
@@ -145,6 +227,7 @@ int BoxOnPlaneSide(vec_t *emins, vec_t *emaxs, mplane_t *p)
 
 	return sides;
 }
+#endif
 
 /* <46f8d> ../engine/mathlib.c:157 */
 NOBODY int InvertMatrix(const float *m, float *out);
@@ -198,7 +281,7 @@ void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 	if (forward)
 	{
 		_mm_storel_pi((__m64 *)forward, _mm_shuffle_ps(cp_mults, cp_mults, 0x08));
-		forward[2] = -s.m128_f32[PITCH];
+		forward[2] = -_mm_cvtss_f32(s);
 	}
 	if (right)
 	{
@@ -208,7 +291,7 @@ void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 	if (up)
 	{
 		_mm_storel_pi((__m64 *)up, _mm_shuffle_ps(m3, m3, 0x0E));
-		up[2] = cp_mults.m128_f32[1];
+		up[2] = _mm_cvtss_f32(_mm_shuffle_ps(cp_mults, cp_mults, 0x01));
 	}
 }
 #else // REHLDS_FIXES
@@ -286,17 +369,17 @@ void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, ve
 
 	if (forward)
 	{
-		forward[0] = cp_mults.m128_f32[0];
+		forward[0] = _mm_cvtss_f32(cp_mults);
 		_mm_storel_pi((__m64*)(forward + 1), m3); // (sr*sp*cy + cr*-sy);
 	}
 	if (right)
 	{
-		right[0] = cp_mults.m128_f32[2];
+		right[0] = _mm_cvtss_f32(_mm_shuffle_ps(cp_mults, cp_mults, 0x02));
 		_mm_storel_pi((__m64*)(right + 1), _mm_shuffle_ps(m3, m3, 0x0E));
 	}
 	if (up)
 	{
-		up[0] = -s.m128_f32[PITCH];
+		up[0] = -_mm_cvtss_f32(s);
 		_mm_storel_pi((__m64 *)&up[1], _mm_shuffle_ps(cp_mults, cp_mults, 0x07));
 	}
 }
@@ -306,18 +389,6 @@ void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, ve
 {
 	float		sr, sp, sy, cr, cp, cy;
 
-#ifdef REHLDS_FIXES
-	// convert to radians
-	avec4_t rad_angles;
-	_mm_store_ps(rad_angles, _mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)));
-
-	sy = sin(rad_angles[YAW]);
-	cy = cos(rad_angles[YAW]);
-	sp = sin(rad_angles[PITCH]);
-	cp = cos(rad_angles[PITCH]);
-	sr = sin(rad_angles[ROLL]);
-	cr = cos(rad_angles[ROLL]);
-#else
 	float angle;
 	angle = (float)(angles[YAW] * (M_PI * 2 / 360));
 	sy = sin(angle);
@@ -328,7 +399,6 @@ void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, ve
 	angle = (float)(angles[ROLL] * (M_PI * 2 / 360));
 	sr = sin(angle);
 	cr = cos(angle);
-#endif
 
 	if (forward)
 	{
@@ -351,23 +421,73 @@ void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, ve
 }
 #endif
 
+#ifdef REHLDS_FIXES
+// parallel SSE version
+void AngleMatrix(const vec_t *angles, float(*matrix)[4])
+{
+#ifndef SWDS
+	g_engdstAddrs.pfnAngleVectors(&angles, &forward, &right, &up);
+#endif // SWDS
+
+	__m128 s, c;
+	sincos_ps(_mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)), &s, &c);
+
+	/*
+	matrix[0][1] = sr * sp * cy - cr * sy;
+	matrix[1][1] = sr * sp * sy + cr * cy;
+	matrix[0][2] = cr * sp * cy + sr * sy;
+	matrix[1][2] = cr * sp * sy - sr * cy;
+	*/
+	__m128 m1;
+	__m128 m2 = _mm_shuffle_ps(s, c, 0x00); // [sp][sp][cp][cp]
+	__m128 m3 = _mm_shuffle_ps(c, s, 0x55); // [cy][cy][sy][sy]
+
+	m1 = _mm_shuffle_ps(s, c, 0xAA);   // [sr][sr][cr][cr]
+	m2 = _mm_shuffle_ps(m2, m2, 0x00); // [sp][sp][sp][sp]
+	m3 = _mm_shuffle_ps(m3, m3, 0xD8); // [cy][sy][cy][sy]
+
+	m2 = _mm_mul_ps(m2, _mm_mul_ps(m1, m3)); // m1*m2*m3
+
+	m1 = _mm_shuffle_ps(m1, m1, 0x1B); // [cr][cr][sr][sr]
+	m3 = _mm_shuffle_ps(m3, m3, 0xB1); // [sy][cy][sy][cy]
+	m3 = _mm_xor_ps(m3, _mm_load_ps((float *)&negmask_1001));
+	m3 = _mm_mul_ps(m3, m1);
+
+	m2 = _mm_add_ps(m2, m3);
+
+	/*
+	matrix[0][0] = cp * cy;
+	matrix[1][0] = cp * sy;
+	matrix[2][1] = sr * cp;
+	matrix[2][2] = cr * cp;
+	*/
+	m1 = _mm_shuffle_ps(s, c, 0x29); // [sy][sr][cr][cp]
+	c = _mm_shuffle_ps(c, c, 0x40);  // [cp][cp][cp][cy]
+	m1 = _mm_mul_ps(m1, c);
+
+	// matrix[0]
+	m3 = _mm_shuffle_ps(m2, m2, 0xE1);
+	_mm_storeu_ps(&matrix[0][0], m3);
+	matrix[0][0] = _mm_cvtss_f32(_mm_shuffle_ps(m1, m1, 0x03));
+	*(int *)&matrix[0][3] = 0;
+
+	// matrix[1]
+	m2 = _mm_shuffle_ps(m2, m2, 0xB4);
+	_mm_storeu_ps(&matrix[1][0], m2);
+	matrix[1][0] = _mm_cvtss_f32(m1);
+	*(int *)&matrix[1][3] = 0;
+
+	// matrix[2]
+	_mm_storeu_ps(&matrix[2][0], m1);
+	matrix[2][0] = -_mm_cvtss_f32(s);
+	*(int *)&matrix[2][3] = 0;
+}
+#else // REHLDS_FIXES
 /* <471e9> ../engine/mathlib.c:340 */
 void AngleMatrix(const vec_t *angles, float(*matrix)[4])
 {
 	float		sr, sp, sy, cr, cp, cy;
 
-#ifdef REHLDS_FIXES
-	// convert to radians
-	avec4_t rad_angles;
-	_mm_store_ps(rad_angles, _mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)));
-
-	sy = sin(rad_angles[ROLL]);
-	cy = cos(rad_angles[ROLL]);
-	sp = sin(rad_angles[YAW]);
-	cp = cos(rad_angles[YAW]);
-	sr = sin(rad_angles[PITCH]);
-	cr = cos(rad_angles[PITCH]);
-#else
 	float angle;
 	angle = (float)(angles[ROLL] * (M_PI * 2 / 360));
 	sy = sin(angle);
@@ -378,29 +498,23 @@ void AngleMatrix(const vec_t *angles, float(*matrix)[4])
 	angle = (float)(angles[PITCH] * (M_PI * 2 / 360));
 	sr = sin(angle);
 	cr = cos(angle);
-#endif
-
-	float tmp1, tmp2;
 
 	matrix[0][0] = cr * cp;
-	matrix[1][0] = cr * sp;
-	matrix[2][0] = -sr;
-
-	tmp1 = sy * sr;
-	matrix[0][1] = tmp1 * cp - cy * sp;
-	matrix[1][1] = tmp1 * sp + cy * cp;
-	matrix[2][1] = sy * cr;
-
-	tmp2 = cy * sr;
-	matrix[0][2] = tmp2 * cp + sy * sp;
-	matrix[1][2] = tmp2 * sp - sy * cp;
-	matrix[2][2] = cy * cr;
-
+	matrix[0][1] = sy * sr * cp - cy * sp;
+	matrix[0][2] = cy * sr * cp + sy * sp;
 	matrix[0][3] = 0.0f;
+
+	matrix[1][0] = cr * sp;
+	matrix[1][1] = sy * sr * sp + cy * cp;
+	matrix[1][2] = cy * sr * sp - sy * cp;
 	matrix[1][3] = 0.0f;
+
+	matrix[2][0] = -sr;
+	matrix[2][1] = sy * cr;
+	matrix[2][2] = cy * cr;
 	matrix[2][3] = 0.0f;
-	
 }
+#endif // REHLDS_FIXES
 
 /* <4729e> ../engine/mathlib.c:370 */
 NOBODY void AngleIMatrix(const vec_t *angles, float *matrix);
@@ -433,24 +547,39 @@ NOBODY void InterpolateAngles(float *start, float *end, float *output, float fra
 //}
 
 /* <47495> ../engine/mathlib.c:457 */
+//#ifndef REHLDS_FIXES
+
+//#else
 void VectorTransform(const vec_t *in1, float *in2, vec_t *out)
 {
-	out[0] = in2[1] * in1[1] + in2[2] * in1[2] + in1[0] * in2[0] + in2[3];
-	out[1] = in2[4] * in1[0] + in2[5] * in1[1] + in2[6] * in1[2] + in2[7];
-	out[2] = in2[8] * in1[0] + in2[9] * in1[1] + in2[10] * in1[2] + in2[11];
+	out[0] = _DotProduct(in1, in2 + 0) + in2[3];
+	out[1] = _DotProduct(in1, in2 + 4) + in2[7];
+	out[2] = _DotProduct(in1, in2 + 8) + in2[11];
 }
+//#endif
 
 /* <474dc> ../engine/mathlib.c:465 */
 int VectorCompare(const vec_t *v1, const vec_t *v2)
 {
+#ifdef REHLDS_FIXES
+	__m128 cmp = _mm_cmpneq_ss(_mm_loadu_ps(v1), _mm_loadu_ps(v2));
+	return !(_mm_movemask_epi8(*(__m128i *)&cmp) & 0xFFFFFF);
+#else
 	for (int i = 0; i < 3; i++)
 	{
 		if (v1[i] != v2[i]) return 0;
 	}
 
 	return 1;
+#endif
 }
 
+#ifdef REHLDS_FIXES
+void VectorMA(const vec_t *veca, float scale, const vec_t *vecm, vec_t *out)
+{
+	xmm2vec(out, _mm_add_ps(_mm_mul_ps(_mm_set_ps1(scale), _mm_loadu_ps(vecm)), _mm_loadu_ps(veca)));
+}
+#else
 /* <47524> ../engine/mathlib.c:476 */
 void VectorMA(const vec_t *veca, float scale, const vec_t *vecm, vec_t *out)
 {
@@ -458,6 +587,7 @@ void VectorMA(const vec_t *veca, float scale, const vec_t *vecm, vec_t *out)
 	out[1] = scale * vecm[1] + veca[1];
 	out[2] = scale * vecm[2] + veca[2];
 }
+#endif
 
 #ifndef REHLDS_FIXES
 /* <4757a> ../engine/mathlib.c:484 */
@@ -488,9 +618,9 @@ NOBODY void _VectorSubtract(vec_t *veca, vec_t *vecb, vec_t *out);
 /* <475fb> ../engine/mathlib.c:496 */
 void _VectorAdd(vec_t *veca, vec_t *vecb, vec_t *out)
 {
-	for (int i = 0; i < 3; i++) {
-		out[i] = veca[i] + vecb[i];
-	}
+	out[0] = veca[0] + vecb[0];
+	out[1] = veca[1] + vecb[1];
+	out[2] = veca[2] + vecb[2];
 }
 
 /* <47642> ../engine/mathlib.c:503 */
@@ -522,6 +652,7 @@ float Length(const vec_t *v)
 	// based on dot product
 	if (cpuinfo.sse4_1)
 		return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(_mm_loadu_ps(v), _mm_loadu_ps(v), 0x71)));
+	return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
 #endif // REHLDS_FIXES
 
 	float length;
@@ -551,10 +682,10 @@ float VectorNormalize(vec3_t v)
 	float	length, ilength;
 
 #ifdef REHLDS_FIXES
-	length = Length(v);
+	length = Length(v); // rsqrt is very inaccurate :(
 #else // REHLDS_FIXES
 	length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
-	length = sqrt(length);		// FIXME
+	length = sqrt(length);
 #endif // REHLDS_FIXES
 
 	if (length)
@@ -643,6 +774,19 @@ NOBODY void R_ConcatRotations(float *in1, float *in2, float *out);
 //}
 
 /* <47a04> ../engine/mathlib.c:660 */
+#ifdef REHLDS_FIXES
+void NOINLINE R_ConcatTransforms(float in1[3][4], float in2[3][4], float out[3][4])
+{
+	for (size_t i = 0; i < 3; i++)
+	{
+		__m128 a1 = _mm_mul_ps(_mm_set_ps1(in1[i][0]), _mm_loadu_ps(in2[0]));
+		__m128 a2 = _mm_mul_ps(_mm_set_ps1(in1[i][1]), _mm_loadu_ps(in2[1]));
+		__m128 a3 = _mm_mul_ps(_mm_set_ps1(in1[i][2]), _mm_loadu_ps(in2[2]));
+		_mm_storeu_ps(out[i], _mm_add_ps(a1, _mm_add_ps(a2, a3)));
+		out[i][3] += in1[i][3];
+	}
+}
+#else // REHLDS_FIXES
 void R_ConcatTransforms(float in1[3][4], float in2[3][4], float out[3][4])
 {
 	out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
@@ -658,6 +802,7 @@ void R_ConcatTransforms(float in1[3][4], float in2[3][4], float out[3][4])
 	out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
 	out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] + in1[2][2] * in2[2][3] + in1[2][3];
 }
+#endif // REHLDS_FIXES
 
 /* <47a4b> ../engine/mathlib.c:699 */
 NOBODY void FloorDivMod(double numer, double denom, int *quotient, int *rem);

From 0d6a95093ffee3b7589a7d570f7762f8b4b26cb3 Mon Sep 17 00:00:00 2001
From: asmodai <asmodai@dedicated-server.ru>
Date: Sat, 6 Feb 2016 03:41:32 +0300
Subject: [PATCH 3/4] Fixed mathlib unit tests Added popcnt support check
 Removed integer division optimization from asmlib.h Fixed typo in
 VectorCompare

---
 rehlds/engine/mathlib.cpp           |  24 ++----
 rehlds/public/asmlib.h              | 101 ------------------------
 rehlds/public/rehlds/sys_shared.cpp |   2 +
 rehlds/public/rehlds/sys_shared.h   |   2 +-
 rehlds/unittests/mathlib_tests.cpp  | 118 +++++++++++-----------------
 5 files changed, 54 insertions(+), 193 deletions(-)

diff --git a/rehlds/engine/mathlib.cpp b/rehlds/engine/mathlib.cpp
index 10e09f3..9a754ae 100644
--- a/rehlds/engine/mathlib.cpp
+++ b/rehlds/engine/mathlib.cpp
@@ -340,10 +340,6 @@ void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 // parallel SSE version
 void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
 {
-#ifndef SWDS
-	g_engdstAddrs.pfnAngleVectors(&angles, &forward, &right, &up);
-#endif // SWDS
-
 	__m128 s, c;
 	sincos_ps(_mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)), &s, &c);
 
@@ -425,10 +421,6 @@ void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, ve
 // parallel SSE version
 void AngleMatrix(const vec_t *angles, float(*matrix)[4])
 {
-#ifndef SWDS
-	g_engdstAddrs.pfnAngleVectors(&angles, &forward, &right, &up);
-#endif // SWDS
-
 	__m128 s, c;
 	sincos_ps(_mm_mul_ps(_mm_loadu_ps(angles), _mm_load_ps(deg2rad)), &s, &c);
 
@@ -547,31 +539,27 @@ NOBODY void InterpolateAngles(float *start, float *end, float *output, float fra
 //}
 
 /* <47495> ../engine/mathlib.c:457 */
-//#ifndef REHLDS_FIXES
-
-//#else
 void VectorTransform(const vec_t *in1, float *in2, vec_t *out)
 {
 	out[0] = _DotProduct(in1, in2 + 0) + in2[3];
 	out[1] = _DotProduct(in1, in2 + 4) + in2[7];
 	out[2] = _DotProduct(in1, in2 + 8) + in2[11];
 }
-//#endif
 
 /* <474dc> ../engine/mathlib.c:465 */
 int VectorCompare(const vec_t *v1, const vec_t *v2)
 {
-#ifdef REHLDS_FIXES
-	__m128 cmp = _mm_cmpneq_ss(_mm_loadu_ps(v1), _mm_loadu_ps(v2));
-	return !(_mm_movemask_epi8(*(__m128i *)&cmp) & 0xFFFFFF);
-#else
+#ifdef REHLDS_OPT_PEDANTIC
+	__m128 cmp = _mm_cmpneq_ps(_mm_loadu_ps(v1), _mm_loadu_ps(v2));
+	return !(_mm_movemask_epi8(*(__m128i *)&cmp) & 0xFFF);
+#else // REHLDS_OPT_PEDANTIC
 	for (int i = 0; i < 3; i++)
 	{
 		if (v1[i] != v2[i]) return 0;
 	}
 
 	return 1;
-#endif
+#endif // REHLDS_OPT_PEDANTIC
 }
 
 #ifdef REHLDS_FIXES
@@ -775,7 +763,7 @@ NOBODY void R_ConcatRotations(float *in1, float *in2, float *out);
 
 /* <47a04> ../engine/mathlib.c:660 */
 #ifdef REHLDS_FIXES
-void NOINLINE R_ConcatTransforms(float in1[3][4], float in2[3][4], float out[3][4])
+void R_ConcatTransforms(float in1[3][4], float in2[3][4], float out[3][4])
 {
 	for (size_t i = 0; i < 3; i++)
 	{
diff --git a/rehlds/public/asmlib.h b/rehlds/public/asmlib.h
index d95d13d..c1bc9af 100644
--- a/rehlds/public/asmlib.h
+++ b/rehlds/public/asmlib.h
@@ -120,105 +120,4 @@ static inline const char * A_strstr(const char * haystack, const char * needle)
 
 #endif // __cplusplus
 
-
-/***********************************************************************
-Function prototypes, integer division functions
-***********************************************************************/
-
-// Turn off name mangling
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void setdivisori32(int buffer[2], int d);                      // Set divisor for repeated division
-int dividefixedi32(const int buffer[2], int x);                // Fast division with previously set divisor
-void setdivisoru32(uint32_t buffer[2], uint32_t d);            // Set divisor for repeated division
-uint32_t dividefixedu32(const uint32_t buffer[2], uint32_t x); // Fast division with previously set divisor
-
-// Test if emmintrin.h is included and __m128i defined
-#if defined(__GNUC__) && defined(_EMMINTRIN_H_INCLUDED) && !defined(__SSE2__)
-#error Please compile with -sse2 or higher 
-#endif
-
-#if defined(_INCLUDED_EMM) || (defined(_EMMINTRIN_H_INCLUDED) && defined(__SSE2__))
-#define VECTORDIVISIONDEFINED
-
-// Integer vector division functions. These functions divide an integer vector by a scalar:
-
-// Set divisor for repeated integer vector division
-void setdivisorV8i16(__m128i buf[2], int16_t d);               // Set divisor for repeated division
-void setdivisorV8u16(__m128i buf[2], uint16_t d);              // Set divisor for repeated division
-void setdivisorV4i32(__m128i buf[2], int32_t d);               // Set divisor for repeated division
-void setdivisorV4u32(__m128i buf[2], uint32_t d);              // Set divisor for repeated division
-
-// Fast division of vector by previously set divisor
-__m128i dividefixedV8i16(const __m128i buf[2], __m128i x);     // Fast division with previously set divisor
-__m128i dividefixedV8u16(const __m128i buf[2], __m128i x);     // Fast division with previously set divisor
-__m128i dividefixedV4i32(const __m128i buf[2], __m128i x);     // Fast division with previously set divisor
-__m128i dividefixedV4u32(const __m128i buf[2], __m128i x);     // Fast division with previously set divisor
-
-#endif // defined(_INCLUDED_EMM) || (defined(_EMMINTRIN_H_INCLUDED) && defined(__SSE2__))
-
-#ifdef __cplusplus
-}  // end of extern "C"
-#endif // __cplusplus
-
-#ifdef __cplusplus
-
-// Define classes and operator '/' for fast division with fixed divisor
-class div_i32;
-class div_u32;
-static inline int32_t  operator / (int32_t  x, div_i32 const &D);
-static inline uint32_t operator / (uint32_t x, div_u32 const & D);
-
-class div_i32 {                                                // Signed 32 bit integer division
-public:
-    div_i32() {                                                // Default constructor
-        buffer[0] = buffer[1] = 0;
-    }
-    div_i32(int d) {                                           // Constructor with divisor
-        setdivisor(d);
-    }
-    void setdivisor(int d) {                                   // Set divisor
-        setdivisori32(buffer, d);
-    }
-protected:
-    int buffer[2];                                             // Internal memory
-    friend int32_t operator / (int32_t x, div_i32 const & D);
-};
-
-static inline int32_t operator / (int32_t x, div_i32 const &D){// Overloaded operator '/'
-    return dividefixedi32(D.buffer, x);
-}
-
-static inline int32_t operator /= (int32_t &x, div_i32 const &D){// Overloaded operator '/='
-    return x = x / D;
-}
-
-class div_u32 {                                                // Unsigned 32 bit integer division
-public:
-    div_u32() {                                                // Default constructor
-        buffer[0] = buffer[1] = 0;
-    }
-    div_u32(uint32_t d) {                                      // Constructor with divisor
-        setdivisor(d);
-    }
-    void setdivisor(uint32_t d) {                              // Set divisor
-        setdivisoru32(buffer, d);
-    }
-protected:
-    uint32_t buffer[2];                                        // Internal memory
-    friend uint32_t operator / (uint32_t x, div_u32 const & D);
-};
-
-static inline uint32_t operator / (uint32_t x, div_u32 const & D){ // Overloaded operator '/'
-    return dividefixedu32(D.buffer, x);
-}
-
-static inline uint32_t operator /= (uint32_t &x, div_u32 const &D){// Overloaded operator '/='
-    return x = x / D;
-}
-
-#endif // __cplusplus
-
 #endif // ASMLIB_H
diff --git a/rehlds/public/rehlds/sys_shared.cpp b/rehlds/public/rehlds/sys_shared.cpp
index e671f46..385b778 100644
--- a/rehlds/public/rehlds/sys_shared.cpp
+++ b/rehlds/public/rehlds/sys_shared.cpp
@@ -35,6 +35,7 @@
 #define SSSE3_FLAG		(1<<9)
 #define SSE4_1_FLAG		(1<<19)
 #define SSE4_2_FLAG		(1<<20)
+#define POPCNT_FLAG		(1<<23)
 #define AVX_FLAG		(1<<28)
 #define AVX2_FLAG		(1<<5)
 
@@ -56,6 +57,7 @@ void Sys_CheckCpuInstructionsSupport(void)
 	cpuinfo.ssse3 = (cpuid_data[2] & SSSE3_FLAG) ? 1 : 0;
 	cpuinfo.sse4_1 = (cpuid_data[2] & SSE4_1_FLAG) ? 1 : 0;
 	cpuinfo.sse4_2 = (cpuid_data[2] & SSE4_2_FLAG) ? 1 : 0;
+	cpuinfo.popcnt = (cpuid_data[2] & POPCNT_FLAG) ? 1 : 0;
 	cpuinfo.avx = (cpuid_data[2] & AVX_FLAG) ? 1 : 0;
 
 #if defined ASMLIB_H
diff --git a/rehlds/public/rehlds/sys_shared.h b/rehlds/public/rehlds/sys_shared.h
index cc9ed72..768b51f 100644
--- a/rehlds/public/rehlds/sys_shared.h
+++ b/rehlds/public/rehlds/sys_shared.h
@@ -31,7 +31,7 @@
 
 typedef struct cpuinfo_s
 {
-	uint8 sse3, ssse3, sse4_1, sse4_2, avx, avx2;
+	uint8 sse3, ssse3, sse4_1, sse4_2, avx, avx2, popcnt;
 } cpuinfo_t;
 
 extern cpuinfo_t cpuinfo;
diff --git a/rehlds/unittests/mathlib_tests.cpp b/rehlds/unittests/mathlib_tests.cpp
index ceeb62c..9b9475a 100644
--- a/rehlds/unittests/mathlib_tests.cpp
+++ b/rehlds/unittests/mathlib_tests.cpp
@@ -4,9 +4,6 @@
 #include <iostream>
 
 TEST(AngleVectorsTest, MathLib, 1000) {
-	Sys_CheckCpuInstructionsSupport();
-	CHECK_WARNING_OUT("SSE4.1 Support", cpuinfo.sse4_1);
-
 	struct testdata_t {
 		vec3_t angles;
 		vec3_t forward, right, up;
@@ -17,33 +14,26 @@ TEST(AngleVectorsTest, MathLib, 1000) {
 			{ { 106.0f, 142.0f, 62.0f }, { 0.21721f, -0.16970f, -0.96126f }, { 0.95785f, -0.15259f, 0.24337f }, { 0.18798f, 0.97361f, -0.12940f } }
 	};
 
-	for (int sse = 0; sse <= 1; sse++) {
-		vec3_t forward, right, up;
+	vec3_t forward, right, up;
 
-		for (int i = 0; i < ARRAYSIZE(testdata); i++) {
-			AngleVectors(testdata[i].angles, forward, right, up);
+	for (int i = 0; i < ARRAYSIZE(testdata); i++) {
+		AngleVectors(testdata[i].angles, forward, right, up);
 
-			DOUBLES_EQUAL("forward[0] mismatch", testdata[i].forward[0], forward[0], 0.00001);
-			DOUBLES_EQUAL("forward[1] mismatch", testdata[i].forward[1], forward[1], 0.00001);
-			DOUBLES_EQUAL("forward[2] mismatch", testdata[i].forward[2], forward[2], 0.00001);
+		DOUBLES_EQUAL("forward[0] mismatch", testdata[i].forward[0], forward[0], 0.00001);
+		DOUBLES_EQUAL("forward[1] mismatch", testdata[i].forward[1], forward[1], 0.00001);
+		DOUBLES_EQUAL("forward[2] mismatch", testdata[i].forward[2], forward[2], 0.00001);
 
-			DOUBLES_EQUAL("right[0] mismatch", testdata[i].right[0], right[0], 0.00001);
-			DOUBLES_EQUAL("right[1] mismatch", testdata[i].right[1], right[1], 0.00001);
-			DOUBLES_EQUAL("right[2] mismatch", testdata[i].right[2], right[2], 0.00001);
+		DOUBLES_EQUAL("right[0] mismatch", testdata[i].right[0], right[0], 0.00001);
+		DOUBLES_EQUAL("right[1] mismatch", testdata[i].right[1], right[1], 0.00001);
+		DOUBLES_EQUAL("right[2] mismatch", testdata[i].right[2], right[2], 0.00001);
 
-			DOUBLES_EQUAL("up[0] mismatch", testdata[i].up[0], up[0], 0.00001);
-			DOUBLES_EQUAL("up[1] mismatch", testdata[i].up[1], up[1], 0.00001);
-			DOUBLES_EQUAL("up[2] mismatch", testdata[i].up[2], up[2], 0.00001);
-		}
-
-		cpuinfo.sse4_1 = 0;
+		DOUBLES_EQUAL("up[0] mismatch", testdata[i].up[0], up[0], 0.00001);
+		DOUBLES_EQUAL("up[1] mismatch", testdata[i].up[1], up[1], 0.00001);
+		DOUBLES_EQUAL("up[2] mismatch", testdata[i].up[2], up[2], 0.00001);
 	}
 }
 
 TEST(AngleVectorsTransposeTest, MathLib, 1000) {
-	Sys_CheckCpuInstructionsSupport();
-	CHECK_WARNING_OUT("SSE4.1 Support", cpuinfo.sse4_1);
-
 	struct testdata_t {
 		vec3_t angles;
 		vec3_t forward, right, up;
@@ -54,33 +44,26 @@ TEST(AngleVectorsTransposeTest, MathLib, 1000) {
 			{ { 106.0f, 142.0f, 62.0f }, { 0.21721f, -0.95785f, 0.18798f }, { -0.16970f, 0.15259f, 0.97361f }, { -0.96126f, -0.24337f, -0.12940f } }
 	};
 
-	for (int sse = 0; sse <= 1; sse++) {
-		vec3_t forward, right, up;
+	vec3_t forward, right, up;
 
-		for (int i = 0; i < ARRAYSIZE(testdata); i++) {
-			AngleVectorsTranspose(testdata[i].angles, forward, right, up);
+	for (int i = 0; i < ARRAYSIZE(testdata); i++) {
+		AngleVectorsTranspose(testdata[i].angles, forward, right, up);
 
-			DOUBLES_EQUAL("forward[0] mismatch", testdata[i].forward[0], forward[0], 0.00001);
-			DOUBLES_EQUAL("forward[1] mismatch", testdata[i].forward[1], forward[1], 0.00001);
-			DOUBLES_EQUAL("forward[2] mismatch", testdata[i].forward[2], forward[2], 0.00001);
+		DOUBLES_EQUAL("forward[0] mismatch", testdata[i].forward[0], forward[0], 0.00001);
+		DOUBLES_EQUAL("forward[1] mismatch", testdata[i].forward[1], forward[1], 0.00001);
+		DOUBLES_EQUAL("forward[2] mismatch", testdata[i].forward[2], forward[2], 0.00001);
 
-			DOUBLES_EQUAL("right[0] mismatch", testdata[i].right[0], right[0], 0.00001);
-			DOUBLES_EQUAL("right[1] mismatch", testdata[i].right[1], right[1], 0.00001);
-			DOUBLES_EQUAL("right[2] mismatch", testdata[i].right[2], right[2], 0.00001);
+		DOUBLES_EQUAL("right[0] mismatch", testdata[i].right[0], right[0], 0.00001);
+		DOUBLES_EQUAL("right[1] mismatch", testdata[i].right[1], right[1], 0.00001);
+		DOUBLES_EQUAL("right[2] mismatch", testdata[i].right[2], right[2], 0.00001);
 
-			DOUBLES_EQUAL("up[0] mismatch", testdata[i].up[0], up[0], 0.00001);
-			DOUBLES_EQUAL("up[1] mismatch", testdata[i].up[1], up[1], 0.00001);
-			DOUBLES_EQUAL("up[2] mismatch", testdata[i].up[2], up[2], 0.00001);
-		}
-
-		cpuinfo.sse4_1 = 0;
+		DOUBLES_EQUAL("up[0] mismatch", testdata[i].up[0], up[0], 0.00001);
+		DOUBLES_EQUAL("up[1] mismatch", testdata[i].up[1], up[1], 0.00001);
+		DOUBLES_EQUAL("up[2] mismatch", testdata[i].up[2], up[2], 0.00001);
 	}
 }
 
 TEST(AngleMatrixTest, MathLib, 1000) {
-	Sys_CheckCpuInstructionsSupport();
-	CHECK_WARNING_OUT("SSE4.1 Support", cpuinfo.sse4_1);
-
 	struct testdata_t {
 		vec3_t angles;
 		vec_t matrix0[4];
@@ -93,29 +76,25 @@ TEST(AngleMatrixTest, MathLib, 1000) {
 			{ { 106.0f, 142.0f, 62.0f }, { 0.21721f, -0.95785f, 0.18798f, 0.0f }, { -0.16970f, 0.15259f, 0.97361f, 0.0f }, { -0.96126f, -0.24337f, -0.12940f, 0.0f } }
 	};
 
-	for (int sse = 0; sse <= 1; sse++) {
-		float rotation_matrix[3][4];
+	float rotation_matrix[3][4];
 
-		for (int i = 0; i < ARRAYSIZE(testdata); i++) {
-			AngleMatrix(testdata[i].angles, rotation_matrix);
+	for (int i = 0; i < ARRAYSIZE(testdata); i++) {
+		AngleMatrix(testdata[i].angles, rotation_matrix);
 
-			DOUBLES_EQUAL("rotationmatrix[0][0] mismatch", testdata[i].matrix0[0], rotation_matrix[0][0], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[0][1] mismatch", testdata[i].matrix0[1], rotation_matrix[0][1], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[0][2] mismatch", testdata[i].matrix0[2], rotation_matrix[0][2], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[0][3] mismatch", testdata[i].matrix0[3], rotation_matrix[0][3], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[0][0] mismatch", testdata[i].matrix0[0], rotation_matrix[0][0], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[0][1] mismatch", testdata[i].matrix0[1], rotation_matrix[0][1], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[0][2] mismatch", testdata[i].matrix0[2], rotation_matrix[0][2], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[0][3] mismatch", testdata[i].matrix0[3], rotation_matrix[0][3], 0.00001);
 
-			DOUBLES_EQUAL("rotationmatrix[1][0] mismatch", testdata[i].matrix1[0], rotation_matrix[1][0], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[1][1] mismatch", testdata[i].matrix1[1], rotation_matrix[1][1], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[1][2] mismatch", testdata[i].matrix1[2], rotation_matrix[1][2], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[1][3] mismatch", testdata[i].matrix1[3], rotation_matrix[1][3], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[1][0] mismatch", testdata[i].matrix1[0], rotation_matrix[1][0], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[1][1] mismatch", testdata[i].matrix1[1], rotation_matrix[1][1], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[1][2] mismatch", testdata[i].matrix1[2], rotation_matrix[1][2], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[1][3] mismatch", testdata[i].matrix1[3], rotation_matrix[1][3], 0.00001);
 
-			DOUBLES_EQUAL("rotationmatrix[2][0] mismatch", testdata[i].matrix2[0], rotation_matrix[2][0], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[2][1] mismatch", testdata[i].matrix2[1], rotation_matrix[2][1], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[2][2] mismatch", testdata[i].matrix2[2], rotation_matrix[2][2], 0.00001);
-			DOUBLES_EQUAL("rotationmatrix[2][3] mismatch", testdata[i].matrix2[3], rotation_matrix[2][3], 0.00001);
-		}
-
-		cpuinfo.sse4_1 = 0;
+		DOUBLES_EQUAL("rotationmatrix[2][0] mismatch", testdata[i].matrix2[0], rotation_matrix[2][0], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[2][1] mismatch", testdata[i].matrix2[1], rotation_matrix[2][1], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[2][2] mismatch", testdata[i].matrix2[2], rotation_matrix[2][2], 0.00001);
+		DOUBLES_EQUAL("rotationmatrix[2][3] mismatch", testdata[i].matrix2[3], rotation_matrix[2][3], 0.00001);
 	}
 }
 
@@ -146,9 +125,6 @@ TEST(DotProductTest, MathLib, 1000) {
 }
 
 TEST(CrossProductTest, MathLib, 1000) {
-	Sys_CheckCpuInstructionsSupport();
-	CHECK_WARNING_OUT("SSE4.1 Support", cpuinfo.sse4_1);
-
 	struct testdata_t {
 		vec3_t v1;
 		vec3_t v2;
@@ -160,18 +136,14 @@ TEST(CrossProductTest, MathLib, 1000) {
 			{ { -16.1f, -0.09f, 1.2f }, { 8.2f, 1.2f, -6.66f }, { -0.84060f, -97.38600f, -18.58200f } },
 	};
 
-	for (int sse = 0; sse <= 1; sse++) {
-		vec3_t res;
+	vec3_t res;
 
-		for (int i = 0; i < ARRAYSIZE(testdata); i++) {
-			CrossProduct(testdata[i].v1, testdata[i].v2, res);
+	for (int i = 0; i < ARRAYSIZE(testdata); i++) {
+		CrossProduct(testdata[i].v1, testdata[i].v2, res);
 
-			DOUBLES_EQUAL("CrossProduct[0] mismatch", testdata[i].res[0], res[0], 0.00001);
-			DOUBLES_EQUAL("CrossProduct[1] mismatch", testdata[i].res[1], res[1], 0.00001);
-			DOUBLES_EQUAL("CrossProduct[2] mismatch", testdata[i].res[2], res[2], 0.00001);
-		}
-
-		cpuinfo.sse4_1 = 0;
+		DOUBLES_EQUAL("CrossProduct[0] mismatch", testdata[i].res[0], res[0], 0.00001);
+		DOUBLES_EQUAL("CrossProduct[1] mismatch", testdata[i].res[1], res[1], 0.00001);
+		DOUBLES_EQUAL("CrossProduct[2] mismatch", testdata[i].res[2], res[2], 0.00001);
 	}
 }
 

From 2e270cb4ad8e38206f1175e6ccd6442f266f2117 Mon Sep 17 00:00:00 2001
From: asmodai <asmodai@dedicated-server.ru>
Date: Mon, 8 Feb 2016 23:08:55 +0300
Subject: [PATCH 4/4] BoxOnPlaneSide was fixed and covered by the flight tests
 Added missed mathlib unit tests

---
 rehlds/engine/mathlib.cpp          | 16 +++---
 rehlds/unittests/mathlib_tests.cpp | 79 +++++++++++++++++++++++++++++-
 2 files changed, 86 insertions(+), 9 deletions(-)

diff --git a/rehlds/engine/mathlib.cpp b/rehlds/engine/mathlib.cpp
index 9a754ae..f33b702 100644
--- a/rehlds/engine/mathlib.cpp
+++ b/rehlds/engine/mathlib.cpp
@@ -93,10 +93,10 @@ void BOPS_Error(void)
 	Sys_Error("BoxOnPlaneSide:  Bad signbits");
 }
 
-#ifdef REHLDS_FIXES
+#ifdef REHLDS_OPT_PEDANTIC
 int BoxOnPlaneSide(vec_t *emins, vec_t *emaxs, mplane_t *p)
 {
-	float	dist1, dist2;
+	double	dist1, dist2;
 	int		sides = 0;
 
 	__m128 emin = _mm_loadu_ps(emins);
@@ -123,17 +123,17 @@ int BoxOnPlaneSide(vec_t *emins, vec_t *emaxs, mplane_t *p)
 		d2[1] = emaxs[1];
 		break;
 	case 3:
-		_mm_store_ps(d1, emax);
-		_mm_store_ps(d2, emin);
-		d1[2] = emins[2];
-		d2[2] = emaxs[2];
-		break;
-	case 4:
 		_mm_store_ps(d1, emin);
 		_mm_store_ps(d2, emax);
 		d1[2] = emaxs[2];
 		d2[2] = emins[2];
 		break;
+	case 4:
+		_mm_store_ps(d1, emax);
+		_mm_store_ps(d2, emin);
+		d1[2] = emins[2];
+		d2[2] = emaxs[2];
+		break;
 	case 5:
 		_mm_store_ps(d1, emin);
 		_mm_store_ps(d2, emax);
diff --git a/rehlds/unittests/mathlib_tests.cpp b/rehlds/unittests/mathlib_tests.cpp
index 9b9475a..e668cb1 100644
--- a/rehlds/unittests/mathlib_tests.cpp
+++ b/rehlds/unittests/mathlib_tests.cpp
@@ -254,4 +254,81 @@ TEST(VectorAnglesTest, MathLib, 1000) {
 
 		cpuinfo.sse4_1 = 0;
 	}
-}
\ No newline at end of file
+}
+
+TEST(VectorCompareTest, MathLib, 1000)
+{
+	struct testdata_t {
+		vec4_t v1;
+		vec4_t v2;
+		int res;
+	};
+
+	testdata_t testdata[4] = {
+		{{41.5f, 7.32f, -9.22f, -16.1f}, {41.5f, 7.32f, -9.22f, -16.1009f}, 1},
+		{{41.5f, 7.32f, -9.22f, -16.1f}, {41.5f, 7.32f, -9.220002f, -16.1f}, 0},
+		{{41.5f, 7.32f, -9.22f, -16.1f}, {41.49f, 7.32f, -9.22f, -16.1f}, 0},
+		{{41.5f, 7.32001f, -9.22f, -16.1f}, {41.5f, 7.32f, -9.22f, -16.1f}, 0}
+	};
+
+	for (int i = 0; i < ARRAYSIZE(testdata); i++) {
+		int res = VectorCompare(testdata[i].v1, testdata[i].v2);
+
+		LONGS_EQUAL("VectorCompare mismatch", testdata[i].res, res);
+	}
+}
+
+TEST(VectorMATest, MathLib, 1000) {
+	struct testdata_t {
+		vec3_t a;
+		vec3_t m;
+		float s;
+		vec3_t res;
+	};
+
+	testdata_t testdata[2] = {
+		{{41.5f, 7.32f, -9.22f}, {-16.1f, -0.09f, 1.2f}, 42.14063f, {-636.964111f, 3.527344f, 41.348755f}},
+		{{0.96204f, 0.16969f, -0.21374f}, {347.65839f, 10.00326f, 0.0f}, 16.10025f, {5598.349121f, 161.224670f, -0.213740f}}
+	};
+
+	for (int i = 0; i < ARRAYSIZE(testdata); i++) {
+		vec3_t out;
+		VectorMA(testdata[i].a, testdata[i].s, testdata[i].m, out);
+
+		DOUBLES_EQUAL("VectorMA[0] mismatch", testdata[i].res[0], out[0], 0.00001);
+		DOUBLES_EQUAL("VectorMA[1] mismatch", testdata[i].res[1], out[1], 0.00001);
+		DOUBLES_EQUAL("VectorMA[2] mismatch", testdata[i].res[2], out[2], 0.00001);
+	}
+}
+
+TEST(R_ConcatTransformsTest, MathLib, 1000) {
+	struct testdata_t {
+		vec4_t in1[3];
+		vec4_t in2[3];
+		vec4_t res[3];
+	};
+
+	testdata_t testdata = {
+		{{0.41f, 13.34f, 41.69f, 14.78f}, {34.67f, 15.00f, 7.24f, 43.58f}, {19.62f, 7.05f, 32.81f, 49.61f}},
+		{{44.64f, 31.45f, 18.27f, 4.91f}, {29.95f, 48.27f, 23.91f, 39.02f}, {19.42f, 4.36f, 46.04f, 1.53f}},
+		{{1227.455200f, 838.584717f, 2245.857666f, 601.105591f},	{2137.519531f, 1845.987793f, 1325.400513f, 810.186890f},	{1724.154541f, 1100.404175f, 2037.595459f, 471.234528f}}
+	};
+
+	vec4_t out[3];
+	R_ConcatTransforms(testdata.in1, testdata.in2, out);
+
+	DOUBLES_EQUAL("R_ConcatTransformsTest[0][0] mismatch", testdata.res[0][0], out[0][0], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[0][1] mismatch", testdata.res[0][1], out[0][1], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[0][2] mismatch", testdata.res[0][2], out[0][2], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[0][3] mismatch", testdata.res[0][3], out[0][3], 0.001);
+
+	DOUBLES_EQUAL("R_ConcatTransformsTest[1][0] mismatch", testdata.res[1][0], out[1][0], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[1][1] mismatch", testdata.res[1][1], out[1][1], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[1][2] mismatch", testdata.res[1][2], out[1][2], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[1][3] mismatch", testdata.res[1][3], out[1][3], 0.001);
+
+	DOUBLES_EQUAL("R_ConcatTransformsTest[2][0] mismatch", testdata.res[2][0], out[2][0], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[2][1] mismatch", testdata.res[2][1], out[2][1], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[2][2] mismatch", testdata.res[2][2], out[2][2], 0.001);
+	DOUBLES_EQUAL("R_ConcatTransformsTest[2][3] mismatch", testdata.res[2][3], out[2][3], 0.001);
+}