add optimized wake_influence kernel

and dual quaternion prototype python code

data/debug.py

@@ -1,11 +1,30 @@
 import numpy as np
 
-lhs = np.array([-1.27324,3.81972e-10,0.424413,3.81972e-10,3.81972e-10,-1.27324,-1.46841e-08,0.424413,0.424413,3.81972e-10,-1.27324,3.81972e-10,1.54481e-08,0.424413,3.81972e-10,-1.27324]).reshape((4,4)).T
+EPS_f = np.float32(1.19209e-07)
+EPS_sqrt_f = np.sqrt(EPS_f)
 
-rhs = np.array([-0.0871557519] * 4)
-wake_buf = np.array([0.0848438,3.80518e-10,0.424593,3.80518e-10,6.40765e-09,0.0848438,1.54434e-08,0.424593]).reshape((2,4)).T
-gamma = np.array([0.102677956, 0.102677956])
+a = np.float32(0.1)
+b = np.float32(0.75)
 
+def func(t: np.float32):
+    return a * np.sin(b * t)
 
-print(wake_buf)
-print(rhs - wake_buf @ gamma)
+def analytical_derivative(t: np.float32):
+    return a * b * np.cos(b * t)
+
+def derivative(f, t):
+    # h = np.max((np.sqrt(t) * EPS_sqrt_f, EPS_f))
+    h = EPS_sqrt_f
+    return (f(t + h) - f(t - h)) / (2 * h)
+
+def complex_step_derivative(f, t, h=EPS_sqrt_f):
+    return np.imag(f(t + h*1j)) / h
+
+n = 500
+vec_t = np.linspace(0, 100, n)
+
+abs_err_fdm = [np.abs(derivative(func, t) - analytical_derivative(t)) for t in vec_t]
+abs_err_csd = [np.abs(complex_step_derivative(func, t) - analytical_derivative(t)) for t in vec_t]
+
+print(f"Avg. abs. error (FDM): {np.mean(abs_err_fdm):.3e}")
+print(f"Avg. abs. error (CSD): {np.mean(abs_err_csd):.3e}")

data/displacement.py

@@ -6,7 +6,7 @@
 class Kinematics:
     joints = [] # list of transformation lambdas
 
-    def add_joint(self, joint, frame):
+    def add_joint(self, joint):
         self.joints.append(joint)
 
     def displacement(self, t):
@@ -50,19 +50,21 @@ def rotation_matrix(center, axis, theta):
 def move_vertices(vertices, displacement):
     return displacement @ vertices
 
-def displacement_wing(t): return translation_matrix([0, 0, np.sin(0.9 * t)])
-def displacement_freestream(t): 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): 
 #     return rotation_matrix([0, 0, 0], [0, 0, 1], 7 * t)
+def displacement_wing(t): return translation_matrix([0, 0, np.sin(0.9 * t)])
+def freestream(t): return translation_matrix([-1 * t, 0, 0])
+alpha = np.radians(5)
+def freestream2(t): return translation_matrix([-np.cos(alpha)*t, 0, -np.sin(alpha)*t])
 def pitching(t): 
-    return rotation_matrix([0, 0, 0], [0, 1, 0], np.sin(t))
+    return rotation_matrix([0, 0, 0], [0, 1, 0], 0.25 * np.pi * np.sin(t))
 
 kinematics = Kinematics()
-# kinematics.add_joint(displacement_freestream, np.identity(4))
-# kinematics.add_joint(displacement_wing, np.identity(4))
-kinematics.add_joint(pitching, np.identity(4))
+kinematics.add_joint(freestream2)
+kinematics.add_joint(pitching)
 
 dt = 0.2
 t_final = 15
@@ -76,6 +78,9 @@ def pitching(t):
 ])
 vertices = np.column_stack((vertices, vertices[:,0]))
 
+# print("Analytical vel: ", [-np.cos(alpha), 0, -np.sin(alpha)])
+print("Numerical vel: ", kinematics.velocity(0, [4.0,0,0,1]))
+
 # Setup animation
 fig = plt.figure()
 ax = fig.add_subplot(111, projection='3d')

