fix: driven atoms now oscillate around their initial equilibrium position

Previously, apply_driving_force set absolute position to A*cos(2π f t + φ),
ignoring the atom's initial coordinates. For atoms not at the origin (e.g.,
atom 120 at x=119), this incorrectly forced them back toward the origin each
step, causing severe distortion and numerical explosion.

Fix: store each driven atom's initial position as eq_pos/eq_x/eq_y/eq_z at
load time; position is now eq + A*cos(2π f t + φ) in all four engines
(Python, C, C++, Fortran).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

compute.py

@@ -69,6 +69,12 @@ camera_keyframes_raw = ""
 camera_distance = 40.0
 camera_elevation = 0.0
 camera_azimuth = 0.0
+camera_center_x = 0.0
+camera_center_y = 0.0
+camera_center_z = 0.0
+camera_pos_x = None
+camera_pos_y = None
+camera_pos_z = None
 FIXED_MASK_X = None
 FIXED_MASK_Y = None
 FIXED_MASK_Z = None
@@ -649,6 +655,8 @@ def load_driver_file(driver_path, atom_ids):
                 "phi": phi_rad,
                 "period_str": period_str,
                 "period_cycles": None if period_str == "all" else float(period_str),
+                # 平衡位置（原子初始坐标），驱动为相对于此位置的偏移
+                "eq_pos": None,
                 # 在模拟中动态记录：冻结步数索引、冻结位置
                 "freeze_step": None,
                 "freeze_pos": None,
@@ -698,8 +706,9 @@ def apply_driving_force(x, y, z, vx, vy, vz, t, step, drivers, dt):
         else:
             period_steps = None  # 全程驱动
 
-        # 当前驱动力下的位置 / 速度
+        # 当前驱动力下的位置 / 速度（偏移叠加在初始平衡坐标上）
-        pos_drive = d["amp"] * np.cos(2.0 * np.pi * d["freq"] * t + d["phi"])
+        eq = d["eq_pos"] if d["eq_pos"] is not None else np.zeros(3)
+        pos_drive = eq + d["amp"] * np.cos(2.0 * np.pi * d["freq"] * t + d["phi"])
         vel_drive = -d["amp"] * 2.0 * np.pi * d["freq"] * np.sin(2.0 * np.pi * d["freq"] * t + d["phi"])
 
         x[idx] = pos_drive[0]
@@ -764,6 +773,8 @@ def run_from_config(config, out_dir=None):
     global ball_radius, ball_color_r, ball_color_g, ball_color_b
     global box_color_r, box_color_g, box_color_b
     global use_marker, camera_keyframes_raw, camera_distance, camera_elevation, camera_azimuth
+    global camera_center_x, camera_center_y, camera_center_z
+    global camera_pos_x, camera_pos_y, camera_pos_z
     global FIXED_MASK_X, FIXED_MASK_Y, FIXED_MASK_Z
     global warmup_steps, sample_start, sample_end
     global GRAVITY_FIELD, GRAVITY_INTERACTION, ELASTIC_FORCE, DAMPING_FORCE, GRAVITY_STRENGTH
@@ -832,6 +843,12 @@ def run_from_config(config, out_dir=None):
     camera_distance = float(config.get("camera_distance", 40.0))
     camera_elevation = float(config.get("camera_elevation", 0.0))
     camera_azimuth = float(config.get("camera_azimuth", 0.0))
+    camera_center_x = float(config.get("camera_center_x", 0.0))
+    camera_center_y = float(config.get("camera_center_y", 0.0))
+    camera_center_z = float(config.get("camera_center_z", 0.0))
+    camera_pos_x = config.get("camera_pos_x", None)
+    camera_pos_y = config.get("camera_pos_y", None)
+    camera_pos_z = config.get("camera_pos_z", None)
 
     # 力开关
     global GRAVITY_FIELD, GRAVITY_INTERACTION, ELASTIC_FORCE, DAMPING_FORCE, GRAVITY_STRENGTH
@@ -851,6 +868,15 @@ def run_from_config(config, out_dir=None):
         if out_dir is not None and not os.path.isabs(driver_rel):
             driver_path = os.path.join(out_dir, driver_rel)
         DRIVER_DATA = load_driver_file(driver_path, ATOM_IDS)
