squad-proto/engine/math.h

#pragma once
// -----------------------------------------------------------------------------
// Базовая математика прототипа. Только декартов мир (top-down), никакой изометрии.
// Header-only: всё inline, никаких аллокаций.
// -----------------------------------------------------------------------------
#include <cmath>
#include <algorithm>

constexpr float PI_F     = 3.14159265358979323846f;
constexpr float TWO_PI_F = 6.28318530717958647692f;
constexpr float DEG2RAD_F = PI_F / 180.0f;

struct Vec2
{
    float x = 0.0f;
    float y = 0.0f;
};

inline Vec2 operator+(Vec2 a, Vec2 b) { return {a.x + b.x, a.y + b.y}; }
inline Vec2 operator-(Vec2 a, Vec2 b) { return {a.x - b.x, a.y - b.y}; }
inline Vec2 operator*(Vec2 a, float s) { return {a.x * s, a.y * s}; }
inline Vec2 operator*(float s, Vec2 a) { return {a.x * s, a.y * s}; }
inline Vec2 operator/(Vec2 a, float s) { return {a.x / s, a.y / s}; }
inline Vec2 operator-(Vec2 a) { return {-a.x, -a.y}; }
inline Vec2& operator+=(Vec2& a, Vec2 b) { a.x += b.x; a.y += b.y; return a; }
inline Vec2& operator-=(Vec2& a, Vec2 b) { a.x -= b.x; a.y -= b.y; return a; }
inline Vec2& operator*=(Vec2& a, float s) { a.x *= s; a.y *= s; return a; }

inline float Dot(Vec2 a, Vec2 b)   { return a.x * b.x + a.y * b.y; }
inline float Cross(Vec2 a, Vec2 b) { return a.x * b.y - a.y * b.x; }
inline float LengthSq(Vec2 a)      { return a.x * a.x + a.y * a.y; }
inline float Length(Vec2 a)        { return std::sqrt(LengthSq(a)); }
inline float DistanceSq(Vec2 a, Vec2 b) { return LengthSq(b - a); }
inline float Distance(Vec2 a, Vec2 b)   { return Length(b - a); }

// Перпендикуляр (поворот на +90°).
inline Vec2 Perp(Vec2 a) { return {-a.y, a.x}; }

inline Vec2 Normalized(Vec2 a, Vec2 fallback = {0.0f, 0.0f})
{
    const float len = Length(a);
    return (len > 1e-6f) ? Vec2{a.x / len, a.y / len} : fallback;
}

// Ограничить длину вектора сверху.
inline Vec2 ClampLength(Vec2 a, float maxLen)
{
    const float len = Length(a);
    if (len > maxLen && len > 1e-6f) return a * (maxLen / len);
    return a;
}

inline float Clampf(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }
inline float Lerpf(float a, float b, float t)    { return a + (b - a) * t; }
inline Vec2  Lerp(Vec2 a, Vec2 b, float t)       { return {Lerpf(a.x, b.x, t), Lerpf(a.y, b.y, t)}; }

// --- углы -------------------------------------------------------------------

inline float AngleOf(Vec2 v) { return std::atan2(v.y, v.x); }
inline Vec2  FromAngle(float a) { return {std::cos(a), std::sin(a)}; }

// Приводит угол к (-pi, pi].
inline float WrapPi(float a)
{
    while (a > PI_F)  a -= TWO_PI_F;
    while (a <= -PI_F) a += TWO_PI_F;
    return a;
}

// Кратчайшая разница углов to-from в (-pi, pi].
inline float AngleDiff(float from, float to) { return WrapPi(to - from); }

inline float RotateToward(float cur, float target, float maxDelta)
{
    const float d = AngleDiff(cur, target);
    if (std::fabs(d) <= maxDelta) return WrapPi(target);
    return WrapPi(cur + (d > 0.0f ? maxDelta : -maxDelta));
}

// --- сегменты ---------------------------------------------------------------

struct Segment
{
    Vec2 a;
    Vec2 b;
};

// Ближайшая точка отрезка ab к p; outT (если нужен) — параметр вдоль отрезка [0..1].
inline Vec2 ClosestPointOnSegment(Vec2 p, Vec2 a, Vec2 b, float* outT = nullptr)
{
    const Vec2  ab   = b - a;
    const float lenSq = LengthSq(ab);
    float t = 0.0f;
    if (lenSq > 1e-12f) t = Clampf(Dot(p - a, ab) / lenSq, 0.0f, 1.0f);
    if (outT) *outT = t;
    return a + ab * t;
}

inline float DistancePointToSegment(Vec2 p, Vec2 a, Vec2 b, float* outT = nullptr)
{
    return Distance(p, ClosestPointOnSegment(p, a, b, outT));
}

inline float DistancePointToSegment(Vec2 p, const Segment& s, float* outT = nullptr)
{
    return DistancePointToSegment(p, s.a, s.b, outT);
}