227 lines
9.7 KiB
C++
227 lines
9.7 KiB
C++
// hmap/hmath.h - tiny dependency-free math types used by libhmap.
|
|
// Conventions: right-handed, +Y up, forward = -Z, column vectors (p' = M * p + t).
|
|
#pragma once
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
|
|
namespace hmap {
|
|
|
|
constexpr float kPi = 3.14159265358979323846f;
|
|
constexpr float kDeg2Rad = kPi / 180.0f;
|
|
constexpr float kRad2Deg = 180.0f / kPi;
|
|
constexpr float kPlaneEps = 0.01f; // world-unit tolerance used by plane classification
|
|
|
|
struct Vec2 {
|
|
float x = 0, y = 0;
|
|
constexpr Vec2() = default;
|
|
constexpr Vec2(float x_, float y_) : x(x_), y(y_) {}
|
|
Vec2 operator+(const Vec2& o) const { return {x + o.x, y + o.y}; }
|
|
Vec2 operator-(const Vec2& o) const { return {x - o.x, y - o.y}; }
|
|
Vec2 operator*(float s) const { return {x * s, y * s}; }
|
|
};
|
|
|
|
struct Vec3 {
|
|
float x = 0, y = 0, z = 0;
|
|
constexpr Vec3() = default;
|
|
constexpr Vec3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {}
|
|
Vec3 operator+(const Vec3& o) const { return {x + o.x, y + o.y, z + o.z}; }
|
|
Vec3 operator-(const Vec3& o) const { return {x - o.x, y - o.y, z - o.z}; }
|
|
Vec3 operator*(float s) const { return {x * s, y * s, z * s}; }
|
|
Vec3 operator/(float s) const { return {x / s, y / s, z / s}; }
|
|
Vec3 operator-() const { return {-x, -y, -z}; }
|
|
Vec3& operator+=(const Vec3& o) { x += o.x; y += o.y; z += o.z; return *this; }
|
|
Vec3& operator-=(const Vec3& o) { x -= o.x; y -= o.y; z -= o.z; return *this; }
|
|
Vec3& operator*=(float s) { x *= s; y *= s; z *= s; return *this; }
|
|
float operator[](int i) const { return i == 0 ? x : (i == 1 ? y : z); }
|
|
float& operator[](int i) { return i == 0 ? x : (i == 1 ? y : z); }
|
|
bool operator==(const Vec3& o) const { return x == o.x && y == o.y && z == o.z; }
|
|
bool operator!=(const Vec3& o) const { return !(*this == o); }
|
|
};
|
|
|
|
inline Vec3 operator*(float s, const Vec3& v) { return v * s; }
|
|
inline float dot(const Vec3& a, const Vec3& b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
|
|
inline Vec3 cross(const Vec3& a, const Vec3& b) {
|
|
return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
|
|
}
|
|
inline float lengthSq(const Vec3& v) { return dot(v, v); }
|
|
inline float length(const Vec3& v) { return std::sqrt(dot(v, v)); }
|
|
inline Vec3 normalize(const Vec3& v) {
|
|
float l = length(v);
|
|
return l > 1e-12f ? v / l : Vec3(0, 0, 0);
|
|
}
|
|
inline Vec3 vmin(const Vec3& a, const Vec3& b) { return {std::min(a.x, b.x), std::min(a.y, b.y), std::min(a.z, b.z)}; }
|
|
inline Vec3 vmax(const Vec3& a, const Vec3& b) { return {std::max(a.x, b.x), std::max(a.y, b.y), std::max(a.z, b.z)}; }
|
|
inline Vec3 lerp(const Vec3& a, const Vec3& b, float t) { return a + (b - a) * t; }
|
|
inline float clampf(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }
|
|
|
|
// Plane: dot(n, p) == d. Solids are on the BACK side (dot(n,p) < d); n points out of the solid.
|
|
struct Plane {
|
|
Vec3 n{0, 1, 0};
|
|
float d = 0;
|
|
float dist(const Vec3& p) const { return dot(n, p) - d; }
|
|
static Plane fromPoints(const Vec3& a, const Vec3& b, const Vec3& c) {
|
|
Vec3 n = normalize(cross(b - a, c - a));
|
|
return Plane{n, dot(n, a)};
|
|
}
|
|
static Plane fromNormalPoint(const Vec3& n, const Vec3& p) {
|
|
Vec3 nn = normalize(n);
|
|
return Plane{nn, dot(nn, p)};
|
|
}
|
|
};
|
|
|
|
struct AABB {
|
|
Vec3 mn{1e30f, 1e30f, 1e30f}, mx{-1e30f, -1e30f, -1e30f};
|
|
bool empty() const { return mn.x > mx.x; }
|
|
void add(const Vec3& p) { mn = vmin(mn, p); mx = vmax(mx, p); }
|
|
void add(const AABB& b) { if (!b.empty()) { add(b.mn); add(b.mx); } }
|
|
Vec3 center() const { return (mn + mx) * 0.5f; }
|
|
Vec3 size() const { return mx - mn; }
|
|
bool overlaps(const AABB& o, float eps = 0) const {
|
|
return !(o.mn.x > mx.x + eps || o.mx.x < mn.x - eps || o.mn.y > mx.y + eps ||
|
|
o.mx.y < mn.y - eps || o.mn.z > mx.z + eps || o.mx.z < mn.z - eps);
|
|
}
|
|
bool contains(const Vec3& p) const {
|
|
return p.x >= mn.x && p.x <= mx.x && p.y >= mn.y && p.y <= mx.y && p.z >= mn.z && p.z <= mx.z;
|
|
}
|
|
AABB expanded(float e) const { AABB r = *this; r.mn = mn - Vec3(e, e, e); r.mx = mx + Vec3(e, e, e); return r; }
|
|
};
|
|
|
|
// Slab test. Returns true when the ray hits; tNear may be negative when the origin is inside.
|
|
inline bool rayAABB(const Vec3& o, const Vec3& d, const AABB& b, float& tNear, float& tFar) {
|
|
float t0 = -1e30f, t1 = 1e30f;
|
|
for (int i = 0; i < 3; ++i) {
|
|
if (std::fabs(d[i]) < 1e-9f) {
|
|
if (o[i] < b.mn[i] || o[i] > b.mx[i]) return false;
|
|
} else {
|
|
float inv = 1.0f / d[i];
|
|
float a = (b.mn[i] - o[i]) * inv, c = (b.mx[i] - o[i]) * inv;
|
|
if (a > c) std::swap(a, c);
|
|
t0 = std::max(t0, a);
|
|
t1 = std::min(t1, c);
|
|
if (t0 > t1) return false;
|
|
}
|
|
}
|
|
tNear = t0;
|
|
tFar = t1;
|
|
return t1 >= 0;
|
|
}
|
|
|
|
// 3x4 affine matrix.
|
|
struct Mat34 {
|
|
float m[3][4] = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}};
|
|
|
|
static Mat34 identity() { return Mat34(); }
|
|
static Mat34 translate(const Vec3& t) {
|
|
Mat34 r;
|
|
r.m[0][3] = t.x; r.m[1][3] = t.y; r.m[2][3] = t.z;
|
|
return r;
|
|
}
|
|
static Mat34 scale(const Vec3& s) {
|
|
Mat34 r;
|
|
r.m[0][0] = s.x; r.m[1][1] = s.y; r.m[2][2] = s.z;
|
|
return r;
|
|
}
|
|
static Mat34 rotate(const Vec3& axis, float deg) { // Rodrigues, right-handed
|
|
Vec3 a = normalize(axis);
|
|
float c = std::cos(deg * kDeg2Rad), s = std::sin(deg * kDeg2Rad), t = 1 - c;
|
|
Mat34 r;
|
|
r.m[0][0] = t * a.x * a.x + c; r.m[0][1] = t * a.x * a.y - s * a.z; r.m[0][2] = t * a.x * a.z + s * a.y;
|
|
r.m[1][0] = t * a.x * a.y + s * a.z; r.m[1][1] = t * a.y * a.y + c; r.m[1][2] = t * a.y * a.z - s * a.x;
|
|
r.m[2][0] = t * a.x * a.z - s * a.y; r.m[2][1] = t * a.y * a.z + s * a.x; r.m[2][2] = t * a.z * a.z + c;
|
|
return r;
|
|
}
|
|
// angles = (pitch, yaw, roll) degrees. R = Ry(yaw) * Rx(pitch) * Rz(roll). Forward (-Z) with
|
|
// yaw 0 / pitch 0 stays -Z; +pitch looks up; +yaw turns left (counter-clockwise from above).
|
|
static Mat34 fromEuler(const Vec3& a) {
|
|
return rotate({0, 1, 0}, a.y) * rotate({1, 0, 0}, a.x) * rotate({0, 0, 1}, a.z);
|
|
}
|
|
Vec3 toEuler() const { // inverse of fromEuler
|
|
float sp = -m[1][2];
|
|
float pitch = std::asin(clampf(sp, -1.0f, 1.0f));
|
|
float yaw, roll;
|
|
if (std::fabs(sp) < 0.99999f) {
|
|
yaw = std::atan2(m[0][2], m[2][2]);
|
|
roll = std::atan2(m[1][0], m[1][1]);
|
|
} else {
|
|
yaw = std::atan2(-m[2][0], m[0][0]);
|
|
roll = 0;
|
|
}
|
|
return {pitch * kRad2Deg, yaw * kRad2Deg, roll * kRad2Deg};
|
|
}
|
|
// Euler angles for the FBX default XYZ order (R = Rz * Ry * Rx), degrees.
|
|
Vec3 toEulerXYZ() const {
|
|
float sb = -m[2][0];
|
|
float b = std::asin(clampf(sb, -1.0f, 1.0f));
|
|
float a, c;
|
|
if (std::fabs(sb) < 0.99999f) {
|
|
a = std::atan2(m[2][1], m[2][2]);
|
|
c = std::atan2(m[1][0], m[0][0]);
|
|
} else {
|
|
a = std::atan2(-m[1][2], m[1][1]);
|
|
c = 0;
|
|
}
|
|
return {a * kRad2Deg, b * kRad2Deg, c * kRad2Deg};
|
|
}
|
|
Vec3 point(const Vec3& p) const {
|
|
return {m[0][0] * p.x + m[0][1] * p.y + m[0][2] * p.z + m[0][3],
|
|
m[1][0] * p.x + m[1][1] * p.y + m[1][2] * p.z + m[1][3],
|
|
m[2][0] * p.x + m[2][1] * p.y + m[2][2] * p.z + m[2][3]};
|
|
}
|
|
Vec3 vector(const Vec3& v) const {
|
|
return {m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z,
|
|
m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z,
|
|
m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z};
|
|
}
|
|
Vec3 translation() const { return {m[0][3], m[1][3], m[2][3]}; }
|
|
// this * b : applies b first, then this.
|
|
Mat34 operator*(const Mat34& b) const {
|
|
Mat34 r;
|
|
for (int i = 0; i < 3; ++i) {
|
|
for (int j = 0; j < 3; ++j)
|
|
r.m[i][j] = m[i][0] * b.m[0][j] + m[i][1] * b.m[1][j] + m[i][2] * b.m[2][j];
|
|
r.m[i][3] = m[i][0] * b.m[0][3] + m[i][1] * b.m[1][3] + m[i][2] * b.m[2][3] + m[i][3];
|
|
}
|
|
return r;
|
|
}
|
|
float det3() const {
|
|
return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
|
|
m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) +
|
|
m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
|
|
}
|
|
bool inverse(Mat34& out) const {
|
|
float det = det3();
|
|
if (std::fabs(det) < 1e-12f) return false;
|
|
float id = 1.0f / det;
|
|
Mat34 r;
|
|
r.m[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) * id;
|
|
r.m[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * id;
|
|
r.m[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * id;
|
|
r.m[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * id;
|
|
r.m[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * id;
|
|
r.m[1][2] = (m[0][2] * m[1][0] - m[0][0] * m[1][2]) * id;
|
|
r.m[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) * id;
|
|
r.m[2][1] = (m[0][1] * m[2][0] - m[0][0] * m[2][1]) * id;
|
|
r.m[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * id;
|
|
Vec3 t = translation();
|
|
Vec3 it = r.vector(t);
|
|
r.m[0][3] = -it.x; r.m[1][3] = -it.y; r.m[2][3] = -it.z;
|
|
out = r;
|
|
return true;
|
|
}
|
|
// Transforms a plane by this affine map (normal uses the inverse transpose).
|
|
Plane transformPlane(const Plane& pl) const {
|
|
Mat34 inv;
|
|
if (!inverse(inv)) return pl;
|
|
Vec3 n(inv.m[0][0] * pl.n.x + inv.m[1][0] * pl.n.y + inv.m[2][0] * pl.n.z,
|
|
inv.m[0][1] * pl.n.x + inv.m[1][1] * pl.n.y + inv.m[2][1] * pl.n.z,
|
|
inv.m[0][2] * pl.n.x + inv.m[1][2] * pl.n.y + inv.m[2][2] * pl.n.z);
|
|
float d = pl.d + dot(n, translation());
|
|
float l = length(n);
|
|
if (l < 1e-12f) return pl;
|
|
return Plane{n / l, d / l};
|
|
}
|
|
};
|
|
|
|
} // namespace hmap
|