+        # 将每个驱动原子的初始坐标作为平衡位置
+        if DRIVER_DATA:
+            for _d in DRIVER_DATA:
+                _i = _d["atom_index"]
+                _d["eq_pos"] = np.array([
+                    ATOM_POSITIONS[_i, 0],
+                    ATOM_POSITIONS[_i, 1],
+                    ATOM_POSITIONS[_i, 2],
+                ], dtype=np.float64)
 
     # 加载运动相机关键帧
     camera_keyframes_raw = ""
@@ -986,6 +1012,9 @@ def run_engine(engine, input_dir, output_dir, config):
         "camera_distance": float(config.get("camera_distance", 40.0)),
         "camera_elevation": float(config.get("camera_elevation", 0)),
         "camera_azimuth": float(config.get("camera_azimuth", 0)),
+        "camera_center_x": float(config.get("camera_center_x", 0.0)),
+        "camera_center_y": float(config.get("camera_center_y", 0.0)),
+        "camera_center_z": float(config.get("camera_center_z", 0.0)),
     }
     param_path = os.path.join(script_dir, "engines", engine, "param.json")
     os.makedirs(os.path.dirname(param_path), exist_ok=True)
@@ -1136,12 +1165,27 @@ def run_engine(engine, input_dir, output_dir, config):
         _npz_path = disp_path.replace("display.txt", "display.npz")
         try:
             _d = load_display_txt(disp_path)
+            # 外部引擎不写摄像机参数到 display.txt，在此从全局变量补入
+            _cam_extra = {
+                "camera_distance":  str(camera_distance),
+                "camera_elevation": str(camera_elevation),
+                "camera_azimuth":   str(camera_azimuth),
+                "camera_center_x":  str(camera_center_x),
+                "camera_center_y":  str(camera_center_y),
+                "camera_center_z":  str(camera_center_z),
+                "camera_keyframes": str(camera_keyframes_raw),
+            }
+            if camera_pos_x is not None:
+                _cam_extra["camera_pos_x"] = str(camera_pos_x)
+                _cam_extra["camera_pos_y"] = str(camera_pos_y)
+                _cam_extra["camera_pos_z"] = str(camera_pos_z)
             save_display_npz(
                 _npz_path,
                 _d["frames_x"], _d["frames_y"], _d["frames_z"],
                 _d["frames_vx"], _d["frames_vy"], _d["frames_vz"],
                 _d["atom_ids"],
                 header_fields={**_d["header_fields"],
+                                **_cam_extra,
                                 "number_of_frames": str(_d["n_total_frames"]),
                                 "number_of_particles": str(_d["n_total_particles"])})
             print(f"[compute] display.npz 已生成: {_npz_path}")
@@ -1727,6 +1771,14 @@ def run_simulation(save_trajectory=0):
                        "camera_distance": str(camera_distance),
                        "camera_elevation": str(camera_elevation),
                        "camera_azimuth": str(camera_azimuth),
+                       "camera_center_x": str(camera_center_x),
+                       "camera_center_y": str(camera_center_y),
+                       "camera_center_z": str(camera_center_z),
+                       **({
+                           "camera_pos_x": str(camera_pos_x),
+                           "camera_pos_y": str(camera_pos_y),
+                           "camera_pos_z": str(camera_pos_z),
+                       } if camera_pos_x is not None else {}),
                        "camera_keyframes": str(camera_keyframes_raw)}
     )
     print(f"[compute] display.txt 已保存至: {disp_path} ({n_frames_actual} 帧)")
@@ -1765,6 +1817,9 @@ def run_simulation(save_trajectory=0):
             "camera_distance": str(camera_distance),
             "camera_elevation": str(camera_elevation),
             "camera_azimuth": str(camera_azimuth),
+            "camera_center_x": str(camera_center_x),
+            "camera_center_y": str(camera_center_y),
+            "camera_center_z": str(camera_center_z),
             "camera_keyframes": str(camera_keyframes_raw),
             "number_of_frames": str(record_steps),
             "number_of_particles": str(n_atoms)}

engines/c/main.c

@@ -84,6 +84,7 @@ typedef struct {
     double *phi_x, *phi_y, *phi_z;   /* radians */
     int    *has_period;    /* 0=all, 1=limited cycles */
     double *period_cycles; /* number of cycles */
+    double *eq_x, *eq_y, *eq_z;       /* 平衡位置（初始坐标） */
     double *freeze_x, *freeze_y, *freeze_z;
 } DriverData;
 
