Mesh rotation function

zigsdof/src/stl.zig

@@ -100,6 +100,16 @@ pub const Mesh = struct {
         mesh.vertices.deinit(allocator);
     }
 };
+
+pub fn rotateMeshAroundPoint(mesh: *Mesh, p: vec3.Vec3F64, q: vec3.QuatF64) void {
+    for (mesh.vertices.items) |*vertex| {
+        const d = vec3.vec3Sub(vertex.*, p);
+        const dprime = vec3.quatApply(q, d);
+        vertex.* = vec3.vec3Add(dprime, p);
+    }
+    return;
+}
+
 test "Verify fixed point conversion works" {
     const vec = vec3.Vec3F64.init(1.0, 2.134, 1010.11);
     const fixed_point = vecToFixedPoint(vec, 2);
@@ -164,3 +174,58 @@ test "Test binary stl is parsed correctly" {
         1e-12,
     ));
 }
+
+test "mesh is rotated correctly" {
+    const allocator = std.testing.allocator;
+
+    var vertices: std.ArrayList(vec3.Vec3F64) = .empty;
+    try vertices.appendSlice(allocator, &.{
+        vec3.Vec3F64.init(0, 0, 0),
+        vec3.Vec3F64.init(0.5, std.math.sqrt(3.0) / 2.0, 0),
+        vec3.Vec3F64.init(1, 0, 0),
+    });
+
+    var triangle_normals: std.ArrayList(vec3.Vec3F64) = .empty;
+    try triangle_normals.appendSlice(allocator, &.{
+        vec3.Vec3F64.init(1, 0, 0),
+    });
+
+    var tri_indices: std.ArrayList([3]usize) = .empty;
+    try tri_indices.appendSlice(allocator, &.{.{ 0, 1, 2 }});
+
+    var mesh: Mesh = .{
+        .vertices = vertices,
+        .triangle_normals = triangle_normals,
+        .triangle_vertices = tri_indices,
+    };
+    defer mesh.deinit(allocator);
+
+    var orig_mesh: Mesh = .{
+        .vertices = try mesh.vertices.clone(allocator),
+        .triangle_normals = try mesh.triangle_normals.clone(allocator),
+        .triangle_vertices = try mesh.triangle_vertices.clone(allocator),
+    };
+    defer orig_mesh.deinit(allocator);
+
+    rotateMeshAroundPoint(
+        &mesh,
+        vec3.Vec3F64.init(0.5, std.math.sqrt(3.0) / 6.0, 0),
+        vec3.yawPitchRollToQuat(2.0 * std.math.pi / 3.0, 0, 0),
+    );
+
+    try std.testing.expect(vec3.vec3ApproxEq(
+        orig_mesh.vertices.items[0],
+        mesh.vertices.items[1],
+        1e-12,
+    ));
+    try std.testing.expect(vec3.vec3ApproxEq(
+        orig_mesh.vertices.items[1],
+        mesh.vertices.items[2],
+        1e-12,
+    ));
+    try std.testing.expect(vec3.vec3ApproxEq(
+        orig_mesh.vertices.items[2],
+        mesh.vertices.items[0],
+        1e-12,
+    ));
+}