#include "math_rtm.h"
#include <algorithm>
#include <cmath>
// reverse-z layout, inf far plane
mat4 perspective(f32 fovy, f32 aspect, f32 world_near_plane, f32 world_far_plane, f32 clip_near_z, f32 clip_far_z) {
f32 f = 1.0f / tanf(fovy * 0.5f);
mat4 r = {};
r.m[0] = f / aspect; // x scale
r.m[5] = f; // y scale
r.m[11] = 1.0f; // z into .w for perspective divice
// finite reverse Z
if (world_far_plane > 0.0f) {
f32 depth_range = world_far_plane - world_near_plane;
r.m[10] = (world_far_plane * clip_far_z - world_near_plane * clip_near_z) / depth_range;
r.m[14] = (world_near_plane * world_far_plane * (clip_near_z - clip_far_z)) / depth_range;
}
// default infinite reverse Z
else {
r.m[10] = clip_far_z;
r.m[11] = 1.0f;
r.m[14] = world_near_plane * (clip_near_z - clip_far_z);
}
return r;
}
// left-handed view mat for (v * M)
// left handed to really just match the convention,
// geom at higher world depth == positive, increasing view space Z
// maps directly into depth buffers without me doing a negative Z negation step
// and generally, it just keeps me sane doing all the view space culling stuff
// world space -> view Space (+X: Right, +Y: Up, +Z: Forward)
mat4 look_at(vec3 eye, vec3 center, vec3 up) {
vec3 f = vec3({center.x - eye.x, center.y - eye.y, center.z - eye.z}).normalized();
vec3 r = cross(f, up).normalized();
vec3 u = cross(r, f).normalized();
mat4 m = {};
// each row: camera basis vectors (r, -u, f)
// clang-format off
m.m[0] = r.x; m.m[1] = -u.x; m.m[2] = f.x; m.m[3] = 0.0f; // x (rgt)
m.m[4] = r.y; m.m[5] = -u.y; m.m[6] = f.y; m.m[7] = 0.0f; // y (up)
m.m[8] = r.z; m.m[9] = -u.z; m.m[10] = f.z; m.m[11] = 0.0f; // z (fwd)
// camera ws translation projected onto it's local axes
m.m[12] = -r.dot(eye); m.m[13] = u.dot(eye); m.m[14] = -f.dot(eye); m.m[15] = 1.0f;
// clang-format on
return m;
}
mat4 ortho(f32 width, f32 height, f32 near_plane, f32 far_plane) {
mat4 r = {};
// row 0, x: [-width/2, width/2] -> [-1.0f, 1.0f]
r.m[0] = 2.0f / width;
r.m[1] = 0.0f;
r.m[2] = 0.0f;
r.m[3] = 0.0f;
// row 1, y: [-height/2, height/2] -> [-1.0, 1.0]
r.m[4] = 0.0f;
r.m[5] = 2.0f / height;
r.m[6] = 0.0f;
r.m[7] = 0.0f;
// row 2, Z slope (reverse-Z: near -> 1.0f, far -> 0.0f)
// same as look_at, left handed +Z forward
r.m[8] = 0.0f;
r.m[9] = 0.0f;
r.m[10] = 1.0f / (near_plane - far_plane);
r.m[11] = 0.0f;
// row 3, translation (Z offset)
// z_ndc = z * (1/(near-far) + (far / (far- near)))
r.m[12] = 0.0f;
r.m[13] = 0.0f;
r.m[14] = far_plane / (far_plane - near_plane);
r.m[15] = 1.0f;
return r;
}
mat4 ortho_offcenter(f32 left, f32 right, f32 bottom, f32 top, f32 near_plane, f32 far_plane) {
mat4 r = {};
f32 inv_w = 1.0f / (right - left);
f32 inv_h = 1.0f / (top - bottom);
// row 0, x
r.m[0] = 2.0f * inv_w;
r.m[1] = 0.0f;
r.m[2] = 0.0f;
r.m[3] = 0.0f;
// row 1, y
r.m[4] = 0.0f;
r.m[5] = 2.0f * inv_h;
r.m[6] = 0.0f;
r.m[7] = 0.0f;
// row 2, reverse z
r.m[8] = 0.0f;
r.m[9] = 0.0f;
r.m[10] = 1.0f / (near_plane - far_plane);
r.m[11] = 0.0f;
// row 3 translations
r.m[12] = -(right + left) * inv_w;
r.m[13] = -(top + bottom) * inv_h;
r.m[14] = far_plane / (far_plane - near_plane);
r.m[15] = 1.0f;
return r;
}
mat4 rotate(vec3 axis, f32 angle_rad) {
vec_work axis_v = rtm::vector_normalize3(rtm::vector_set(axis.x, axis.y, axis.z));
rtm::quatf q = rtm::quat_from_axis_angle(axis_v, angle_rad);
rtm::matrix3x4f rot = rtm::matrix_from_rotation(q);
return mat4::from_matrix(rtm::matrix_cast(rot));
}
mat4 inverse(mat4 m) {
rtm::matrix4x4f inv = rtm::matrix_inverse(m.to_matrix());
mat4 r;
rtm::vector_store(inv.x_axis, &r.m[0]);
rtm::vector_store(inv.y_axis, &r.m[4]);
rtm::vector_store(inv.z_axis, &r.m[8]);
rtm::vector_store(inv.w_axis, &r.m[12]);
return r;
}
// w 1.0
vec4 transform_point(const vec3 &v, const mat4 &m) {
vec_work vv = rtm::vector_set(v.x, v.y, v.z, 1.0f);
return vec4::from_vec4(rtm::matrix_mul_vector(vv, m.to_matrix()));
}
vec3 transform_direction(const vec3 &v, const mat4 &m) {
vec_work vv = rtm::vector_set(v.x, v.y, v.z, 0.0f);
return vec3::from_vec4(rtm::matrix_mul_vector(vv, m.to_matrix()));
}
mat4 trs_to_mat4(const TRS &trs) {
rtm::matrix3x4f t =
rtm::matrix_from_translation(rtm::vector_set(trs.translation.x, trs.translation.y, trs.translation.z));
rtm::matrix3x4f r = rtm::matrix_from_rotation(trs.rotation);
rtm::matrix3x4f s = rtm::matrix_from_scale(rtm::vector_set(trs.scale.x, trs.scale.y, trs.scale.z));
// s * r * t
rtm::matrix3x4f sr = rtm::matrix_mul(s, r);
rtm::matrix3x4f trs_m = rtm::matrix_mul(sr, t);
return mat4::from_matrix(rtm::matrix_cast(trs_m));
}
TRS mat4_to_trs(const mat4 &m) {
rtm::matrix4x4f m4 = m.to_matrix();
rtm::matrix3x4f m34;
m34.x_axis = m4.x_axis;
m34.y_axis = m4.y_axis;
m34.z_axis = m4.z_axis;
m34.w_axis = m4.w_axis;
// translation is column 3
vec3 t = vec3::from_vec4(m34.w_axis);
// rot from 3x3 part
quat r;
r.q = rtm::quat_from_matrix(m34);
r.q = rtm::quat_normalize(r.q);
// scale = length of each column vector of the 3x3 part
vec3 s(
rtm::scalar_cast(rtm::vector_length3_as_scalar(m34.x_axis)),
rtm::scalar_cast(rtm::vector_length3_as_scalar(m34.y_axis)),
rtm::scalar_cast(rtm::vector_length3_as_scalar(m34.z_axis))
);
return {t, r, s};
}
vec3 quat_to_euler(const quat &q) {
const mat4 m = trs_to_mat4(TRS{vec3(0.0f), q, vec3(1.0f)});
const f32 pitch = std::asin(std::clamp(m.m[2], -1.0f, 1.0f));
const f32 yaw = std::atan2(m.m[1], m.m[0]);
const f32 roll = std::atan2(-m.m[6], m.m[10]);
return vec3(degrees(pitch), degrees(yaw), degrees(roll));
}
quat euler_to_quat(const vec3 &euler) {
quat qz = quat::from_axis_angle(vec3(0, 0, 1), radians(euler.z));
quat qy = quat::from_axis_angle(vec3(0, 1, 0), radians(euler.y));
quat qx = quat::from_axis_angle(vec3(1, 0, 0), radians(euler.x));
return (qz * qy * qx).normalized();
}