@@ -129,12 +130,16 @@ static DriverData read_driver(const char *input_dir, const AtomData *atoms) {
     d.phi_z = (double*)xmalloc(n_lines * sizeof(double));
     d.has_period = (int*)xmalloc(n_lines * sizeof(int));
     d.period_cycles = (double*)xmalloc(n_lines * sizeof(double));
+    d.eq_x = (double*)xmalloc(n_lines * sizeof(double));
+    d.eq_y = (double*)xmalloc(n_lines * sizeof(double));
+    d.eq_z = (double*)xmalloc(n_lines * sizeof(double));
     d.freeze_x = (double*)xmalloc(n_lines * sizeof(double));
     d.freeze_y = (double*)xmalloc(n_lines * sizeof(double));
     d.freeze_z = (double*)xmalloc(n_lines * sizeof(double));
 
-    /* 初始化 freeze 数组 */
+    /* 初始化 freeze/eq 数组 */
     for (int i = 0; i < n_lines; i++) {
+        d.eq_x[i] = d.eq_y[i] = d.eq_z[i] = 0.0;
         d.freeze_x[i] = d.freeze_y[i] = d.freeze_z[i] = 0.0;
     }
 
@@ -177,6 +182,9 @@ static DriverData read_driver(const char *input_dir, const AtomData *atoms) {
         if (ii < 0) continue;
 
         d.atom_idx[idx] = ii;
+        d.eq_x[idx] = atoms->pos_0[ii*3+0];
+        d.eq_y[idx] = atoms->pos_0[ii*3+1];
+        d.eq_z[idx] = atoms->pos_0[ii*3+2];
         d.amp_x[idx] = amp_x;
         d.amp_y[idx] = amp_y;
         d.amp_z[idx] = amp_z;
@@ -738,9 +746,9 @@ static void apply_driving_force(
             }
         }
 
-        double px = drivers->amp_x[d] * cos(2*M_PI*drivers->freq_x[d]*t + drivers->phi_x[d]);
+        double px = drivers->eq_x[d] + drivers->amp_x[d] * cos(2*M_PI*drivers->freq_x[d]*t + drivers->phi_x[d]);
-        double py = drivers->amp_y[d] * cos(2*M_PI*drivers->freq_y[d]*t + drivers->phi_y[d]);
+        double py = drivers->eq_y[d] + drivers->amp_y[d] * cos(2*M_PI*drivers->freq_y[d]*t + drivers->phi_y[d]);
-        double pz = drivers->amp_z[d] * cos(2*M_PI*drivers->freq_z[d]*t + drivers->phi_z[d]);
+        double pz = drivers->eq_z[d] + drivers->amp_z[d] * cos(2*M_PI*drivers->freq_z[d]*t + drivers->phi_z[d]);
         double vpx = -drivers->amp_x[d]*2*M_PI*drivers->freq_x[d]*sin(2*M_PI*drivers->freq_x[d]*t + drivers->phi_x[d]);
         double vpy = -drivers->amp_y[d]*2*M_PI*drivers->freq_y[d]*sin(2*M_PI*drivers->freq_y[d]*t + drivers->phi_y[d]);
         double vpz = -drivers->amp_z[d]*2*M_PI*drivers->freq_z[d]*sin(2*M_PI*drivers->freq_z[d]*t + drivers->phi_z[d]);

engines/cpp/main.cpp

@@ -86,6 +86,7 @@ struct DriverData {
     std::vector<double> phi_x, phi_y, phi_z;  // radians
     std::vector<int> has_period;     // 0=all, 1=limited
     std::vector<double> period_cycles;
+    std::vector<double> eq_x, eq_y, eq_z;           // 平衡位置（初始坐标）
     std::vector<double> freeze_x, freeze_y, freeze_z;
 };
 
@@ -314,6 +315,9 @@ static DriverData read_driver(const std::string &input_dir, const AtomData &atom
             continue;
         }
         d.atom_idx.push_back(idx);
+        d.eq_x.push_back(atoms.pos_0[idx*3+0]);
+        d.eq_y.push_back(atoms.pos_0[idx*3+1]);
+        d.eq_z.push_back(atoms.pos_0[idx*3+2]);
         d.amp_x.push_back(ax); d.amp_y.push_back(ay); d.amp_z.push_back(az);
         d.freq_x.push_back(fx); d.freq_y.push_back(fy); d.freq_z.push_back(fz);
         // Convert degrees to radians
@@ -700,9 +704,9 @@ static void apply_driving_force(
         }
 
         const double TWO_PI = 2.0 * M_PI;
-        double px = drivers.amp_x[d] * std::cos(TWO_PI * drivers.freq_x[d] * t + drivers.phi_x[d]);
+        double px = drivers.eq_x[d] + drivers.amp_x[d] * std::cos(TWO_PI * drivers.freq_x[d] * t + drivers.phi_x[d]);
-        double py = drivers.amp_y[d] * std::cos(TWO_PI * drivers.freq_y[d] * t + drivers.phi_y[d]);
+        double py = drivers.eq_y[d] + drivers.amp_y[d] * std::cos(TWO_PI * drivers.freq_y[d] * t + drivers.phi_y[d]);
-        double pz = drivers.amp_z[d] * std::cos(TWO_PI * drivers.freq_z[d] * t + drivers.phi_z[d]);
+        double pz = drivers.eq_z[d] + drivers.amp_z[d] * std::cos(TWO_PI * drivers.freq_z[d] * t + drivers.phi_z[d]);
         double vpx = -drivers.amp_x[d] * TWO_PI * drivers.freq_x[d] * std::sin(TWO_PI * drivers.freq_x[d] * t + drivers.phi_x[d]);
         double vpy = -drivers.amp_y[d] * TWO_PI * drivers.freq_y[d] * std::sin(TWO_PI * drivers.freq_y[d] * t + drivers.phi_y[d]);
         double vpz = -drivers.amp_z[d] * TWO_PI * drivers.freq_z[d] * std::sin(TWO_PI * drivers.freq_z[d] * t + drivers.phi_z[d]);

engines/fortran/main.f90

@@ -41,6 +41,7 @@ program dynamics_f90
     double precision, allocatable :: drv_phi_x(:), drv_phi_y(:), drv_phi_z(:)
     integer, allocatable :: drv_has_period(:)
     double precision, allocatable :: drv_period_cycles(:)
+    double precision, allocatable :: drv_eq_x(:), drv_eq_y(:), drv_eq_z(:)
     double precision, allocatable :: drv_freeze_x(:), drv_freeze_y(:), drv_freeze_z(:)
 
     ! 运行时位置/速度
@@ -82,11 +83,12 @@ program dynamics_f90
     ! 读取驱动力
     n_drivers = 0
     if (driving_force /= 0) then
-        call read_driver(input_dir, n_atoms, atom_ids, n_drivers, &
+        call read_driver(input_dir, n_atoms, atom_ids, pos_0, n_drivers, &
                          drv_atom_idx, drv_amp_x, drv_amp_y, drv_amp_z, &
                          drv_freq_x, drv_freq_y, drv_freq_z, &
                          drv_phi_x, drv_phi_y, drv_phi_z, &
                          drv_has_period, drv_period_cycles, &
+                         drv_eq_x, drv_eq_y, drv_eq_z, &
                          drv_freeze_x, drv_freeze_y, drv_freeze_z)
     end if
 
@@ -134,6 +136,7 @@ program dynamics_f90
                            drv_freq_x, drv_freq_y, drv_freq_z, &
                            drv_phi_x, drv_phi_y, drv_phi_z, &
                            drv_has_period, drv_period_cycles, &
+                           drv_eq_x, drv_eq_y, drv_eq_z, &
                            drv_freeze_x, drv_freeze_y, drv_freeze_z)
     end if
 
@@ -146,6 +149,7 @@ program dynamics_f90
                                drv_freq_x, drv_freq_y, drv_freq_z, &
                                drv_phi_x, drv_phi_y, drv_phi_z, &
                                drv_has_period, drv_period_cycles, &
+                               drv_eq_x, drv_eq_y, drv_eq_z, &
                                drv_freeze_x, drv_freeze_y, drv_freeze_z)
         end if
         call apply_step(method, n, x, y, z, vx, vy, vz, masses, G, B, &
@@ -171,6 +175,7 @@ program dynamics_f90
                                drv_freq_x, drv_freq_y, drv_freq_z, &
                                drv_phi_x, drv_phi_y, drv_phi_z, &
                                drv_has_period, drv_period_cycles, &
+                               drv_eq_x, drv_eq_y, drv_eq_z, &
                                drv_freeze_x, drv_freeze_y, drv_freeze_z)
         end if
         traj_x(s, :)  = x;  traj_y(s, :)  = y;  traj_z(s, :)  = z