data/displacement_quaternions.py

@@ -0,0 +1,98 @@
+import numpy as np
+
+class DualQuaternion:
+    def __init__(self, real, dual):
+        self.real = real  # Quaternion
+        self.dual = dual  # Quaternion
+
+    @staticmethod
+    def from_translation(translation):
+        real = np.array([1, 0, 0, 0], dtype=np.complex64)
+        translation = np.array(translation, dtype=np.complex64)  # Convert to NumPy array
+        dual = np.hstack(([0], 0.5 * translation))
+        return DualQuaternion(real, dual)
+
+    @staticmethod
+    def from_rotation(axis_origin, axis, angle):
+        axis = np.array(axis, dtype=np.float32)
+        axis /= np.linalg.norm(axis)
+        sin_half_angle = np.sin(angle / 2)
+        real = np.hstack(([np.cos(angle / 2)], sin_half_angle * axis)).astype(np.complex64)
+
+        # Calculate the dual part
+        origin = np.array(axis_origin, dtype=np.float32)
+        dual_part = np.cross(-origin, sin_half_angle * axis)
+        dual = np.hstack(([0], dual_part)).astype(np.complex64)
+
+        return DualQuaternion(real, dual)
+
+    def transform_point(self, point):
+        p = np.array([0, point[0], point[1], point[2]], dtype=np.complex64)
+        transformed_p = self._quaternion_mul(self._quaternion_mul(self.real, p), self._quaternion_conj(self.real)) + 2 * self._quaternion_mul(self.dual, self._quaternion_conj(self.real))
+        return transformed_p[1:4]
+
+    def __mul__(self, other):
+        real = self._quaternion_mul(self.real, other.real)
+        dual = self._quaternion_mul(self.real, other.dual) + self._quaternion_mul(self.dual, other.real)
+        return DualQuaternion(real, dual)
+
+    @staticmethod
+    def _quaternion_mul(q1, q2):
+        w1, x1, y1, z1 = q1
+        w2, x2, y2, z2 = q2
+        w = w1*w2 - x1*x2 - y1*y2 - z1*z2
+        x = w1*x2 + x1*w2 + y1*z2 - z1*y2
+        y = w1*y2 - x1*z2 + y1*w2 + z1*x2
+        z = w1*z2 + x1*y2 - y1*x2 + z1*w2
+        return np.array([w, x, y, z], dtype=np.complex64)
+
+    @staticmethod
+    def _quaternion_conj(q):
+        return np.array([q[0], -q[1], -q[2], -q[3]], dtype=q.dtype)
+
+alpha = np.radians(5)  # Example angle in radians
+
+def freestream2(t):
+    return DualQuaternion.from_translation([-np.cos(alpha) * t, 0, -np.sin(alpha) * t])
+
+def pitching(t):
+    return DualQuaternion.from_rotation([0, 0, 0], [0, 1, 0], 0.25 * np.pi * np.sin(t))
+
+class Kinematics:
+    def __init__(self):
+        self.m_joints = []
+
+    def add_joint(self, joint):
+        self.m_joints.append(joint)
+
+    def displacement(self, t, n=0):
+        result = DualQuaternion(np.array([1, 0, 0, 0], dtype=np.complex64), np.array([0, 0, 0, 0], dtype=np.complex64))
+        end_joint = len(self.m_joints) if n == 0 else n
+        for i in range(end_joint):
+            result = result * self.m_joints[i](t)
+        return result
+
+    def relative_displacement(self, t0, t1, n=0):
+        displacement_t1 = self.displacement(t1, n)
+        displacement_t0 = self.displacement(t0, n)
+        displacement_t0_inv = DualQuaternion(displacement_t0.real, -displacement_t0.dual)
+        return displacement_t1 * displacement_t0_inv
+
+    def velocity(self, t, vertex, n=0):
+        EPS = np.sqrt(np.finfo(np.float32).eps)
+        complex_t = t + 1j * EPS
+        displacement_with_complex_step = self.relative_displacement(t, complex_t, n)
+        return displacement_with_complex_step.transform_point(vertex).imag / EPS
+
+    def velocity_magnitude(self, t, vertex):
+        return np.linalg.norm(self.velocity(t, vertex))
+
+# Example usage
+kinematics = Kinematics()
+kinematics.add_joint(freestream2)
+kinematics.add_joint(pitching)
+
+vertex = np.array([4.0, 0, 0], dtype=np.float32)  # Vertex without homogeneous coordinate
+t = 0.0
+print("Numerical vel: ", kinematics.velocity(t, vertex))
+print("Velocity Magnitude:", kinematics.velocity_magnitude(t, vertex))

