405 lines
6.9 KiB
C++
405 lines
6.9 KiB
C++
#pragma once
|
|
|
|
namespace math
|
|
{
|
|
class Vector3
|
|
{
|
|
public:
|
|
Vector3 operator-(const Vector3& other) const
|
|
{
|
|
return Vector3{ this->x - other.x, this->y - other.y, this->z - other.z };
|
|
}
|
|
|
|
Vector3 operator+(const Vector3& other) const
|
|
{
|
|
return Vector3{ this->x + other.x, this->y + other.y, this->z + other.z };
|
|
}
|
|
|
|
Vector3& operator*=(const float multiplier)
|
|
{
|
|
x *= multiplier;
|
|
y *= multiplier;
|
|
z *= multiplier;
|
|
|
|
return *this;
|
|
}
|
|
|
|
Vector3 operator*(const Vector3& other) const
|
|
{
|
|
return { x * other.x, y * other.y, z * other.z };
|
|
}
|
|
|
|
Vector3 operator*(const float& other) const
|
|
{
|
|
return { x * other, y * other, z * other };
|
|
}
|
|
|
|
[[nodiscard]] FLOAT Length() CONST {
|
|
return (x + y + z);
|
|
}
|
|
|
|
[[nodiscard]] FLOAT Length2d() CONST {
|
|
return math::sqrtf(x * x + y * y + z * z);
|
|
}
|
|
|
|
[[nodiscard]] FLOAT LengthSquare() CONST {
|
|
return (x * x + y * y + z * z);
|
|
}
|
|
|
|
[[nodiscard]] BOOL Empty() CONST {
|
|
return (Length() <= 0.0f);
|
|
}
|
|
|
|
[[nodiscard]] FLOAT DistanceTo(Vector3& vOther) CONST
|
|
{
|
|
CONST AUTO delta = (vOther - *this);
|
|
|
|
return math::fabsf(delta.LengthSquare());
|
|
}
|
|
|
|
[[nodiscard]] FLOAT DistanceTo(CONST Vector3& vOther) CONST
|
|
{
|
|
CONST AUTO delta = (vOther - *this);
|
|
|
|
return math::fabsf(delta.LengthSquare());
|
|
}
|
|
|
|
[[nodiscard]] floating_point_holder3_t<FLOAT> Data() {
|
|
return { x, y, z };
|
|
}
|
|
|
|
Vector3 Normalized();
|
|
public:
|
|
float x, y, z;
|
|
protected:
|
|
private:
|
|
};
|
|
|
|
class Angle3 {
|
|
public:
|
|
Angle3 operator-(const Angle3& other) const
|
|
{
|
|
return Angle3{ this->x - other.x, this->y - other.y, this->z - other.z };
|
|
}
|
|
|
|
Angle3 operator+(const Angle3& other) const
|
|
{
|
|
return Angle3{ this->x + other.x, this->y + other.y, this->z + other.z };
|
|
}
|
|
|
|
Angle3& operator*=(const float multiplier)
|
|
{
|
|
x *= multiplier;
|
|
y *= multiplier;
|
|
z *= multiplier;
|
|
|
|
return *this;
|
|
}
|
|
|
|
Angle3 operator*(const Angle3& other) const
|
|
{
|
|
return { x * other.x, y * other.y, z * other.z };
|
|
}
|
|
|
|
Angle3 operator*(const float& other) const
|
|
{
|
|
return { x * other, y * other, z * other };
|
|
}
|
|
|
|
[[nodiscard]] floating_point_holder3_t<FLOAT> Data() {
|
|
return { x, y, z };
|
|
}
|
|
|
|
[[nodiscard]] FLOAT Length() CONST {
|
|
return (x + y + z);
|
|
}
|
|
|
|
[[nodiscard]] FLOAT Length2d() CONST {
|
|
return math::sqrtf(x * x + y * y + z * z);
|
|
}
|
|
|
|
[[nodiscard]] FLOAT LengthSquare() CONST {
|
|
return (x * x + y * y + z * z);
|
|
}
|
|
|
|
[[nodiscard]] BOOL Empty() CONST {
|
|
return (Length() <= 0.0f);
|
|
}
|
|
public:
|
|
float x, y, z;
|
|
protected:
|
|
private:
|
|
};
|
|
|
|
class Vector4 : public Vector3
|
|
{
|
|
public:
|
|
public:
|
|
float w;
|
|
protected:
|
|
private:
|
|
};
|
|
|
|
struct matrix3x4
|
|
{
|
|
float* operator[](int i)
|
|
{
|
|
return m_value[i];
|
|
}
|
|
|
|
const float* operator[](int i) const
|
|
{
|
|
return m_value[i];
|
|
}
|
|
|
|
float* Base()
|
|
{
|
|
return &m_value[0][0];
|
|
}
|
|
|
|
const float* Base() const
|
|
{
|
|
return &m_value[0][0];
|
|
}
|
|
|
|
float m_value[3][4];
|
|
};
|
|
|
|
struct matrix4x4
|
|
{
|
|
float* operator[](int i)
|
|
{
|
|
return m_value[i];
|
|
}
|
|
|
|
const float* operator[](int i) const
|
|
{
|
|
return m_value[i];
|
|
}
|
|
|
|
float* Base()
|
|
{
|
|
return &m_value[0][0];
|
|
}
|
|
|
|
const float* Base() const
|
|
{
|
|
return &m_value[0][0];
|
|
}
|
|
|
|
float m_value[4][4];
|
|
};
|
|
|
|
struct __declspec(align(16)) matrix3x4a : matrix3x4
|
|
{
|
|
};
|
|
|
|
inline constexpr float deg2rad(const float v)
|
|
{
|
|
return (v * static_cast<float>(M_PI)) / 180.0f;
|
|
}
|
|
|
|
inline constexpr float rad2deg(const float v)
|
|
{
|
|
return (v * 180.0f) / static_cast<float>(M_PI);
|
|
}
|
|
|
|
inline void sin_cos(float radians, float* sine, float* cosine)
|
|
{
|
|
*sine = sinf(radians);
|
|
*cosine = cosf(radians);
|
|
}
|
|
|
|
inline void VectorAngles(const math::Vector3& forward, math::Angle3& angles)
|
|
{
|
|
float temp, yaw, pitch;
|
|
|
|
if (forward.y == 0.0f && forward.x == 0.0f)
|
|
{
|
|
yaw = 0.0f;
|
|
|
|
if (forward.z > 0.0f)
|
|
{
|
|
pitch = 270.0f;
|
|
}
|
|
else
|
|
{
|
|
pitch = 90.0f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
yaw = math::atan2f(forward.y, forward.x) * 180.0f / static_cast<float>(M_PI);
|
|
|
|
if (yaw < 0.0f)
|
|
{
|
|
yaw += 360.0f;
|
|
}
|
|
|
|
temp = math::sqrtf(forward.x * forward.x + forward.y * forward.y);
|
|
pitch = math::atan2f(-forward.z, temp) * 180.0f / static_cast<float>(M_PI);
|
|
|
|
if (pitch < 0.0f)
|
|
{
|
|
pitch += 360.0f;
|
|
}
|
|
|
|
angles.x = pitch;
|
|
angles.y = yaw;
|
|
angles.z = 0.0f;
|
|
}
|
|
}
|
|
|
|
inline void AngleVectors(const math::Angle3& in, math::Vector3& out)
|
|
{
|
|
float sp, sy, cp, cy;
|
|
|
|
sin_cos(deg2rad(in.y), &sy, &cy);
|
|
sin_cos(deg2rad(in.x), &sp, &cp);
|
|
|
|
out.x = cp * cy;
|
|
out.y = cp * sy;
|
|
out.z = -sp;
|
|
}
|
|
|
|
inline void ClampAngle(Vector3& angle)
|
|
{
|
|
angle.x = stl::clamp(angle.x, -89.0f, +89.0f);
|
|
angle.y = stl::clamp(angle.y, -180.0f, +180.0f);
|
|
angle.z = stl::clamp(angle.z, 0.0f, 0.0f);
|
|
}
|
|
|
|
inline void ClampAngle(Angle3& angle)
|
|
{
|
|
angle.x = stl::clamp(angle.x, -89.0f, +89.0f);
|
|
angle.y = stl::clamp(angle.y, -180.0f, +180.0f);
|
|
angle.z = stl::clamp(angle.z, 0.0f, 0.0f);
|
|
}
|
|
|
|
inline void NormalizeVector(math::Vector3& in)
|
|
{
|
|
if (in.x > 89.0f)
|
|
{
|
|
in.x -= 360.0f;
|
|
}
|
|
|
|
if (in.x < -89.0f)
|
|
{
|
|
in.x += 360.0f;
|
|
}
|
|
|
|
while (in.y > 180.0f)
|
|
{
|
|
in.y -= 360.0f;
|
|
}
|
|
|
|
while (in.y < -180.0f)
|
|
{
|
|
in.y += 360.0f;
|
|
}
|
|
}
|
|
|
|
inline void NormalizeAngle(math::Angle3& angle)
|
|
{
|
|
if (angle.x > 89.0f)
|
|
{
|
|
angle.x -= 360.0f;
|
|
}
|
|
|
|
if (angle.x < -89.0f)
|
|
{
|
|
angle.x += 360.0f;
|
|
}
|
|
|
|
while (angle.y > 180.0f)
|
|
{
|
|
angle.y -= 360.0f;
|
|
}
|
|
|
|
while (angle.y < -180.0f)
|
|
{
|
|
angle.y += 360.0f;
|
|
}
|
|
}
|
|
constexpr float DotProduct(const math::Vector3& first, const math::Vector3& second)
|
|
{
|
|
return ((first.x * second.x) + (first.y * second.y) + (first.z * second.z));
|
|
}
|
|
|
|
inline Vector3 Vector3::Normalized()
|
|
{
|
|
auto temp = *this;
|
|
|
|
NormalizeVector(temp);
|
|
|
|
return temp;
|
|
}
|
|
|
|
namespace SE {
|
|
FORCEINLINE FLOAT VectorNormalize(Vector3& vec) {
|
|
FLOAT radius = math::sqrtf(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
|
|
FLOAT iradius = 1.f / (radius + FLT_EPSILON);
|
|
|
|
vec.x *= iradius;
|
|
vec.y *= iradius;
|
|
vec.z *= iradius;
|
|
|
|
return radius;
|
|
}
|
|
|
|
FORCEINLINE FLOAT AngleNormalize(FLOAT angle)
|
|
{
|
|
angle = std::fmodf(angle, 360.0f);
|
|
|
|
if (angle > 180)
|
|
angle -= 360;
|
|
|
|
if (angle < -180)
|
|
angle += 360;
|
|
|
|
return angle;
|
|
}
|
|
|
|
FORCEINLINE FLOAT AngleNormalizePositive(FLOAT angle)
|
|
{
|
|
angle = std::fmodf(angle, 360.0f);
|
|
|
|
if (angle < 0.0f)
|
|
angle += 360.0f;
|
|
|
|
return angle;
|
|
}
|
|
|
|
FORCEINLINE VOID AngleNormalize(Angle3& vec) {
|
|
vec.x = stl::clamp(vec.x, -89.f, +89.f);
|
|
vec.y = remainderf(vec.y, 360.f);
|
|
vec.z = 0.f;
|
|
}
|
|
|
|
__forceinline auto get_fov_difference(Angle3 angle, Vector3 start, Vector3 end)
|
|
{
|
|
Angle3 aim_angle{};
|
|
Vector3 view_position{}, aim_position{};
|
|
|
|
const Vector3 delta = end - start;
|
|
|
|
VectorAngles(delta, aim_angle);
|
|
|
|
AngleVectors(angle, view_position);
|
|
AngleVectors(aim_angle, aim_position);
|
|
|
|
const float aimplsq = aim_position.Length2d();
|
|
const auto dotp = static_cast<float>(DotProduct(aim_position, view_position) / aimplsq);
|
|
|
|
const float vcos = math::acos(dotp);
|
|
const float drad = math::rad2deg(vcos);
|
|
|
|
if (!math::isnan(vcos))
|
|
return drad;
|
|
|
|
return 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|