From c453c06f26904784393999a0173257258035bb11 Mon Sep 17 00:00:00 2001
From: havoc <havoc@d7cf8633-e32d-0410-b094-e92efae38249>
Date: Thu, 6 Dec 2007 17:32:45 +0000
Subject: [PATCH] added DP_QC_VECTOANGLES_WITH_ROLL

git-svn-id: svn://svn.icculus.org/twilight/trunk/darkplaces@7774 d7cf8633-e32d-0410-b094-e92efae38249
---
 mathlib.c   | 68 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 mathlib.h   |  2 ++
 prvm_cmds.c | 42 +++------------------------------
 svvm_cmds.c |  1 +
 4 files changed, 74 insertions(+), 39 deletions(-)

diff --git a/mathlib.c b/mathlib.c
index c1e65b5a..cbd57db1 100644
--- a/mathlib.c
+++ b/mathlib.c
@@ -518,6 +518,74 @@ void AngleVectorsFLU (const vec3_t angles, vec3_t forward, vec3_t left, vec3_t u
 	}
 }
 
+// LordHavoc: calculates pitch/yaw/roll angles from forward and up vectors
+void AnglesFromVectors (vec3_t angles, const vec3_t forward, const vec3_t up, qboolean flippitch)
+{
+	if (forward[0] == 0 && forward[1] == 0)
+	{
+		angles[PITCH] = forward[2] > 0 ? -M_PI * 0.5 : M_PI * 0.5;
+		angles[YAW] = up ? atan2(-up[1], -up[0]) : 0;
+		angles[ROLL] = 0;
+	}
+	else
+	{
+		angles[YAW] = atan2(forward[1], forward[0]);
+		angles[PITCH] = -atan2(forward[2], sqrt(forward[0]*forward[0] + forward[1]*forward[1]));
+		if (up)
+		{
+			vec_t cp = cos(angles[PITCH]), sp = sin(angles[PITCH]);
+			vec_t cy = cos(angles[YAW]), sy = sin(angles[YAW]);
+			vec3_t tleft, tup;
+			tleft[0] = -sy;
+			tleft[1] = cy;
+			tleft[2] = 0;
+			tup[0] = sp*cy;
+			tup[1] = sp*sy;
+			tup[2] = cp;
+			angles[ROLL] = -atan2(DotProduct(up, tleft), DotProduct(up, tup));
+		}
+		else
+			angles[ROLL] = 0;
+	}
+
+	// now convert radians to degrees, and make all values positive
+	VectorScale(angles, 180.0 / M_PI, angles);
+	if (flippitch)
+		angles[PITCH] *= -1;
+	if (angles[PITCH] < 0) angles[PITCH] += 360;
+	if (angles[YAW] < 0) angles[YAW] += 360;
+	if (angles[ROLL] < 0) angles[ROLL] += 360;
+
+#if 0
+{
+	// debugging code
+	vec3_t tforward, tleft, tup, nforward, nup;
+	VectorCopy(forward, nforward);
+	VectorNormalize(nforward);
+	if (up)
+	{
+		VectorCopy(up, nup);
+		VectorNormalize(nup);
+		AngleVectors(angles, tforward, tleft, tup);
+		if (VectorDistance(tforward, nforward) > 0.01 || VectorDistance(tup, nup) > 0.01)
+		{
+			Con_Printf("vectoangles('%f %f %f', '%f %f %f') = %f %f %f\n", nforward[0], nforward[1], nforward[2], nup[0], nup[1], nup[2], angles[0], angles[1], angles[2]);
+			Con_Printf("^3But that is '%f %f %f', '%f %f %f'\n", tforward[0], tforward[1], tforward[2], tup[0], tup[1], tup[2]);
+		}
+	}
+	else
+	{
+		AngleVectors(angles, tforward, tleft, tup);
+		if (VectorDistance(tforward, nforward) > 0.01)
+		{
+			Con_Printf("vectoangles('%f %f %f') = %f %f %f\n", nforward[0], nforward[1], nforward[2], angles[0], angles[1], angles[2]);
+			Con_Printf("^3But that is '%f %f %f'\n", tforward[0], tforward[1], tforward[2]);
+		}
+	}
+}
+#endif
+}
+
 #if 0
 void AngleMatrix (const vec3_t angles, const vec3_t translate, vec_t matrix[][4])
 {
diff --git a/mathlib.h b/mathlib.h
index 4516a276..1e0f98c0 100644
--- a/mathlib.h
+++ b/mathlib.h
@@ -215,6 +215,8 @@ void AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
 void AngleVectorsFLU (const vec3_t angles, vec3_t forward, vec3_t left, vec3_t up);
 // LordHavoc: builds a [3][4] matrix
 void AngleMatrix (const vec3_t angles, const vec3_t translate, vec_t matrix[][4]);
+// LordHavoc: calculates pitch/yaw/roll angles from forward and up vectors
+void AnglesFromVectors (vec3_t angles, const vec3_t forward, const vec3_t up, qboolean flippitch);
 
 // LordHavoc: like AngleVectors, but taking a forward vector instead of angles, useful!
 void VectorVectors(const vec3_t forward, vec3_t right, vec3_t up);
diff --git a/prvm_cmds.c b/prvm_cmds.c
index 1b2f87ae..3a8b38a3 100644
--- a/prvm_cmds.c
+++ b/prvm_cmds.c
@@ -320,50 +320,14 @@ void VM_vectoyaw (void)
 =================
 VM_vectoangles
 
-vector vectoangles(vector)
+vector vectoangles(vector[, vector])
 =================
 */
 void VM_vectoangles (void)
 {
-	float	*value1;
-	float	forward;
-	float	yaw, pitch;
-
-	VM_SAFEPARMCOUNT(1,VM_vectoangles);
-
-	value1 = PRVM_G_VECTOR(OFS_PARM0);
-
-	if (value1[1] == 0 && value1[0] == 0)
-	{
-		yaw = 0;
-		if (value1[2] > 0)
-			pitch = 90;
-		else
-			pitch = 270;
-	}
-	else
-	{
-		// LordHavoc: optimized a bit
-		if (value1[0])
-		{
-			yaw = (atan2(value1[1], value1[0]) * 180 / M_PI);
-			if (yaw < 0)
-				yaw += 360;
-		}
-		else if (value1[1] > 0)
-			yaw = 90;
-		else
-			yaw = 270;
-
-		forward = sqrt(value1[0]*value1[0] + value1[1]*value1[1]);
-		pitch = (atan2(value1[2], forward) * 180 / M_PI);
-		if (pitch < 0)
-			pitch += 360;
-	}
+	VM_SAFEPARMCOUNTRANGE(1, 2,VM_vectoangles);
 
-	PRVM_G_FLOAT(OFS_RETURN+0) = pitch;
-	PRVM_G_FLOAT(OFS_RETURN+1) = yaw;
-	PRVM_G_FLOAT(OFS_RETURN+2) = 0;
+	AnglesFromVectors(PRVM_G_VECTOR(OFS_RETURN), PRVM_G_VECTOR(OFS_PARM0), prog->argc >= 2 ? PRVM_G_VECTOR(OFS_PARM1) : NULL, true);
 }
 
 /*
diff --git a/svvm_cmds.c b/svvm_cmds.c
index dffc4fda..7c5af070 100644
--- a/svvm_cmds.c
+++ b/svvm_cmds.c
@@ -80,6 +80,7 @@ char *vm_sv_extensions =
 "DP_QC_TRACE_MOVETYPE_HITMODEL "
 "DP_QC_TRACE_MOVETYPE_WORLDONLY "
 "DP_QC_UNLIMITEDTEMPSTRINGS "
+"DP_QC_VECTOANGLES_WITH_ROLL "
 "DP_QC_VECTORVECTORS "
 "DP_QUAKE2_MODEL "
 "DP_QUAKE2_SPRITE "
-- 
2.39.2