debug in python

data/displacement.py

@@ -0,0 +1,106 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+
+class Kinematics:
+    joints = [] # list of transformation lambdas
+    frames = [np.identity(4)] # list of joint frames in global coordinates
+
+    def add_joint(self, joint, frame):
+        self.joints.append(joint)
+        self.frames.append(frame)
+
+    def displacement(self, t, move_frames=True):
+        disp = np.identity(4)
+        for i in range(len(self.joints)):
+            disp = disp @ self.joints[i](t, self.frames[i])
+            if move_frames: 
+                self.frames[i+1] = self.joints[i](t, self.frames[i]) @ self.frames[i+1]
+        return disp
+
+    def relative_displacement(self, t0, t1):
+        base = np.linalg.inv(self.displacement(t0, True))
+        return self.displacement(t1, False) @ base
+
+def skew_matrix(v):
+    return np.array([
+        [0, -v[2], v[1]],
+        [v[2], 0, -v[0]],
+        [-v[1], v[0], 0]
+    ])
+
+def translation_matrix(v):
+    return np.array([[1, 0, 0, v[0]],
+                     [0, 1, 0, v[1]],
+                     [0, 0, 1, v[2]],
+                     [0, 0, 0, 1]])
+
+def rotation_matrix(center, axis, theta):
+    axis = axis[:3]
+    center = center[:3]
+    mat = np.identity(4)
+    rodrigues = np.identity(3) + np.sin(theta) * skew_matrix(axis) + (1 - np.cos(theta)) * skew_matrix(axis) @ skew_matrix(axis)
+    mat[:3, :3] = rodrigues
+    mat[:3, 3] = (np.identity(3) - rodrigues) @ center # matvec
+    return mat
+
+def move_vertices(vertices, displacement):
+    return displacement @ vertices
+
+def displacement_wing(t, frame): return translation_matrix([0, 0, np.sin(0.9 * t)])
+def displacement_freestream(t, frame): return translation_matrix([-1 * t, 0, 0])
+def displacement_rotor(t, frame): return rotation_matrix(frame @ [0, 0, 0, 1], frame @ [0, 0, 1, 0], 1 * t)
+#def displacement_rotor(t, frame): return rotation_matrix([0, 0, 0, 1], [0, 0, 1, 0], 1 * t)
+
+kinematics = Kinematics()
+kinematics.add_joint(displacement_freestream, np.identity(4))
+kinematics.add_joint(displacement_wing, np.identity(4))
+# kinematics.add_joint(displacement_rotor, np.identity(4))
+
+dt = 0.5
+t_final = 15
+
+# vertices of a single panel (clockwise) (initial position) (global coordinates)
+vertices = np.array([
+    [0.0, 0.5, 1.0, 1.0], # x
+    [0.0, 10.0, 10.0, 0.0], # y
+    [0.0, 0.0, 0.0, 0.0], # z
+    [1.0, 1.0, 1.0, 1.0]  # homogeneous coordinates
+])
+
+body_frame = np.identity(4)
+
+vertices = np.column_stack((vertices, vertices[:,0]))
+
+wing_frame = np.identity(4)
+
+# Setup animation
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+
+# Setting the initial view to be isometric
+ax.view_init(elev=30, azim=135)  # Isometric view angle
+ax.set_xlabel('X axis')
+ax.set_ylabel('Y axis')
+ax.set_zlabel('Z axis')
+ax.set_xlim(-15, 0)
+ax.set_ylim(-15, 15)
+ax.set_zlim(-5, 5)
+ax.invert_xaxis()  # Invert x axis
+ax.invert_yaxis()  # Invert y axis
+
+# Initial plot - create a line object
+line, = ax.plot3D(vertices[0, :], vertices[1, :], vertices[2, :], 'o-') 
+
+def update(frame):
+    t = frame * dt
+    current_displacement = kinematics.relative_displacement(0, t)
+    moved_vertices = move_vertices(vertices, current_displacement)
+    # Update the line object for 3D
+    line.set_data(moved_vertices[0, :], moved_vertices[1, :])  # y and z for 2D part of set_data
+    line.set_3d_properties(moved_vertices[2, :])  # x for the 3rd dimension
+    return line,
+
+ani = animation.FuncAnimation(fig, update, frames=np.arange(0, t_final/dt), blit=False)
+
+plt.show()

vlm/include/vlm_mesh.hpp

@@ -87,7 +87,6 @@ class Mesh {
     void compute_metrics_wing(); // fill colloc, normal, area
     void compute_metrics_wake();
     void compute_metrics_i(u64 i);
-    void shed_wake();
     void move(const linalg::alias::float4x4& transform);
     void resize_wake(const u64 nw);
 

vlm/src/vlm_mesh.cpp

@@ -370,10 +370,28 @@ void Mesh::io_read(const std::string& filename) {
 }
 
 void Mesh::move(const linalg::alias::float4x4& transform) {
+    assert(current_nw < nw); // check if we have capacity
+
+    // Shed wake before moving    
+    const u64 src_begin = nb_vertices_wing() - (ns + 1);
+    const u64 src_end = nb_vertices_wing();
+    const u64 dst_begin = (nc + nw - current_nw) * (ns + 1);
+    std::copy(v.x.data() + src_begin, v.x.data() + src_end, v.x.data() + dst_begin);
+    std::copy(v.y.data() + src_begin, v.y.data() + src_end, v.y.data() + dst_begin);
+    std::copy(v.z.data() + src_begin, v.z.data() + src_end, v.z.data() + dst_begin);
+
+    // Perform the movement
     for (u64 i = 0; i < nb_vertices_wing(); i++) {
-        linalg::alias::float4 global_vertex{v.x[i], v.y[i], v.z[i], 1.f};
-        linalg::alias::float4 local_vertex = frame.col(3);
-
+        const linalg::alias::float4 global_vertex{v.x[i], v.y[i], v.z[i], 1.f};
+        const linalg::alias::float4 local_frame_center = frame.col(3); // in global coordinates
+        const linalg::alias::float4 center_to_vertex = global_vertex - local_frame_center;
+        const linalg::alias::float4 local_vertex = {
+            linalg::dot(frame.col(0), center_to_vertex),
+            linalg::dot(frame.col(1), center_to_vertex),
+            linalg::dot(frame.col(2), center_to_vertex),
+            1.f
+        }; // vertex in local coordinates
+
         const linalg::alias::float4 transformed_pt = linalg::mul(transform, linalg::alias::float4{v.x[i], v.y[i], v.z[i], 1.f});
         v.x[i] = transformed_pt.x;
         v.y[i] = transformed_pt.y;
@@ -383,17 +401,5 @@ void Mesh::move(const linalg::alias::float4x4& transform) {
 
 void Mesh::resize_wake(const u64 nw_) {
     nw = nw_;
-    alloc();
+    alloc(); // resizes the buffers
 }
-
-// Duplicate the trailing edge vertices and release them in the buffer
-void Mesh::shed_wake() {
-    assert(current_nw < nw); // check if we have capacity
-    const u64 src_begin = nb_vertices_wing() - (ns + 1);
-    const u64 src_end = nb_vertices_wing();
-    const u64 dst_begin = (nc + nw - current_nw) * (ns + 1);
-    std::copy(v.x.data() + src_begin, v.x.data() + src_end, v.x.data() + dst_begin);
-    std::copy(v.y.data() + src_begin, v.y.data() + src_end, v.y.data() + dst_begin);
-    std::copy(v.z.data() + src_begin, v.z.data() + src_end, v.z.data() + dst_begin);
-}
-