#pragma once
#include <rtm/math.h>
#include <rtm/matrix3x4f.h>
#include <rtm/matrix4x4f.h>
#include <rtm/quatf.h>
#include <rtm/vector4f.h>
#include "core/globals.h"
constexpr double PI = 3.14159265358979323846;
using vec_work = rtm::vector4f;
inline f32 radians(f32 deg) {
return deg * 0.01745329251994329577f; // π / 180
}
inline f32 degrees(f32 rad) {
return rad * 57.295779513082320876f; // 180 / π
}
struct vec2 {
f32 x, y;
//
// constructors
//
constexpr vec2() : x(0), y(0) {
}
constexpr explicit vec2(f32 s) : x(s), y(s) {
}
constexpr vec2(f32 x_, f32 y_) : x(x_), y(y_) {
}
//
// SIMD bridge
//
static inline vec2 from_vec4(const vec_work &v) {
vec2 result;
rtm::vector_store2(v, &result.x);
return result;
}
inline vec_work to_vec4() const {
return rtm::vector_load2(&x);
}
//
// operators
//
inline vec2 operator+(const vec2 &rhs) const {
return from_vec4(rtm::vector_add(to_vec4(), rhs.to_vec4()));
}
inline vec2 operator-(const vec2 &rhs) const {
return from_vec4(rtm::vector_sub(to_vec4(), rhs.to_vec4()));
}
inline vec2 operator-() const {
return from_vec4(rtm::vector_neg(to_vec4()));
}
inline vec2 operator*(f32 s) const {
return from_vec4(rtm::vector_mul(to_vec4(), s));
}
inline vec2 operator/(f32 s) const {
return from_vec4(rtm::vector_div(to_vec4(), rtm::vector_set(s)));
}
inline vec2 &operator+=(const vec2 &rhs) {
*this = *this + rhs;
return *this;
}
inline vec2 &operator-=(const vec2 &rhs) {
*this = *this - rhs;
return *this;
}
inline vec2 &operator*=(f32 s) {
*this = *this * s;
return *this;
}
inline vec2 &operator/=(f32 s) {
*this = *this / s;
return *this;
}
//
// methods
//
inline f32 dot(const vec2 &rhs) const {
return rtm::scalar_cast(rtm::vector_dot2_as_scalar(to_vec4(), rhs.to_vec4()));
}
inline f32 length() const {
vec_work vv = to_vec4();
rtm::scalarf len_sq = rtm::vector_dot2_as_scalar(vv, vv);
return rtm::scalar_cast(rtm::scalar_sqrt(len_sq));
}
inline f32 length_squared() const {
return rtm::scalar_cast(rtm::vector_dot2_as_scalar(to_vec4(), to_vec4()));
}
inline vec2 normalized() const {
return from_vec4(rtm::vector_normalize2(to_vec4()));
}
};
inline vec2 operator*(f32 s, const vec2 &v) {
return v * s;
}
struct vec3 {
f32 x, y, z;
//
// constructors
//
constexpr vec3() : x(0), y(0), z(0) {
}
constexpr explicit vec3(f32 s) : x(s), y(s), z(s) {
}
constexpr vec3(f32 x_, f32 y_, f32 z_) : x(x_), y(y_), z(z_) {
}
//
// SIMD bridge
//
static inline vec3 from_vec4(const vec_work &v) {
vec3 result;
rtm::vector_store3(v, &result.x);
return result;
}
inline vec_work to_vec4() const {
return rtm::vector_load3(&x);
}
//
// operators
//
inline vec3 operator+(const vec3 &rhs) const {
return from_vec4(rtm::vector_add(to_vec4(), rhs.to_vec4()));
}
inline vec3 operator-(const vec3 &rhs) const {
return from_vec4(rtm::vector_sub(to_vec4(), rhs.to_vec4()));
}
inline vec3 operator-() const {
return from_vec4(rtm::vector_neg(to_vec4()));
}
inline vec3 operator*(f32 s) const {
return from_vec4(rtm::vector_mul(to_vec4(), s));
}
inline vec3 operator-(f32 s) const {
return from_vec4(rtm::vector_sub(to_vec4(), rtm::vector_set(s)));
}
inline vec3 operator/(f32 s) const {
return from_vec4(rtm::vector_div(to_vec4(), rtm::vector_set(s)));
}
inline vec3 &operator+=(const vec3 &rhs) {
*this = *this + rhs;
return *this;
}
inline vec3 &operator-=(const vec3 &rhs) {
*this = *this - rhs;
return *this;
}
inline vec3 &operator*=(f32 s) {
*this = *this * s;
return *this;
}
inline vec3 &operator/=(f32 s) {
*this = *this / s;
return *this;
}
//
// methods
//
inline f32 dot(const vec3 &rhs) const {
return rtm::scalar_cast(rtm::vector_dot3_as_scalar(to_vec4(), rhs.to_vec4()));
}
inline f32 length() const {
vec_work vv = to_vec4();
rtm::scalarf len_sq = rtm::vector_dot3_as_scalar(vv, vv);
return rtm::scalar_cast(rtm::scalar_sqrt(len_sq));
}
inline f32 length_squared() const {
return rtm::scalar_cast(rtm::vector_dot3_as_scalar(to_vec4(), to_vec4()));
}
inline vec3 normalized() const {
return from_vec4(rtm::vector_normalize3(to_vec4()));
}
};
inline vec3 operator*(f32 s, const vec3 &v) {
return v * s;
}
inline vec3 cross(const vec3 &a, const vec3 &b) {
return vec3::from_vec4(rtm::vector_cross3(a.to_vec4(), b.to_vec4()));
}
struct vec4 {
f32 x, y, z, w;
//
// constructors
//
constexpr vec4() : x(0), y(0), z(0), w(0) {
}
constexpr explicit vec4(f32 s) : x(s), y(s), z(s), w(s) {
}
constexpr vec4(f32 x_, f32 y_, f32 z_, f32 w_) : x(x_), y(y_), z(z_), w(w_) {
}
//
// SIMD bridge
//
static inline vec4 from_vec4(const vec_work &v) {
vec4 result;
rtm::vector_store(v, &result.x);
return result;
}
inline vec_work to_vec4() const {
return rtm::vector_load(&x);
}
//
// operators
//
inline vec4 operator+(const vec4 &rhs) const {
return from_vec4(rtm::vector_add(to_vec4(), rhs.to_vec4()));
}
inline vec4 operator-(const vec4 &rhs) const {
return from_vec4(rtm::vector_sub(to_vec4(), rhs.to_vec4()));
}
inline vec4 operator-() const {
return from_vec4(rtm::vector_neg(to_vec4()));
}
inline vec4 operator*(f32 s) const {
return from_vec4(rtm::vector_mul(to_vec4(), s));
}
inline vec4 operator/(f32 s) const {
return from_vec4(rtm::vector_div(to_vec4(), rtm::vector_set(s)));
}
inline vec4 &operator+=(const vec4 &rhs) {
*this = *this + rhs;
return *this;
}
inline vec4 &operator-=(const vec4 &rhs) {
*this = *this - rhs;
return *this;
}
inline vec4 &operator*=(f32 s) {
*this = *this * s;
return *this;
}
inline vec4 &operator/=(f32 s) {
*this = *this / s;
return *this;
}
//
// methods
//
inline f32 dot(const vec4 &rhs) const {
return rtm::scalar_cast(rtm::vector_dot_as_scalar(to_vec4(), rhs.to_vec4()));
}
inline f32 length() const {
vec_work vv = to_vec4();
rtm::scalarf len_sq = rtm::vector_dot_as_scalar(vv, vv);
return rtm::scalar_cast(rtm::scalar_sqrt(len_sq));
}
inline f32 length_squared() const {
return rtm::scalar_cast(rtm::vector_dot_as_scalar(to_vec4(), to_vec4()));
}
inline vec4 normalized() const {
return from_vec4(rtm::vector_normalize(to_vec4()));
}
};
inline vec4 operator*(f32 s, const vec4 &v) {
return v * s;
}
struct mat3 {
f32 m[9]; // column-major: col * 3 + row
mat3() = default;
explicit mat3(f32 diag) {
m[0] = diag;
m[3] = 0;
m[6] = 0;
m[1] = 0;
m[4] = diag;
m[7] = 0;
m[2] = 0;
m[5] = 0;
m[8] = diag;
}
};
struct mat4 {
f32 m[16];
//
// column access
//
inline f32 *operator[](int col) {
return &m[col * 4];
}
inline const f32 *operator[](int col) const {
return &m[col * 4];
}
//
// SIMD bridge
//
static inline mat4 from_matrix(const rtm::matrix4x4f &mat) {
mat4 result;
rtm::vector_store(mat.x_axis, &result.m[0]);
rtm::vector_store(mat.y_axis, &result.m[4]);
rtm::vector_store(mat.z_axis, &result.m[8]);
rtm::vector_store(mat.w_axis, &result.m[12]);
return result;
}
inline rtm::matrix4x4f to_matrix() const {
return rtm::matrix4x4f{
rtm::vector_load(&m[0]), rtm::vector_load(&m[4]), rtm::vector_load(&m[8]), rtm::vector_load(&m[12])
};
}
};
inline mat4 identity() {
return mat4::from_matrix(rtm::matrix_identity());
}
inline mat4 translate(vec3 t) {
rtm::matrix3x4f m34 = rtm::matrix_from_translation(rtm::vector_set(t.x, t.y, t.z));
return mat4::from_matrix(rtm::matrix_cast(m34));
}
inline mat4 scale(vec3 s) {
rtm::matrix3x4f m34 = rtm::matrix_from_scale(rtm::vector_set(s.x, s.y, s.z));
return mat4::from_matrix(rtm::matrix_cast(m34));
}
mat4 rotate(vec3 axis, f32 angle_rad);
mat4 inverse(mat4 m);
// w 1.0
vec4 transform_point(const vec3 &v, const mat4 &m);
// w 0.0
vec3 transform_direction(const vec3 &v, const mat4 &m);
inline vec4 operator*(const mat4 &m, const vec4 &v) {
return vec4::from_vec4(rtm::matrix_mul_vector(v.to_vec4(), m.to_matrix()));
}
inline mat4 operator*(const mat4 &a, const mat4 &b) {
return mat4::from_matrix(rtm::matrix_mul(a.to_matrix(), b.to_matrix()));
}
inline mat4 transpose(const mat4 &m) {
return mat4::from_matrix(rtm::matrix_transpose(m.to_matrix()));
}
inline mat3 m3_from_mat4(const mat4 &m) {
mat3 r;
// column 0
r.m[0] = m.m[0];
r.m[1] = m.m[1];
r.m[2] = m.m[2];
// column 1
r.m[3] = m.m[4];
r.m[4] = m.m[5];
r.m[5] = m.m[6];
// column 2
r.m[6] = m.m[8];
r.m[7] = m.m[9];
r.m[8] = m.m[10];
return r;
}
inline vec3 operator*(const mat3 &m, const vec3 &v) {
return {
m.m[0] * v.x + m.m[3] * v.y + m.m[6] * v.z,
m.m[1] * v.x + m.m[4] * v.y + m.m[7] * v.z,
m.m[2] * v.x + m.m[5] * v.y + m.m[8] * v.z
};
}
mat4 ortho(f32 width, f32 height, f32 near_plane, f32 far_plane);
mat4 look_at(vec3 eye, vec3 center, vec3 up);
mat4 perspective(
f32 fovy,
f32 aspect,
f32 world_near_plane,
f32 world_far_plane = 0.0f,
f32 clip_near_z = 1.0f,
f32 clip_far_z = 0.0f
);
// [0, 1] helper
static inline mat4 perspective_standard_z(f32 fovy, f32 aspect, f32 near_plane, f32 far_plane) {
return perspective(fovy, aspect, near_plane, far_plane, 0.0f, 1.0f);
}
mat4 ortho_offcenter(f32 left, f32 right, f32 bottom, f32 top, f32 near_plane, f32 far_plane);
static_assert(sizeof(vec2) == 8, "vec2 must be 8 bytes");
static_assert(sizeof(vec3) == 12, "vec3 must be 12 bytes");
static_assert(sizeof(vec4) == 16, "vec4 must be 16 bytes");
static_assert(sizeof(mat3) == 36, "mat3 must be 36 bytes");
static_assert(sizeof(mat4) == 64, "mat4 must be 64 bytes");
static_assert(std::is_trivially_copyable_v<vec2>);
static_assert(std::is_trivially_copyable_v<vec3>);
static_assert(std::is_trivially_copyable_v<vec4>);
static_assert(std::is_trivially_copyable_v<mat3>);
static_assert(std::is_trivially_copyable_v<mat4>);
static_assert(std::is_standard_layout_v<vec2>);
static_assert(std::is_standard_layout_v<vec3>);
static_assert(std::is_standard_layout_v<vec4>);
static_assert(std::is_standard_layout_v<mat3>);
static_assert(std::is_standard_layout_v<mat4>);
struct quat {
rtm::quatf q;
quat() : q(rtm::quat_identity()) {
}
quat(const rtm::quatf &q_) : q(q_) {
}
static quat from_axis_angle(const vec3 &axis, f32 angle_rad) {
return rtm::quat_from_axis_angle(rtm::vector_normalize3(rtm::vector_set(axis.x, axis.y, axis.z)), angle_rad);
}
quat operator*(const quat &rhs) const {
return rtm::quat_mul(q, rhs.q);
}
quat normalized() const {
return rtm::quat_normalize(q);
}
operator rtm::quatf() const {
return q;
}
bool same_rotation(const quat &rhs, f32 epsilon = 1e-6f) const {
f32 dot = rtm::scalar_cast(rtm::quat_dot_as_scalar(q, rhs.q));
return dot * dot > 1.0f - epsilon;
}
};
static_assert(sizeof(quat) == 16, "quat must be 16 bytes");
static_assert(std::is_trivially_copyable_v<quat>);
struct TRS {
vec3 translation = vec3(0.0f);
quat rotation;
vec3 scale = vec3(1.0f);
};
mat4 trs_to_mat4(const TRS &trs);
TRS mat4_to_trs(const mat4 &m);
// euler angles in degrees, (pitch, yaw, roll) = (x, y, z).
// matches rtm::quat_from_euler's convention (pitch about Y, yaw about Z, roll about X).
vec3 quat_to_euler(const quat &q);
quat euler_to_quat(const vec3 &euler);