Port navmesh debug renderer from primedev (#626)

Adds support for rendering navmeshes in-game using debug overlay

Cherry-picked from primedev, originally written by F1F7Y

Co-authored-by: F1F7Y <filip.bartos07@proton.me>
Co-authored-by: Maya <11448698+RoyalBlue1@users.noreply.github.com>

3 files changed, 421 insertions, 24 deletions

diff --git a/primedev/core/math/math_pfns.h b/primedev/core/math/math_pfns.h
new file mode 100644
index 00000000..4dca9806
--- /dev/null
+++ b/primedev/core/math/math_pfns.h
@@ -0,0 +1,7 @@
+#pragma once
+
+inline float FastSqrt(float x)
+{
+	__m128 root = _mm_sqrt_ss(_mm_load_ss(&x));
+	return *(reinterpret_cast<float*>(&root));
+}
diff --git a/primedev/core/math/vector.h b/primedev/core/math/vector.h
index 8684908f..e62f2c93 100644
--- a/primedev/core/math/vector.h
+++ b/primedev/core/math/vector.h
@@ -1,47 +1,331 @@
+#pragma once
+
+#include "bits.h"
+#include "math_pfns.h"
+
 #include <cmath>
 
-#pragma once
+#define DEG2RAD(a) (a) * (3.14159265358979323846f / 180.0f)
+#define RAD2DEG(a) (a) * (180.0f / 3.14159265358979323846f)
 
-union Vector3
+class Vector3
 {
-	struct
+public:
+	float x, y, z;
+
+	Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {}
+	Vector3(float _f) : x(_f), y(_f), z(_f) {}
+	Vector3() : x(0.0f), y(0.0f), z(0.0f) {}
+
+	inline bool IsValid()
 	{
-		float x;
-		float y;
-		float z;
-	};
+		return IsFinite(x) && IsFinite(y) && IsFinite(z);
+	}
 
-	float raw[3];
+	inline void Init(float fX = 0.0f, float fY = 0.0f, float fZ = 0.0f)
+	{
+		x = fX;
+		y = fY;
+		z = fZ;
+	}
 
-	void MakeValid()
+	inline float Length()
 	{
-		for (auto& fl : raw)
-			if (std::isnan(fl))
-				fl = 0;
+		return FastSqrt(x * x + y * y + z * z);
 	}
 
-	// todo: more operators maybe
-	bool operator==(const Vector3& other)
+	inline float DistTo(const Vector3& vOther)
 	{
-		return x == other.x && y == other.y && z == other.z;
+		Vector3 vDelta;
+		vDelta.x = vOther.x - x;
+		vDelta.y = vOther.y - y;
+		vDelta.z = vOther.z - z;
+
+		return vDelta.Length();
+	}
+
+	float Dot(const Vector3& vOther) const
+	{
+		return x * vOther.x + y * vOther.y + z * vOther.z;
 	}
+
+	// Cross product between two vectors.
+	Vector3 Cross(const Vector3& vOther) const;
+
+	void Normalize();
+
+	bool operator==(const Vector3& v) const;
+	bool operator!=(const Vector3& v) const;
+
+	FORCEINLINE Vector3& operator+=(const Vector3& v);
+	FORCEINLINE Vector3& operator-=(const Vector3& v);
+	FORCEINLINE Vector3& operator*=(const Vector3& v);
+	FORCEINLINE Vector3& operator*=(float s);
+	FORCEINLINE Vector3& operator/=(const Vector3& v);
+	FORCEINLINE Vector3& operator/=(float s);
+	FORCEINLINE Vector3& operator+=(float fl); ///< broadcast add
+	FORCEINLINE Vector3& operator-=(float fl);
+
+	// arithmetic operations
+	Vector3 operator-(void) const;
+
+	Vector3 operator+(const Vector3& v) const;
+	Vector3 operator-(const Vector3& v) const;
+	Vector3 operator*(const Vector3& v) const;
+	Vector3 operator/(const Vector3& v) const;
+	Vector3 operator*(float fl) const;
+	Vector3 operator/(float fl) const;
 };
 
