From 09b57ae1148806eb41b51825831163a7655af18f Mon Sep 17 00:00:00 2001 From: matthew Date: Sun, 3 Aug 2025 14:33:37 +1000 Subject: [PATCH] fixed camera (using cglm for maths) --- src/gears/game.d | 48 +- src/shared/math.d | 329 ++-------- src/to_rework/math.d | 1378 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 1430 insertions(+), 325 deletions(-) create mode 100644 src/to_rework/math.d diff --git a/src/gears/game.d b/src/gears/game.d index 79ad2ac..0a8f309 100644 --- a/src/gears/game.d +++ b/src/gears/game.d @@ -161,7 +161,7 @@ Cycle(Game* g) projection[1, 1] *= -1.0; g.globals.projection_matrix = projection; - g.globals.view_matrix = Mat4Identity(); //g.camera.view; + g.globals.view_matrix = g.camera.view; g.globals.res.x = ext.x; g.globals.res.y = ext.y; SetUniform(&g.rd, &g.globals); @@ -219,6 +219,7 @@ struct Camera { Mat4 view = Mat4Identity(); Vec3 pos = Vec3(0.0, 0.0, 2.0); + Vec3 velocity = Vec3(0.0); Vec3 front = Vec3(0.0, 0.0, -1.0); Vec3 up = Vec3(0.0, 1.0, 0.0); Vec3 right = Vec3(0.0); @@ -231,50 +232,21 @@ struct Camera pragma(inline): void HandleInputs(Camera* cam, Inputs* inputs) { - f32 velocity = cam.speed * g_DELTA; - - Logf("pos %s pitch %s yaw %s", cam.pos.v, cam.pitch, cam.yaw); - foreach(i; 0 .. inputs.count) { InputEvent event = inputs.events[i]; switch(event.key) { - case Input.W: - { - if (!event.pressed) continue; - cam.pos += cam.front * velocity; - } break; - case Input.S: - { - if (!event.pressed) continue; - cam.pos -= cam.front * velocity; - } break; - case Input.A: - { - if (!event.pressed) continue; - cam.pos -= cam.right * velocity; - } break; - case Input.D: - { - if (!event.pressed) continue; - cam.pos += cam.right * velocity; - } break; - case Input.Space: - { - if (!event.pressed) continue; - cam.pos += cam.up * velocity; - } break; - case Input.LeftCtrl: - { - if (!event.pressed) continue; - cam.pos -= cam.up * velocity; - } break; + case Input.W: cam.velocity.z = event.pressed ? -1.0 : 0.0; break; + case Input.S: cam.velocity.z = event.pressed ? 1.0 : 0.0; break; + case Input.A: cam.velocity.x = event.pressed ? -1.0 : 0.0; break; + case Input.D: cam.velocity.x = event.pressed ? 1.0 : 0.0; break; + case Input.Space: cam.velocity.y = event.pressed ? 1.0 : 0.0; break; + case Input.LeftCtrl: cam.velocity.y = event.pressed ? -1.0 : 0.0; break; case Input.MouseMotion: { - Logf("Mouse Motion"); // (0, 0) top left - cam.yaw += cast(f32)(event.rel_x) / 20.0; + cam.yaw -= cast(f32)(event.rel_x) / 20.0; cam.pitch += cast(f32)(event.rel_y) / 20.0; if (cam.pitch > 90.0) @@ -296,6 +268,8 @@ Update(Camera* cam) { Vec3 front = Vec3(0.0); + cam.pos += cam.velocity * g_DELTA; + front.x = cosf(Radians(cam.yaw)) * cosf(Radians(cam.pitch)); front.y = sinf(Radians(cam.pitch)); front.z = sinf(Radians(cam.yaw)) * cosf(Radians(cam.pitch)); diff --git a/src/shared/math.d b/src/shared/math.d index 93d1081..0d48a26 100644 --- a/src/shared/math.d +++ b/src/shared/math.d @@ -1,4 +1,5 @@ import aliases; +import includes; import util; import std.math; import std.math.algebraic; @@ -19,10 +20,6 @@ f32 Radians(f32 deg) return deg * (PI / 180.0); } -// TODO: Clean this mess up - -enum IsVector(T) = is(T : Vector!U, U...); - struct Vector(T, int N) { static assert(N > 0 && N <= 4); @@ -58,8 +55,6 @@ struct Vector(T, int N) } } - nothrow @nogc: - this(Vec3, f32)(Vec3 v3, f32 f) if (N == 4 && is(T: f32)) { x = v3.x; @@ -91,36 +86,18 @@ struct Vector(T, int N) } } - ref Vector opAssign(U)(U x) if (isAssignable!(T, U)) + ref Vector opAssign(U)(U x) if (is(U: T)) { mixin(GenerateLoop!("v[@] = x;", N)()); return this; } - ref Vector opAssign(U)(U arr) if (isStaticArray!(U) && isAssignable!(T, typeof(arr[0])) && arr.length == N) - { - mixin(GenerateLoop!("v[@] = arr[@];", N)()); - return this; - } - - ref Vector opAssign(U)(U arr) if (isDynamicArray!(U) && isAssignable!(T, typeof(arr[0]))) - { - mixin(GenerateLoop!("v[@] = arr[@];", N)()); - return this; - } - ref Vector opAssign(U)(U u) if (is(U : Vector)) { v[] = u.v[]; return this; } - ref Vector opAssign(U)(U x) if (IsVector!(U) && isAssignable!(T, U._T) && (!is(U: Vector)) && (U._N == _N)) - { - mixin(GenerateLoop!("v[@] = x.v[@];", N)()); - return this; - } - inout(T)* ptr() inout @property { return v.ptr; @@ -201,7 +178,7 @@ struct Vector(T, int N) f32* r = &operand.x; f32* res = &result.x; - asm @nogc nothrow + asm { mov R8, l; mov R9, r; @@ -209,11 +186,11 @@ struct Vector(T, int N) movups XMM0, x.offsetof[R8]; movups XMM1, operand.x.offsetof[R9]; } - static if (op == "*") asm @nogc nothrow { mulps XMM0, XMM1; } - else static if (op == "-") asm @nogc nothrow { subps XMM0, XMM1; } - else static if (op == "+") asm @nogc nothrow { addps XMM0, XMM1; } - else static if (op == "/") asm @nogc nothrow { divps XMM0, XMM1; } - asm @nogc nothrow + static if (op == "*") asm { mulps XMM0, XMM1; } + else static if (op == "-") asm { subps XMM0, XMM1; } + else static if (op == "+") asm { addps XMM0, XMM1; } + else static if (op == "/") asm { divps XMM0, XMM1; } + asm { movups result.x.offsetof[R10], XMM0; } @@ -229,7 +206,7 @@ struct Vector(T, int N) f32* r = &other.x; f32* res = &result.x; - asm @nogc nothrow + asm { mov R8, l; mov R9, r; @@ -237,11 +214,11 @@ struct Vector(T, int N) movups XMM0, x.offsetof[R8]; movups XMM1, other.x.offsetof[R9]; } - static if (op == "*") asm @nogc nothrow { mulps XMM0, XMM1; } - else static if (op == "-") asm @nogc nothrow { subps XMM0, XMM1; } - else static if (op == "+") asm @nogc nothrow { addps XMM0, XMM1; } - else static if (op == "/") asm @nogc nothrow { divps XMM0, XMM1; } - asm @nogc nothrow + static if (op == "*") asm { mulps XMM0, XMM1; } + else static if (op == "-") asm { subps XMM0, XMM1; } + else static if (op == "+") asm { addps XMM0, XMM1; } + else static if (op == "/") asm { divps XMM0, XMM1; } + asm { movups result.x.offsetof[R8], XMM0; } @@ -378,7 +355,8 @@ struct Vector(T, int N) } } -struct Matrix(T, int D) +// TODO: fix alignment for non mat4s +align(16) struct Matrix(T, int D) { static assert(D > 0 && D <= 4); @@ -394,9 +372,12 @@ struct Matrix(T, int D) T[N] v; Row[D] rows; MatrixVec[D] vec; + static if (D == 4) mat4 glm_mat; + static if (D == 3) mat3 glm_mat; + static if (D == 2) mat2 glm_mat; } - // TODO: setup @nogc nothrow + // TODO: setup this(U...)(U values) { @@ -505,44 +486,8 @@ struct Matrix(T, int D) { Vec4 opBinary(string op, U)(U x) if (is(U: Vec4) && is(T: f32) && (op == "*")) { - Vec4 result; - - auto res = &result; - auto m = &this; - auto v = &x; - - asm @trusted @nogc nothrow - { - mov R8, m; - mov R9, v; - mov R10, res; - - movups XMM0, vec.offsetof[R8]+00; - movups XMM1, vec.offsetof[R8]+16; - movups XMM2, vec.offsetof[R8]+32; - movups XMM3, vec.offsetof[R8]+48; - - movups XMM4, x.x.offsetof[R9]; - movups XMM5, XMM4; - shufps XMM5, XMM5, 0; - movups XMM6, XMM4; - shufps XMM6, XMM6, 85; - movups XMM7, XMM4; - shufps XMM7, XMM7, 170; - movups XMM8, XMM4; - shufps XMM8, XMM8, 255; - - mulps XMM3, XMM8; - mulps XMM2, XMM7; - addps XMM3, XMM2; - mulps XMM1, XMM6; - addps XMM3, XMM1; - mulps XMM0, XMM5; - addps XMM3, XMM0; - - movups result.x.offsetof[R10], XMM3; - } - + Vec4 result = 0.0; + glm_mat4_mulv(glm_mat.ptr, x.v.ptr, result.v.ptr); return result; } @@ -551,35 +496,7 @@ struct Matrix(T, int D) Matrix result; MatZero(&result); - Row[D] m1 = rows; - Row[D] m2 = x.rows; - - f32 a00 = m1[0][0], a01 = m1[0][1], a02 = m1[0][2], a03 = m1[0][3], - a10 = m1[1][0], a11 = m1[1][1], a12 = m1[1][2], a13 = m1[1][3], - a20 = m1[2][0], a21 = m1[2][1], a22 = m1[2][2], a23 = m1[2][3], - a30 = m1[3][0], a31 = m1[3][1], a32 = m1[3][2], a33 = m1[3][3], - - b00 = m2[0][0], b01 = m2[0][1], b02 = m2[0][2], b03 = m2[0][3], - b10 = m2[1][0], b11 = m2[1][1], b12 = m2[1][2], b13 = m2[1][3], - b20 = m2[2][0], b21 = m2[2][1], b22 = m2[2][2], b23 = m2[2][3], - b30 = m2[3][0], b31 = m2[3][1], b32 = m2[3][2], b33 = m2[3][3]; - - result[0, 0] = a00 * b00 + a10 * b01 + a20 * b02 + a30 * b03; - result[0, 1] = a01 * b00 + a11 * b01 + a21 * b02 + a31 * b03; - result[0, 2] = a02 * b00 + a12 * b01 + a22 * b02 + a32 * b03; - result[0, 3] = a03 * b00 + a13 * b01 + a23 * b02 + a33 * b03; - result[1, 0] = a00 * b10 + a10 * b11 + a20 * b12 + a30 * b13; - result[1, 1] = a01 * b10 + a11 * b11 + a21 * b12 + a31 * b13; - result[1, 2] = a02 * b10 + a12 * b11 + a22 * b12 + a32 * b13; - result[1, 3] = a03 * b10 + a13 * b11 + a23 * b12 + a33 * b13; - result[2, 0] = a00 * b20 + a10 * b21 + a20 * b22 + a30 * b23; - result[2, 1] = a01 * b20 + a11 * b21 + a21 * b22 + a31 * b23; - result[2, 2] = a02 * b20 + a12 * b21 + a22 * b22 + a32 * b23; - result[2, 3] = a03 * b20 + a13 * b21 + a23 * b22 + a33 * b23; - result[3, 0] = a00 * b30 + a10 * b31 + a20 * b32 + a30 * b33; - result[3, 1] = a01 * b30 + a11 * b31 + a21 * b32 + a31 * b33; - result[3, 2] = a02 * b30 + a12 * b31 + a22 * b32 + a32 * b33; - result[3, 3] = a03 * b30 + a13 * b31 + a23 * b32 + a33 * b33; + glm_mat4_mul(glm_mat.ptr, x.glm_mat.ptr, result.glm_mat.ptr); return result; } @@ -600,8 +517,6 @@ struct Matrix(T, int D) struct Quat { - @nogc nothrow: - union { f32[4] v; @@ -685,7 +600,7 @@ Mat4MulASM(Mat4 l, Mat4 r) auto res = &result; // TODO: fix this - asm @trusted @nogc nothrow + asm @trusted { mov R8, lp; mov R9, rp; @@ -788,7 +703,7 @@ Mat4MulASM(Mat4 l, Mat4 r) return result; } -nothrow @nogc pragma(inline): Mat4 +pragma(inline): Mat4 Mat4Identity() { return Mat4( @@ -810,7 +725,7 @@ Mat4Identity(Mat4* mat) (*mat)[3, 3] = 1.0; } -nothrow @nogc pragma(inline): Mat3 + pragma(inline): Mat3 Mat3Identity() { return Mat3( @@ -820,7 +735,7 @@ Mat3Identity() ); } -nothrow @nogc pragma(inline): Mat2 + pragma(inline): Mat2 Mat2Identity() { return Mat2( @@ -918,19 +833,10 @@ Normalize(Quat q) pragma(inline): Mat4 Perspective(f32 fov, f32 aspect, f32 near, f32 far) { - Mat4 mat; - MatZero(&mat); - - f32 f = 1.0 / tanf(fov * 0.5); - f32 fn = 1.0 / (near - far); - - mat[0, 0] = f / aspect; - mat[1, 1] = f; - mat[2, 2] = far * fn; - mat[2, 3] = -1.0; - mat[3, 2] = near * far * fn; - - return mat; + Mat4 res; + MatZero(&res); + glm_perspective(fov, aspect, near, far, res.glm_mat.ptr); + return res; } pragma(inline): Vec3 @@ -954,18 +860,10 @@ Rotate(Quat q, Vec3 vec) pragma(inline): Mat4 LookAt(Vec3 eye, Vec3 center, Vec3 up) { - Vec3 f = Normalize(center - eye); - Vec3 s = CrossN(f, up); - Vec3 u = Cross(s, f); - - Mat4 mat; - - return Mat4( - +s.x, +s.y, +s.z, -Dot(&s, &eye), - +u.x, +u.y, +u.z, -Dot(&u, &eye), - -f.x, -f.y, -f.z, -Dot(&f, &eye), - 0.0, 0.0, 0.0, 1.0, - ); + Mat4 result; + MatZero(&result); + glm_lookat(eye.v.ptr, center.v.ptr, up.v.ptr, result.glm_mat.ptr); + return result; } pragma(inline): Mat4 @@ -995,22 +893,20 @@ pragma(inline): void CrossN(Vec3 a, Vec3 b, Vec3* dst) { Cross(a, b, dst); - Normalize(dst); + glm_vec3_normalize(dst.v.ptr); } pragma(inline): void Cross(Vec3 a, Vec3 b, Vec3* dst) { - dst.x = a.y * b.z - a.z * b.y; - dst.y = a.z * b.x - a.x * b.z; - dst.z = a.x * b.y - a.y * b.x; + glm_vec3_cross(a.v.ptr, b.v.ptr, dst.v.ptr); } pragma(inline): void MatZero(Mat4* mat) { auto v = &mat.vec; - asm @nogc nothrow + asm { mov R8, v; xorps XMM0, XMM0; @@ -1024,114 +920,15 @@ MatZero(Mat4* mat) pragma(inline): void Translate(Mat4* mat, Vec3 vec) { - Mat4Identity(mat); - (*mat)[3, 0] = vec[0]; - (*mat)[3, 1] = vec[1]; - (*mat)[3, 2] = vec[2]; -} - -pragma(inline): void -Rotate(Mat4* mat, f32 angle, Vec3 axis) -{ - Mat4 rot = Rotate(angle, axis); - MultiplyRotate(mat, &rot, mat); -} - -pragma(inline): void -MultiplyRotate(Mat4* m1, Mat4* m2, Mat4* dst) -{ - f32 a00 = (*m1)[0, 0], a01 = (*m1)[0, 1], a02 = (*m1)[0, 2], a03 = (*m1)[0, 3], - a10 = (*m1)[1, 0], a11 = (*m1)[1, 1], a12 = (*m1)[1, 2], a13 = (*m1)[1, 3], - a20 = (*m1)[2, 0], a21 = (*m1)[2, 1], a22 = (*m1)[2, 2], a23 = (*m1)[2, 3], - a30 = (*m1)[3, 0], a31 = (*m1)[3, 1], a32 = (*m1)[3, 2], a33 = (*m1)[3, 3], - - b00 = (*m2)[0, 0], b01 = (*m2)[0, 1], b02 = (*m2)[0, 2], - b10 = (*m2)[1, 0], b11 = (*m2)[1, 1], b12 = (*m2)[1, 2], - b20 = (*m2)[2, 0], b21 = (*m2)[2, 1], b22 = (*m2)[2, 2]; - - (*dst)[0, 0] = a00 * b00 + a10 * b01 + a20 * b02; - (*dst)[0, 1] = a01 * b00 + a11 * b01 + a21 * b02; - (*dst)[0, 2] = a02 * b00 + a12 * b01 + a22 * b02; - (*dst)[0, 3] = a03 * b00 + a13 * b01 + a23 * b02; - - (*dst)[1, 0] = a00 * b10 + a10 * b11 + a20 * b12; - (*dst)[1, 1] = a01 * b10 + a11 * b11 + a21 * b12; - (*dst)[1, 2] = a02 * b10 + a12 * b11 + a22 * b12; - (*dst)[1, 3] = a03 * b10 + a13 * b11 + a23 * b12; - - (*dst)[2, 0] = a00 * b20 + a10 * b21 + a20 * b22; - (*dst)[2, 1] = a01 * b20 + a11 * b21 + a21 * b22; - (*dst)[2, 2] = a02 * b20 + a12 * b21 + a22 * b22; - (*dst)[2, 3] = a03 * b20 + a13 * b21 + a23 * b22; - - (*dst)[3, 0] = a30; - (*dst)[3, 1] = a31; - (*dst)[3, 2] = a32; - (*dst)[3, 3] = a33; -} - -pragma(inline): Mat4 -Rotate(f32 angle, Vec3 axis) -{ - f32 c = cosf(angle); - - Vec3 axis_normal = Normalize(axis); - Vec3 v = axis_normal * (1.0 - c); - Vec3 vs = axis_normal * sinf(angle); - - Mat4 mat; - mat.vec[0].xyz = axis_normal * v.x; - mat.vec[1].xyz = axis_normal * v.y; - mat.vec[2].xyz = axis_normal * v.z; - - mat[0, 0] += c; mat[1, 0] -= vs.z; mat[2, 0] += vs.y; - mat[0, 1] += vs.z; mat[1, 1] += c; mat[2, 1] -= vs.x; - mat[0, 2] -= vs.y; mat[1, 2] += vs.x; mat[2, 2] += c; - - mat[0, 3] = mat[1, 3] = mat[3, 0] = mat[3, 2] = 0.0; - mat[3, 3] = 1.0; - - return mat; + glm_translate(mat.glm_mat.ptr, vec.v.ptr); } pragma(inline): Mat4 Inverse(Mat4 mat) { - // TODO: SIMD this - f32 a = mat[0, 0], b = mat[0, 1], c = mat[0, 2], d = mat[0, 3], - e = mat[1, 0], f = mat[1, 1], g = mat[1, 2], h = mat[1, 3], - i = mat[2, 0], j = mat[2, 1], k = mat[2, 2], l = mat[2, 3], - m = mat[3, 0], n = mat[3, 1], o = mat[3, 2], p = mat[3, 3]; - - f32 c1 = k * p - l * o, c2 = c * h - d * g, c3 = i * p - l * m, - c4 = a * h - d * e, c5 = j * p - l * n, c6 = b * h - d * f, - c7 = i * n - j * m, c8 = a * f - b * e, c9 = j * o - k * n, - c10 = b * g - c * f, c11 = i * o - k * m, c12 = a * g - c * e; - - f32 idt = 1.0/(c8*c1+c4*c9+c10*c3+c2*c7-c12*c5-c6*c11), ndt = -idt; - Mat4 res; - - res[0, 0] = (f * c1 - g * c5 + h * c9) * idt; - res[0, 1] = (b * c1 - c * c5 + d * c9) * ndt; - res[0, 2] = (n * c2 - o * c6 + p * c10) * idt; - res[0, 3] = (j * c2 - k * c6 + l * c10) * ndt; - - res[1, 0] = (e * c1 - g * c3 + h * c11) * ndt; - res[1, 1] = (a * c1 - c * c3 + d * c11) * idt; - res[1, 2] = (m * c2 - o * c4 + p * c12) * ndt; - res[1, 3] = (i * c2 - k * c4 + l * c12) * idt; - - res[2, 0] = (e * c5 - f * c3 + h * c7) * idt; - res[2, 1] = (a * c5 - b * c3 + d * c7) * ndt; - res[2, 2] = (m * c6 - n * c4 + p * c8) * idt; - res[2, 3] = (i * c6 - j * c4 + l * c8) * ndt; - - res[3, 0] = (e * c9 - f * c11 + g * c7) * ndt; - res[3, 1] = (a * c9 - b * c11 + c * c7) * idt; - res[3, 2] = (m * c10 - n * c12 + o * c8) * ndt; - res[3, 3] = (i * c10 - j * c12 + k * c8) * idt; - + MatZero(&res); + glm_mat4_inv(mat.glm_mat.ptr, res.glm_mat.ptr); return res; } @@ -1154,20 +951,6 @@ Remap(f32 v, f32 in_min, f32 in_max, f32 out_min, f32 out_max) return Mix(out_min, out_max, t); } -pragma(inline): T -SquaredMagnitude(T)(T v) if (IsVector!(T)) -{ - T sum_squares = 0; - mixin(GenerateLoop!("sum_squares += v.v[@] * v.v[@];", v._N)()); - return sum_squares; -} - -pragma(inline): T -SquaredDistTo(T)(T l, T r) if (IsVector!(T)) -{ - return SquaredMagnitude(r - l); -} - unittest { enum FLOAT_MAX = f32.max; @@ -1199,19 +982,6 @@ unittest return cast(f32)(rand())/cast(f32)(RAND_MAX + 1.0); } - Mat4 RandomMat4() - { - Mat4 mat; - Mat4Identity(&mat); - mat[3, 0] = RandomFloat(); - mat[3, 1] = RandomFloat(); - mat[3, 2] = RandomFloat(); - - Rotate(&mat, RandomFloat(), Vec3(RandomFloat(), RandomFloat(), RandomFloat())); - - return mat; - } - { // Vec2 arithmetic Vec2 v1 = Vec2(5.0, 10.0); Vec2 v2 = Vec2(2.5, 5.0); @@ -1358,21 +1128,4 @@ unittest assert(v3 == v4, "Vec3 CrossN failure"); } - - { // LookAt - Vec3 eye = Vec3(0.024, 14.6, 67.04); - Vec3 dir = Vec3(0.0, 0.0, -1.0); - Vec3 up = Vec3(0.0, 1.0, 0.0); - - Vec3 center = eye + dir; - Mat4 view1 = LookAt(eye, center, up); - Mat4 view2 = Mat4( - 1.0, -0.0, 0.0, -0.024, - 0.0, 1.0, 0.0, -14.6, - -0.0, -0.0, 1.0, 67.04, - 0.0, 0.0, 0.0, 1.0 - ); - - assert(view1 == view2, "LookAt failure"); - } } diff --git a/src/to_rework/math.d b/src/to_rework/math.d new file mode 100644 index 0000000..93d1081 --- /dev/null +++ b/src/to_rework/math.d @@ -0,0 +1,1378 @@ +import aliases; +import util; +import std.math; +import std.math.algebraic; +import core.stdc.math : tanf, cosf, sinf, sqrtf; +import std.traits; +import inteli; +import std.meta; +import std.format; +import std.stdio; + +T AlignPow2(T)(T v, T a) +{ + return (v + a - 1) & ~(a - 1); +} + +f32 Radians(f32 deg) +{ + return deg * (PI / 180.0); +} + +// TODO: Clean this mess up + +enum IsVector(T) = is(T : Vector!U, U...); + +struct Vector(T, int N) +{ + static assert(N > 0 && N <= 4); + + enum _N = N; + alias T _T; + + union + { + T[N] v; + struct + { + T x; + alias x r; + + static if (N > 1) + { + T y; + alias y g; + } + + static if (N > 2) + { + T z; + alias z b; + } + + static if (N > 3) + { + T w; + alias w a; + } + } + } + + nothrow @nogc: + + this(Vec3, f32)(Vec3 v3, f32 f) if (N == 4 && is(T: f32)) + { + x = v3.x; + y = v3.y; + z = v3.z; + w = f; + } + + this(Args...)(Args args) + { + static if (args.length == 1) + { + opAssign!(Args[0])(args[0]); + } + else static if (args.length == N) + { + mixin(GenerateLoop!("v[@] = args[@];", N)()); + } + else static if (args.length == 2 && N == 4) + { + v[0] = args[0]; + v[1] = args[0]; + v[2] = args[0]; + v[3] = args[1]; + } + else + { + static assert(false, "Invalid Vector constructor"); + } + } + + ref Vector opAssign(U)(U x) if (isAssignable!(T, U)) + { + mixin(GenerateLoop!("v[@] = x;", N)()); + return this; + } + + ref Vector opAssign(U)(U arr) if (isStaticArray!(U) && isAssignable!(T, typeof(arr[0])) && arr.length == N) + { + mixin(GenerateLoop!("v[@] = arr[@];", N)()); + return this; + } + + ref Vector opAssign(U)(U arr) if (isDynamicArray!(U) && isAssignable!(T, typeof(arr[0]))) + { + mixin(GenerateLoop!("v[@] = arr[@];", N)()); + return this; + } + + ref Vector opAssign(U)(U u) if (is(U : Vector)) + { + v[] = u.v[]; + return this; + } + + ref Vector opAssign(U)(U x) if (IsVector!(U) && isAssignable!(T, U._T) && (!is(U: Vector)) && (U._N == _N)) + { + mixin(GenerateLoop!("v[@] = x.v[@];", N)()); + return this; + } + + inout(T)* ptr() inout @property + { + return v.ptr; + } + + bool opEquals(U)(U other) if (is(U: Vector!(T, N))) + { + bool result = true; + + foreach(i; 0 .. N) + { + if (fabs(v[i] - other.v[i]) > 0.0000009) + { + result = false; + break; + } + } + + return result; + } + + int opDollar() + { + return N; + } + + T[] opSlice() + { + return v[]; + } + + Vector opUnary(string op)() if (op == "+" || op == "-" || op == "~" || op == "!") + { + Vector result; + mixin(GenerateLoop!("res.v[@] = " ~ op ~ " v[@];", N)()); + return res; + } + + ref Vector opOpAssign(string op, U)(U value) if (is(U: Vector)) + { + mixin(GenerateLoop!("v[@] " ~ op ~ "= value.v[@];", N)()); + return this; + } + + ref Vector opOpAssign(string op, U)(U value) if (IsConvertible!(U)) + { + Vector conv = value; + return opOpAssign!(op)(conv); + } + + @property auto opDispatch(string op, U = void)() if (ValidSwizzle!(op) && op.length <= 4) + { + Vector!(T, op.length) result; + enum index_tuple = SwizzleTuple!(op); + static foreach(i, index; index_tuple) + { + result.v[i] = v[index]; + } + return result; + } + + @property void opDispatch(string op, U)(U x) if ((op.length > 1) && ValidUniqueSwizzle!(op) && is(typeof(Vector!(T, op.length)(x)))) + { + Vector!(T, op.length) conv = x; + enum index_tuple = SwizzleTuple!(op); + static foreach(i, index; index_tuple) + { + v[index] = conv[i]; + } + } + + static if (N == 4) + { + Vector opBinary(string op, U)(U operand) if ((is(U: Vector!(f32, 4)) && is(T: f32)) && (op == "*" || op == "+" || op == "-" || op == "/")) + { + Vector result; + f32* l = &x; + f32* r = &operand.x; + f32* res = &result.x; + + asm @nogc nothrow + { + mov R8, l; + mov R9, r; + mov R10, res; + movups XMM0, x.offsetof[R8]; + movups XMM1, operand.x.offsetof[R9]; + } + static if (op == "*") asm @nogc nothrow { mulps XMM0, XMM1; } + else static if (op == "-") asm @nogc nothrow { subps XMM0, XMM1; } + else static if (op == "+") asm @nogc nothrow { addps XMM0, XMM1; } + else static if (op == "/") asm @nogc nothrow { divps XMM0, XMM1; } + asm @nogc nothrow + { + movups result.x.offsetof[R10], XMM0; + } + + return result; + } + + Vector opBinary(string op, U)(U operand) if (IsConvertible!(U) && (op == "*" || op == "+" || op == "-" || op == "/")) + { + Vector result; + Vector other = operand; + f32* l = &x; + f32* r = &other.x; + f32* res = &result.x; + + asm @nogc nothrow + { + mov R8, l; + mov R9, r; + mov R10, res; + movups XMM0, x.offsetof[R8]; + movups XMM1, other.x.offsetof[R9]; + } + static if (op == "*") asm @nogc nothrow { mulps XMM0, XMM1; } + else static if (op == "-") asm @nogc nothrow { subps XMM0, XMM1; } + else static if (op == "+") asm @nogc nothrow { addps XMM0, XMM1; } + else static if (op == "/") asm @nogc nothrow { divps XMM0, XMM1; } + asm @nogc nothrow + { + movups result.x.offsetof[R8], XMM0; + } + + return result; + } + } + else + { + Vector opBinary(string op, U)(U operand) if (is(U: Vector) && U._N == N && (op == "*" || op == "+" || op == "-" || op == "/")) + { + Vector res; + mixin(GenerateLoop!("res.v[@] = v[@] " ~ op ~ " operand.v[@];", N)()); + return res; + } + + Vector opBinary(string op, U)(U operand) if (IsConvertible!(U) && (op == "*" || op == "+" || op == "-" || op == "/")) + { + Vector res; + Vector other = operand; + mixin(GenerateLoop!("res.v[@] = v[@] " ~ op ~ " other.v[@];", N)()); + return res; + } + } + + + + ref T opIndex(size_t i) + { + return v[i]; + } + + T opIndexAssign(U : T)(U x, size_t i) + { + return v[i] = x; + } + + U opCast(U)() if (IsVector!(U) && (U._N == _N)) + { + U result; + mixin(GenerateLoop!("res.v[@] = cast(U._T)v[@];", N)()); + return result; + } + + template IsConvertible(T) + { + enum bool IsConvertible = (!is(T : Vector)) && is(typeof({ T x; Vector v = x; }())); + } + + template SwizzleIndex(char c) + { + static if ((c == 'x' || c == 'r') && N > 0) + enum SwizzleIndex = 0; + else static if ((c == 'y' || c == 'g') && N > 1) + enum SwizzleIndex = 1; + else static if ((c == 'z' || c == 'b') && N > 2) + enum SwizzleIndex = 2; + else static if ((c == 'w' || c == 'a') && N > 3) + enum SwizzleIndex = 3; + else + enum SwizzleIndex = -1; + } + + template SwizzleSet(char c) + { + static if (c == 'x' || c == 'y' || c == 'z' || c == 'w') + enum SwizzleSet = 0; + else static if (c == 'r' || c == 'g' || c == 'b' || c == 'a') + enum SwizzleSet = 1; + else + enum SwizzleSet = -1; + } + + template SwizzleTuple(string op) + { + enum op_length = op.length; + static if (op.length == 0) + enum SwizzleTuple = []; + else + enum SwizzleTuple = [ SwizzleIndex!(op[0])] ~ SwizzleTuple!(op[1 .. op.length]); + } + + template SearchString(char c, string s) + { + static if (s.length == 0) + { + enum bool result = false; + } + else + { + enum string tail = s[1 .. s.length]; + enum bool result = (s[0] == c) || SearchString!(c, tail).result; + } + } + + template UniqueChars(string s) + { + static if (s.length == 1) + { + enum bool result = true; + } + else + { + enum tail = s[1 .. s.length]; + enum bool result = !(SearchString!(s[0], tail).result) && UniqueChars!(tail).result; + } + } + + template ValidSwizzle(string op, int last_swizzle = -1) + { + static if (op.length == 0) + { + enum bool ValidSwizzle = true; + } + else + { + enum length = op.length; + enum int swizzle_set = SwizzleSet!(op[0]); + enum bool valid_swizzle_set = (last_swizzle == -1 || (swizzle_set == last_swizzle)); + enum bool ValidSwizzle = (SwizzleIndex!(op[0]) != -1) && valid_swizzle_set && ValidSwizzle!(op[1 .. length], swizzle_set); + } + } + + template ValidUniqueSwizzle(string op) + { + static if (ValidSwizzle!(op)) + { + enum ValidUniqueSwizzle = UniqueChars!(op).result; + } + else + { + enum ValidUniqueSwizzle = false; + } + } +} + +struct Matrix(T, int D) +{ + static assert(D > 0 && D <= 4); + + alias Vector!(T, D) MatrixVec; + alias T _T; + alias T[4] Row; + + enum N = D*D; + enum _D = D; + + union + { + T[N] v; + Row[D] rows; + MatrixVec[D] vec; + } + + // TODO: setup @nogc nothrow + + this(U...)(U values) + { + static if ((U.length == N) && allSatisfy!(IsTypeAssignable, U)) + { + static foreach(i, x; values) + { + v[i] = x; + } + } + else static if ((U.length == 1) && (isAssignable!(U[0])) && (!is(U[0] : Matrix))) + { + v[] = values[0]; + } + else static assert(false, "Cannot construct matrix with provided parameters"); + } + + this(U)(T x) + { + static foreach(i; 0 .. N) + { + v[i] = x; + } + } + + @property inout(T)* ptr() inout + { + return v.ptr; + } + + ref Matrix opAssign(U : T)(U x) + { + static foreach(i; 0 .. N) + { + v[i] = x; + } + + return this; + } + + ref Matrix opAssign(U : Matrix)(U x) + { + static foreach(i; 0 .. N) + { + v[i] = x.v[i]; + } + + return this; + } + + ref Matrix opAssign(U)(U x) if (IsMatrixInstantiation!(U) && is(U._T : _T) && (!is(U: Matrix) && (U.N != N))) + { + static foreach(i; 0 .. N) + { + v[i] = x.v[i]; + } + + return this; + } + + ref T opIndex(size_t i, size_t j) + { + return v[(i * D) + j]; + } + + T opIndexAssign(U: T)(U x, size_t i, size_t j) + { + return v[(i * D) + j] = x; + } + + bool opEquals(U)(U other) if (is(U: Matrix!(T, D))) + { + bool result = true; + + static foreach(i; 0 .. N) + { + if (fabs(this.v[i] - other.v[i]) > 0.0000009) + { + result = false; + } + } + + return result; + } + + Matrix opBinary(string op)(T scalar) if (op == "*") + { + Matrix result; + + static foreach(i; 0 .. N) + { + result.v[i] = v[i] * scalar; + } + + return result; + } + + MatrixVec opBinary(string op)(T factor) if (op == "*") + { + Matrix result; + + return result; + } + + static if (D == 4) + { + Vec4 opBinary(string op, U)(U x) if (is(U: Vec4) && is(T: f32) && (op == "*")) + { + Vec4 result; + + auto res = &result; + auto m = &this; + auto v = &x; + + asm @trusted @nogc nothrow + { + mov R8, m; + mov R9, v; + mov R10, res; + + movups XMM0, vec.offsetof[R8]+00; + movups XMM1, vec.offsetof[R8]+16; + movups XMM2, vec.offsetof[R8]+32; + movups XMM3, vec.offsetof[R8]+48; + + movups XMM4, x.x.offsetof[R9]; + movups XMM5, XMM4; + shufps XMM5, XMM5, 0; + movups XMM6, XMM4; + shufps XMM6, XMM6, 85; + movups XMM7, XMM4; + shufps XMM7, XMM7, 170; + movups XMM8, XMM4; + shufps XMM8, XMM8, 255; + + mulps XMM3, XMM8; + mulps XMM2, XMM7; + addps XMM3, XMM2; + mulps XMM1, XMM6; + addps XMM3, XMM1; + mulps XMM0, XMM5; + addps XMM3, XMM0; + + movups result.x.offsetof[R10], XMM3; + } + + return result; + } + + Matrix opBinary(string op, U)(U x) if (is(U: Matrix!(T, D)) && is(T: f32) && D == 4 && (op == "*")) + { + Matrix result; + MatZero(&result); + + Row[D] m1 = rows; + Row[D] m2 = x.rows; + + f32 a00 = m1[0][0], a01 = m1[0][1], a02 = m1[0][2], a03 = m1[0][3], + a10 = m1[1][0], a11 = m1[1][1], a12 = m1[1][2], a13 = m1[1][3], + a20 = m1[2][0], a21 = m1[2][1], a22 = m1[2][2], a23 = m1[2][3], + a30 = m1[3][0], a31 = m1[3][1], a32 = m1[3][2], a33 = m1[3][3], + + b00 = m2[0][0], b01 = m2[0][1], b02 = m2[0][2], b03 = m2[0][3], + b10 = m2[1][0], b11 = m2[1][1], b12 = m2[1][2], b13 = m2[1][3], + b20 = m2[2][0], b21 = m2[2][1], b22 = m2[2][2], b23 = m2[2][3], + b30 = m2[3][0], b31 = m2[3][1], b32 = m2[3][2], b33 = m2[3][3]; + + result[0, 0] = a00 * b00 + a10 * b01 + a20 * b02 + a30 * b03; + result[0, 1] = a01 * b00 + a11 * b01 + a21 * b02 + a31 * b03; + result[0, 2] = a02 * b00 + a12 * b01 + a22 * b02 + a32 * b03; + result[0, 3] = a03 * b00 + a13 * b01 + a23 * b02 + a33 * b03; + result[1, 0] = a00 * b10 + a10 * b11 + a20 * b12 + a30 * b13; + result[1, 1] = a01 * b10 + a11 * b11 + a21 * b12 + a31 * b13; + result[1, 2] = a02 * b10 + a12 * b11 + a22 * b12 + a32 * b13; + result[1, 3] = a03 * b10 + a13 * b11 + a23 * b12 + a33 * b13; + result[2, 0] = a00 * b20 + a10 * b21 + a20 * b22 + a30 * b23; + result[2, 1] = a01 * b20 + a11 * b21 + a21 * b22 + a31 * b23; + result[2, 2] = a02 * b20 + a12 * b21 + a22 * b22 + a32 * b23; + result[2, 3] = a03 * b20 + a13 * b21 + a23 * b22 + a33 * b23; + result[3, 0] = a00 * b30 + a10 * b31 + a20 * b32 + a30 * b33; + result[3, 1] = a01 * b30 + a11 * b31 + a21 * b32 + a31 * b33; + result[3, 2] = a02 * b30 + a12 * b31 + a22 * b32 + a32 * b33; + result[3, 3] = a03 * b30 + a13 * b31 + a23 * b32 + a33 * b33; + + return result; + } + } + + template IsTypeAssignable(U) + { + enum bool IsTypeAssignable = std.traits.isAssignable!(T, U); + } + + template IsMatrixInstantiation(U) + { + static void IsMatrix(T, int D)(Matrix!(T, D) x) {} + + enum bool IsMatrixInstantiation = is(typeof(IsMatrix(U.init))); + } +} + +struct Quat +{ + @nogc nothrow: + + union + { + f32[4] v; + Vec4 vec; + struct + { + f32 x; + f32 y; + f32 z; + f32 w; + }; + }; + + this(f32 w, f32 x, f32 y, f32 z) + { + vec.x = x; + vec.y = y; + vec.z = z; + vec.w = w; + } + + U opCast(U)() if (is(U: Mat4)) + { + f32 norm = Norm(&vec); + f32 s = norm > 0.0 ? 2.0 / norm : 0.0; + + f32 _x = x; + f32 _y = y; + f32 _z = z; + f32 _w = w; + + f32 xx = s * x * x; f32 xy = s * x * y; f32 wx = s * w * x; + f32 yy = s * y * y; f32 yz = s * y * z; f32 wy = s * w * y; + f32 zz = s * z * z; f32 xz = s * x * z; f32 wz = s * w * z; + + U mat; + + mat[0, 0] = 1.0 - yy - zz; + mat[1, 1] = 1.0 - xx - zz; + mat[2, 2] = 1.0 - xx - yy; + + mat[0, 1] = xy + wz; + mat[1, 2] = yz + wx; + mat[2, 0] = xz + wy; + + mat[1, 0] = xy - wz; + mat[2, 1] = yz - wx; + mat[0, 2] = xz - wy; + + mat[0, 3] = 0.0; + mat[1, 3] = 0.0; + mat[2, 3] = 0.0; + mat[3, 0] = 0.0; + mat[3, 1] = 0.0; + mat[3, 2] = 0.0; + mat[3, 3] = 1.0; + + return mat; + } + + Quat opBinary(string op, U)(U r) if (op == "*" && is(U: Quat)) + { + Quat q; + + q.x = this.w * r.x + this.x * r.w + this.y * r.z - this.z * r.y; + q.y = this.w * r.y - this.x * r.z + this.y * r.w + this.z * r.x; + q.z = this.w * r.z + this.x * r.y - this.y * r.x + this.z * r.w; + q.w = this.w * r.w - this.x * r.x - this.y * r.y - this.z * r.z; + + return q; + } +} + +Mat4 +Mat4MulASM(Mat4 l, Mat4 r) +{ + Mat4 result; + + auto lp = &l; + auto rp = &r; + auto res = &result; + + // TODO: fix this + asm @trusted @nogc nothrow + { + mov R8, lp; + mov R9, rp; + mov R10, res; + + movups XMM0, [R8]; + movups XMM1, [R9+00]; + movups XMM2, [R9+16]; + movups XMM3, [R9+32]; + movups XMM4, [R9+48]; + + movups XMM6, XMM1; + shufps XMM6, XMM6, 0; // XMM5 = vec.xxxx; + mulps XMM6, XMM0; // XMM6 = col1; + + movups XMM7, XMM2; + shufps XMM7, XMM7, 0; + mulps XMM7, XMM0; // XMM7 = col2; + + movups XMM8, XMM3; + shufps XMM8, XMM8, 0; + mulps XMM8, XMM0; // XMM8 = col3; + + movups XMM9, XMM3; + shufps XMM9, XMM9, 0; + mulps XMM9, XMM0; // XMM9 = col4; + + movups XMM0, [R8+16]; + + movups XMM5, XMM1; + shufps XMM5, XMM5, 85; // XMM5 = vec.yyyy; + mulps XMM5, XMM0; + addps XMM6, XMM5; + + movups XMM5, XMM2; + shufps XMM5, XMM5, 85; + mulps XMM5, XMM0; + addps XMM7, XMM5; + + movups XMM5, XMM3; + shufps XMM5, XMM5, 85; + mulps XMM5, XMM0; + addps XMM8, XMM5; + + movups XMM5, XMM4; + shufps XMM5, XMM5, 85; + mulps XMM5, XMM0; + addps XMM9, XMM5; + + movups XMM0, [R8+32]; + + movups XMM5, XMM1; + shufps XMM5, XMM5, 170; // XMM5 = vec.zzzz; + mulps XMM5, XMM0; + addps XMM6, XMM5; + + movups XMM5, XMM2; + shufps XMM5, XMM5, 170; + mulps XMM5, XMM0; + addps XMM7, XMM5; + + movups XMM5, XMM3; + shufps XMM5, XMM5, 170; + mulps XMM5, XMM0; + addps XMM8, XMM5; + + movups XMM5, XMM4; + shufps XMM5, XMM5, 170; + mulps XMM5, XMM0; + addps XMM9, XMM5; + + movups XMM0, [R8+48]; + + movups XMM5, XMM1; + shufps XMM5, XMM5, 255; // XMM5 = vec.wwww; + mulps XMM5, XMM0; + addps XMM6, XMM5; + + movups XMM5, XMM2; + shufps XMM5, XMM5, 255; + mulps XMM5, XMM0; + addps XMM7, XMM5; + + movups XMM5, XMM3; + shufps XMM5, XMM5, 255; + mulps XMM5, XMM0; + addps XMM8, XMM5; + + movups XMM5, XMM4; + shufps XMM5, XMM5, 255; + mulps XMM5, XMM0; + addps XMM9, XMM5; + + movups [R10+00], XMM6; + movups [R10+16], XMM7; + movups [R10+32], XMM8; + movups [R10+48], XMM9; + } + + return result; +} + +nothrow @nogc pragma(inline): Mat4 +Mat4Identity() +{ + return Mat4( + 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0 + ); +} + +pragma(inline): void +Mat4Identity(Mat4* mat) +{ + MatZero(mat); + + (*mat)[0, 0] = 1.0; + (*mat)[1, 1] = 1.0; + (*mat)[2, 2] = 1.0; + (*mat)[3, 3] = 1.0; +} + +nothrow @nogc pragma(inline): Mat3 +Mat3Identity() +{ + return Mat3( + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 + ); +} + +nothrow @nogc pragma(inline): Mat2 +Mat2Identity() +{ + return Mat2( + 1.0, 0.0, + 0.0, 1.0 + ); +} + +pragma(inline): void +QuatFromAxis(Quat* q, f32 angle, Vec3 axis) +{ + Vec3 k; + + f32 a = angle * 0.5f; + f32 c = cosf(a); + f32 s = sinf(a); + + k = Normalize(axis); + + q.x = s * k.x; + q.y = s * k.y; + q.z = s * k.z; + q.w = c; +} + +pragma(inline): f32 +Dot(Vec2* l, Vec2* r) +{ + return l.x * r.x + l.y * r.y; +} + +pragma(inline): f32 +Dot(Vec3* l, Vec3* r) +{ + return l.x * r.x + l.y * r.y + l.z * r.z; +} + +pragma(inline): f32 +Dot(Vec4* l, Vec4* r) +{ + // TODO: SIMD this + return l.x * r.x + l.y * r.y + l.z * r.z + l.w * r.w; +} + +pragma(inline): f32 +Norm(Vec3* v) +{ + return sqrtf(Dot(v, v)); +} + +pragma(inline): f32 +Norm(Vec4* v) +{ + // TODO: SIMD this + return sqrtf(Dot(v, v)); +} + +pragma(inline): void +Normalize(T)(T* vec) if (is(T: Vec2) || is(T: Vec3) || is(T: Vec4)) +{ + f32 length = Norm(vec); + + if (length < f32.epsilon) + { + mixin(GenerateLoop!("vec.v[@] = 0.0;", vec._N)()); + } + else + { + mixin(GenerateLoop!("vec.v[@] *= (1.0 / length);", vec._N)()); + } +} + +pragma(inline): T +Normalize(T)(T vec) if (is(T: Vec2) || is(T: Vec3) || is(T: Vec4)) +{ + Normalize(&vec); + return vec; +} + +pragma(inline): Quat +Normalize(Quat q) +{ + f32 dot = Norm(&q.vec); + + if (dot <= 0.0) + { + q = Quat(1.0, 0.0, 0.0, 0.0); + } + + q.vec *= 1.0 / sqrtf(dot); + + return q; +} + +pragma(inline): Mat4 +Perspective(f32 fov, f32 aspect, f32 near, f32 far) +{ + Mat4 mat; + MatZero(&mat); + + f32 f = 1.0 / tanf(fov * 0.5); + f32 fn = 1.0 / (near - far); + + mat[0, 0] = f / aspect; + mat[1, 1] = f; + mat[2, 2] = far * fn; + mat[2, 3] = -1.0; + mat[3, 2] = near * far * fn; + + return mat; +} + +pragma(inline): Vec3 +Rotate(Quat q, Vec3 vec) +{ + Quat p = Normalize(q); + + Vec3 i = p.vec.xyz; + f32 r = p.vec.w; + + Vec3 v1 = i * (2.0 * Dot(&i, &vec)); + Vec3 v2 = i * (r * r - Dot(&i, &i)); + v1 += v2; + + v2 = i * vec; + v2 = v2 * (2.0 * r); + + return v1 + v2; +} + +pragma(inline): Mat4 +LookAt(Vec3 eye, Vec3 center, Vec3 up) +{ + Vec3 f = Normalize(center - eye); + Vec3 s = CrossN(f, up); + Vec3 u = Cross(s, f); + + Mat4 mat; + + return Mat4( + +s.x, +s.y, +s.z, -Dot(&s, &eye), + +u.x, +u.y, +u.z, -Dot(&u, &eye), + -f.x, -f.y, -f.z, -Dot(&f, &eye), + 0.0, 0.0, 0.0, 1.0, + ); +} + +pragma(inline): Mat4 +Look(Vec3 eye, Vec3 dir, Vec3 up) +{ + return LookAt(eye, eye + dir, up); +} + +pragma(inline): Vec3 +Cross(Vec3 a, Vec3 b) +{ + Vec3 c; + Cross(a, b, &c); + return c; +} + +pragma(inline): Vec3 +CrossN(Vec3 a, Vec3 b) +{ + Vec3 c; + Cross(a, b, &c); + Normalize(&c); + return c; +} + +pragma(inline): void +CrossN(Vec3 a, Vec3 b, Vec3* dst) +{ + Cross(a, b, dst); + Normalize(dst); +} + +pragma(inline): void +Cross(Vec3 a, Vec3 b, Vec3* dst) +{ + dst.x = a.y * b.z - a.z * b.y; + dst.y = a.z * b.x - a.x * b.z; + dst.z = a.x * b.y - a.y * b.x; +} + +pragma(inline): void +MatZero(Mat4* mat) +{ + auto v = &mat.vec; + asm @nogc nothrow + { + mov R8, v; + xorps XMM0, XMM0; + movups mat.vec.offsetof[R8]+00, XMM0; + movups mat.vec.offsetof[R8]+16, XMM0; + movups mat.vec.offsetof[R8]+32, XMM0; + movups mat.vec.offsetof[R8]+48, XMM0; + } +} + +pragma(inline): void +Translate(Mat4* mat, Vec3 vec) +{ + Mat4Identity(mat); + (*mat)[3, 0] = vec[0]; + (*mat)[3, 1] = vec[1]; + (*mat)[3, 2] = vec[2]; +} + +pragma(inline): void +Rotate(Mat4* mat, f32 angle, Vec3 axis) +{ + Mat4 rot = Rotate(angle, axis); + MultiplyRotate(mat, &rot, mat); +} + +pragma(inline): void +MultiplyRotate(Mat4* m1, Mat4* m2, Mat4* dst) +{ + f32 a00 = (*m1)[0, 0], a01 = (*m1)[0, 1], a02 = (*m1)[0, 2], a03 = (*m1)[0, 3], + a10 = (*m1)[1, 0], a11 = (*m1)[1, 1], a12 = (*m1)[1, 2], a13 = (*m1)[1, 3], + a20 = (*m1)[2, 0], a21 = (*m1)[2, 1], a22 = (*m1)[2, 2], a23 = (*m1)[2, 3], + a30 = (*m1)[3, 0], a31 = (*m1)[3, 1], a32 = (*m1)[3, 2], a33 = (*m1)[3, 3], + + b00 = (*m2)[0, 0], b01 = (*m2)[0, 1], b02 = (*m2)[0, 2], + b10 = (*m2)[1, 0], b11 = (*m2)[1, 1], b12 = (*m2)[1, 2], + b20 = (*m2)[2, 0], b21 = (*m2)[2, 1], b22 = (*m2)[2, 2]; + + (*dst)[0, 0] = a00 * b00 + a10 * b01 + a20 * b02; + (*dst)[0, 1] = a01 * b00 + a11 * b01 + a21 * b02; + (*dst)[0, 2] = a02 * b00 + a12 * b01 + a22 * b02; + (*dst)[0, 3] = a03 * b00 + a13 * b01 + a23 * b02; + + (*dst)[1, 0] = a00 * b10 + a10 * b11 + a20 * b12; + (*dst)[1, 1] = a01 * b10 + a11 * b11 + a21 * b12; + (*dst)[1, 2] = a02 * b10 + a12 * b11 + a22 * b12; + (*dst)[1, 3] = a03 * b10 + a13 * b11 + a23 * b12; + + (*dst)[2, 0] = a00 * b20 + a10 * b21 + a20 * b22; + (*dst)[2, 1] = a01 * b20 + a11 * b21 + a21 * b22; + (*dst)[2, 2] = a02 * b20 + a12 * b21 + a22 * b22; + (*dst)[2, 3] = a03 * b20 + a13 * b21 + a23 * b22; + + (*dst)[3, 0] = a30; + (*dst)[3, 1] = a31; + (*dst)[3, 2] = a32; + (*dst)[3, 3] = a33; +} + +pragma(inline): Mat4 +Rotate(f32 angle, Vec3 axis) +{ + f32 c = cosf(angle); + + Vec3 axis_normal = Normalize(axis); + Vec3 v = axis_normal * (1.0 - c); + Vec3 vs = axis_normal * sinf(angle); + + Mat4 mat; + mat.vec[0].xyz = axis_normal * v.x; + mat.vec[1].xyz = axis_normal * v.y; + mat.vec[2].xyz = axis_normal * v.z; + + mat[0, 0] += c; mat[1, 0] -= vs.z; mat[2, 0] += vs.y; + mat[0, 1] += vs.z; mat[1, 1] += c; mat[2, 1] -= vs.x; + mat[0, 2] -= vs.y; mat[1, 2] += vs.x; mat[2, 2] += c; + + mat[0, 3] = mat[1, 3] = mat[3, 0] = mat[3, 2] = 0.0; + mat[3, 3] = 1.0; + + return mat; +} + +pragma(inline): Mat4 +Inverse(Mat4 mat) +{ + // TODO: SIMD this + f32 a = mat[0, 0], b = mat[0, 1], c = mat[0, 2], d = mat[0, 3], + e = mat[1, 0], f = mat[1, 1], g = mat[1, 2], h = mat[1, 3], + i = mat[2, 0], j = mat[2, 1], k = mat[2, 2], l = mat[2, 3], + m = mat[3, 0], n = mat[3, 1], o = mat[3, 2], p = mat[3, 3]; + + f32 c1 = k * p - l * o, c2 = c * h - d * g, c3 = i * p - l * m, + c4 = a * h - d * e, c5 = j * p - l * n, c6 = b * h - d * f, + c7 = i * n - j * m, c8 = a * f - b * e, c9 = j * o - k * n, + c10 = b * g - c * f, c11 = i * o - k * m, c12 = a * g - c * e; + + f32 idt = 1.0/(c8*c1+c4*c9+c10*c3+c2*c7-c12*c5-c6*c11), ndt = -idt; + + Mat4 res; + + res[0, 0] = (f * c1 - g * c5 + h * c9) * idt; + res[0, 1] = (b * c1 - c * c5 + d * c9) * ndt; + res[0, 2] = (n * c2 - o * c6 + p * c10) * idt; + res[0, 3] = (j * c2 - k * c6 + l * c10) * ndt; + + res[1, 0] = (e * c1 - g * c3 + h * c11) * ndt; + res[1, 1] = (a * c1 - c * c3 + d * c11) * idt; + res[1, 2] = (m * c2 - o * c4 + p * c12) * ndt; + res[1, 3] = (i * c2 - k * c4 + l * c12) * idt; + + res[2, 0] = (e * c5 - f * c3 + h * c7) * idt; + res[2, 1] = (a * c5 - b * c3 + d * c7) * ndt; + res[2, 2] = (m * c6 - n * c4 + p * c8) * idt; + res[2, 3] = (i * c6 - j * c4 + l * c8) * ndt; + + res[3, 0] = (e * c9 - f * c11 + g * c7) * ndt; + res[3, 1] = (a * c9 - b * c11 + c * c7) * idt; + res[3, 2] = (m * c10 - n * c12 + o * c8) * ndt; + res[3, 3] = (i * c10 - j * c12 + k * c8) * idt; + + return res; +} + +pragma(inline): f32 +Mix(f32 x, f32 y, f32 a) +{ + return x * (1 - a) + y * a; +} + +pragma(inline): f32 +InverseLerp(f32 v, f32 min, f32 max) +{ + return (v - min) / (max - min); +} + +pragma(inline): f32 +Remap(f32 v, f32 in_min, f32 in_max, f32 out_min, f32 out_max) +{ + f32 t = InverseLerp(v, in_min, in_max); + return Mix(out_min, out_max, t); +} + +pragma(inline): T +SquaredMagnitude(T)(T v) if (IsVector!(T)) +{ + T sum_squares = 0; + mixin(GenerateLoop!("sum_squares += v.v[@] * v.v[@];", v._N)()); + return sum_squares; +} + +pragma(inline): T +SquaredDistTo(T)(T l, T r) if (IsVector!(T)) +{ + return SquaredMagnitude(r - l); +} + +unittest +{ + enum FLOAT_MAX = f32.max; + enum FLOAT_MIN = -f32.max; + + import core.stdc.stdio; + import core.stdc.stdlib; + import core.stdc.time; + import std.range : take; + import std.algorithm.iteration : sum; + + void PrintMatrix(Mat4 mat) + { + foreach(i; 0 .. mat.N) + { + if (i % 4 == 0) + { + printf("\n"); + } + printf("%.08f ", mat.v[i]); + } + printf("\n"); + } + + srand(cast(u32)time(null)); + + f32 RandomFloat() + { + return cast(f32)(rand())/cast(f32)(RAND_MAX + 1.0); + } + + Mat4 RandomMat4() + { + Mat4 mat; + Mat4Identity(&mat); + mat[3, 0] = RandomFloat(); + mat[3, 1] = RandomFloat(); + mat[3, 2] = RandomFloat(); + + Rotate(&mat, RandomFloat(), Vec3(RandomFloat(), RandomFloat(), RandomFloat())); + + return mat; + } + + { // Vec2 arithmetic + Vec2 v1 = Vec2(5.0, 10.0); + Vec2 v2 = Vec2(2.5, 5.0); + + Vec2 result = v1 * v2; + + assert(result == Vec2(12.5, 50.0), "Vec2 mul failure"); + + result = v1 + v2; + + assert(result == Vec2(7.5, 15.0), "Vec2 add failure"); + + result = v1 - v2; + + assert(result == Vec2(2.5, 5.0), "Vec2 sub failure"); + + result = v1 / v2; + + assert(result == Vec2(2.0), "Vec2 div failure"); + } + + { // Vec3 Arithmetic + Vec3 v1 = Vec3(5.0, 10.0, 15.0); + Vec3 v2 = Vec3(2.5, 5.0, 7.5); + + Vec3 result = v1 * v2; + + assert(result == Vec3(12.5, 50.0, 112.5), "Vec3 mul failure"); + + result = v1 + v2; + + assert(result == Vec3(7.5, 15.0, 22.5), "Vec3 add failure"); + + result = v1 - v2; + + assert(result == Vec3(2.5, 5.0, 7.5), "Vec3 sub failure"); + + result = v1 / v2; + + assert(result == Vec3(2.0), "Vec3 div failure"); + } + + { // Vec3 Arithmetic + Vec4 v1 = Vec4(5.0, 10.0, 15.0, 20.0); + Vec4 v2 = Vec4(2.5, 5.0, 7.5, 10.0); + + Vec4 result = v1 * v2; + + assert(result == Vec4(12.5, 50.0, 112.5, 200.0), "Vec4 mul failure"); + + result = v1 + v2; + + assert(result == Vec4(7.5, 15.0, 22.5, 30.0), "Vec4 add failure"); + + result = v1 - v2; + + assert(result == Vec4(2.5, 5.0, 7.5, 10.0), "Vec4 sub failure"); + + result = v1 / v2; + + assert(result == Vec4(2.0), "Vec4 div failure"); + } + + { // Mat4 Arithmetic + Mat4 m1 = RandomMat4(); + Mat4 m2 = RandomMat4(); + Mat4 m3 = m1 * m2; + Mat4 m4; + + MatZero(&m4); + + for(u32 i = 0; i < 4; i += 1) + { + for(u32 j = 0; j < 4; j += 1) + { + for(u32 k = 0; k < 4; k += 1) + { + m4.rows[i][j] += m1.rows[k][j] * m2.rows[i][k]; + } + } + } + + assert(m3 == m4, "Mat4 mul failure"); + } + + { // Translate + Mat4 mat = Mat4Identity(); + Vec4 vec = Vec4(1.0, 2.0, 3.0, 1.0); + + Translate(&mat, Vec3(13.0, 11.0, 7.0)); + Vec4 result = mat * vec; + + assert(result == Vec4(14.0, 13.0, 10.0, 1.0)); + + mat = Mat4Identity(); + Translate(&mat, Vec3(1.0, -1.0, -5.0)); + result = mat * result; + + assert(result == Vec4(15.0, 12.0, 5.0, 1.0)); + } + + { // Identity + Mat4 identity = Mat4( + 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0 + ); + Mat4 mat = Mat4Identity(); + + assert(identity == mat); + } + + + { // Inverse + foreach(i; 0 .. 1000) + { + Mat4 m1 = RandomMat4(); + + Mat4 m1_inv = Inverse(m1); + Mat4 m1_reinv = Inverse(m1_inv); + + assert(m1 == m1_reinv, "Inverse test failed"); + } + } + + { // Cross + Vec3 v1 = Vec3(2.0, -3.0, 4.0); + Vec3 v2 = Vec3(12.0, -31.0, 43.0); + + Vec3 v3 = Cross(v1, v2); + + Vec3 v4 = Vec3( + v1.y * v2.z - v1.z * v2.y, + v1.z * v2.x - v1.x * v2.z, + v1.x * v2.y - v1.y * v2.x + ); + + assert(v3 == v4, "Vec3 Cross failure"); + + v3 = CrossN(v1, v2); + + Normalize(&v4); + + assert(v3 == v4, "Vec3 CrossN failure"); + } + + { // LookAt + Vec3 eye = Vec3(0.024, 14.6, 67.04); + Vec3 dir = Vec3(0.0, 0.0, -1.0); + Vec3 up = Vec3(0.0, 1.0, 0.0); + + Vec3 center = eye + dir; + Mat4 view1 = LookAt(eye, center, up); + Mat4 view2 = Mat4( + 1.0, -0.0, 0.0, -0.024, + 0.0, 1.0, 0.0, -14.6, + -0.0, -0.0, 1.0, 67.04, + 0.0, 0.0, 0.0, 1.0 + ); + + assert(view1 == view2, "LookAt failure"); + } +}