From 61d4844826aa66006388b58eb3c1619c20d72cd1 Mon Sep 17 00:00:00 2001
From: Alibek Omarov <a1ba.omarov@gmail.com>
Date: Thu, 7 Jun 2018 18:52:30 +0300
Subject: [PATCH 1/5] Theoretically better unlag based on bose position
 restoring

---
 rehlds/engine/r_studio.cpp | 31 +++++++++++++
 rehlds/engine/server.h     | 14 ++++++
 rehlds/engine/sv_main.cpp  |  3 ++
 rehlds/engine/sv_user.cpp  | 94 +++++++++++++++++++++++++++++++++++++-
 rehlds/engine/sv_user.h    | 13 ++++++
 5 files changed, 153 insertions(+), 2 deletions(-)

diff --git a/rehlds/engine/r_studio.cpp b/rehlds/engine/r_studio.cpp
index 30a83ffbc..b69d443d2 100644
--- a/rehlds/engine/r_studio.cpp
+++ b/rehlds/engine/r_studio.cpp
@@ -627,7 +627,11 @@ hull_t *R_StudioHull(model_t *pModel, float frame, int sequence, const vec_t *an
 	pstudiohdr = (studiohdr_t *)Mod_Extradata(pModel);
 
 	vec_t angles2[3] = { -angles[0], angles[1], angles[2] };
+#ifdef REHLDS_FIXES
+	SV_StudioSetupUnlagBones( pModel, frame, sequence, angles2, origin, pcontroller, pblending, -1, pEdict );
+#else
 	g_pSvBlendingAPI->SV_StudioSetupBones(pModel, frame, sequence, angles2, origin, pcontroller, pblending, -1, pEdict);
+#endif
 
 	mstudiobbox_t *pbbox = (mstudiobbox_t *)((char *)pstudiohdr + pstudiohdr->hitboxindex);
 	for (int i = 0; i < pstudiohdr->numhitboxes; i++)
@@ -874,6 +878,17 @@ void EXT_FUNC AnimationAutomove(const edict_t *pEdict, float flTime)
 void EXT_FUNC GetBonePosition(const edict_t *pEdict, int iBone, float *rgflOrigin, float *rgflAngles)
 {
 	pstudiohdr = (studiohdr_t *)Mod_Extradata(g_psv.models[pEdict->v.modelindex]);
+#ifdef REHLDS_FIXES
+	SV_StudioSetupUnlagBones( g_psv.models[pEdict->v.modelindex],
+		pEdict->v.frame,
+		pEdict->v.sequence,
+		pEdict->v.angles,
+		pEdict->v.origin,
+		pEdict->v.controller,
+		pEdict->v.blending,
+		iBone,
+		pEdict );
+#else
 	g_pSvBlendingAPI->SV_StudioSetupBones(
 		g_psv.models[pEdict->v.modelindex],
 		pEdict->v.frame,
@@ -885,6 +900,7 @@ void EXT_FUNC GetBonePosition(const edict_t *pEdict, int iBone, float *rgflOrigi
 		iBone,
 		pEdict
 	);
+#endif
 
 	if (rgflOrigin)
 	{
@@ -907,6 +923,20 @@ void EXT_FUNC GetAttachment(const edict_t *pEdict, int iAttachment, float *rgflO
 	pattachment = (mstudioattachment_t *)((char *)pstudiohdr + pstudiohdr->attachmentindex);
 	pattachment += iAttachment;
 
+#ifdef REHLDS_FIXES
+	SV_StudioSetupUnlagBones( 
+		g_psv.models[pEdict->v.modelindex],
+		pEdict->v.frame,
+		pEdict->v.sequence,
+		angles,
+		pEdict->v.origin,
+		pEdict->v.controller,
+		pEdict->v.blending,
+		pattachment->bone,
+		pEdict 
+	);
+#else
+
 	g_pSvBlendingAPI->SV_StudioSetupBones(
 		g_psv.models[pEdict->v.modelindex],
 		pEdict->v.frame,
@@ -918,6 +948,7 @@ void EXT_FUNC GetAttachment(const edict_t *pEdict, int iAttachment, float *rgflO
 		pattachment->bone,
 		pEdict
 	);
+#endif
 
 	if (rgflOrigin)
 		VectorTransform(pattachment->org, bonetransform[pattachment->bone], rgflOrigin);
diff --git a/rehlds/engine/server.h b/rehlds/engine/server.h
index 7adca46e5..3e008c427 100644
--- a/rehlds/engine/server.h
+++ b/rehlds/engine/server.h
@@ -53,6 +53,7 @@ const int MAX_NAME   = 32;
 #include "pm_defs.h"
 #include "inst_baseline.h"
 #include "net_ws.h"
+#include "studio_rehlds.h"
 
 const int DEFAULT_SOUND_PACKET_VOLUME			= 255;
 const float DEFAULT_SOUND_PACKET_ATTENUATION	= 1.0f;
@@ -175,6 +176,16 @@ struct rehlds_server_t {
 #endif
 };
 
+#ifdef REHLDS_FIXES
+// a1batross: "bone position"-based unlag
+typedef struct client_bone_state_s
+{
+	bonetransform_t bonetransform;
+	float rotationmatrix[3][4];
+	qboolean valid;
+} client_bone_state_t;
+#endif // REHLDS_FIXES
+
 typedef struct client_frame_s
 {
 	double senttime;
@@ -182,6 +193,9 @@ typedef struct client_frame_s
 	clientdata_t clientdata;
 	weapon_data_t weapondata[64];
 	packet_entities_t entities;
+#ifdef REHLDS_FIXES // a1batross: "bone position"-based unlag
+	client_bone_state_t bonestate;
+#endif // REHLDS_FIXES
 } client_frame_t;
 
 typedef struct client_s
diff --git a/rehlds/engine/sv_main.cpp b/rehlds/engine/sv_main.cpp
index 8bec12a51..580863e45 100644
--- a/rehlds/engine/sv_main.cpp
+++ b/rehlds/engine/sv_main.cpp
@@ -7717,6 +7717,9 @@ void SV_Init(void)
 	Cvar_RegisterVariable(&sv_instancedbaseline);
 	Cvar_RegisterVariable(&sv_contact);
 	Cvar_RegisterVariable(&sv_unlag);
+#ifdef REHLDS_FIXES
+	Cvar_RegisterVariable(&sv_bone_unlag);
+#endif
 	Cvar_RegisterVariable(&sv_maxunlag);
 	Cvar_RegisterVariable(&sv_unlagpush);
 	Cvar_RegisterVariable(&sv_unlagsamples);
diff --git a/rehlds/engine/sv_user.cpp b/rehlds/engine/sv_user.cpp
index 381e014d6..ceb89ef6c 100644
--- a/rehlds/engine/sv_user.cpp
+++ b/rehlds/engine/sv_user.cpp
@@ -59,6 +59,9 @@ cvar_t sv_footsteps = { "mp_footsteps", "1", FCVAR_SERVER, 0.0f, NULL };
 cvar_t sv_rollspeed = { "sv_rollspeed", "0.0", 0, 0.0f, NULL };
 cvar_t sv_rollangle = { "sv_rollangle", "0.0", 0, 0.0f, NULL };
 cvar_t sv_unlag = { "sv_unlag", "1", 0, 0.0f, NULL };
+#ifdef REHLDS_FIXES
+cvar_t sv_bone_unlag = { "sv_bone_unlag", "0", 0, 0.0f, NULL };
+#endif // REHLDS_FIXES
 cvar_t sv_maxunlag = { "sv_maxunlag", "0.5", 0, 0.0f, NULL };
 cvar_t sv_unlagpush = { "sv_unlagpush", "0.0", 0, 0.0f, NULL };
 cvar_t sv_unlagsamples = { "sv_unlagsamples", "1", 0, 0.0f, NULL };
@@ -1008,6 +1011,10 @@ void SV_RunCmd(usercmd_t *ucmd, int random_seed)
 	gGlobalVariables.frametime = frametime;
 	gEntityInterface.pfnPlayerPostThink(sv_player);
 	gEntityInterface.pfnCmdEnd(sv_player);
+	
+#ifdef REHLDS_FIXES
+	SV_SaveBoneState( host_client, sv_player );
+#endif
 
 	if (!host_client->fakeclient)
 		SV_RestoreMove(host_client);
@@ -1320,7 +1327,7 @@ void SV_SetupMove(client_t *_host_client)
 		frame = nextFrame;
 		frac = 0.0;
 	}
-
+	
 	for (i = 0; i < nextFrame->entities.num_entities; i++)
 	{
 		state = &nextFrame->entities.entities[i];
@@ -1342,6 +1349,25 @@ void SV_SetupMove(client_t *_host_client)
 		}
 
 		pnextstate = SV_FindEntInPack(state->number, &frame->entities);
+		
+#ifdef REHLDS_FIXES
+		if( nextFrame->bonestate.valid )
+		{
+			if( frame->bonestate.valid )
+			{
+				// TODO: interpolate
+				pos->bonestate = nextFrame->bonestate;
+			}
+			else
+			{
+				pos->bonestate = nextFrame->bonestate;
+			}
+		}
+		else
+		{
+			pos->bonestate.valid = false;
+		}
+#endif // REHLDS_FIXES
 
 		if (pnextstate)
 		{
@@ -1409,7 +1435,11 @@ void SV_RestoreMove(client_t *_host_client)
 			Con_DPrintf("SV_RestoreMove:  Tried to restore 'inactive' player %i/%s\n", i, &cli->name[4]);
 			continue;
 		}
-
+		
+#ifdef REHLDS_FIXES
+		pos->bonestate.valid = false;
+#endif // REHLDS_FIXES
+		
 		if (VectorCompare(pos->initial_correction_org, cli->edict->v.origin))
 		{
 			cli->edict->v.origin[0] = pos->oldorg[0];
@@ -1910,3 +1940,63 @@ void SV_FullUpdate_f(void)
 	gEntityInterface.pfnClientCommand(sv_player);
 #endif // REHLDS_FIXES
 }
+
+#ifdef REHLDS_FIXES
+void SV_SaveBoneState(client_t *_host_client, const edict_t *edict)
+{
+	int num = NUM_FOR_EDICT( edict );
+	client_frame_t *frame;
+	extern bonetransform_t bonetransform; // in r_studio.cpp
+	extern float rotationmatrix[3][4]; // in r_studio.cpp
+
+	if( !SV_IsPlayerIndex( num )) // just in case
+		return;
+	
+	// get last outgoing frame
+	frame = &_host_client->frames[SV_UPDATE_MASK & (_host_client->netchan.outgoing_sequence)];
+	
+	// set up bones
+	g_pSvBlendingAPI->SV_StudioSetupBones(
+		g_psv.models[edict->v.modelindex],
+		edict->v.frame, edict->v.sequence, edict->v.angles, edict->v.origin,
+		edict->v.controller, edict->v.blending, -1, edict
+	);
+	
+	// copy bones
+	frame->bonestate.valid = true;
+	Q_memcpy( frame->bonestate.bonetransform, bonetransform, sizeof( bonetransform ));
+	Q_memcpy( frame->bonestate.rotationmatrix, rotationmatrix, sizeof( rotationmatrix ));
+
+}
+
+void SV_StudioSetupUnlagBones( model_t *pModel, float frame, int sequence, const vec_t *angles, const vec_t *origin, const unsigned char *pcontroller, const unsigned char *pblending, int iBone, const edict_t *edict )
+{
+	// a1ba: carefully check everything, otherwise we may get broken bones!
+	if( edict && sv_bone_unlag.value ) // check is this a server or disabled
+	{
+		if( !nofind ) // unlag is enabled
+		{			
+			if( edict->v.flags & FL_CLIENT ) 
+			{
+				int num = NUM_FOR_EDICT( edict );	
+
+				if( truepositions[num].active &&
+					!truepositions[num].needrelink && // TODO: will this work for moving clients?
+					truepositions[num].bonestate.valid ) // bones are correct
+				{
+					// in r_studio.cpp
+					extern bonetransform_t bonetransform;
+					extern float rotationmatrix[3][4];
+					
+					Q_memcpy( bonetransform, truepositions[num].bonestate.bonetransform, sizeof( bonetransform ));
+					Q_memcpy( rotationmatrix, truepositions[num].bonestate.rotationmatrix, sizeof( rotationmatrix ));
+					return;
+				}
+			}
+		}
+	}
+	
+	// fallback to original SV_StudioSetupBones
+	g_pSvBlendingAPI->SV_StudioSetupBones(pModel, frame, sequence, angles, origin, pcontroller, pblending, iBone, edict);
+}
+#endif // REHLDS_FIXES
diff --git a/rehlds/engine/sv_user.h b/rehlds/engine/sv_user.h
index 8da33602c..66bde0a58 100644
--- a/rehlds/engine/sv_user.h
+++ b/rehlds/engine/sv_user.h
@@ -49,6 +49,9 @@ typedef struct sv_adjusted_positions_s
 	int deadflag;
 	vec3_t temp_org;
 	int temp_org_setflag;
+#ifdef REHLDS_FIXES
+	client_bone_state_t bonestate;
+#endif // REHLDS_FIXES
 } sv_adjusted_positions_t;
 
 typedef struct clc_func_s
@@ -71,6 +74,9 @@ extern cvar_t sv_footsteps;
 extern cvar_t sv_rollspeed;
 extern cvar_t sv_rollangle;
 extern cvar_t sv_unlag;
+#ifdef REHLDS_FIXES
+extern cvar_t sv_bone_unlag;
+#endif
 extern cvar_t sv_maxunlag;
 extern cvar_t sv_unlagpush;
 extern cvar_t sv_unlagsamples;
@@ -117,3 +123,10 @@ qboolean SV_SetPlayer(int idnum);
 void SV_ShowServerinfo_f(void);
 void SV_SendEnts_f(void);
 void SV_FullUpdate_f(void);
+
+#ifdef REHLDS_FIXES
+void SV_SaveBoneState( client_t *cl, const edict_t *edict );
+
+// "bone position"-based sv_unlag
+void SV_StudioSetupUnlagBones( model_t *pModel, float frame, int sequence, const vec_t *angles, const vec_t *origin, const unsigned char *pcontroller, const unsigned char *pblending, int iBone, const edict_t *edict );
+#endif // REHLDS_FIXES

From a4f4e34fae0ad4770607d258e3e6d89095e1535a Mon Sep 17 00:00:00 2001
From: Alibek Omarov <a1ba.omarov@gmail.com>
Date: Thu, 7 Jun 2018 21:05:05 +0300
Subject: [PATCH 2/5] Implement bone position interpolation

---
 rehlds/engine/server.h    |  1 +
 rehlds/engine/sv_user.cpp | 87 ++++++++++++++++++++++++++++++++++++++-
 2 files changed, 86 insertions(+), 2 deletions(-)

diff --git a/rehlds/engine/server.h b/rehlds/engine/server.h
index 3e008c427..ea6980dec 100644
--- a/rehlds/engine/server.h
+++ b/rehlds/engine/server.h
@@ -182,6 +182,7 @@ typedef struct client_bone_state_s
 {
 	bonetransform_t bonetransform;
 	float rotationmatrix[3][4];
+	int numbones;
 	qboolean valid;
 } client_bone_state_t;
 #endif // REHLDS_FIXES
diff --git a/rehlds/engine/sv_user.cpp b/rehlds/engine/sv_user.cpp
index ceb89ef6c..265f29dfb 100644
--- a/rehlds/engine/sv_user.cpp
+++ b/rehlds/engine/sv_user.cpp
@@ -1159,6 +1159,89 @@ entity_state_t *SV_FindEntInPack(int index, packet_entities_t *pack)
 	return NULL;
 }
 
+#ifdef REHLDS_FIXES
+void VectorsAngles( const vec3_t forward, const vec3_t right, const vec3_t up, vec3_t angles )
+{
+	float	pitch, cpitch, yaw, roll;
+
+	pitch = -asin( forward[2] );
+	cpitch = cos( pitch );
+
+	if( fabs( cpitch ) > 0.001f )	// gimball lock?
+	{
+		cpitch = 1.0f / cpitch;
+		pitch = RAD2DEG( pitch );
+		yaw = RAD2DEG( atan2( forward[1] * cpitch, forward[0] * cpitch ));
+		roll = RAD2DEG( atan2( -right[2] * cpitch, up[2] * cpitch ));
+	}
+	else
+	{
+		pitch = forward[2] > 0 ? -90.0f : 90.0f;
+		yaw = RAD2DEG( atan2( right[0], -right[1] ));
+		roll = 180.0f;
+	}
+
+	angles[0] = pitch;
+	angles[1] = yaw;
+	angles[2] = roll;
+}
+
+static void LerpRotationMatrix(const float from[3][4], const float to[3][4], float s, float out[3][4])
+{
+	vec3_t f1, r1, u1, pos1, ang1;
+	vec3_t f2, r2, u2, pos2, ang2;
+	vec4_t q1, q2, q3;
+	float s1 = 1.0f - s; // backlerp
+	
+	for( int j = 0; j < 3; j++ )
+	{
+		f1[j]   = from[j][0];
+		r1[j]   = from[j][1];
+		u2[j]   = from[j][2];
+		pos1[j] = from[j][3];
+		
+		f1[j]   = to[j][0];
+		r1[j]   = to[j][1];
+		u1[j]   = to[j][2];
+		pos2[j] = to[j][3];
+	}
+		
+	VectorsAngles( f1, r1, u1, ang1 );
+	VectorsAngles( f2, r2, u2, ang2 );
+	
+	AngleQuaternion( ang1, q1 );
+	AngleQuaternion( ang2, q2 );
+	
+	QuaternionSlerp(q1, q2, s, q3);
+	pos1[0] = pos1[0] * s1 + pos2[0] * s;
+	pos1[1] = pos1[1] * s1 + pos2[1] * s;
+	pos1[2] = pos1[2] * s1 + pos2[2] * s;
+	
+	// result
+	QuaternionMatrix(q3, out);
+	out[0][3] = pos1[0];
+	out[1][3] = pos1[1];
+	out[2][3] = pos1[2];
+}
+
+static client_bone_state_t SV_StudioUnlagSlerpBones(const client_bone_state_t *from, const client_bone_state_t *to, float s)
+{
+	client_bone_state_t ret;
+	
+	ret.valid = true;
+	ret.numbones = from->numbones;
+	
+	for (int i = 0; i < from->numbones; i++)
+	{
+		LerpRotationMatrix(from->bonetransform[i], to->bonetransform[i], s, ret.bonetransform[i]);
+	}
+	
+	LerpRotationMatrix(from->rotationmatrix, to->rotationmatrix, s, ret.rotationmatrix);
+	
+	return ret;
+}
+#endif // REHLDS_FIXES
+
 void SV_SetupMove(client_t *_host_client)
 {
 	struct client_s *cl;
@@ -1355,8 +1438,7 @@ void SV_SetupMove(client_t *_host_client)
 		{
 			if( frame->bonestate.valid )
 			{
-				// TODO: interpolate
-				pos->bonestate = nextFrame->bonestate;
+				pos->bonestate = SV_StudioUnlagSlerpBones(&frame->bonestate, &nextFrame->bonestate, frac);
 			}
 			else
 			{
@@ -1964,6 +2046,7 @@ void SV_SaveBoneState(client_t *_host_client, const edict_t *edict)
 	
 	// copy bones
 	frame->bonestate.valid = true;
+	frame->bonestate.numbones = pstudiohdr->numbones;
 	Q_memcpy( frame->bonestate.bonetransform, bonetransform, sizeof( bonetransform ));
 	Q_memcpy( frame->bonestate.rotationmatrix, rotationmatrix, sizeof( rotationmatrix ));
 

From 0799c568f0e55a82382fca7e70ff0d1adef6f0d4 Mon Sep 17 00:00:00 2001
From: Alibek Omarov <a1ba.omarov@gmail.com>
Date: Tue, 12 Jun 2018 12:29:33 +0300
Subject: [PATCH 3/5] Fix crash in saving bone state for current frame

---
 rehlds/engine/sv_user.cpp | 24 ++++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/rehlds/engine/sv_user.cpp b/rehlds/engine/sv_user.cpp
index 265f29dfb..ce13846db 100644
--- a/rehlds/engine/sv_user.cpp
+++ b/rehlds/engine/sv_user.cpp
@@ -1012,12 +1012,12 @@ void SV_RunCmd(usercmd_t *ucmd, int random_seed)
 	gEntityInterface.pfnPlayerPostThink(sv_player);
 	gEntityInterface.pfnCmdEnd(sv_player);
 	
+	if (!host_client->fakeclient)
+		SV_RestoreMove(host_client);
+	
 #ifdef REHLDS_FIXES
 	SV_SaveBoneState( host_client, sv_player );
 #endif
-
-	if (!host_client->fakeclient)
-		SV_RestoreMove(host_client);
 }
 
 int SV_ValidateClientCommand(char *pszCommand)
@@ -2037,6 +2037,22 @@ void SV_SaveBoneState(client_t *_host_client, const edict_t *edict)
 	// get last outgoing frame
 	frame = &_host_client->frames[SV_UPDATE_MASK & (_host_client->netchan.outgoing_sequence)];
 	
+	// not a studio model
+	if( g_psv.models[i]->type != mod_studio )
+	{
+		frame->bonestate.valid = false;
+		return;
+	}
+	
+	studiohdr_t *hdr = (studiohdr_t*)Mod_Extradata(g_psv.models[edict->v.modelindex]);
+	
+	// shouldn't really happen
+	if( !hdr )
+	{
+		frame->bonestate.valid = false;
+		return;
+	}
+	
 	// set up bones
 	g_pSvBlendingAPI->SV_StudioSetupBones(
 		g_psv.models[edict->v.modelindex],
@@ -2046,7 +2062,7 @@ void SV_SaveBoneState(client_t *_host_client, const edict_t *edict)
 	
 	// copy bones
 	frame->bonestate.valid = true;
-	frame->bonestate.numbones = pstudiohdr->numbones;
+	frame->bonestate.numbones = hdr->numbones;
 	Q_memcpy( frame->bonestate.bonetransform, bonetransform, sizeof( bonetransform ));
 	Q_memcpy( frame->bonestate.rotationmatrix, rotationmatrix, sizeof( rotationmatrix ));
 

From b13d85e0c87433945d91cc954fba69b479348c1f Mon Sep 17 00:00:00 2001
From: Alibek Omarov <alibek.omarov@autogramma.ru>
Date: Wed, 13 Jun 2018 12:21:55 +0300
Subject: [PATCH 4/5] Fix compiling

---
 rehlds/engine/sv_user.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rehlds/engine/sv_user.cpp b/rehlds/engine/sv_user.cpp
index ce13846db..3945c0cfb 100644
--- a/rehlds/engine/sv_user.cpp
+++ b/rehlds/engine/sv_user.cpp
@@ -2038,7 +2038,7 @@ void SV_SaveBoneState(client_t *_host_client, const edict_t *edict)
 	frame = &_host_client->frames[SV_UPDATE_MASK & (_host_client->netchan.outgoing_sequence)];
 	
 	// not a studio model
-	if( g_psv.models[i]->type != mod_studio )
+	if( g_psv.models[edict->v.modelindex]->type != mod_studio )
 	{
 		frame->bonestate.valid = false;
 		return;

From b09519164ea114540231c4b17255eb2d5b367a77 Mon Sep 17 00:00:00 2001
From: Alibek Omarov <a1ba.omarov@gmail.com>
Date: Mon, 16 Jul 2018 23:42:30 +0300
Subject: [PATCH 5/5] Don't hide math functions under REHLDS_FIXES

---
 rehlds/engine/sv_user.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/rehlds/engine/sv_user.cpp b/rehlds/engine/sv_user.cpp
index 3945c0cfb..427b9d2bf 100644
--- a/rehlds/engine/sv_user.cpp
+++ b/rehlds/engine/sv_user.cpp
@@ -1159,7 +1159,7 @@ entity_state_t *SV_FindEntInPack(int index, packet_entities_t *pack)
 	return NULL;
 }
 
-#ifdef REHLDS_FIXES
+
 void VectorsAngles( const vec3_t forward, const vec3_t right, const vec3_t up, vec3_t angles )
 {
 	float	pitch, cpitch, yaw, roll;
@@ -1224,6 +1224,7 @@ static void LerpRotationMatrix(const float from[3][4], const float to[3][4], flo
 	out[2][3] = pos1[2];
 }
 
+#ifdef REHLDS_FIXES
 static client_bone_state_t SV_StudioUnlagSlerpBones(const client_bone_state_t *from, const client_bone_state_t *to, float s)
 {
 	client_bone_state_t ret;