diff --git a/README.md b/README.md index 861bf41..cf148d7 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,7 @@ Current provides the following types: - `Vec2` - `Vec3` - `Vec4` +- `Quat` - `Mat2` - `Mat3` - `Mat4` diff --git a/src/test.zig b/src/test.zig index 32a5148..a13f133 100644 --- a/src/test.zig +++ b/src/test.zig @@ -7,10 +7,12 @@ const math_f32 = math.as(f32); const vec2 = math_f32.vec2; const vec3 = math_f32.vec3; const vec4 = math_f32.vec4; +const quat = math_f32.quat; const Vec2 = math_f32.Vec2; const Vec3 = math_f32.Vec3; const Vec4 = math_f32.Vec4; +const Quat = math_f32.Quat; const Mat3 = math_f32.Mat3; const Mat4 = math_f32.Mat4; @@ -264,3 +266,115 @@ test "mat4 format" { }; try std.testing.expectFmt("mat4{ (1.00 2.00 3.00 4.00) (5.00 6.00 7.00 8.00) (9.00 10.00 11.00 12.00) (13.00 14.00 15.00 16.00) }", "{f}", .{mat}); } + +test "quat constructor" { + const q = quat(1, 2, 3, 4); + assert(q.x == 1); + assert(q.y == 2); + assert(q.z == 3); + assert(q.w == 4); +} + +test "quat identity" { + const id = Quat.identity; + assert(id.x == 0); + assert(id.y == 0); + assert(id.z == 0); + assert(id.w == 1); +} + +test "quat length" { + const q = quat(1, 2, 3, 4); + assert(Quat.length2(q) == 30.0); + assert(Quat.length(q) == std.math.sqrt(30.0)); +} + +test "quat normalize" { + const q = quat(0, 0, 0, 2); + const n = Quat.normalize(q); + assert(n.x == 0); + assert(n.y == 0); + assert(n.z == 0); + assert(n.w == 1); +} + +test "quat conjugate" { + const q = quat(1, 2, 3, 4); + const c = Quat.conjugate(q); + assert(c.x == -1); + assert(c.y == -2); + assert(c.z == -3); + assert(c.w == 4); +} + +test "quat mul identity" { + const q = quat(1, 2, 3, 4); + const id = Quat.identity; + + const q_times_id = Quat.mul(q, id); + const id_times_q = Quat.mul(id, q); + + assert(std.meta.eql(q_times_id, q)); + assert(std.meta.eql(id_times_q, q)); +} + +test "quat mul" { + const a = quat(1, 2, 3, 4); + const b = quat(5, 6, 7, 8); + + const result = Quat.mul(a, b); + + // Quaternion multiplication by hand: + // w = 4*8 - 1*5 - 2*6 - 3*7 = 32 - 5 - 12 - 21 = -6 + // x = 4*5 + 1*8 + 2*7 - 3*6 = 20 + 8 + 14 - 18 = 24 + // y = 4*6 - 1*7 + 2*8 + 3*5 = 24 - 7 + 16 + 15 = 48 + // z = 4*7 + 1*6 - 2*5 + 3*8 = 28 + 6 - 10 + 24 = 48 + assert(result.x == 24); + assert(result.y == 48); + assert(result.z == 48); + assert(result.w == -6); +} + +test "quat fromAxisAngle" { + const pi = std.math.pi; + + // 180 degree rotation around Z axis + const q = Quat.fromAxisAngle(vec3(0, 0, 1), pi); + + // sin(pi/2) = 1, cos(pi/2) = 0 + try std.testing.expectApproxEqAbs(@as(f32, 0), q.x, 1e-6); + try std.testing.expectApproxEqAbs(@as(f32, 0), q.y, 1e-6); + try std.testing.expectApproxEqAbs(@as(f32, 1), q.z, 1e-6); + try std.testing.expectApproxEqAbs(@as(f32, 0), q.w, 1e-6); +} + +test "quat rotateVec3" { + const pi = std.math.pi; + + // 90 degree rotation around Z axis + const q = Quat.fromAxisAngle(vec3(0, 0, 1), pi / 2.0); + const v = vec3(1, 0, 0); + const rotated = Quat.rotateVec3(q, v); + + // Rotating (1,0,0) by 90 degrees around Z should give (0,1,0) + try std.testing.expectApproxEqAbs(@as(f32, 0), rotated.x, 1e-6); + try std.testing.expectApproxEqAbs(@as(f32, 1), rotated.y, 1e-6); + try std.testing.expectApproxEqAbs(@as(f32, 0), rotated.z, 1e-6); +} + +test "quat slerp" { + const id = Quat.identity; + const q = quat(0, 0, 0.7071068, 0.7071068); // ~90 degrees around Z + + // slerp at t=0 should give first quaternion + const s0 = Quat.slerp(id, q, 0); + assert(Quat.approxEqAbs(s0, id, 1e-6)); + + // slerp at t=1 should give second quaternion + const s1 = Quat.slerp(id, q, 1); + assert(Quat.approxEqAbs(s1, q, 1e-6)); +} + +test "quat format" { + try std.testing.expectFmt("quat(1.00, 2.00, 3.00, 4.00)", "{f}", .{quat(1, 2, 3, 4)}); +} diff --git a/src/zlm.zig b/src/zlm.zig index fa2e9dd..c04c0fd 100644 --- a/src/zlm.zig +++ b/src/zlm.zig @@ -829,6 +829,219 @@ pub fn as(comptime Real: type) type { } }; + /// Quaternion type for representing rotations. + pub const Quat = extern struct { + const Self = @This(); + + x: Real, + y: Real, + z: Real, + w: Real, + + /// Identity quaternion (no rotation). + pub const identity = Self.new(0, 0, 0, 1); + + pub fn new(x: Real, y: Real, z: Real, w: Real) Self { + return Self{ + .x = x, + .y = y, + .z = z, + .w = w, + }; + } + + pub fn format(value: Self, writer: *std.Io.Writer) !void { + try writer.print("quat({d:.2}, {d:.2}, {d:.2}, {d:.2})", .{ value.x, value.y, value.z, value.w }); + } + + /// Returns the squared magnitude of the quaternion. + pub fn length2(q: Self) Real { + return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; + } + + /// Returns the magnitude of the quaternion. + pub fn length(q: Self) Real { + return @sqrt(q.length2()); + } + + /// Normalizes the quaternion to unit length. + pub fn normalize(q: Self) Self { + const len = q.length(); + if (len == 0) { + return Self.identity; + } + const inv_len = 1.0 / len; + return Self{ + .x = q.x * inv_len, + .y = q.y * inv_len, + .z = q.z * inv_len, + .w = q.w * inv_len, + }; + } + + /// Returns the conjugate of the quaternion. + pub fn conjugate(q: Self) Self { + return Self{ + .x = -q.x, + .y = -q.y, + .z = -q.z, + .w = q.w, + }; + } + + /// Returns the inverse of the quaternion. + pub fn inverse(q: Self) Self { + const len2 = q.length2(); + if (len2 == 0) { + return Self.identity; + } + const conj = q.conjugate(); + const inv_len2 = 1.0 / len2; + return Self{ + .x = conj.x * inv_len2, + .y = conj.y * inv_len2, + .z = conj.z * inv_len2, + .w = conj.w * inv_len2, + }; + } + + /// Multiplies two quaternions (composes rotations). + pub fn mul(a: Self, b: Self) Self { + return Self{ + .x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + .y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + .z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, + .w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z, + }; + } + + /// Creates a quaternion from an axis and angle (in radians). + pub fn fromAxisAngle(axis: Vec3, angle: Real) Self { + const normalized = axis.normalize(); + const half_angle = angle * 0.5; + const s = @sin(half_angle); + const c = @cos(half_angle); + return Self{ + .x = normalized.x * s, + .y = normalized.y * s, + .z = normalized.z * s, + .w = c, + }; + } + + /// Creates a quaternion from Euler angles (in radians). + /// Order: roll (X), pitch (Y), yaw (Z). + pub fn fromEulerAngles(roll: Real, pitch: Real, yaw: Real) Self { + const cr = @cos(roll * 0.5); + const sr = @sin(roll * 0.5); + const cp = @cos(pitch * 0.5); + const sp = @sin(pitch * 0.5); + const cy = @cos(yaw * 0.5); + const sy = @sin(yaw * 0.5); + + return Self{ + .x = sr * cp * cy - cr * sp * sy, + .y = cr * sp * cy + sr * cp * sy, + .z = cr * cp * sy - sr * sp * cy, + .w = cr * cp * cy + sr * sp * sy, + }; + } + + /// Rotates a Vec3 by this quaternion. + pub fn rotateVec3(q: Self, v: Vec3) Vec3 { + // q * v * q^-1, optimized + const qv = Vec3.new(q.x, q.y, q.z); + const uv = Vec3.cross(qv, v); + const uuv = Vec3.cross(qv, uv); + return Vec3{ + .x = v.x + (uv.x * q.w + uuv.x) * 2.0, + .y = v.y + (uv.y * q.w + uuv.y) * 2.0, + .z = v.z + (uv.z * q.w + uuv.z) * 2.0, + }; + } + + /// Converts the quaternion to a 4x4 rotation matrix. + pub fn toMat4(q: Self) Mat4 { + const xx = q.x * q.x; + const yy = q.y * q.y; + const zz = q.z * q.z; + const xy = q.x * q.y; + const xz = q.x * q.z; + const yz = q.y * q.z; + const wx = q.w * q.x; + const wy = q.w * q.y; + const wz = q.w * q.z; + + return Mat4{ + .fields = [4][4]Real{ + [4]Real{ 1.0 - 2.0 * (yy + zz), 2.0 * (xy + wz), 2.0 * (xz - wy), 0 }, + [4]Real{ 2.0 * (xy - wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz + wx), 0 }, + [4]Real{ 2.0 * (xz + wy), 2.0 * (yz - wx), 1.0 - 2.0 * (xx + yy), 0 }, + [4]Real{ 0, 0, 0, 1 }, + }, + }; + } + + /// Spherical linear interpolation between two quaternions. + pub fn slerp(a: Self, b: Self, t: Real) Self { + var cos_theta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; + + // If the dot product is negative, negate one quaternion + // to take the shorter path + var b_adj = b; + if (cos_theta < 0) { + b_adj = Self{ + .x = -b.x, + .y = -b.y, + .z = -b.z, + .w = -b.w, + }; + cos_theta = -cos_theta; + } + + // If quaternions are very close, use linear interpolation + if (cos_theta > 0.9995) { + const lerped = Self{ + .x = a.x + t * (b_adj.x - a.x), + .y = a.y + t * (b_adj.y - a.y), + .z = a.z + t * (b_adj.z - a.z), + .w = a.w + t * (b_adj.w - a.w), + }; + return lerped.normalize(); + } + + const theta = std.math.acos(cos_theta); + const sin_theta = @sin(theta); + const wa = @sin((1.0 - t) * theta) / sin_theta; + const wb = @sin(t * theta) / sin_theta; + + return Self{ + .x = wa * a.x + wb * b_adj.x, + .y = wa * a.y + wb * b_adj.y, + .z = wa * a.z + wb * b_adj.z, + .w = wa * a.w + wb * b_adj.w, + }; + } + + /// Returns the dot product of two quaternions. + pub fn dot(a: Self, b: Self) Real { + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; + } + + /// Checks if two quaternions are equal. + pub fn eql(a: Self, b: Self) bool { + return a.x == b.x and a.y == b.y and a.z == b.z and a.w == b.w; + } + + /// Checks if two quaternions are approximately equal (absolute tolerance). + pub fn approxEqAbs(a: Self, b: Self, tolerance: Real) bool { + return std.math.approxEqAbs(Real, a.x, b.x, tolerance) and + std.math.approxEqAbs(Real, a.y, b.y, tolerance) and + std.math.approxEqAbs(Real, a.z, b.z, tolerance) and + std.math.approxEqAbs(Real, a.w, b.w, tolerance); + } + }; + /// constructs a new Vec2. pub const vec2 = Vec2.new; @@ -838,6 +1051,9 @@ pub fn as(comptime Real: type) type { /// constructs a new Vec4. pub const vec4 = Vec4.new; + /// constructs a new Quat. + pub const quat = Quat.new; + /// Converts degrees to radian pub fn toRadians(deg: Real) Real { return std.math.pi * deg / 180.0;