-union QAngle
+FORCEINLINE void VectorAdd(const Vector3& a, const Vector3& b, Vector3& result);
+FORCEINLINE void VectorSubtract(const Vector3& a, const Vector3& b, Vector3& result);
+FORCEINLINE void VectorMultiply(const Vector3& a, float b, Vector3& result);
+FORCEINLINE void VectorMultiply(const Vector3& a, const Vector3& b, Vector3& result);
+FORCEINLINE void VectorDivide(const Vector3& a, float b, Vector3& result);
+FORCEINLINE void VectorDivide(const Vector3& a, const Vector3& b, Vector3& result);
+
+inline bool Vector3::operator==(const Vector3& src) const
+{
+	return (src.x == x) && (src.y == y) && (src.z == z);
+}
+
+inline bool Vector3::operator!=(const Vector3& src) const
+{
+	return (src.x != x) || (src.y != y) || (src.z != z);
+}
+
+FORCEINLINE Vector3& Vector3::operator+=(const Vector3& v)
+{
+	x += v.x;
+	y += v.y;
+	z += v.z;
+	return *this;
+}
+
+FORCEINLINE Vector3& Vector3::operator-=(const Vector3& v)
+{
+	x -= v.x;
+	y -= v.y;
+	z -= v.z;
+	return *this;
+}
+
+FORCEINLINE Vector3& Vector3::operator*=(float fl)
+{
+	x *= fl;
+	y *= fl;
+	z *= fl;
+	return *this;
+}
+
+FORCEINLINE Vector3& Vector3::operator*=(const Vector3& v)
+{
+	x *= v.x;
+	y *= v.y;
+	z *= v.z;
+	return *this;
+}
+
+// this ought to be an opcode.
+FORCEINLINE Vector3& Vector3::operator+=(float fl)
+{
+	x += fl;
+	y += fl;
+	z += fl;
+	return *this;
+}
+
+FORCEINLINE Vector3& Vector3::operator-=(float fl)
+{
+	x -= fl;
+	y -= fl;
+	z -= fl;
+	return *this;
+}
+
+FORCEINLINE Vector3& Vector3::operator/=(float fl)
+{
+	float oofl = 1.0f / fl;
+	x *= oofl;
+	y *= oofl;
+	z *= oofl;
+	return *this;
+}
+
+FORCEINLINE Vector3& Vector3::operator/=(const Vector3& v)
+{
+	x /= v.x;
+	y /= v.y;
+	z /= v.z;
+	return *this;
+}
+
+inline Vector3 Vector3::operator-(void) const
+{
+	return Vector3(-x, -y, -z);
+}
+
+inline Vector3 Vector3::operator+(const Vector3& v) const
 {
-	struct
+	Vector3 res;
+	VectorAdd(*this, v, res);
+	return res;
+}
+
+inline Vector3 Vector3::operator-(const Vector3& v) const
+{
+	Vector3 res;
+	VectorSubtract(*this, v, res);
+	return res;
+}
+
+inline Vector3 Vector3::operator*(float fl) const
+{
+	Vector3 res;
+	VectorMultiply(*this, fl, res);
+	return res;
+}
+
+inline Vector3 Vector3::operator*(const Vector3& v) const
+{
+	Vector3 res;
+	VectorMultiply(*this, v, res);
+	return res;
+}
+
+inline Vector3 Vector3::operator/(float fl) const
+{
+	Vector3 res;
+	VectorDivide(*this, fl, res);
+	return res;
+}
+
+inline Vector3 Vector3::operator/(const Vector3& v) const
+{
+	Vector3 res;
+	VectorDivide(*this, v, res);
+	return res;
+}
+
+inline Vector3 operator*(float fl, const Vector3& v)
+{
+	return v * fl;
+}
+
+inline Vector3 Vector3::Cross(const Vector3& vOther) const
+{
+	return Vector3(y * vOther.z - z * vOther.y, z * vOther.x - x * vOther.z, x * vOther.y - y * vOther.x);
+}
+
+inline void Vector3::Normalize()
+{
+	float fLen = Length();
+	x /= fLen;
+	y /= fLen;
+	z /= fLen;
+}
+
+inline Vector3 StringToVector(char* pString)
+{
+	Vector3 vRet;
+
+	int length = 0;
+	while (pString[length])
 	{
-		float x;
-		float y;
-		float z;
-		float w;
-	};
+		if ((pString[length] == '<') || (pString[length] == '>'))
+			pString[length] = '\0';
+		length++;
+	}
+
+	int startOfFloat = 1;
+	int currentIndex = 1;
+
+	while (pString[currentIndex] && (pString[currentIndex] != ','))
+		currentIndex++;
+	pString[currentIndex] = '\0';
+	vRet.x = std::stof(&pString[startOfFloat]);
+	startOfFloat = ++currentIndex;
 