data/theodorsen.py

@@ -38,11 +38,28 @@ def theo_fun(k):
 a = ar / c # full span
 pitch_axis = -0.5 # leading edge
 
-def atime(t: float): return 2. * u_inf * t / c
+cycles = 4 # number of periods
+
+uvlm_cl = []
+uvlm_t = []
+uvlm_z = []
+uvlm_alpha = []
+with open("build/windows/x64/release/cl_data.txt", "r") as f:
+    freq = float(f.readline()) # get reduced frequency of the simulation
+    for line in f:
+        time_step, z, cl, alpha = line.split()
+        uvlm_t.append(float(time_step))
+        uvlm_z.append(float(z))
+        uvlm_cl.append(float(cl))
+        uvlm_alpha.append(float(alpha))
+
+n = len(uvlm_t) # number of time steps
+uvlm_cycle_idx = int((1 - 1 / cycles) * n - 1)
+uvlm_alpha = np.array(uvlm_alpha)
+uvlm_cl = np.array(uvlm_cl)
 
-cycles = 4
 amplitudes = [0.1] 
-reduced_frequencies = [0.5]
+reduced_frequencies = [freq]
 
 t_final = cycles * 2 * np.pi / (reduced_frequencies[0] * 2.0 * u_inf / c) # 4 cycles
 nb_pts = 500
@@ -61,12 +78,12 @@ def atime(t: float): return 2. * u_inf * t / c
     omega = k * 2.0 * u_inf / c # pitch frequency
 
     # sudden acceleration
-    # def pitch(t): return np.radians(5)
-    # def heave(t): return 0
+    def pitch(t): return np.radians(5)
+    def heave(t): return 0
 
     # pure heaving
-    def pitch(t): return 0
-    def heave(t): return -amplitude * np.sin(omega * t)
+    # def pitch(t): return 0
+    # def heave(t): return -amplitude * np.sin(omega * t)
 
     # pure pitching
     # def pitch(t): return np.radians(np.sin(omega * t))
@@ -96,23 +113,6 @@ def cl_theodorsen(t: float): # using Wagner functions and Kholodar formulation
     axs["time"].plot(vec_t, cl_theo, label=f"k={k}")
     axs["heave"].plot(coord_z[cycle_idx:], cl_theo[cycle_idx:], label=f"k={k}")
 
-uvlm_cl = []
-uvlm_t = []
-uvlm_z = []
-uvlm_alpha = []
-with open("build/windows/x64/release/cl_data.txt", "r") as f:
-    for line in f:
-        time_step, z, cl, alpha = line.split()
-        uvlm_t.append(float(time_step))
-        uvlm_z.append(float(z))
-        uvlm_cl.append(float(cl))
-        uvlm_alpha.append(float(alpha))
-
-n = len(uvlm_t) # number of time steps
-uvlm_cycle_idx = int((1 - 1 / cycles) * n - 1)
-uvlm_alpha = np.array(uvlm_alpha)
-uvlm_cl = np.array(uvlm_cl)
-
 analytical_cl = np.array([np.interp(ut, vec_t, cl_theo) for ut in uvlm_t[uvlm_cycle_idx:]])
 difference = uvlm_cl[uvlm_cycle_idx:] - analytical_cl
 error = np.sqrt(np.dot(difference, difference) / (n-uvlm_cycle_idx))