From 2abe6523e64bd2f1acd1e8bd3da87101ced225d5 Mon Sep 17 00:00:00 2001 From: Samuel Ayala Date: Thu, 28 Mar 2024 17:36:25 -0400 Subject: [PATCH] debug in python --- data/displacement.py | 106 +++++++++++++++++++++++++++++++++++++++ vlm/include/vlm_mesh.hpp | 1 - vlm/src/vlm_mesh.cpp | 38 ++++++++------ 3 files changed, 128 insertions(+), 17 deletions(-) create mode 100644 data/displacement.py diff --git a/data/displacement.py b/data/displacement.py new file mode 100644 index 0000000..ed5a492 --- /dev/null +++ b/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() diff --git a/vlm/include/vlm_mesh.hpp b/vlm/include/vlm_mesh.hpp index 7bf0eb9..1cc7f3a 100644 --- a/vlm/include/vlm_mesh.hpp +++ b/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); diff --git a/vlm/src/vlm_mesh.cpp b/vlm/src/vlm_mesh.cpp index b124066..ddfd82f 100644 --- a/vlm/src/vlm_mesh.cpp +++ b/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); -} -