-	float raw[4];
+	while (pString[currentIndex] && (pString[currentIndex] != ','))
+		currentIndex++;
+	pString[currentIndex] = '\0';
+	vRet.y = std::stof(&pString[startOfFloat]);
+	startOfFloat = ++currentIndex;
+
+	while (pString[currentIndex] && (pString[currentIndex] != ','))
+		currentIndex++;
+	pString[currentIndex] = '\0';
+	vRet.z = std::stof(&pString[startOfFloat]);
+	startOfFloat = ++currentIndex;
+
+	return vRet;
+}
+
+FORCEINLINE void VectorAdd(const Vector3& a, const Vector3& b, Vector3& c)
+{
+	c.x = a.x + b.x;
+	c.y = a.y + b.y;
+	c.z = a.z + b.z;
+}
+
+FORCEINLINE void VectorSubtract(const Vector3& a, const Vector3& b, Vector3& c)
+{
+	c.x = a.x - b.x;
+	c.y = a.y - b.y;
+	c.z = a.z - b.z;
+}
+
+FORCEINLINE void VectorMultiply(const Vector3& a, float b, Vector3& c)
+{
+	c.x = a.x * b;
+	c.y = a.y * b;
+	c.z = a.z * b;
+}
+
+FORCEINLINE void VectorMultiply(const Vector3& a, const Vector3& b, Vector3& c)
+{
+	c.x = a.x * b.x;
+	c.y = a.y * b.y;
+	c.z = a.z * b.z;
+}
+
+FORCEINLINE void VectorDivide(const Vector3& a, float b, Vector3& c)
+{
+	float oob = 1.0f / b;
+	c.x = a.x * oob;
+	c.y = a.y * oob;
+	c.z = a.z * oob;
+}
+
+FORCEINLINE void VectorDivide(const Vector3& a, const Vector3& b, Vector3& c)
+{
+	c.x = a.x / b.x;
+	c.y = a.y / b.y;
+	c.z = a.z / b.z;
+}
+
+class QAngle
+{
+public:
+	float x;
+	float y;
+	float z;
+
+	QAngle(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {}
+	QAngle(float _f) : x(_f), y(_f), z(_f) {}
+	QAngle() : x(0.0f), y(0.0f), z(0.0f) {}
+
+	Vector3 GetNormal() const;
 
 	// todo: more operators maybe
 	bool operator==(const QAngle& other)
 	{
-		return x == other.x && y == other.y && z == other.z && w == other.w;
+		return x == other.x && y == other.y && z == other.z;
 	}
 };
+
+inline Vector3 QAngle::GetNormal() const
+{
+	Vector3 ret(cos(DEG2RAD(y)), sin(DEG2RAD(y)), -sin(DEG2RAD(x)));
+	return ret;
+}
diff --git a/primedev/core/math/vplane.h b/primedev/core/math/vplane.h
new file mode 100644
index 00000000..8b8de423
--- /dev/null
+++ b/primedev/core/math/vplane.h
@@ -0,0 +1,106 @@
+#pragma once
+
+typedef int SideType;
+
+// Used to represent sides of things like planes.
+#define SIDE_FRONT 0
+#define SIDE_BACK 1
+#define SIDE_ON 2
+
+#define VP_EPSILON 0.01f
+
+class VPlane
+{
+public:
+	VPlane();
+	VPlane(const Vector3& vNormal, float dist);
+	VPlane(const Vector3& vPoint, const QAngle& ang);
+
+	void Init(const Vector3& vNormal, float dist);
+
+	// Return the distance from the point to the plane.
+	float DistTo(const Vector3& vVec) const;
+
+	// Copy.
+	VPlane& operator=(const VPlane& thePlane);
+
+	// Returns SIDE_ON, SIDE_FRONT, or SIDE_BACK.
+	// The epsilon for SIDE_ON can be passed in.
+	SideType GetPointSide(const Vector3& vPoint, float sideEpsilon = VP_EPSILON) const;
+
+	// Returns SIDE_FRONT or SIDE_BACK.
+	SideType GetPointSideExact(const Vector3& vPoint) const;
+
+	// Get a point on the plane (normal*dist).
+	Vector3 GetPointOnPlane() const;
+
+	// Snap the specified point to the plane (along the plane's normal).
+	Vector3 SnapPointToPlane(const Vector3& vPoint) const;
+
+public:
+	Vector3 m_Normal;
+	float m_Dist;
+};
+
+//-----------------------------------------------------------------------------
+// Inlines.
+//-----------------------------------------------------------------------------
+inline VPlane::VPlane() {}
+
+inline VPlane::VPlane(const Vector3& vNormal, float dist)
+{
+	m_Normal = vNormal;
+	m_Dist = dist;
+}
+
+inline VPlane::VPlane(const Vector3& vPoint, const QAngle& ang)
+{
+	m_Normal = ang.GetNormal();
+	m_Dist = vPoint.x * m_Normal.x + vPoint.y * m_Normal.y + vPoint.z * m_Normal.z;
+}
+
+inline void VPlane::Init(const Vector3& vNormal, float dist)
+{
+	m_Normal = vNormal;
+	m_Dist = dist;
+}
+
+inline float VPlane::DistTo(const Vector3& vVec) const
+{
+	return vVec.Dot(m_Normal) - m_Dist;
+}
+
+inline VPlane& VPlane::operator=(const VPlane& thePlane)
+{
+	m_Normal = thePlane.m_Normal;
+	m_Dist = thePlane.m_Dist;
+	return *this;
+}
+
+inline Vector3 VPlane::GetPointOnPlane() const
+{
+	return m_Normal * m_Dist;
+}
+
+inline Vector3 VPlane::SnapPointToPlane(const Vector3& vPoint) const
+{
+	return vPoint - m_Normal * DistTo(vPoint);
+}
+
+inline SideType VPlane::GetPointSide(const Vector3& vPoint, float sideEpsilon) const
+{
+	float fDist;
+
+	fDist = DistTo(vPoint);
+	if (fDist >= sideEpsilon)
+		return SIDE_FRONT;
+	else if (fDist <= -sideEpsilon)
+		return SIDE_BACK;
+	else
+		return SIDE_ON;
+}
+
+inline SideType VPlane::GetPointSideExact(const Vector3& vPoint) const
+{
+	return DistTo(vPoint) > 0.0f ? SIDE_FRONT : SIDE_BACK;
+}