fix kinematics

README.md

@@ -11,15 +11,15 @@
 
 - [X] VLM
 - [X] NL-VLM
-- [ ] UVLM
+- [X] UVLM
 - [ ] NL-UVLM
 - [ ] NLHB-UVLM
 
 # Correctors
 
 - [X] High angle of attack correction
 - [X] Dihedral / Anhedral Wings (local coordinate projection in force calculation)
-- [ ] Swept Wings (coordinate rotation & non linear correction)
+- [X] Swept Wings (coordinate rotation)
 - [ ] Compressibility Effects (Prandtl Glauert corrector)
 
 # Backends

data/displacement.py

@@ -1,6 +1,7 @@
 import numpy as np
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
+from matplotlib import cm
 
 class Kinematics:
     joints = [] # list of transformation lambdas
@@ -16,6 +17,14 @@ def displacement(self, t):
 
     def relative_displacement(self, t0, t1):
         return self.displacement(t1) @ np.linalg.inv(self.displacement(t0))
+
+    # f32 absolute_velocity(f32 t, const linalg::alias::float4& vertex) {
+    #     return linalg::length((linalg::mul(displacement(t+EPS_sqrt_f), vertex)-linalg::mul(displacement(t), vertex))/EPS_sqrt_f);
+    # }
+
+    def absolute_velocity(self, t, vertex):
+        EPS_sqrt_f = np.sqrt(1.19209e-07)
+        return np.linalg.norm((self.relative_displacement(t, t+EPS_sqrt_f) @ vertex - vertex) / EPS_sqrt_f)
 
 def skew_matrix(v):
     return np.array([
@@ -58,16 +67,16 @@ def displacement_freestream(t): return translation_matrix([-2 * t, 0, 0])
 #     return rotation_matrix(frame @ [0, 0, 0, 1], frame @ [0, 0, 1, 0], 1 * t)
 def displacement_rotor(t): 
     return rotation_matrix([0, 0, 0], [0, 0, 1], 7 * t)
-# def displacement_rotor(t, frame):
-#     return rotation_matrix2([0, 0, 1], 1 * t)
+def pitching(t): 
+    return rotation_matrix([0, 0, 0], [0, 1, 0], 0.5 * np.pi * np.sin(2.0 * 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))
+# kinematics.add_joint(displacement_wing, np.identity(4))
+kinematics.add_joint(pitching, np.identity(4))
 
-dt = 0.01
-t_final = 20
+dt = 0.1
+t_final = 15
 
 # vertices of a single panel (clockwise) (initial position) (global coordinates)
 vertices = np.array([
@@ -78,11 +87,6 @@ def displacement_rotor(t):
 ])
 vertices = np.column_stack((vertices, vertices[:,0]))
 
-body_frame = np.identity(4)
-
-
-wing_frame = np.identity(4)
-
 # Setup animation
 fig = plt.figure()
 ax = fig.add_subplot(111, projection='3d')
@@ -94,22 +98,39 @@ def displacement_rotor(t):
 ax.set_zlabel('Z axis')
 ax.set_xlim(-30, 0)
 ax.set_ylim(-15, 15)
-ax.set_zlim(-5, 5)
+ax.set_zlim(-15, 15)
 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-') 
+line, = ax.plot3D(vertices[0, :], vertices[1, :], vertices[2, :], '-') 
+scatter = ax.scatter(vertices[0, :], vertices[1, :], vertices[2, :], c='r', marker='o')
 
+current_frame = 0
 def update(frame):
-    global vertices, kinematics
+    global vertices, kinematics, current_frame
     t = frame * dt
-    print(f"t = {t:.2f}/{t_final}", end='\r')
-    vertices = move_vertices(vertices, kinematics.relative_displacement(t, t+dt))
+
+    vertices_velocity = np.array([kinematics.absolute_velocity(t, vertex) for vertex in vertices.T])
+    if frame == current_frame: # otherwise invalid velocity value
+        print(f"frame: {frame} | vel: {vertices_velocity[:-1]}")
+
+    norm = plt.Normalize(vertices_velocity.min(), vertices_velocity.max())
+    colors = cm.viridis(norm(vertices_velocity))
+
     # Update the line object for 3D
     line.set_data(vertices[0, :], vertices[1, :])  # y and z for 2D part of set_data
     line.set_3d_properties(vertices[2, :])  # x for the 3rd dimension
-    return line,
+
+    scatter._offsets3d = (vertices[0, :], vertices[1, :], vertices[2, :])
+    scatter.set_facecolor(colors)
+
+    if (frame == current_frame): # fix double frame 0 issue
+        print(f"t = {t:.2f}/{t_final}", end='\r')
+        vertices = move_vertices(vertices, kinematics.relative_displacement(t, t+dt))
+        current_frame += 1
+
+    return line, scatter
 
 ani = animation.FuncAnimation(fig, update, frames=np.arange(0, t_final/dt), blit=False, repeat=False)
 # ani.save('animation.mp4', fps=30, extra_args=['-vcodec', 'libx264'])

headeronly/tinytypes.hpp

@@ -0,0 +1,28 @@
+#include <cstdint>
+#include <cmath>
+
+namespace tiny {
+
+using u8  = std::uint8_t;
+using u16 = std::uint16_t;
+using u32 = std::uint32_t;
+using u64 = std::uint64_t;
+
+using i8  = std::int8_t;
+using i16 = std::int16_t;
+using i32 = std::int32_t;
+using i64 = std::int64_t;
+
+using f32 = float;
+using f64 = double;
+
+static const f64 PI_d = 3.14159265358979;
+static const f32 PI_f = 3.141593f;
+static const f64 EPS_d = 2.22045e-16;
+static const f32 EPS_f = 1.19209e-07f;
+static const f64 EPS_sqrt_d = std::sqrt(EPS_d);
+static const f32 EPS_sqrt_f = std::sqrt(EPS_f);
+static const f64 EPS_cbrt_d = std::cbrt(EPS_d);
+static const f32 EPS_cbrt_f = std::cbrt(EPS_f);
+
+} // namespace tiny

tests/test_uvlm_theodorsen.cpp

@@ -3,6 +3,7 @@
 #include <fstream>
 #include <cmath>
 #include <cstdio>
+#include <functional> // std::function
 
 #include "tinycombination.hpp"
 
@@ -12,6 +13,35 @@
 using namespace vlm;
 using namespace linalg::ostream_overloads;
 
+using tmatrix = linalg::alias::float4x4; // transformation matrix
+
+class Kinematics {
+    public:
+    std::vector<std::function<tmatrix(f32 t)>> joints;
+    Kinematics() = default;
+    ~Kinematics() = default;
+
+    void add(std::function<tmatrix(f32 t)>&& joint) {
+        joints.push_back(std::move(joint));
+    }
+
+    tmatrix displacement(f32 t) {
+        tmatrix result = linalg::identity;
+        for (const auto& joint : joints) {
+            result = linalg::mul(result, joint(t));
+        }
+        return result;
+    }
+
+    tmatrix relative_displacement(f32 t0, f32 t1) {
+        return linalg::mul(displacement(t1), linalg::inverse(displacement(t0)));
+    }
+
+    f32 absolute_velocity(f32 t, const linalg::alias::float4& vertex) {
+        return linalg::length((linalg::mul(relative_displacement(t, t+EPS_sqrt_f), vertex)-vertex)/EPS_sqrt_f);
+    }
+};
+
 int main() {
     const std::vector<std::string> meshes = {"../../../../mesh/infinite_rectangular_5x200.x"};
     const std::vector<std::string> backends = get_available_backends();
@@ -21,54 +51,40 @@ int main() {
     // Define simulation length
     const f32 t_final = 10.0f;
 
+    Kinematics kinematics{};
+
     // Define wing motion
-    auto displacement_wing = [&](f32 t) -> linalg::alias::float4x4 {
+    kinematics.add([](f32 t) {
         return linalg::translation_matrix(linalg::alias::float3{
             0.0f,
             0.0f,
             std::sin(0.2f * t)
         });
-    };
+    });
 
-    // For the moment take AoA=0 and U_inf=1
-    auto displacement_freestream = [&](f32 t) -> linalg::alias::float4x4 {
+    kinematics.add([](f32 t) {
         return linalg::translation_matrix(linalg::alias::float3{
             -1.0f*t,
             0.0f,
             0.0f
         });
-    };
-
-    auto displacement = [&](f32 t) -> linalg::alias::float4x4 {
-        return linalg::mul(displacement_freestream(t), displacement_wing(t));
-    };
-
-    auto relative_displacement = [&](f32 t0, f32 t1) -> linalg::alias::float4x4 {
-        return linalg::mul(displacement(t1), linalg::inverse(displacement(t0)));
-    };
-
-    // Magnitude of the velocity of a vertex at time t calculated by finite difference
-    auto absolute_velocity = [&](f32 t, const linalg::alias::float4& vertex) -> f32 {
-        return linalg::length((linalg::mul(displacement(t+EPS_sqrt_f), vertex)-linalg::mul(displacement(t), vertex))/EPS_sqrt_f);
-    };
+    });
 
     for (const auto& [mesh_name, backend_name] : solvers) {
         UVLM solver{};
         solver.mesh = create_mesh(mesh_name);
-        solver.backend = create_backend(backend_name, *solver.mesh);
-
-        std::vector<f32> vec_t; // pre-calculated timesteps
 
         // Pre-calculate timesteps to determine wake size
+        std::vector<f32> vec_t; // timesteps
         vec_t.push_back(0.0f);
         for (f32 t = 0.0f; t < t_final;) {
-            const f32 dt = (0.5f * (solver.mesh->chord_length(0) + solver.mesh->chord_length(1))) / absolute_velocity(t, {0.f, 0.f, 0.f, 1.f});
+            const f32 dt = (0.5f * (solver.mesh->chord_length(0) + solver.mesh->chord_length(1))) / kinematics.absolute_velocity(t, {0.f, 0.f, 0.f, 1.f});
             t += dt;
             vec_t.push_back(t);
         }
 
-        // TODO: think about splitting mesh body and wake mesh
         solver.mesh->resize_wake(vec_t.size()-1); // +1 for the initial pose
+        solver.backend = create_backend(backend_name, *solver.mesh); // create after mesh has been resized
 
         // f32 t = 0.0f;
         // for (u64 i = 0; i < vec_dt.size(); i++) {

vlm/src/vlm_mesh.cpp

@@ -382,21 +382,16 @@ void Mesh::move(const linalg::alias::float4x4& transform) {
 
     // Perform the movement
     for (u64 i = 0; i < nb_vertices_wing(); i++) {
-        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;
         v.z[i] = transformed_pt.z;
     }
+
+    // Copy new trailing edge vertices on the wake buffer
+    std::copy(v.x.data() + src_begin, v.x.data() + src_end, v.x.data() + dst_begin - (ns + 1));
+    std::copy(v.y.data() + src_begin, v.y.data() + src_end, v.y.data() + dst_begin - (ns + 1));
+    std::copy(v.z.data() + src_begin, v.z.data() + src_end, v.z.data() + dst_begin - (ns + 1));
 }
 
 void Mesh::resize_wake(const u64 nw_) {