3D viewport, milestone 1
This commit is contained in:
parent
38156dd570
commit
e277663ba0
27 changed files with 7389 additions and 8 deletions
630
src/3d/Math3D.h
Normal file
630
src/3d/Math3D.h
Normal file
|
|
@ -0,0 +1,630 @@
|
|||
// Math3D.h - Minimal 3D math library for McRogueFace
|
||||
// Header-only implementation of vec3, mat4, and quat
|
||||
// Column-major matrices for OpenGL compatibility
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace mcrf {
|
||||
|
||||
// =============================================================================
|
||||
// vec2 - 2D vector
|
||||
// =============================================================================
|
||||
|
||||
struct vec2 {
|
||||
float x, y;
|
||||
|
||||
vec2() : x(0), y(0) {}
|
||||
vec2(float x_, float y_) : x(x_), y(y_) {}
|
||||
explicit vec2(float v) : x(v), y(v) {}
|
||||
|
||||
vec2 operator+(const vec2& other) const { return vec2(x + other.x, y + other.y); }
|
||||
vec2 operator-(const vec2& other) const { return vec2(x - other.x, y - other.y); }
|
||||
vec2 operator*(float s) const { return vec2(x * s, y * s); }
|
||||
vec2 operator/(float s) const { return vec2(x / s, y / s); }
|
||||
|
||||
float dot(const vec2& other) const { return x * other.x + y * other.y; }
|
||||
float length() const { return std::sqrt(x * x + y * y); }
|
||||
float lengthSquared() const { return x * x + y * y; }
|
||||
|
||||
vec2 normalized() const {
|
||||
float len = length();
|
||||
if (len > 0.0001f) return vec2(x / len, y / len);
|
||||
return vec2(0, 0);
|
||||
}
|
||||
};
|
||||
|
||||
// =============================================================================
|
||||
// vec3 - 3D vector
|
||||
// =============================================================================
|
||||
|
||||
struct vec3 {
|
||||
float x, y, z;
|
||||
|
||||
vec3() : x(0), y(0), z(0) {}
|
||||
vec3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {}
|
||||
explicit vec3(float v) : x(v), y(v), z(v) {}
|
||||
|
||||
vec3 operator+(const vec3& other) const {
|
||||
return vec3(x + other.x, y + other.y, z + other.z);
|
||||
}
|
||||
|
||||
vec3 operator-(const vec3& other) const {
|
||||
return vec3(x - other.x, y - other.y, z - other.z);
|
||||
}
|
||||
|
||||
vec3 operator*(float s) const {
|
||||
return vec3(x * s, y * s, z * s);
|
||||
}
|
||||
|
||||
vec3 operator/(float s) const {
|
||||
return vec3(x / s, y / s, z / s);
|
||||
}
|
||||
|
||||
vec3 operator-() const {
|
||||
return vec3(-x, -y, -z);
|
||||
}
|
||||
|
||||
vec3& operator+=(const vec3& other) {
|
||||
x += other.x; y += other.y; z += other.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
vec3& operator-=(const vec3& other) {
|
||||
x -= other.x; y -= other.y; z -= other.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
vec3& operator*=(float s) {
|
||||
x *= s; y *= s; z *= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
float dot(const vec3& other) const {
|
||||
return x * other.x + y * other.y + z * other.z;
|
||||
}
|
||||
|
||||
vec3 cross(const vec3& other) const {
|
||||
return vec3(
|
||||
y * other.z - z * other.y,
|
||||
z * other.x - x * other.z,
|
||||
x * other.y - y * other.x
|
||||
);
|
||||
}
|
||||
|
||||
float lengthSquared() const {
|
||||
return x * x + y * y + z * z;
|
||||
}
|
||||
|
||||
float length() const {
|
||||
return std::sqrt(lengthSquared());
|
||||
}
|
||||
|
||||
vec3 normalized() const {
|
||||
float len = length();
|
||||
if (len > 0.0001f) {
|
||||
return *this / len;
|
||||
}
|
||||
return vec3(0, 0, 0);
|
||||
}
|
||||
|
||||
// Component-wise operations
|
||||
vec3 hadamard(const vec3& other) const {
|
||||
return vec3(x * other.x, y * other.y, z * other.z);
|
||||
}
|
||||
|
||||
// Linear interpolation
|
||||
static vec3 lerp(const vec3& a, const vec3& b, float t) {
|
||||
return a + (b - a) * t;
|
||||
}
|
||||
};
|
||||
|
||||
// Left-hand scalar multiplication
|
||||
inline vec3 operator*(float s, const vec3& v) {
|
||||
return v * s;
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// vec4 - 4D vector (for homogeneous coordinates)
|
||||
// =============================================================================
|
||||
|
||||
struct vec4 {
|
||||
float x, y, z, w;
|
||||
|
||||
vec4() : x(0), y(0), z(0), w(0) {}
|
||||
vec4(float x_, float y_, float z_, float w_) : x(x_), y(y_), z(z_), w(w_) {}
|
||||
vec4(const vec3& v, float w_) : x(v.x), y(v.y), z(v.z), w(w_) {}
|
||||
|
||||
vec3 xyz() const { return vec3(x, y, z); }
|
||||
|
||||
// Perspective divide
|
||||
vec3 perspectiveDivide() const {
|
||||
if (std::abs(w) > 0.0001f) {
|
||||
return vec3(x / w, y / w, z / w);
|
||||
}
|
||||
return vec3(x, y, z);
|
||||
}
|
||||
};
|
||||
|
||||
// =============================================================================
|
||||
// mat4 - 4x4 matrix (column-major for OpenGL)
|
||||
// =============================================================================
|
||||
|
||||
struct mat4 {
|
||||
// Column-major storage: m[col][row] but stored as m[col*4 + row]
|
||||
// This matches OpenGL's expected layout
|
||||
float m[16];
|
||||
|
||||
mat4() {
|
||||
for (int i = 0; i < 16; i++) m[i] = 0;
|
||||
}
|
||||
|
||||
// Access element at column c, row r
|
||||
float& at(int c, int r) { return m[c * 4 + r]; }
|
||||
const float& at(int c, int r) const { return m[c * 4 + r]; }
|
||||
|
||||
// Get column as vec4
|
||||
vec4 col(int c) const {
|
||||
return vec4(m[c*4], m[c*4+1], m[c*4+2], m[c*4+3]);
|
||||
}
|
||||
|
||||
// Get raw data pointer (for OpenGL uniforms)
|
||||
const float* data() const { return m; }
|
||||
float* data() { return m; }
|
||||
|
||||
static mat4 identity() {
|
||||
mat4 result;
|
||||
result.at(0, 0) = 1.0f;
|
||||
result.at(1, 1) = 1.0f;
|
||||
result.at(2, 2) = 1.0f;
|
||||
result.at(3, 3) = 1.0f;
|
||||
return result;
|
||||
}
|
||||
|
||||
static mat4 translate(const vec3& v) {
|
||||
mat4 result = identity();
|
||||
result.at(3, 0) = v.x;
|
||||
result.at(3, 1) = v.y;
|
||||
result.at(3, 2) = v.z;
|
||||
return result;
|
||||
}
|
||||
|
||||
static mat4 translate(float x, float y, float z) {
|
||||
return translate(vec3(x, y, z));
|
||||
}
|
||||
|
||||
static mat4 scale(const vec3& v) {
|
||||
mat4 result = identity();
|
||||
result.at(0, 0) = v.x;
|
||||
result.at(1, 1) = v.y;
|
||||
result.at(2, 2) = v.z;
|
||||
return result;
|
||||
}
|
||||
|
||||
static mat4 scale(float x, float y, float z) {
|
||||
return scale(vec3(x, y, z));
|
||||
}
|
||||
|
||||
static mat4 scale(float s) {
|
||||
return scale(vec3(s, s, s));
|
||||
}
|
||||
|
||||
static mat4 rotateX(float radians) {
|
||||
mat4 result = identity();
|
||||
float c = std::cos(radians);
|
||||
float s = std::sin(radians);
|
||||
result.at(1, 1) = c;
|
||||
result.at(2, 1) = -s;
|
||||
result.at(1, 2) = s;
|
||||
result.at(2, 2) = c;
|
||||
return result;
|
||||
}
|
||||
|
||||
static mat4 rotateY(float radians) {
|
||||
mat4 result = identity();
|
||||
float c = std::cos(radians);
|
||||
float s = std::sin(radians);
|
||||
result.at(0, 0) = c;
|
||||
result.at(2, 0) = s;
|
||||
result.at(0, 2) = -s;
|
||||
result.at(2, 2) = c;
|
||||
return result;
|
||||
}
|
||||
|
||||
static mat4 rotateZ(float radians) {
|
||||
mat4 result = identity();
|
||||
float c = std::cos(radians);
|
||||
float s = std::sin(radians);
|
||||
result.at(0, 0) = c;
|
||||
result.at(1, 0) = -s;
|
||||
result.at(0, 1) = s;
|
||||
result.at(1, 1) = c;
|
||||
return result;
|
||||
}
|
||||
|
||||
// Perspective projection matrix
|
||||
// fov: vertical field of view in radians
|
||||
// aspect: width / height
|
||||
// near, far: clipping planes
|
||||
static mat4 perspective(float fov, float aspect, float near, float far) {
|
||||
mat4 result;
|
||||
float tanHalfFov = std::tan(fov / 2.0f);
|
||||
|
||||
result.at(0, 0) = 1.0f / (aspect * tanHalfFov);
|
||||
result.at(1, 1) = 1.0f / tanHalfFov;
|
||||
result.at(2, 2) = -(far + near) / (far - near);
|
||||
result.at(2, 3) = -1.0f;
|
||||
result.at(3, 2) = -(2.0f * far * near) / (far - near);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Orthographic projection matrix
|
||||
static mat4 ortho(float left, float right, float bottom, float top, float near, float far) {
|
||||
mat4 result = identity();
|
||||
|
||||
result.at(0, 0) = 2.0f / (right - left);
|
||||
result.at(1, 1) = 2.0f / (top - bottom);
|
||||
result.at(2, 2) = -2.0f / (far - near);
|
||||
result.at(3, 0) = -(right + left) / (right - left);
|
||||
result.at(3, 1) = -(top + bottom) / (top - bottom);
|
||||
result.at(3, 2) = -(far + near) / (far - near);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// View matrix (camera transformation)
|
||||
static mat4 lookAt(const vec3& eye, const vec3& target, const vec3& up) {
|
||||
vec3 zaxis = (eye - target).normalized(); // Forward (camera looks down -Z)
|
||||
vec3 xaxis = up.cross(zaxis).normalized(); // Right
|
||||
vec3 yaxis = zaxis.cross(xaxis); // Up
|
||||
|
||||
mat4 result;
|
||||
|
||||
// Rotation part (transposed because we need the inverse)
|
||||
result.at(0, 0) = xaxis.x;
|
||||
result.at(1, 0) = xaxis.y;
|
||||
result.at(2, 0) = xaxis.z;
|
||||
|
||||
result.at(0, 1) = yaxis.x;
|
||||
result.at(1, 1) = yaxis.y;
|
||||
result.at(2, 1) = yaxis.z;
|
||||
|
||||
result.at(0, 2) = zaxis.x;
|
||||
result.at(1, 2) = zaxis.y;
|
||||
result.at(2, 2) = zaxis.z;
|
||||
|
||||
// Translation part
|
||||
result.at(3, 0) = -xaxis.dot(eye);
|
||||
result.at(3, 1) = -yaxis.dot(eye);
|
||||
result.at(3, 2) = -zaxis.dot(eye);
|
||||
result.at(3, 3) = 1.0f;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Matrix multiplication
|
||||
mat4 operator*(const mat4& other) const {
|
||||
mat4 result;
|
||||
for (int c = 0; c < 4; c++) {
|
||||
for (int r = 0; r < 4; r++) {
|
||||
float sum = 0.0f;
|
||||
for (int k = 0; k < 4; k++) {
|
||||
sum += at(k, r) * other.at(c, k);
|
||||
}
|
||||
result.at(c, r) = sum;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// Transform a point (assumes w=1, returns xyz)
|
||||
vec3 transformPoint(const vec3& p) const {
|
||||
vec4 v(p, 1.0f);
|
||||
vec4 result(
|
||||
at(0, 0) * v.x + at(1, 0) * v.y + at(2, 0) * v.z + at(3, 0) * v.w,
|
||||
at(0, 1) * v.x + at(1, 1) * v.y + at(2, 1) * v.z + at(3, 1) * v.w,
|
||||
at(0, 2) * v.x + at(1, 2) * v.y + at(2, 2) * v.z + at(3, 2) * v.w,
|
||||
at(0, 3) * v.x + at(1, 3) * v.y + at(2, 3) * v.z + at(3, 3) * v.w
|
||||
);
|
||||
return result.perspectiveDivide();
|
||||
}
|
||||
|
||||
// Transform a direction (assumes w=0)
|
||||
vec3 transformDirection(const vec3& d) const {
|
||||
return vec3(
|
||||
at(0, 0) * d.x + at(1, 0) * d.y + at(2, 0) * d.z,
|
||||
at(0, 1) * d.x + at(1, 1) * d.y + at(2, 1) * d.z,
|
||||
at(0, 2) * d.x + at(1, 2) * d.y + at(2, 2) * d.z
|
||||
);
|
||||
}
|
||||
|
||||
// Transform a vec4
|
||||
vec4 operator*(const vec4& v) const {
|
||||
return vec4(
|
||||
at(0, 0) * v.x + at(1, 0) * v.y + at(2, 0) * v.z + at(3, 0) * v.w,
|
||||
at(0, 1) * v.x + at(1, 1) * v.y + at(2, 1) * v.z + at(3, 1) * v.w,
|
||||
at(0, 2) * v.x + at(1, 2) * v.y + at(2, 2) * v.z + at(3, 2) * v.w,
|
||||
at(0, 3) * v.x + at(1, 3) * v.y + at(2, 3) * v.z + at(3, 3) * v.w
|
||||
);
|
||||
}
|
||||
|
||||
// Transpose
|
||||
mat4 transposed() const {
|
||||
mat4 result;
|
||||
for (int c = 0; c < 4; c++) {
|
||||
for (int r = 0; r < 4; r++) {
|
||||
result.at(r, c) = at(c, r);
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// Inverse (for general 4x4 matrix - used for camera)
|
||||
// Returns identity if matrix is singular
|
||||
mat4 inverse() const {
|
||||
mat4 inv;
|
||||
const float* m = this->m;
|
||||
float* out = inv.m;
|
||||
|
||||
out[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] -
|
||||
m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
|
||||
m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
|
||||
out[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] +
|
||||
m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
|
||||
m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
|
||||
out[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] -
|
||||
m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
|
||||
m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
|
||||
out[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] +
|
||||
m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
|
||||
m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
|
||||
out[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] +
|
||||
m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
|
||||
m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
|
||||
out[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] -
|
||||
m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
|
||||
m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
|
||||
out[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] +
|
||||
m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
|
||||
m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
|
||||
out[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] -
|
||||
m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
|
||||
m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
|
||||
out[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] -
|
||||
m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
|
||||
m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
|
||||
out[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] +
|
||||
m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
|
||||
m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
|
||||
out[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] -
|
||||
m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
|
||||
m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
|
||||
out[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] +
|
||||
m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
|
||||
m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
|
||||
out[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] +
|
||||
m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
|
||||
m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
|
||||
out[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] -
|
||||
m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
|
||||
m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
|
||||
out[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] +
|
||||
m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
|
||||
m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
|
||||
out[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] -
|
||||
m[4] * m[1] * m[10] + m[4] * m[2] * m[9] +
|
||||
m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
|
||||
|
||||
float det = m[0] * out[0] + m[1] * out[4] + m[2] * out[8] + m[3] * out[12];
|
||||
|
||||
if (std::abs(det) < 0.0001f) {
|
||||
return identity();
|
||||
}
|
||||
|
||||
det = 1.0f / det;
|
||||
for (int i = 0; i < 16; i++) {
|
||||
out[i] *= det;
|
||||
}
|
||||
|
||||
return inv;
|
||||
}
|
||||
};
|
||||
|
||||
// =============================================================================
|
||||
// quat - Quaternion for rotations
|
||||
// =============================================================================
|
||||
|
||||
struct quat {
|
||||
float x, y, z, w; // w is the scalar part
|
||||
|
||||
quat() : x(0), y(0), z(0), w(1) {} // Identity quaternion
|
||||
quat(float x_, float y_, float z_, float w_) : x(x_), y(y_), z(z_), w(w_) {}
|
||||
|
||||
// Create from axis and angle (angle in radians)
|
||||
static quat fromAxisAngle(const vec3& axis, float angle) {
|
||||
float halfAngle = angle * 0.5f;
|
||||
float s = std::sin(halfAngle);
|
||||
vec3 n = axis.normalized();
|
||||
return quat(n.x * s, n.y * s, n.z * s, std::cos(halfAngle));
|
||||
}
|
||||
|
||||
// Create from Euler angles (in radians, applied as yaw-pitch-roll / Y-X-Z)
|
||||
static quat fromEuler(float pitch, float yaw, float roll) {
|
||||
float cy = std::cos(yaw * 0.5f);
|
||||
float sy = std::sin(yaw * 0.5f);
|
||||
float cp = std::cos(pitch * 0.5f);
|
||||
float sp = std::sin(pitch * 0.5f);
|
||||
float cr = std::cos(roll * 0.5f);
|
||||
float sr = std::sin(roll * 0.5f);
|
||||
|
||||
return quat(
|
||||
sr * cp * cy - cr * sp * sy,
|
||||
cr * sp * cy + sr * cp * sy,
|
||||
cr * cp * sy - sr * sp * cy,
|
||||
cr * cp * cy + sr * sp * sy
|
||||
);
|
||||
}
|
||||
|
||||
float lengthSquared() const {
|
||||
return x * x + y * y + z * z + w * w;
|
||||
}
|
||||
|
||||
float length() const {
|
||||
return std::sqrt(lengthSquared());
|
||||
}
|
||||
|
||||
quat normalized() const {
|
||||
float len = length();
|
||||
if (len > 0.0001f) {
|
||||
float invLen = 1.0f / len;
|
||||
return quat(x * invLen, y * invLen, z * invLen, w * invLen);
|
||||
}
|
||||
return quat();
|
||||
}
|
||||
|
||||
quat conjugate() const {
|
||||
return quat(-x, -y, -z, w);
|
||||
}
|
||||
|
||||
quat inverse() const {
|
||||
float lenSq = lengthSquared();
|
||||
if (lenSq > 0.0001f) {
|
||||
float invLenSq = 1.0f / lenSq;
|
||||
return quat(-x * invLenSq, -y * invLenSq, -z * invLenSq, w * invLenSq);
|
||||
}
|
||||
return quat();
|
||||
}
|
||||
|
||||
// Quaternion multiplication
|
||||
quat operator*(const quat& other) const {
|
||||
return quat(
|
||||
w * other.x + x * other.w + y * other.z - z * other.y,
|
||||
w * other.y - x * other.z + y * other.w + z * other.x,
|
||||
w * other.z + x * other.y - y * other.x + z * other.w,
|
||||
w * other.w - x * other.x - y * other.y - z * other.z
|
||||
);
|
||||
}
|
||||
|
||||
// Rotate a vector by this quaternion
|
||||
vec3 rotate(const vec3& v) const {
|
||||
// q * v * q^-1
|
||||
quat vq(v.x, v.y, v.z, 0);
|
||||
quat result = (*this) * vq * conjugate();
|
||||
return vec3(result.x, result.y, result.z);
|
||||
}
|
||||
|
||||
// Convert to rotation matrix
|
||||
mat4 toMatrix() const {
|
||||
mat4 result = mat4::identity();
|
||||
|
||||
float xx = x * x;
|
||||
float yy = y * y;
|
||||
float zz = z * z;
|
||||
float xy = x * y;
|
||||
float xz = x * z;
|
||||
float yz = y * z;
|
||||
float wx = w * x;
|
||||
float wy = w * y;
|
||||
float wz = w * z;
|
||||
|
||||
result.at(0, 0) = 1.0f - 2.0f * (yy + zz);
|
||||
result.at(0, 1) = 2.0f * (xy + wz);
|
||||
result.at(0, 2) = 2.0f * (xz - wy);
|
||||
|
||||
result.at(1, 0) = 2.0f * (xy - wz);
|
||||
result.at(1, 1) = 1.0f - 2.0f * (xx + zz);
|
||||
result.at(1, 2) = 2.0f * (yz + wx);
|
||||
|
||||
result.at(2, 0) = 2.0f * (xz + wy);
|
||||
result.at(2, 1) = 2.0f * (yz - wx);
|
||||
result.at(2, 2) = 1.0f - 2.0f * (xx + yy);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Spherical linear interpolation
|
||||
static quat slerp(const quat& a, const quat& b, float t) {
|
||||
float dot = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
||||
|
||||
quat b2 = b;
|
||||
if (dot < 0.0f) {
|
||||
// Take the shorter path
|
||||
b2.x = -b.x;
|
||||
b2.y = -b.y;
|
||||
b2.z = -b.z;
|
||||
b2.w = -b.w;
|
||||
dot = -dot;
|
||||
}
|
||||
|
||||
const float DOT_THRESHOLD = 0.9995f;
|
||||
if (dot > DOT_THRESHOLD) {
|
||||
// Linear interpolation for very similar quaternions
|
||||
return quat(
|
||||
a.x + (b2.x - a.x) * t,
|
||||
a.y + (b2.y - a.y) * t,
|
||||
a.z + (b2.z - a.z) * t,
|
||||
a.w + (b2.w - a.w) * t
|
||||
).normalized();
|
||||
}
|
||||
|
||||
float theta_0 = std::acos(dot);
|
||||
float theta = theta_0 * t;
|
||||
float sin_theta = std::sin(theta);
|
||||
float sin_theta_0 = std::sin(theta_0);
|
||||
|
||||
float s0 = std::cos(theta) - dot * sin_theta / sin_theta_0;
|
||||
float s1 = sin_theta / sin_theta_0;
|
||||
|
||||
return quat(
|
||||
a.x * s0 + b2.x * s1,
|
||||
a.y * s0 + b2.y * s1,
|
||||
a.z * s0 + b2.z * s1,
|
||||
a.w * s0 + b2.w * s1
|
||||
);
|
||||
}
|
||||
|
||||
// Linear interpolation (faster but less accurate for large angles)
|
||||
static quat lerp(const quat& a, const quat& b, float t) {
|
||||
float dot = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
||||
|
||||
quat result;
|
||||
if (dot < 0.0f) {
|
||||
result = quat(
|
||||
a.x - (b.x + a.x) * t,
|
||||
a.y - (b.y + a.y) * t,
|
||||
a.z - (b.z + a.z) * t,
|
||||
a.w - (b.w + a.w) * t
|
||||
);
|
||||
} else {
|
||||
result = quat(
|
||||
a.x + (b.x - a.x) * t,
|
||||
a.y + (b.y - a.y) * t,
|
||||
a.z + (b.z - a.z) * t,
|
||||
a.w + (b.w - a.w) * t
|
||||
);
|
||||
}
|
||||
return result.normalized();
|
||||
}
|
||||
};
|
||||
|
||||
// =============================================================================
|
||||
// Utility constants and functions
|
||||
// =============================================================================
|
||||
|
||||
constexpr float PI = 3.14159265358979323846f;
|
||||
constexpr float TWO_PI = PI * 2.0f;
|
||||
constexpr float HALF_PI = PI * 0.5f;
|
||||
constexpr float DEG_TO_RAD = PI / 180.0f;
|
||||
constexpr float RAD_TO_DEG = 180.0f / PI;
|
||||
|
||||
inline float radians(float degrees) { return degrees * DEG_TO_RAD; }
|
||||
inline float degrees(float radians) { return radians * RAD_TO_DEG; }
|
||||
|
||||
inline float clamp(float v, float min, float max) {
|
||||
return std::min(std::max(v, min), max);
|
||||
}
|
||||
|
||||
} // namespace mcrf
|
||||
Loading…
Add table
Add a link
Reference in a new issue