Finish quaternion rotation library

src/types/vec3.zig

@@ -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(),
+        vec3Dot(a.row1, b),
-        b.x() * a.row2.x() + b.y() * a.row2.y() + b.z() * a.row2.z(),
+        vec3Dot(a.row2, b),
-        b.x() * a.row3.x() + b.y() * a.row3.y() + b.z() * a.row3.z(),
+        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) {
+pub fn yawPitchRollToQuat(yaw: f32, pitch: f32, roll: f32) QuatF32 {
-//    const cx = @cos(roll);
+    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(std.math.approxEqAbs(f32, c.x(), 2, 1e-7));
-    try std.testing.expect(c.y() == 5);
+    try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 5, 1e-7));
-    try std.testing.expect(c.z() == 3);
+    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(std.math.approxEqAbs(f32, c.w(), -2, 1e-7));
-    try std.testing.expect(c.x() == -2);
+    try std.testing.expect(std.math.approxEqAbs(f32, c.x(), -2, 1e-7));
-    try std.testing.expect(c.y() == 4);
+    try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 4, 1e-7));
-    try std.testing.expect(c.z() == 6);
+    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));
 }