@@ -205,6 +210,7 @@ program dynamics_f90
         deallocate(drv_freq_x, drv_freq_y, drv_freq_z)
         deallocate(drv_phi_x, drv_phi_y, drv_phi_z)
         deallocate(drv_has_period, drv_period_cycles)
+        deallocate(drv_eq_x, drv_eq_y, drv_eq_z)
         deallocate(drv_freeze_x, drv_freeze_y, drv_freeze_z)
     end if
 
@@ -804,14 +810,16 @@ end subroutine apply_step
 ! ========================================================================
 ! 读取驱动力 driver.txt
 ! ========================================================================
-subroutine read_driver(input_dir, n_atoms, atom_ids, n_drivers, &
+subroutine read_driver(input_dir, n_atoms, atom_ids, pos_0, n_drivers, &
                        drv_atom_idx, drv_amp_x, drv_amp_y, drv_amp_z, &
                        drv_freq_x, drv_freq_y, drv_freq_z, &
                        drv_phi_x, drv_phi_y, drv_phi_z, &
                        drv_has_period, drv_period_cycles, &
+                       drv_eq_x, drv_eq_y, drv_eq_z, &
                        drv_freeze_x, drv_freeze_y, drv_freeze_z)
     character(len=*), intent(in) :: input_dir
     integer, intent(in) :: n_atoms, atom_ids(n_atoms)
+    double precision, intent(in) :: pos_0(n_atoms, 3)
     integer, intent(out) :: n_drivers
     integer, allocatable, intent(out) :: drv_atom_idx(:)
     double precision, allocatable, intent(out) :: drv_amp_x(:), drv_amp_y(:), drv_amp_z(:)
@@ -819,6 +827,7 @@ subroutine read_driver(input_dir, n_atoms, atom_ids, n_drivers, &
     double precision, allocatable, intent(out) :: drv_phi_x(:), drv_phi_y(:), drv_phi_z(:)
     integer, allocatable, intent(out) :: drv_has_period(:)
     double precision, allocatable, intent(out) :: drv_period_cycles(:)
+    double precision, allocatable, intent(out) :: drv_eq_x(:), drv_eq_y(:), drv_eq_z(:)
     double precision, allocatable, intent(out) :: drv_freeze_x(:), drv_freeze_y(:), drv_freeze_z(:)
 
     character(len=512) :: path, line, period_str
@@ -831,6 +840,7 @@ subroutine read_driver(input_dir, n_atoms, atom_ids, n_drivers, &
     double precision :: pxx_tmp(MX), pyy_tmp(MX), pzz_tmp(MX)
     double precision :: pc_tmp(MX)
     double precision :: fzx_tmp(MX), fzy_tmp(MX), fzz_tmp2(MX)
+    double precision :: eqx_tmp(MX), eqy_tmp(MX), eqz_tmp(MX)
 
     n_drivers = 0
     path = trim(input_dir) // '/driver.txt'
@@ -859,6 +869,9 @@ subroutine read_driver(input_dir, n_atoms, atom_ids, n_drivers, &
         if (idx < 0) cycle
 
         idx_tmp(n_drivers) = idx - 1  ! 0-based index
+        eqx_tmp(n_drivers) = pos_0(idx, 1)
+        eqy_tmp(n_drivers) = pos_0(idx, 2)
+        eqz_tmp(n_drivers) = pos_0(idx, 3)
         ax_tmp(n_drivers) = ax; ay_tmp(n_drivers) = ay; az_tmp(n_drivers) = az
         fxx_tmp(n_drivers) = fx; fyy_tmp(n_drivers) = fy; fzz_tmp(n_drivers) = fz
         ! degrees to radians
@@ -886,6 +899,7 @@ subroutine read_driver(input_dir, n_atoms, atom_ids, n_drivers, &
     allocate(drv_freq_x(n_drivers), drv_freq_y(n_drivers), drv_freq_z(n_drivers))
     allocate(drv_phi_x(n_drivers), drv_phi_y(n_drivers), drv_phi_z(n_drivers))
     allocate(drv_has_period(n_drivers), drv_period_cycles(n_drivers))
+    allocate(drv_eq_x(n_drivers), drv_eq_y(n_drivers), drv_eq_z(n_drivers))
     allocate(drv_freeze_x(n_drivers), drv_freeze_y(n_drivers), drv_freeze_z(n_drivers))
 
     drv_atom_idx = idx_tmp(1:n_drivers)
@@ -894,6 +908,7 @@ subroutine read_driver(input_dir, n_atoms, atom_ids, n_drivers, &
     drv_phi_x = pxx_tmp(1:n_drivers); drv_phi_y = pyy_tmp(1:n_drivers); drv_phi_z = pzz_tmp(1:n_drivers)
     drv_has_period = per_tmp(1:n_drivers)
     drv_period_cycles = pc_tmp(1:n_drivers)
+    drv_eq_x = eqx_tmp(1:n_drivers); drv_eq_y = eqy_tmp(1:n_drivers); drv_eq_z = eqz_tmp(1:n_drivers)
     drv_freeze_x = fzx_tmp(1:n_drivers); drv_freeze_y = fzy_tmp(1:n_drivers); drv_freeze_z = fzz_tmp2(1:n_drivers)
 
     write(*, '("[Fortran-engine] 已加载驱动力: ", i0, " 条定义")') n_drivers
@@ -906,6 +921,7 @@ subroutine apply_driving(n, x, y, z, vx, vy, vz, t, step, dt, &
                          drv_freq_x, drv_freq_y, drv_freq_z, &
                          drv_phi_x, drv_phi_y, drv_phi_z, &
                          drv_has_period, drv_period_cycles, &
+                         drv_eq_x, drv_eq_y, drv_eq_z, &
                          drv_freeze_x, drv_freeze_y, drv_freeze_z)
     integer, intent(in) :: n, step, n_drivers
     integer, intent(in) :: drv_atom_idx(n_drivers), drv_has_period(n_drivers)
@@ -915,6 +931,7 @@ subroutine apply_driving(n, x, y, z, vx, vy, vz, t, step, dt, &
     double precision, intent(in) :: drv_freq_x(n_drivers), drv_freq_y(n_drivers), drv_freq_z(n_drivers)
     double precision, intent(in) :: drv_phi_x(n_drivers), drv_phi_y(n_drivers), drv_phi_z(n_drivers)
     double precision, intent(in) :: drv_period_cycles(n_drivers)
+    double precision, intent(in) :: drv_eq_x(n_drivers), drv_eq_y(n_drivers), drv_eq_z(n_drivers)
     double precision, intent(inout) :: drv_freeze_x(n_drivers), drv_freeze_y(n_drivers), drv_freeze_z(n_drivers)
 
     integer :: d, idx, period_steps
@@ -941,9 +958,9 @@ subroutine apply_driving(n, x, y, z, vx, vy, vz, t, step, dt, &
             end if
         end if
 
-        px = drv_amp_x(d) * cos(TWO_PI * drv_freq_x(d) * t + drv_phi_x(d))
+        px = drv_eq_x(d) + drv_amp_x(d) * cos(TWO_PI * drv_freq_x(d) * t + drv_phi_x(d))
-        py = drv_amp_y(d) * cos(TWO_PI * drv_freq_y(d) * t + drv_phi_y(d))
+        py = drv_eq_y(d) + drv_amp_y(d) * cos(TWO_PI * drv_freq_y(d) * t + drv_phi_y(d))
-        pz = drv_amp_z(d) * cos(TWO_PI * drv_freq_z(d) * t + drv_phi_z(d))
+        pz = drv_eq_z(d) + drv_amp_z(d) * cos(TWO_PI * drv_freq_z(d) * t + drv_phi_z(d))
         vpx = -drv_amp_x(d) * TWO_PI * drv_freq_x(d) * sin(TWO_PI * drv_freq_x(d) * t + drv_phi_x(d))
         vpy = -drv_amp_y(d) * TWO_PI * drv_freq_y(d) * sin(TWO_PI * drv_freq_y(d) * t + drv_phi_y(d))
         vpz = -drv_amp_z(d) * TWO_PI * drv_freq_z(d) * sin(TWO_PI * drv_freq_z(d) * t + drv_phi_z(d))