WIP stl parser

zigsdof/src/io.zig

@@ -25,7 +25,7 @@ pub fn jsonFileToSimConfig(
     io: std.Io,
 ) !drone.QuadCopterSim {
     const cwd = std.Io.Dir.cwd();
-    cons file = try cwd.openFile(io, file_path, .{ .mode = .read_only });
+    const file = try cwd.openFile(io, file_path, .{ .mode = .read_only });
     defer file.close(io);
 
     var file_reader = file.reader(io, &.{});

zigsdof/src/main.zig

@@ -6,12 +6,5 @@ test "main" {
     _ = @import("./forces.zig");
     _ = @import("./integration.zig");
     _ = @import("./io.zig");
-}
+    _ = @import("./stl.zig");
-
-test "how ptrs work" {
-    var ptr: *usize = 0;
-    var val = 1;
-    ptr = val;
-    val = 2;
-    std.testing.expect(*ptr == 1);
 }

zigsdof/src/stl.zig

@@ -0,0 +1,153 @@
+const std = @import("std");
+const vec3 = @import("./vec3.zig");
+
+fn vecToFixedPoint(vec: vec3.Vec3F64, digits: u5) [3]u64 {
+    return .{
+        @round(vec.x() * std.math.pow(f64, 10.0, digits)),
+        @round(vec.y() * std.math.pow(f64, 10.0, digits)),
+        @round(vec.z() * std.math.pow(f64, 10.0, digits)),
+    };
+}
+pub const Mesh = struct {
+    vertices: std.ArrayList(vec3.Vec3F64),
+    triangle_vertices: std.ArrayList([3]usize),
+    triangle_normals: std.ArrayList(vec3.Vec3F64),
+
+    pub fn fromFile(
+        file_path: []const u8,
+        allocator: std.mem.Allocator,
+        io: std.Io,
+    ) !Mesh {
+        const cwd = std.Io.Dir.cwd();
+        const file = try cwd.openFile(io, file_path, .{ .mode = .read_only });
+        defer file.close(io);
+
+        var file_reader = file.reader(io, &.{});
+        const bytes = try file_reader.interface.allocRemaining(allocator, .unlimited);
+        defer allocator.free(bytes);
+
+        return fromByteArray(bytes, allocator);
+    }
+
+    pub fn fromByteArray(
+        bytes: []const u8,
+        allocator: std.mem.Allocator,
+    ) !Mesh {
+        // Skip header
+        var offset: usize = 80;
+
+        const triangle_num: u32 = std.mem.readInt(u32, @ptrCast(bytes[offset .. offset + 4].ptr), .little);
+        offset += 4;
+
+        var triangle_vertices: std.ArrayList([3]usize) = try .initCapacity(allocator, triangle_num);
+        var triangle_normals: std.ArrayList(vec3.Vec3F64) = try .initCapacity(allocator, triangle_num);
+        var vertices: std.ArrayList(vec3.Vec3F64) = try .initCapacity(allocator, triangle_num * 3);
+
+        var vertex_map: std.AutoHashMap([3]u64, usize) = .init(allocator);
+        defer vertex_map.deinit();
+
+        for (0..triangle_num) |_| {
+            var normal = vec3.Vec3F64.init(0, 0, 0);
+            inline for (0..3) |i| {
+                const bits = std.mem.readInt(
+                    u32,
+                    @ptrCast(bytes[offset .. offset + 4].ptr),
+                    .little,
+                );
+                offset += 4;
+                const val: f32 = @bitCast(bits);
+                normal.data[i] = val;
+            }
+            var indices: [3]usize = .{ 0, 0, 0 };
+            inline for (0..3) |i| {
+                var vertex = vec3.Vec3F64.init(0, 0, 0);
+                inline for (0..3) |j| {
+                    const bits = std.mem.readInt(
+                        u32,
+                        @ptrCast(bytes[offset .. offset + 4].ptr),
+                        .little,
+                    );
+                    offset += 4;
+                    const val: f32 = @bitCast(bits);
+                    vertex.data[j] = val;
+                }
+                const gop = try vertex_map.getOrPut(vecToFixedPoint(vertex, 5));
+                if (gop.found_existing) {
+                    indices[i] = gop.value_ptr.*;
+                } else {
+                    const new_idx = vertices.items.len;
+                    try vertices.append(allocator, vertex);
+                    gop.value_ptr.* = new_idx;
+                    indices[i] = new_idx;
+                }
+            }
+            try triangle_vertices.append(allocator, indices);
+            try triangle_normals.append(allocator, normal);
+        }
+
+        std.debug.print("{}", .{vertices});
+        return .{
+            .triangle_vertices = triangle_vertices,
+            .triangle_normals = triangle_normals,
+            .vertices = vertices,
+        };
+    }
+
+    pub fn deinit(mesh: *Mesh, allocator: std.mem.Allocator) void {
+        mesh.triangle_vertices.deinit(allocator);
+        mesh.triangle_normals.deinit(allocator);
+        mesh.vertices.deinit(allocator);
+    }
+};
+test "Verify fixed point conversion works" {
+    const vec = vec3.Vec3F64.init(1.0, 2.134, 1010.11);
+    const fixed_point = vecToFixedPoint(vec, 2);
+    try std.testing.expect(fixed_point[0] == 100);
+    try std.testing.expect(fixed_point[1] == 213);
+    try std.testing.expect(fixed_point[2] == 101011);
+}
+
+test "Test binary stl is parsed correctly" {
+    const binary_stl = "\x42\x69\x6e\x61\x72\x79\x20\x53\x54\x4c\x3a\x20\x32\x20\x74\x72\x69\x61" ++
+        "\x6e\x67\x6c\x65\x73\x3b\x20\x6e\x6f\x72\x6d\x61\x6c\x73\x20\x66\x72\x6f" ++
+        "\x6d\x20\x72\x69\x67\x68\x74\x2d\x68\x61\x6e\x64\x2d\x72\x75\x6c\x65\x20" ++
+        "\x76\x65\x72\x74\x65\x78\x20\x77\x69\x6e\x64\x69\x6e\x67\x00\x00\x00\x00" ++
+        "\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x00\x00\x00\x00\x80\x3f\x00\x00" ++
+        "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" ++
+        "\x00\x00\x00\x00\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" ++
+        "\x80\x3f\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\xf3\x04\x35\xbf\xf3\x04" ++
+        "\x35\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80\x3f" ++
+        "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80\x3f\x00\x00\x80\x3f\x00\x00" ++
+        "\x80\x3f\x00\x00";
+
+    const allocator = std.testing.allocator;
+    var mesh = try Mesh.fromByteArray(binary_stl, allocator);
+    defer mesh.deinit(allocator);
+
+    try std.testing.expect(vec3.vec3ApproxEq(
+        mesh.vertices.items[0],
+        vec3.Vec3F64.init(0, 0, 0),
+        1e-12,
+    ));
+    try std.testing.expect(vec3.vec3ApproxEq(
+        mesh.vertices.items[1],
+        vec3.Vec3F64.init(0, 1, 0),
+        1e-12,
+    ));
+    try std.testing.expect(vec3.vec3ApproxEq(
+        mesh.vertices.items[2],
+        vec3.Vec3F64.init(0, 1, 1),
+        1e-12,
+    ));
+
+    try std.testing.expect(vec3.vec3ApproxEq(
+        mesh.vertices.items[3],
+        vec3.Vec3F64.init(1, 0, 0),
+        1e-12,
+    ));
+    try std.testing.expect(vec3.vec3ApproxEq(
+        mesh.vertices.items[4],
+        vec3.Vec3F64.init(1, 1, 1),
+        1e-12,
+    ));
+}