aselimov/zigsdof
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Finish quaternion rotation library

Browse source
This commit is contained in:
Alex Selimov 2026-08-02 22:24:05 -04:00
parent 8b8eeb25c9
commit 0fc5ccd020
1 changed files with 66 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

79
src/types/vec3.zig
View file

@ -85,9 +85,9 @@ pub fn mat3Mul(a: Mat3F32, b: Mat3F32) Mat3F32 {
pub fn vec3MulMat3(a: Mat3F32, b: Vec3F32) Vec3F32 {
return .init(
b.x() * a.row1.x() + b.y() * a.row1.y() + b.z() * a.row1.z(),
b.x() * a.row2.x() + b.y() * a.row2.y() + b.z() * a.row2.z(),
b.x() * a.row3.x() + b.y() * a.row3.y() + b.z() * a.row3.z(),
vec3Dot(a.row1, b),
vec3Dot(a.row2, b),
vec3Dot(a.row3, b),
);
}
@ -121,11 +121,27 @@ const QuatF32 = struct {
const magnitude = q.mag();
return .{ .data = q.data / @as(@Vector(4, f32), @splat(magnitude)) };
}
pub fn conjugate(q: QuatF32) QuatF32 {
return .{ .data = q.data * @as(@Vector(4, f32), .{ -1, -1, -1, 1 }) };
}
};
//pub fn yawPitchRollToQuat(comptime T: comptime_float, yaw: T, pitch: T, roll: T) Quat(T) {
// const cx = @cos(roll);
//}
pub fn yawPitchRollToQuat(yaw: f32, pitch: f32, roll: f32) QuatF32 {
const cx = @cos(roll / 2);
const sx = @sin(roll / 2);
const cy = @cos(pitch / 2);
const sy = @sin(pitch / 2);
const cz = @cos(yaw / 2);
const sz = @sin(yaw / 2);
return .init(
sx * cy * cz - cx * sy * sz,
cx * sy * cz + sx * cy * sz,
cx * cy * sz - sx * sy * cz,
cx * cy * cz + sx * sy * sz,
);
}
pub fn quatMul(a: QuatF32, b: QuatF32) QuatF32 {
return .init(
@ -136,6 +152,11 @@ pub fn quatMul(a: QuatF32, b: QuatF32) QuatF32 {
);
}
pub fn quatApply(a: QuatF32, b: Vec3F32) Vec3F32 {
const quat_v = quatMul(quatMul(a, .init(b.x(), b.y(), b.z(), 0)), a.conjugate());
return .init(quat_v.data[0], quat_v.data[1], quat_v.data[2]);
}
test "vec3Add adds properly" {
const a = Vec3F32.init(1, 2, 3);
const b = Vec3F32.init(3, 1, 0);
@ -255,9 +276,9 @@ test "vec3 mul mat3 works" {
const c = vec3MulMat3(b, a);
try std.testing.expect(c.x() == 2);
try std.testing.expect(c.y() == 5);
try std.testing.expect(c.z() == 3);
try std.testing.expect(std.math.approxEqAbs(f32, c.x(), 2, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 5, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 3, 1e-7));
}
test "quat magnitude" {
@ -288,8 +309,40 @@ test "quat mul" {
const c = quatMul(a, b);
try std.testing.expect(c.w() == -2);
try std.testing.expect(c.x() == -2);
try std.testing.expect(c.y() == 4);
try std.testing.expect(c.z() == 6);
try std.testing.expect(std.math.approxEqAbs(f32, c.w(), -2, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, c.x(), -2, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 4, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 6, 1e-7));
}
test "quat euler" {
const quat = yawPitchRollToQuat(0.5, 0.5, 1);
try std.testing.expect(std.math.approxEqAbs(f32, quat.x(), 0.3963648, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, quat.y(), 0.3252922, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, quat.z(), 0.0954433, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, quat.w(), 0.8532119, 1e-7));
}
test "quat conjugate" {
const quat: QuatF32 = .init(1, 1, 1, 1);
const conj = quat.conjugate();
try std.testing.expect(std.math.approxEqAbs(f32, conj.x(), -1, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, conj.y(), -1, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, conj.z(), -1, 1e-7));
try std.testing.expect(std.math.approxEqAbs(f32, conj.w(), 1, 1e-7));
}
test "quat apply to vec " {
const quat: QuatF32 = yawPitchRollToQuat(std.math.pi / 2.0, 0, 0);
try std.testing.expect(std.math.approxEqAbs(f32, quat.x(), 0, 1e-6));
try std.testing.expect(std.math.approxEqAbs(f32, quat.y(), 0, 1e-6));
try std.testing.expect(std.math.approxEqAbs(f32, quat.z(), @sin(std.math.pi / 4.0), 1e-6));
try std.testing.expect(std.math.approxEqAbs(f32, quat.w(), @cos(std.math.pi / 4.0), 1e-6));
const rotated = quatApply(quat, .init(1, 2, 1));
try std.testing.expect(std.math.approxEqAbs(f32, rotated.x(), -2, 1e-6));
try std.testing.expect(std.math.approxEqAbs(f32, rotated.y(), 1, 1e-6));
try std.testing.expect(std.math.approxEqAbs(f32, rotated.z(), 1, 1e-6));
}