diff --git a/copter/Cargo.toml b/copter/Cargo.toml index 9af1ebb..4c219df 100644 --- a/copter/Cargo.toml +++ b/copter/Cargo.toml @@ -14,6 +14,7 @@ i2cdev-bmp280 = { path = "./local/i2cdev-sensors/i2cdev-bmp280" } i2cdev-l3gd20 = { path = "./local/i2cdev-sensors/i2cdev-l3gd20" } i2cdev-lsm303d = { path = "./local/i2cdev-sensors/i2cdev-lsm303d" } i2cdev-lsm9ds0 = { path = "./local/i2cdev-sensors/i2cdev-lsm9ds0" } +i2cdev-lsm303dlhc = { path = "./local/i2cdev-sensors/i2cdev-lsm303dlhc" } pca9685 = { path = "./local/rust-pca9685" } ads111x = { path = "./local/rust-ads1115" } unbounded-gpsd = "0.4.2" diff --git a/copter/config.json b/copter/config.json deleted file mode 100644 index aff7b72..0000000 --- a/copter/config.json +++ /dev/null @@ -1,45 +0,0 @@ -{ - "roll_kp": 3.7, - "roll_ki": 1.7, - "roll_kd": 0.6, - "pitch_kp": 3.7, - "pitch_ki": 1.7, - "pitch_kd": 0.6, - "yaw_kp": 0.7, - "yaw_kd": 0.05, - "alt_kp": 0.0, - "alt_ki": 0.0, - "alt_kd": 0.0, - "desired_angle": 0, - "motors": [ - 26, - 19, - 16, - 20 - ], - "motor_cutoff": 75.0, - "motors_on": false, - "integral_decay_time": 0.8, - "integral_bandwidth": 10.0, - "server_address": "10.0.0.209:7070", - "hover_power": 1100, - "max_motor_speed": 1250, - "take_off_time": 3.0, - "debug_websocket_port": 27070, - "sea_level_pressure": 1.01325, - "derivative_sampling": 0.001, - "integral_decay": 1.0, - "sensors": [ - "BMP280", - "LSM9DS0" - ], - "angle_offset_x": 0.0, - "angle_offset_y": -0.0, - "logging": false, - "logging_freq": 2, - "real_time_debugging": false, - "motor_frequency": 200, - "sensor_sample_frequency": 100, - "imu_frequency": 2000, - "pid_frequency": 200 -} \ No newline at end of file diff --git a/copter/configurations/src/calibrations.rs b/copter/configurations/src/calibrations.rs index 782a0c1..604c121 100644 --- a/copter/configurations/src/calibrations.rs +++ b/copter/configurations/src/calibrations.rs @@ -8,46 +8,46 @@ use na::{Matrix3, Vector3}; #[derive(Debug, Deserialize, Serialize)] pub struct Simple { - pub offsets: Vec, + pub offsets: Vec } impl Simple { - pub fn new(offsets: Vector3) -> Simple { + pub fn new(offsets: Vector3) -> Simple { Simple { offsets: offsets.as_slice().to_vec(), } } - pub fn get_offsets(&self) -> Vector3 { + pub fn get_offsets(&self) -> Vector3 { Vector3::from_column_slice(&self.offsets) } } #[derive(Debug, Deserialize, Serialize)] pub struct Ellipsoid { - pub offsets: Vec, - pub rotation: Vec, - pub gains: Vec, + pub offsets: Vec, + pub rotation: Vec, + pub gains: Vec } impl Ellipsoid { - pub fn new(offsets: Vector3, rotation: Matrix3, gains: Vector3) -> Ellipsoid { + pub fn new(offsets: Vector3, rotation: Matrix3, gains: Vector3) -> Ellipsoid { Ellipsoid { offsets: offsets.as_slice().to_vec(), rotation: rotation.as_slice().to_vec(), - gains: gains.as_slice().to_vec(), + gains: gains.as_slice().to_vec() } } - pub fn get_offsets(&self) -> Vector3 { + pub fn get_offsets(&self) -> Vector3 { Vector3::from_column_slice(&self.offsets) } - pub fn get_rotation(&self) -> Matrix3 { + pub fn get_rotation(&self) -> Matrix3 { Matrix3::from_column_slice(&self.rotation) } - pub fn get_gains(&self) -> Vector3 { + pub fn get_gains(&self) -> Vector3 { Vector3::from_column_slice(&self.gains) } } diff --git a/copter/ekf_matlab/ekf_derivation b/copter/ekf_matlab/ekf_derivation new file mode 100644 index 0000000..bc26381 --- /dev/null +++ b/copter/ekf_matlab/ekf_derivation @@ -0,0 +1 @@ +% SafeFlight EKF derivation \ No newline at end of file diff --git a/copter/ekf_matlab/ekf_derivation.m~ b/copter/ekf_matlab/ekf_derivation.m~ new file mode 100644 index 0000000..1e0a1d5 --- /dev/null +++ b/copter/ekf_matlab/ekf_derivation.m~ @@ -0,0 +1,137 @@ +% SafeFlight EKF derivation +% Written by Martin Deegan +clc + +helper_functions; + +% 19 state EKF +% Position: latitude, longitude, altitude +% Velocity: North, East, Vertical +% Attitude: quaternion representation +% Biases: Gyroscope, Accelerometer +% Magnetic Field +syms lat lon alt vn ve vz qx qy qz qw gbx gby gbz abx aby abz mx my mz +syms dlat dlon dalt dvn dve dvz dtx dty dtz dgbx dgby dgbz dabx daby dabz dmx dmy dmz +syms dt +syms x +syms dx +x = [lat; lon; alt; vn; ve; vz; qx; qy; qz; qw; gbx; gby; gbz; abx; aby; abz; mx; my; mz]; +dx = [lat lon alt vn ve vz qx qy qz qw gbx gby gbz abx aby abz mx my mz]; + +% Control Variables +syms gxp gyp gzp axp ayp azp +syms gx gy gz ax ay az +syms up un +up = [gxp; gyp; gzp; axp; ayp; azp]; +un = [gx; gy; gz; ax; ay; az]; + +% transition functions +syms remove_bias(r,b) +remove_bias(r, b) = r-b; +syms vec_mean(v1, v2) +vec_mean(v1, v2) = (v1+v2)/2; +syms predict_state(p, v, q, gb, ab, gp, ap, g, a, dt) +syms predict_state_corrected(p, v, q, gp, g, a, dt) +syms predict_state_velocity(qx, qy, qz, qw, vn, ve, vz, ax, ay, az, dt) +syms predict_state_attitude(qx, qy, qz, qw, gxp, gyp, gzp, gx, gy, gz, dt) +syms predict_state_position(lat, lon, alt, vn, ve, vz, dt) + +dXddx = jacobian(add_e_state([lat; lon; alt; vn; ve; vz; qx; qy; qz; qw;... + gbx; gby; gbz; abx; aby; abz; mx; my; mz], ... + [dlat; dlon; dalt; dvn; dve; dvz; dtx; dty; dtz; ... + dgbx; dgby; dgbz; dabx; daby; dabz; dmx; dmy; dmz]), ... + [dlat dlon dalt dvn dve dvz dtx dty dtz dgbx dgby dgbz dabx daby dabz dmx dmy dmz]); + +A = jacobian(predict_state(x, up, un, dt), dx); +A(7:10,7:13) = 0; + +syms fx fy fz +H_field = jacobian(rotate_field(x, [fx; fy; fz]), dx); +V_field = jacobian(rotate_field + +disp("Jacobian of the state with respect to the error state (X_dx):"); + + +disp("Linearization of the state transition function (A):"); +% disp(dXddx); +disp(H_field); + +% --------- Prediction Step ---------------- +function integrate_gyroscope_f = integrate_gyroscope(q, gp, gn, dt) + w = (gp + gn)/2; + omega_mean = omega_matrix(w); + omega_prev = omega_matrix(gp); + omega_next = omega_matrix(gn); + expon = expm(omega_mean); + second_term = (1/48)*(omega_next*omega_prev-omega_prev*omega_next)*dt^2; + integrate_gyroscope_f = (expon + second_term)*q; +end + +function predict_velocity_f = predict_velocity(v, a, q, dt) + rot = rot_matrix(q); + a_w = rot*a; + v_p = v + a_w * dt; + predict_velocity_f = v_p; +end + +function predict_position_f = predict_position(p, v, dt) + p_p = p + v*dt; + predict_position_f = p_p; +end + +function predict_state_f = predict_state(x, up, un, dt) + un = un - x(11:16); + q = integrate_gyroscope(x(7:10), up(1:3), un(1:3), dt); + v = predict_velocity(x(4:6), un(4:6), x(7:10), dt); + p = predict_position(x(1:3), x(4:6), dt); + predict_state_f = [p; v; q; x(11:19)]; +end + +% -------------------- Updates ------------------------- + +function rotate_field_f = rotate_field(x, f) + rot = rot_matrix(x(7:10)); + rotate_field_f = rot * f; +end + + +% -------------------------------Helper functions and variables -------------------------------------- + + +% Helper Matrices + +function rot_matrix_f = rot_matrix(q) +rot_matrix_f = [1-2*q(2)^2-2*q(3)^2 2*q(1)*q(2)-2*q(3)*q(4) 2*q(1)*q(3)+2*q(2)*q(4); + 2*q(1)*q(2)+2*q(3)*q(4) 1-2*q(1)^2-2*q(3)^2 2*q(2)*q(3)-2*q(1)*q(4); + 2*q(1)*q(3)-2*q(2)*q(4) 2*q(2)*q(3)+2*q(1)*q(4) 1-2*q(1)^2-2*q(2)^2]; +end + +function q_prod_l_f = q_prod_l(q) + q_prod_l_f = [q(4) q(3) -q(2) q(1); -q(3) q(4) q(1) q(2); q(2) -q(1) q(4) q(3); -q(1) -q(2) -q(3) q(4)]; +end + +function q_prod_r_f = q_prod_r(q) + q_prod_r_f = [q(4) -q(3) q(2) q(1); q(3) q(4) -q(1) q(2); -q(2) q(1) q(4) q(3); -q(1) -q(2) -q(3) q(4)]; +end + +function skew_matrix_f = skew_matrix(w) + skew_matrix_f = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; +end + +function omega_matrix_f = omega_matrix(w) + omega_matrix_f = [0 w(3) -w(2) w(1); -w(3) 0 w(1) w(2); w(2) -w(1) 0 w(3); -w(1) -w(2) -w(3) 0]; +end + +function small_e_to_dq_f = small_e_to_dq(theta) + small_e_to_dq_f = [0.5*theta(1); 0.5*theta(2); 0.5*theta(3); 1]; +end + +function add_e_state_f = add_e_state(x, dx) + p_prod = q_prod_r(x(7:10)); + dq = small_e_to_dq(dx(7:9)); + q_fixed = p_prod * dq; + dx_dim = [dx(1:6); 0; 0; 0; 0; dx(10:18)]; + x_fixed = x + dx_dim; + x_fixed(7:10) = q_fixed; + add_e_state_f = x_fixed; +end diff --git a/copter/ekf_matlab/helper_functions.m b/copter/ekf_matlab/helper_functions.m new file mode 100644 index 0000000..139597f --- /dev/null +++ b/copter/ekf_matlab/helper_functions.m @@ -0,0 +1,2 @@ + + diff --git a/copter/ekf_matlab/helper_functions.m~ b/copter/ekf_matlab/helper_functions.m~ new file mode 100644 index 0000000..c710894 --- /dev/null +++ b/copter/ekf_matlab/helper_functions.m~ @@ -0,0 +1,49 @@ +% Helper functions +clc +% Quaternion Helpers +syms quaternion_to_rotation_matrix(qx, qy, qz, qw) +quaternion_to_rotation_matrix(qx, qy, qz, qw) = [1-2*qy^2-2*qz^2 2*qx*qy-2*qz*qw 2*qx*qz+2*qy*qw; + 2*qx*qy+2*qz*qw 1-2*qx^2-2*qz^2 2*qy*qz-2*qx*qw; + 2*qx*qz-2*qy*qw 2*qy*qz+2*qx*qw 1-2*qx^2-2*qy^2]; + +syms left_product_matrix(qx, qy, qz, qw) +left_product_matrix(qx, qy, qz, qw) = [qw qz -qy qx; -qz qw qx qy; qy -qx qw qz; -qx -qy -qz qw]; +syms right_product_matrix(q) +right_product_matrix(q) = [q(4) -q(3) q(2) q(1); q(3) q(4) -q(1) q(2); -q(2) q(1) q(4) q(3); -q(1) -q(2) -q(3) q(4)]; + +syms sqew_matrix(x, y, z) +sqew_matrix(x, y, z) = [0 -z y; z 0 -x; -y x 0]; + +syms omega_matrix(x, y, z) +omega_matrix(qx, qy, qz, qw) = [0 z -y x; -z 0 x y; y -x 0 z; -x -y -z 0]; + +syms small_error_to_error_quaternion(theta) +small_error_to_error_quaternion(theta) = [0.5*theta(1); 0.5*theta(2); 0.5*theta(3); 1]; + +syms add_error_state(lat, lon, alt, vn, ve, vz, qx, qy, qz, qw, gbx, gby, gbz, abx, aby, abz, mx, my, mz, dlat, dlon, dalt, dvn, dve, dvz, dtx, dty, dtz, dgbx, dgby, dgbz, dabx, daby, dabz, dmx, dmy, dmz) +add_error_state(lat, lon, alt, vn, ve, vz, qx, qy, qz, qw, gbx, gby, gbz, abx, aby, abz, mx, my, mz, ... + dlat, dlon, dalt, dvn, dve, dvz, dtx, dty, dtz, dgbx, dgby, dgbz, dabx, daby, dabz, dmx, dmy, dmz) = ... + [lat+dlat; lon+dlon; alt+dalt; vn+dvn; ve+dve; vz+dvz; ... + right_product_matrix(qx, qy, qz, qw)*small_error_to_error_quaternion(dtx, dty, dtz); ... + gbx+dgbx; gby+dgby; gbz+dgbz; abx+dabx; aby+daby; abz+dabz; mx+dmx; my+dmy; mz+dmz]; + +dXddx = jacobian(add_e_state_f([lat; lon; alt; vn; ve; vz; qx; qy; qz; qw; gbx; gby; gbz;... + abx; aby; abz; mx; my; mz; dlat, dlon, dalt, dvn, dve, dvz, dtx, dty, dtz, dgbx, dgby, dgbz, dabx, daby, dabz, dmx, dmy; dmz]), [dx]) +% Helper Matrices +I3 = eye(3); +I4 = eye(4); + +syms func(x, dx) +%func = [x(1)+2*dx(1)+3*dx(2); x(2)-dx(3); x(3)*dx(1); x(4)-dx(1)+dx(2)]; + +function add_e_state_f = add_e_state(x, dx) + p_prod = right_product_matrix(x(7:10)); + dq = small_error_to_error_quaternion(dx(7:9)); + q_fixed = p_prod * dq; + dx_dim = [dx(1:6); 0; 0; 0; 0; dx(10:)]; + x_fixed = x + dx_dim; + x_fixed(7:10) = q_fixed; + add_e_state_f = x_fixed; +end + + diff --git a/copter/ekf_matlab/transition_jacobian.txt b/copter/ekf_matlab/transition_jacobian.txt new file mode 100644 index 0000000..a1b374e --- /dev/null +++ b/copter/ekf_matlab/transition_jacobian.txt @@ -0,0 +1,46 @@ + 0, 0, 0, 0, 0, 0, 1] + +ekf_derivation +Jacobian of the state with respect to the error state (X_dx): +Linearization of the state transition function (A): +[ 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 1, 0, 0, -dt*(2*qy*(aby - ay) + 2*qz*(abz - az)), -dt*(2*qx*(aby - ay) - 4*qy*(abx - ax) + 2*qw*(abz - az)), dt*(2*qw*(aby - ay) + 4*qz*(abx - ax) - 2*qx*(abz - az)), dt*(2*qz*(aby - ay) - 2*qy*(abz - az)), 0, 0, 0, dt*(2*qy^2 + 2*qz^2 - 1), dt*(2*qw*qz - 2*qx*qy), -dt*(2*qw*qy + 2*qx*qz), 0, 0, 0] +[ 0, 0, 0, 0, 1, 0, dt*(4*qx*(aby - ay) - 2*qy*(abx - ax) + 2*qw*(abz - az)), -dt*(2*qx*(abx - ax) + 2*qz*(abz - az)), -dt*(2*qw*(abx - ax) - 4*qz*(aby - ay) + 2*qy*(abz - az)), -dt*(2*qz*(abx - ax) - 2*qx*(abz - az)), 0, 0, 0, -dt*(2*qw*qz + 2*qx*qy), dt*(2*qx^2 + 2*qz^2 - 1), dt*(2*qw*qx - 2*qy*qz), 0, 0, 0] +[ 0, 0, 0, 0, 0, 1, -dt*(2*qw*(aby - ay) + 2*qz*(abx - ax) - 4*qx*(abz - az)), dt*(2*qw*(abx - ax) - 2*qz*(aby - ay) + 4*qy*(abz - az)), -dt*(2*qx*(abx - ax) + 2*qy*(aby - ay)), dt*(2*qy*(abx - ax) - 2*qx*(aby - ay)), 0, 0, 0, dt*(2*qw*qy - 2*qx*qz), -dt*(2*qw*qx + 2*qy*qz), dt*(2*qx^2 + 2*qy^2 - 1), 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, qz*((dt^2*gzp)/24 - ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gx - 2*gbx + 2*gxp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) + qy*((dt^2*gyp)/24 + ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qx*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) + qw*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qw*((dt^2*gzp)/24 + ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qy*((dt^2*gxp)/24 - ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gy - 2*gby + 2*gyp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qx*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qz*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qy*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qz*((dt^2*gxp)/24 + ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qx*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qw*((dt^2*gyp)/24 - ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gz - 2*gbz + 2*gzp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, qz*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qx*((dt^2*gyp)/24 + ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qy*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qw*((dt^2*gzp)/24 - ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gx - 2*gbx + 2*gxp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qz*((dt^2*gzp)/24 + ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) + qx*((dt^2*gxp)/24 - ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gy - 2*gby + 2*gyp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qy*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) + qw*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qw*((dt^2*gxp)/24 + ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qz*((dt^2*gyp)/24 - ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gz - 2*gbz + 2*gzp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qy*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qx*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, qw*((dt^2*gyp)/24 + ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qx*((dt^2*gzp)/24 - ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gx - 2*gbx + 2*gxp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qz*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qy*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qx*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qw*((dt^2*gxp)/24 - ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gy - 2*gby + 2*gyp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qz*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qy*((dt^2*gzp)/24 + ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qy*((dt^2*gyp)/24 - ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gz - 2*gbz + 2*gzp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) + qx*((dt^2*gxp)/24 + ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qz*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) + qw*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, qy*((dt^2*gzp)/24 - ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gx - 2*gbx + 2*gxp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qz*((dt^2*gyp)/24 + ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qw*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qx*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gx - 2*gbx + 2*gxp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gx - 2*gbx + 2*gxp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qz*((dt^2*gxp)/24 - ((gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gy - 2*gby + 2*gyp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qx*((dt^2*gzp)/24 + ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qw*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qy*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gy - 2*gby + 2*gyp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gy - 2*gby + 2*gyp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), qx*((dt^2*gyp)/24 - ((gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + ((2*gz - 2*gbz + 2*gzp)*(gbx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gx*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gx*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gxp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gxp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qy*((dt^2*gxp)/24 + ((gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gby*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gby*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gy*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gy*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gyp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gyp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))) - qw*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(8*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2))) - qz*((exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - (gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - (gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) + (gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)*(2*gz - 2*gbz + 2*gzp))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)))/(2*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)) - ((2*gz - 2*gbz + 2*gzp)*(gbz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gbz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gz*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gz*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) - gzp*exp(-(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2) + gzp*exp((- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(1/2)/2)))/(4*(- gbx^2 + 2*gbx*gx + 2*gbx*gxp - gby^2 + 2*gby*gy + 2*gby*gyp - gbz^2 + 2*gbz*gz + 2*gbz*gzp - gx^2 - 2*gx*gxp - gxp^2 - gy^2 - 2*gy*gyp - gyp^2 - gz^2 - 2*gz*gzp - gzp^2)^(3/2))), 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1] + +ekf_derivation +Jacobian of the state with respect to the error state (X_dx): +Linearization of the state transition function (A): +[ 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 1, 0, 0, -dt*(2*qy*(aby - ay) + 2*qz*(abz - az)), -dt*(2*qx*(aby - ay) - 4*qy*(abx - ax) + 2*qw*(abz - az)), dt*(2*qw*(aby - ay) + 4*qz*(abx - ax) - 2*qx*(abz - az)), dt*(2*qz*(aby - ay) - 2*qy*(abz - az)), 0, 0, 0, dt*(2*qy^2 + 2*qz^2 - 1), dt*(2*qw*qz - 2*qx*qy), -dt*(2*qw*qy + 2*qx*qz), 0, 0, 0] +[ 0, 0, 0, 0, 1, 0, dt*(4*qx*(aby - ay) - 2*qy*(abx - ax) + 2*qw*(abz - az)), -dt*(2*qx*(abx - ax) + 2*qz*(abz - az)), -dt*(2*qw*(abx - ax) - 4*qz*(aby - ay) + 2*qy*(abz - az)), -dt*(2*qz*(abx - ax) - 2*qx*(abz - az)), 0, 0, 0, -dt*(2*qw*qz + 2*qx*qy), dt*(2*qx^2 + 2*qz^2 - 1), dt*(2*qw*qx - 2*qy*qz), 0, 0, 0] +[ 0, 0, 0, 0, 0, 1, -dt*(2*qw*(aby - ay) + 2*qz*(abx - ax) - 4*qx*(abz - az)), dt*(2*qw*(abx - ax) - 2*qz*(aby - ay) + 4*qy*(abz - az)), -dt*(2*qx*(abx - ax) + 2*qy*(aby - ay)), dt*(2*qy*(abx - ax) - 2*qx*(aby - ay)), 0, 0, 0, dt*(2*qw*qy - 2*qx*qz), -dt*(2*qw*qx + 2*qy*qz), dt*(2*qx^2 + 2*qy^2 - 1), 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0] +[ 0, 0, 0, 0, 0, 0, 0, 0, 0, \ No newline at end of file diff --git a/copter/get_dependencies.sh b/copter/get_dependencies.sh deleted file mode 100644 index 1a24852..0000000 --- a/copter/get_dependencies.sh +++ /dev/null @@ -1 +0,0 @@ -#!/bin/sh diff --git a/copter/src/flight/altitude.rs b/copter/src/flight/altitude.rs new file mode 100644 index 0000000..1d4c140 --- /dev/null +++ b/copter/src/flight/altitude.rs @@ -0,0 +1,46 @@ +use configurations::Config; + +const MAX_DESCEND_RATE: f32 = -0.03; +const MAX_ASCEND_RATE: f32 = 0.25; + +pub struct Altitude { + alt_kp: f32, + alt_ki: f32, + alt_kd: f32, + last_altitude: f32, + integral: f32, + pub last_mid_level: f32 +} + +impl Altitude { + pub fn new(altitude: f32) -> Altitude { + let config = Config::new(); + + Altitude { + alt_kp: config.alt_kp, + alt_ki: config.alt_ki, + alt_kd: config.alt_kd, + last_altitude: altitude, + integral: 0.0, + last_mid_level: 0.0 + } + } + + pub fn get_mid_level(&mut self, current_altitude: f32, desired_altitude: f32, climb: Option, dt: f32) -> f32 { + let mut proportional = desired_altitude - current_altitude; + proportional = proportional * proportional * proportional; + self.integral += proportional * dt; + let derivative = (self.last_altitude - current_altitude) / dt; + + let error = proportional * self.alt_kp + self.integral * self.alt_ki + derivative * self.alt_kd; + + if error < MAX_DESCEND_RATE { + self.last_mid_level += MAX_DESCEND_RATE; + self.integral -= proportional * dt; + } else { + self.last_mid_level += error; + } + + self.last_mid_level + } +} diff --git a/copter/src/flight/imu.rs b/copter/src/flight/imu.rs new file mode 100644 index 0000000..ee1298c --- /dev/null +++ b/copter/src/flight/imu.rs @@ -0,0 +1,476 @@ +use hardware::sensors::{MultiSensorData,start_sensors,SensorInput}; + +use configurations::Config; +use std::sync::mpsc::{Sender, Receiver, channel}; +use std::f32::consts::PI; +// x: roll +// y: pitch +// z: yaw +pub type Attitude = MultiSensorData; + +// Position in space relative to the tracking start +// x: North/South +// y: East/West +// z: Altitude +pub type Position = MultiSensorData; + +pub fn magnitude(vector: MultiSensorData) -> f32 { + (vector.x * vector.x + vector.y * vector.y + vector.z * vector.z).sqrt() +} + +const RADIAN_TO_DEGREES: f32 = 180.0 / PI; +const DEGREE_TO_RADIAN: f32 = 1.0 / RADIAN_TO_DEGREES; + +pub struct AttitudeOutput { + current_attitude: Option, + current_angular_rate: Option, + current_altitude: Option +} + +pub struct IMU { + // Input + input_rx: Receiver, + + // Previous data + pub last_angular_rate: MultiSensorData, + pub last_acceleration: MultiSensorData, + pub last_magnetic_reading: MultiSensorData, + pub last_temperature: f32, + pub last_pressure: f32, + + // Previous states + pub last_attitude: Attitude, + pub last_position: Position, + pub last_altitude: f32, + + // Dead reckoning + tracking: bool, + pub relative_location: MultiSensorData, + pub north_reading: MultiSensorData, + + pub start_altitude: f32, + pub sea_level_pressure: f32, + pub yaw_offset: f32 +} + + + +impl IMU { + pub fn new(sensor_stream: Receiver) -> IMU { + let mut imu = IMU { + input_rx: sensor_stream, + last_angular_rate: MultiSensorData::zeros(), + last_acceleration: MultiSensorData::zeros(), + last_magnetic_reading: MultiSensorData::zeros(), + last_temperature: 0.0, + last_pressure: 0.0, + last_attitude: Attitude::zeros(), + last_position: Position::zeros(), + last_altitude: 0.0, + tracking: false, + relative_location: MultiSensorData::zeros(), + start_altitude: 0.0, + sea_level_pressure: 101.325, + north_reading: MultiSensorData::zeros(), + yaw_offset: 0.0 + }; + #[cfg(not(test))] + for i in 0..10 { + imu.read_data(); + imu.compute_intertial_reference(1.0); + } + + imu.north_reading = imu.last_magnetic_reading; + imu + } + + pub fn read_data(&mut self) { + match self.input_rx.recv() { + Ok(input) => { + self.last_angular_rate = input.angular_rate; + self.last_acceleration = input.acceleration; + if input.magnetic_reading.is_some() { + self.last_magnetic_reading = input.magnetic_reading.unwrap(); + } + self.last_pressure = input.pressure; + }, + Err(e) => {} + } + + loop { + match self.input_rx.try_recv() { + Ok(input) => { + self.last_angular_rate = input.angular_rate; + self.last_acceleration = input.acceleration; + if input.magnetic_reading.is_some() { + self.last_magnetic_reading = input.magnetic_reading.unwrap(); + } + self.last_pressure = input.pressure; + }, + Err(e) => { break; } + } + } + } + + pub fn compute_intertial_reference(&mut self, dt: f32) { + self.compute_attitude(dt); + self.compute_altitude(); + self.compute_position(dt); + } + + fn compute_attitude(&mut self, dt: f32) { + let alpha = 0.02; + // Trig calculations + // let s_roll = self.last_attitude.x.sin(); let c_roll = self.last_attitude.x.cos(); + // let s_pitch = self.last_attitude.y.sin(); let c_pitch = self.last_attitude.y.cos(); + + // Estimate + self.last_attitude = self.last_attitude + self.last_angular_rate * dt; + + // Compute true values + let A_x = self.last_acceleration.x; + let A_y = self.last_acceleration.y; + let A_z = self.last_acceleration.z; + + // Rotation about the x axis + let mut roll = A_y.atan2(A_z) * RADIAN_TO_DEGREES; + self.last_attitude.x = roll * alpha + self.last_attitude.x * (1.0 - alpha); + roll = self.last_attitude.x * DEGREE_TO_RADIAN; + + // Rotation about the y axis + let gz_2 = A_y * (roll).sin() + A_z * (roll).cos(); + let mut pitch = A_x.atan2(gz_2) * -1.0 * RADIAN_TO_DEGREES; + self.last_attitude.y = pitch * alpha + self.last_attitude.y * (1.0 - alpha); + pitch = self.last_attitude.y * DEGREE_TO_RADIAN; + + roll *= -1.0; + + let yaw_alpha = 0.5; + let mag_x_comp = self.last_magnetic_reading.x * pitch.cos() + self.last_magnetic_reading.z * pitch.sin(); + let mag_y_comp = self.last_magnetic_reading.x * roll.sin() * pitch.sin() + + self.last_magnetic_reading.y * roll.cos() - + self.last_magnetic_reading.z * roll.sin() * pitch.cos(); + + let yaw = mag_y_comp.atan2(mag_x_comp) * RADIAN_TO_DEGREES - self.yaw_offset; + self.last_attitude.z = yaw * yaw_alpha + self.last_attitude.z * (1.0 - yaw_alpha); + } + + fn compute_altitude(&mut self) { + let pressure = self.last_pressure * 1000.0; + let sea_level_pa = 101.325 * 1000.0; + self.last_altitude = 44330. * (1. - (pressure / sea_level_pa).powf(0.1903)) - self.start_altitude; + } + + fn compute_position(&mut self, dt: f32) { + + // println!("{},{},{}", self.relative_location.x, self.relative_location.y, self.relative_location.z); + } +} + +fn linearalgebraize(vec3: MultiSensorData) -> Vector3 { + Vector3::new(vec3.x, vec3.y, vec3.z) +} + +fn delinearalgebraize(vec3: Vector3) -> MultiSensorData { + MultiSensorData { x: vec3.data[0], y: vec3.data[1], z: vec3.data[2] } +} + +// fn compute_true_yaw(mut M: MultiSensorData, mut start_magnetic_reading: MultiSensorData, attitude: Attitude) -> f32 { +// M = M * DEGREE_TO_RADIAN; +// start_magnetic_reading = start_magnetic_reading * DEGREE_TO_RADIAN; +// let sr = attitude.x.sin(); let cr = attitude.x.cos(); +// let sp = attitude.y.sin(); let cp = attitude.y.cos(); +// +// let roll_pitch_rotation_matrix = Matrix3::new(cp, sr*sp, cr*sp, +// 0.0, cr, -1.0*sr, +// -sp, sr*cp, cr*cp); +// let M_n = linearalgebraize(start_magnetic_reading); +// let M_ = roll_pitch_rotation_matrix * M_n; +// +// let A = (M.y*M_.x - M.x*M_.y); +// let B = (M.y*M_.y + M.x*M_.y); +// let yaw = A.atan2(B) * RADIAN_TO_DEGREES; +// // R_3 * M = M_ +// yaw +// } + +fn world_space_to_drone_space(vector: MultiSensorData, attitude: Attitude) -> MultiSensorData { + let navec3 = linearalgebraize(vector); + // Compute Trig values for efficiency + let sr = attitude.x.sin(); let cr = attitude.x.cos(); + let sp = attitude.y.sin(); let cp = attitude.y.cos(); + let sy = attitude.z.sin(); let cy = attitude.z.cos(); + + let rotation_matrix = Matrix3::new(cp*cy, -1.0*cr*sy + sr*sp*cy, sr*sy + cr*sp*cy, + cp*sy, cr*cy + sr*sp*sy, -1.0*sr *cy + cr*sp*sy, + -1.0*sp, sr*cp, cr*cp); + + delinearalgebraize(rotation_matrix * navec3) +} + +fn drone_space_to_world_space(vector: MultiSensorData, attitude: Attitude) -> MultiSensorData { + let navec3 = linearalgebraize(vector); + + let sr = attitude.x.sin(); let cr = attitude.x.cos(); + let sp = attitude.y.sin(); let cp = attitude.y.cos(); + let sy = attitude.z.sin(); let cy = attitude.z.cos(); + + let rotation_matrix = Matrix3::new( cp*cy, cp*sy, -1.0*sp, + -1.0*cr*sy + sr*sp*cy, cr*cy + sr*sp*sy, sr*cp, + sr*sy + cr*sp*cy, -1.0*sr *cy + cr*sp*sy, cr*cp); + + delinearalgebraize(rotation_matrix * navec3) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn createTestData(gx: f32, gy: f32, gz: f32, ax: f32, ay: f32, az: f32) -> SensorInput { + SensorInput { + angular_rate: MultiSensorData { + x: gx, + y: gy, + z: gz + }, + acceleration: MultiSensorData { + x: ax, + y: ay, + z: az + }, + magnetic_reading: None, + temperature: 0.0, + pressure: 0.0 + } + } + + #[test] + fn testSimpleData () { + let (tx, rx) : (Sender, Receiver) = channel(); + let mut imu = IMU::new(rx); + let data = SensorInput { + angular_rate: MultiSensorData::zeros(), + acceleration: MultiSensorData { + x: 0.0, + y: 0.0, + z: 1.0 + }, + magnetic_reading: None, + temperature: 0.0, + pressure: 0.0 + }; + tx.send(data); + imu.read_data(); + imu.compute_intertial_reference(1.0); + assert_eq!(imu.last_attitude.x, 0.0); + assert_eq!(imu.last_attitude.y, 0.0); + assert_eq!(imu.last_attitude.z, 0.0); + } + + #[test] + fn testFlushAllSensorData() { + let (tx, rx) : (Sender, Receiver) = channel(); + let mut imu = IMU::new(rx); + let data1 = createTestData(1.0, 1.0, 1.0, 1.0, 1.0, 1.0); + let data2 = createTestData(2.0, 2.0, 2.0, 2.0, 2.0, 2.0); + tx.send(data1); + tx.send(data2); + imu.read_data(); + + assert_eq!(imu.last_angular_rate.x, 2.0); + assert_eq!(imu.last_angular_rate.y, 2.0); + assert_eq!(imu.last_angular_rate.z, 2.0); + } + + #[test] + fn testMovingGyroData () { + let (tx, rx) : (Sender, Receiver) = channel(); + let mut imu = IMU::new(rx); + + let alpha = 0.02; + let first_tilt = 20.0; + let data1 = createTestData(first_tilt, first_tilt, first_tilt, 0.0, 0.0, 1.0); + + tx.send(data1); + imu.read_data(); + imu.compute_intertial_reference(1.0); + assert_eq!(imu.last_attitude.x, first_tilt * (1.0 - alpha)); + assert_eq!(imu.last_attitude.y, first_tilt * (1.0 - alpha)); + assert_eq!(imu.last_attitude.z, first_tilt * (1.0 - alpha)); + + // Next data point. Even though we have tilted first_tilt deg, we are still pointing down according to the accelerometer readings. This isn't realistic. Go figure! + let data2 = createTestData(0.5, 0.5, 0.5, 0.0, 0.0, 1.0); + tx.send(data2); + imu.read_data(); + imu.compute_intertial_reference(0.5); + let new_angle = (first_tilt * (1.0 - alpha) + (0.5 * 0.5)) * (1.0 - alpha); + + assert_eq!(imu.last_attitude.x, new_angle); + assert_eq!(imu.last_attitude.y, new_angle); + // TODO: Test this value. + // assert_eq!(imu.last_attitude.z, -2.3946); + } + + #[test] + fn testMovingAccelerometerData () { + let (tx, rx) : (Sender, Receiver) = channel(); + let mut imu = IMU::new(rx); + + let alpha = 0.02; + let tilt = 0.0; + let data1 = createTestData(tilt, tilt, tilt, 0.0, 0.0, 1.0); + + tx.send(data1); + imu.read_data(); + imu.compute_intertial_reference(1.0); + assert_eq!(imu.last_attitude.x, 0.0); + assert_eq!(imu.last_attitude.y, 0.0); + assert_eq!(imu.last_attitude.z, 0.0); + + // All of a suden, we are flipped 90 degrees! + let data2 = createTestData(tilt, tilt, tilt, -1.0, 0.0, 0.0); + tx.send(data2); + imu.read_data(); + imu.compute_intertial_reference(0.5); + + assert_eq!(imu.last_attitude.x, 0.0); + assert_eq!(imu.last_attitude.y, 90.0 * alpha); + assert_eq!(imu.last_attitude.z, 0.0); + + imu.compute_intertial_reference(0.5); + + assert_eq!(imu.last_attitude.x, 0.0); + assert_eq!(imu.last_attitude.y, (90.0 * alpha) * (1.0 - alpha) + 90.0 * alpha); + assert_eq!(imu.last_attitude.z, 0.0); + + } + + #[test] + fn testYawComputationSimple () { + let original_magnetic_reading = MultiSensorData { + x: 1.0, + y: 0.0, + z: 0.0 + }; + + let new_magnetic_reading = MultiSensorData { + x: 1.0, + y: 0.0, + z: 0.0 + }; + + let current_rotation = Attitude { + x: 0.0, + y: 0.0, + z: 0.0 // unused + }; + + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading, original_magnetic_reading), 0.0); + } + + #[test] + fn testYawComputationRotation () { + let original_magnetic_reading = MultiSensorData { + x: 1.0, + y: 0.0, + z: 0.0 + }; + + let current_rotation = Attitude { + x: 0.0, + y: 0.0, + z: 0.0 // unused + }; + + let new_magnetic_reading1= MultiSensorData { + x: 0.0, + y: 1.0, + z: 0.0 + }; + + let new_magnetic_reading2 = MultiSensorData { + x: 0.0, + y: -1.0, + z: 0.0 + }; + + let new_magnetic_reading3 = MultiSensorData { + x: -1.0, + y: 0.0, + z: 0.0 + }; + + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading1, original_magnetic_reading), 90.0); + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading2, original_magnetic_reading), -90.0); + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading3, original_magnetic_reading).abs(), 180.0); + } + + #[test] + fn testYawComputationRotationWithZ () { + let original_magnetic_reading = MultiSensorData { + x: 1.0, + y: 0.0, + z: 0.1 + }; + + let current_rotation = Attitude { + x: 0.0, + y: 0.0, + z: 0.0 // unused + }; + + let new_magnetic_reading1= MultiSensorData { + x: 0.0, + y: 1.0, + z: 0.1 + }; + + let new_magnetic_reading2 = MultiSensorData { + x: 0.0, + y: -1.0, + z: 0.1 + }; + + let new_magnetic_reading3 = MultiSensorData { + x: -1.0, + y: 0.0, + z: 0.1 + }; + + let new_magnetic_reading4 = MultiSensorData { + x: 1.0, + y: 1.0, + z: 0.1 + }; + + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading1, original_magnetic_reading), 90.0); + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading2, original_magnetic_reading), -90.0); + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading3, original_magnetic_reading), 180.0); + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading4, original_magnetic_reading), 45.0); + } + + #[test] + fn testYawComputationRotation2 () { + let original_magnetic_reading = MultiSensorData { + x: 1.0, + y: 0.0, + z: 0.1 + }; + + let current_rotation = Attitude { + x: 45.0, + y: 0.0, + z: 0.0 // unused + }; + + let new_magnetic_reading1 = MultiSensorData { + x: 0.0, + y: 1.0, + z: 0.1 + }; + + assert_eq!(compute_yaw(current_rotation, new_magnetic_reading1, original_magnetic_reading), 90.0); + } + + // TODO: Ideally, have an integration test that uses real world values. +} diff --git a/copter/src/flight/kalman.rs b/copter/src/flight/kalman.rs index 8e0a72a..76f48af 100644 --- a/copter/src/flight/kalman.rs +++ b/copter/src/flight/kalman.rs @@ -15,21 +15,21 @@ use hardware::GPSData; use logger::ModuleLogger; -const G_TO_MPSPS: f64 = 9.80665; +const G_TO_MPSPS: f32 = 9.80665; -type TransitionJacobian = MatrixN; -type CovarianceMatrix = MatrixN; -type StateJacobian = MatrixMN; +type TransitionJacobian = MatrixN; +type CovarianceMatrix = MatrixN; +type StateJacobian = MatrixMN; // Keep track of Location: (lat, lon, altitude), Velocity: (track, climb), Attitude(w, i, j, k), gyro bias, accel bias, magnetic field #[derive(Debug)] pub struct State { - pub position: Vector3, - pub velocity: Vector3, - pub attitude: UnitQuaternion, - pub gyro_bias: Vector3, - pub acc_bias: Vector3, - pub magnetic_field: Vector3, + pub position: Vector3, + pub velocity: Vector3, + pub attitude: UnitQuaternion, + pub gyro_bias: Vector3, + pub acc_bias: Vector3, + pub magnetic_field: Vector3, } impl Default for State { @@ -54,8 +54,8 @@ pub struct KalmanFilter { state_jacobian: StateJacobian, F: TransitionJacobian, Q: CovarianceMatrix, - H_field: MatrixMN, - thrust: f64, + H_field: MatrixMN, + thrust: f32, } impl KalmanFilter { @@ -73,14 +73,14 @@ impl KalmanFilter { bottom_right.fill_with_identity(); } - let mut F_i: MatrixMN = MatrixMN::zero(); + let mut F_i: MatrixMN = MatrixMN::zero(); F_i.fixed_slice_mut::(3, 0).fill_with_identity(); - let acc_noise = (0.5 as f64); - let gyro_noise = (0.1 as f64).to_radians(); - let acc_bias_walk = (0.0 as f64); - let gyro_bias_walk = (0.0 as f64); - let mut Q_i: MatrixN = MatrixN::zero(); + let acc_noise = (0.5 as f32); + let gyro_noise = (0.1 as f32).to_radians(); + let acc_bias_walk = (0.0 as f32); + let gyro_bias_walk = (0.0 as f32); + let mut Q_i: MatrixN = MatrixN::zero(); { let mut acc_noise_mat = Q_i.fixed_slice_mut::(0, 0); acc_noise_mat.fill_with_identity(); @@ -120,7 +120,7 @@ impl KalmanFilter { } fn update_state_jacobian(&mut self) { - let q: &Vector4 = &self.x.attitude.coords; + let q: &Vector4 = &self.x.attitude.coords; let mut q_jacob = self.state_jacobian.fixed_slice_mut::(6, 6); @@ -133,7 +133,7 @@ impl KalmanFilter { q_jacob.copy_from(&update); } - fn omega_matrix(w: &Vector3) -> Matrix4 { + fn omega_matrix(w: &Vector3) -> Matrix4 { #[rustfmt_skip] Matrix4::new(0.0, w.z, -w.y, w.x, -w.z, 0.0, w.x, w.y, @@ -141,18 +141,18 @@ impl KalmanFilter { -w.x, -w.y, -w.z, 0.0) } - fn sqew_matrix(a: &Vector3) -> Matrix3 { + fn sqew_matrix(a: &Vector3) -> Matrix3 { #[rustfmt_skip] Matrix3::new(0.0, -a.z, a.y, a.z, 0.0, -a.x, -a.y, a.x, 0.0) } - fn quaternion_rate(w: Vector3) -> UnitQuaternion { + fn quaternion_rate(w: Vector3) -> UnitQuaternion { UnitQuaternion::from_axis_angle(&Unit::new_normalize(w), w.norm()) } - pub fn predict(&mut self, dt: f64) { + pub fn predict(&mut self, dt: f32) { // let current_state = self.get_state(); let mut u = self.prediction_rx.recv().unwrap(); u.angular_rate -= self.x.gyro_bias; @@ -170,14 +170,14 @@ impl KalmanFilter { // 3 axis angular rate integration let taylor_order: i32 = 3; - let mut Theta: Matrix4 = Matrix4::identity(); + let mut Theta: Matrix4 = Matrix4::identity(); let mut fac = 1.0; let mut omega = omega_mean; for i in 1..taylor_order { Theta = Theta + (1.0 / fac) * omega; omega *= omega_mean; - fac *= i as f64; + fac *= i as f32; } Theta += (1.0 / 48.0 * dt * dt) * (omega_next * omega_prev - omega_prev * omega_next); let attitude_p = @@ -223,7 +223,7 @@ impl KalmanFilter { }; } - fn add_error_state(&mut self, error_state: VectorN) { + fn add_error_state(&mut self, error_state: VectorN) { let dp = Vector3::new( error_state.data[0], error_state.data[1], @@ -272,11 +272,11 @@ impl KalmanFilter { // self.P = G * self.P * G.transpose(); } - fn correct_field_reading(&mut self, field: Vector3, measurement: Vector3) { + fn correct_field_reading(&mut self, field: Vector3, measurement: Vector3) { let predicted_measurement = self.x.attitude.transform_vector(&field); let z = measurement - predicted_measurement; - let acc_noise = (2.0 as f64); + let acc_noise = (2.0 as f32); let rot = self.x.attitude.to_rotation_matrix().unwrap(); let R = Matrix3::identity() * acc_noise; let V = rot * R * rot.transpose(); @@ -291,7 +291,7 @@ impl KalmanFilter { let qz = self.x.attitude.coords.data[2]; let qw = self.x.attitude.coords.data[3]; #[rustfmt_skip] - let H_x: Matrix3x4 = + let H_x: Matrix3x4 = Matrix3x4::new( 2.0*(fy*qy+qz*fz), 2.0*(fy*qx-2.0*fx*qy+qw*fz), 2.0*(qx*fz-2.0*fx*qz-fy*qw), 2.0*(qy*fz-fy*qz), 2.0*(fx*qy-2.0*fy*qx-qw*fz), 2.0*(fx*qx+qz*fz), 2.0*(fx*qw-2.0*fy*qz+qy*fz), 2.0*(fx*qz-qx*fz), 2.0*(fy*qw+fx*qz-2.0*qx*fz), 2.0*(fy*qz-fx*qw-2.0*qy*fz), 2.0*(fx*qx+fy*qy), 2.0*(fy*qx-fx*qy)); @@ -311,30 +311,18 @@ impl KalmanFilter { } None => {} } - println!("P: {:?}", self.P); } - fn update_accelerometer(&mut self, acceleration: Vector3, dt: f64) { + fn update_accelerometer(&mut self, acceleration: Vector3, dt: f32) { let gravity = Vector3::new(0.0, 0.0, G_TO_MPSPS); self.correct_field_reading(gravity, acceleration); } - fn update_magnetometer(&mut self, magnetic_reading: Vector3) {} + fn update_magnetometer(&mut self, magnetic_reading: Vector3) {} fn update_gps(&mut self, gps_measurement: GPSData) { - let mut ned_measurement = Vector3::new( - gps_measurement.latitude, - gps_measurement.longitude, - self.x.position.data[2] as f64, - ); - match gps_measurement.altitude { - Some(alt) => { - ned_measurement.data[2] = alt; - } - None => {} - }; - - let z = ned_measurement - self.x.position; + let lat = gps_measurement.latitude; + let lon = gps_measurement.longitude; } // fn update_magnetometer(&mut self, magnetic_reading: Vector3, dt: f32) { @@ -354,10 +342,10 @@ impl KalmanFilter { // fn update_gps(&mut self, dt: f32) {} - pub fn update(&mut self, dt: f64) { + pub fn update(&mut self, dt: f32) { let update = self.update_rx.recv().unwrap(); - self.update_accelerometer(update.acceleration, dt); +// self.update_accelerometer(update.acceleration, dt); // if update.magnetic_reading.is_some() { // self.update_magnetometer(update.magnetic_reading.unwrap(), dt); // } diff --git a/copter/src/flight/mod.rs b/copter/src/flight/mod.rs index eaa50e3..6afdd94 100644 --- a/copter/src/flight/mod.rs +++ b/copter/src/flight/mod.rs @@ -1,5 +1,7 @@ +// mod altitude; // mod pid; -mod navigation; +// mod imu; +// mod navigation; mod kalman; // use self::altitude::Altitude; @@ -26,7 +28,7 @@ use std::fmt; use std::net::{Ipv4Addr, SocketAddrV4, UdpSocket}; use std::io::prelude::*; -const MICROSECONDS_PER_SECOND: f64 = 1000000.0; +const MICROSECONDS_PER_SECOND: f32 = 1000000.0; #[derive(Clone, Copy)] pub enum FlightMode { @@ -78,13 +80,11 @@ fn control_loop( 'control: loop { let current_time = PreciseTime::now(); let diff = prev_time.to(current_time); - let dt = (diff.num_microseconds().unwrap() as f64) / MICROSECONDS_PER_SECOND; + let dt = (diff.num_microseconds().unwrap() as f32) / MICROSECONDS_PER_SECOND; kalman_filter.predict(dt); kalman_filter.update(dt); prev_time = current_time; - println!("{:?}", kalman_filter.x); - motor_tx.send(MotorCommand::SetPower(0.0, 0.0, 0.0, 0.0)); let att_vec = kalman_filter.x.attitude.coords; @@ -104,10 +104,10 @@ fn control_loop( // let config = Config::new(); // let start_power = config.hover_power; // let take_off_max = config.max_motor_speed; -// let mut mid_level: f64 = 1000.0; +// let mut mid_level: f32 = 1000.0; // let mut start_altitude = 0.0; // let logging = config.logging; -// let loop_time = 1000000.0 / config.sensor_sample_frequency as f64; +// let loop_time = 1000000.0 / config.sensor_sample_frequency as f32; // let power_diff = take_off_max - start_power; // let take_off_time = config.take_off_time * 1000000.0; // let loops_to_run = take_off_time / loop_time; @@ -120,7 +120,7 @@ fn control_loop( // control_loop( // &mode_rx, // logging, -// take_off_max as f64, +// take_off_max as f32, // start_power, // ramp_amount, // ); @@ -132,9 +132,9 @@ fn control_loop( // fn control_loop( // mode_rx: &Receiver, // logging: bool, -// take_off_max: f64, -// take_off_start: f64, -// ramp_amount: f64, +// take_off_max: f32, +// take_off_start: f32, +// ramp_amount: f32, // ) { // let mut flight_mode = FlightMode::Off; @@ -157,7 +157,7 @@ fn control_loop( // 'control: loop { // let current_time = PreciseTime::now(); -// let dt = (last_time.to(current_time).num_microseconds().unwrap() as f64) / 1000000.0; +// let dt = (last_time.to(current_time).num_microseconds().unwrap() as f32) / 1000000.0; // check_flight_mode( // &mode_rx, @@ -234,9 +234,9 @@ fn control_loop( // motor_manager: &mut MotorManager, // pid_controller: &mut PID, // altitude_holder: &mut Altitude, -// take_off_max: f64, -// ramp_amount: f64, -// dt: f64, +// take_off_max: f32, +// ramp_amount: f32, +// dt: f32, // ) { // let mid_level = altitude_holder.get_mid_level(imu.last_altitude, 1.5, None, dt); @@ -255,7 +255,7 @@ fn control_loop( // motor_manager: &mut MotorManager, // pid_controller: &mut PID, // altitude_holder: &mut Altitude, -// dt: f64, +// dt: f32, // ) { // let mid_level = altitude_holder.get_mid_level(imu.last_altitude, -1.0, None, dt); @@ -311,7 +311,7 @@ fn control_loop( // } // } -// fn send_log_data(logger: &Sender, imu: &IMU, motor_manager: &MotorManager, time: f64) { +// fn send_log_data(logger: &Sender, imu: &IMU, motor_manager: &MotorManager, time: f32) { // let data = DebugInfo { // t: time, // m1: motor_manager.last_m1, diff --git a/copter/src/flight/navigation.rs b/copter/src/flight/navigation.rs index d28ac5d..915a23e 100644 --- a/copter/src/flight/navigation.rs +++ b/copter/src/flight/navigation.rs @@ -1,120 +1,59 @@ -use std::vec::Vec; -use std::sync::mpsc::{Receiver, Sender}; +use std::f32::consts::PI; +use hardware::gps::{GPSData,get_gps}; +use std::sync::mpsc::Receiver; + +#[derive(Clone,Copy)] +pub struct Destination { + lat: Option, + lon: Option, + altitude: Option, + relative_x: Option, + relative_y: Option +} -use MotorCommand; -use super::kalman::State; +const EARTH_RADIUS: f32 = 6371000.0; // Meters +const DEGREES_TO_RADIANS: f32 = PI / 180.0; +const RADIANS_TO_DEGREES: f32 = 180.0 / PI; -use na::geometry::UnitQuaternion; +//Distance +pub fn lat_lon_distance(curr_lat: f32, curr_lon: f32, dest_lat: f32, dest_lon: f32) -> f32 { + // Haversine formula + let phi1 = curr_lat * DEGREES_TO_RADIANS; + let phi2 = dest_lat * DEGREES_TO_RADIANS; + let dphi = (dest_lat - curr_lat) * DEGREES_TO_RADIANS; + let dlambda = (dest_lon - curr_lon) * DEGREES_TO_RADIANS; -type Path = Vec; + let a = (dphi / 2.0).sin() * (dphi / 2.0).sin() + + phi1.cos() * phi2.cos() * + (dlambda / 2.0).sin() * (dlambda / 2.0).sin(); -pub struct NavigationInstructions { - desired_attitude: UnitQuaternion, - desired_thrust: f64, -} + let c = 2.0 * (a.sqrt().atan2((1.0 - a).sqrt())); -pub enum FlightState { - Off, - Powered, - Takeoff, - Landing, - Navigating, + EARTH_RADIUS * c } -pub struct Location { - lat: f64, - lon: f64, - alt: f64, +//Initial bearing +pub fn lat_lon_bearing(mut curr_lat: f32, mut curr_lon: f32, mut dest_lat: f32, mut dest_lon: f32) -> f32 { + curr_lat *= DEGREES_TO_RADIANS; curr_lon *= DEGREES_TO_RADIANS; dest_lat *= DEGREES_TO_RADIANS; dest_lon *= DEGREES_TO_RADIANS; + let y = (dest_lon - curr_lon).sin() * dest_lat.cos(); + let x = curr_lat.cos() * dest_lat.sin() - + curr_lat.sin() * dest_lat.cos() * (dest_lon - curr_lon).cos(); + + y.atan2(x) * RADIANS_TO_DEGREES } pub struct Navigator { - state: FlightState, - path: Path, - motor_command: Sender, - path_receiver: Receiver, + gps: Receiver } impl Navigator { - fn new(motor_command: Sender, path_receiver: Receiver) -> Navigator { + pub fn new() -> Navigator { Navigator { - state: FlightState::Off, - path: Path::new(), - motor_command: motor_command, - path_receiver: path_receiver, - } - } - - pub fn update(&mut self, state: &State) -> NavigationInstructions { - match self.path_receiver.try_recv() { - Ok(new_path) => { - self.path = new_path; - match self.state { - FlightState::Off => { - self.set_power(); - } - FlightState::Powered => { - self.state = FlightState::Takeoff; - } - FlightState::Landing => { - self.state = FlightState::Takeoff; - } - _ => {} - }; - } - Err(_) => {} - }; - - match self.state { - FlightState::Off => self.handle_off(state), - FlightState::Powered => self.handle_powered(state), - FlightState::Takeoff => self.handle_takeoff(state), - FlightState::Landing => self.handle_landing(state), - FlightState::Navigating => self.handle_navigation(state), - } - } - - fn handle_off(&mut self, state: &State) -> NavigationInstructions { - NavigationInstructions { - desired_attitude: UnitQuaternion::identity(), - desired_thrust: 0.0, - } - } - fn handle_powered(&mut self, state: &State) -> NavigationInstructions { - NavigationInstructions { - desired_attitude: state.attitude, - desired_thrust: 1000.0, } } - fn handle_takeoff(&mut self, state: &State) -> NavigationInstructions { - NavigationInstructions { - desired_attitude: UnitQuaternion::identity(), - desired_thrust: 0.0, - } - } - - fn handle_landing(&mut self, state: &State) -> NavigationInstructions { - NavigationInstructions { - desired_attitude: UnitQuaternion::identity(), - desired_thrust: 0.0, - } - } - - fn handle_navigation(&mut self, state: &State) -> NavigationInstructions { - NavigationInstructions { - desired_attitude: UnitQuaternion::identity(), - desired_thrust: 0.0, - } - } - - fn set_off(&mut self) { - self.motor_command.send(MotorCommand::PowerDown); - self.state = FlightState::Off; - } - - fn set_power(&mut self) { - self.motor_command.send(MotorCommand::Arm); - self.state = FlightState::Powered; + pub fn get_location(&mut self) { + } } diff --git a/copter/src/flight/pid.rs b/copter/src/flight/pid.rs index 9a748cf..9d3fe4c 100644 --- a/copter/src/flight/pid.rs +++ b/copter/src/flight/pid.rs @@ -8,13 +8,23 @@ use num::traits::Zero; use time::{Duration, PreciseTime}; -const MICROSECONDS_PER_SECOND: f64 = 1000000.0; +const MICROSECONDS_PER_SECOND: f32 = 1000000.0; pub struct PID { - kp: f64, - ki: f64, - kd: f64, - integral: Vector3, + // integral_on: bool, + // roll_kp: f32, + // roll_ki: f32, + // roll_kd: f32, + // pitch_kp: f32, + // pitch_ki: f32, + // pitch_kd: f32, + // yaw_kp: f32, + // yaw_kd: f32, + // integral: MultiSensorData, + kp: f32, + ki: f32, + kd: f32, + integral: Vector3, last_update: PreciseTime, } @@ -31,10 +41,10 @@ impl PID { // Thank you to Jaeyoon Kim for helping formulate this PID // Compute the torque to put on the quadcopter in order to fix the attitude - fn compute_torque(&mut self, error: Vector3, angular_rate: Vector3) -> Vector3 { + fn compute_torque(&mut self, error: Vector3, angular_rate: Vector3) -> Vector3 { let now = PreciseTime::now(); let diff = self.last_update.to(now); - let dt = (diff.num_microseconds() as f64) / MICROSECONDS_PER_SECOND; + let dt = (diff.num_microseconds() as f32) / MICROSECONDS_PER_SECOND; self.integral = self.integral + error * dt let torque = -self.kp * error - self.kd * angular_rate - self.ki * self.integral; @@ -48,10 +58,94 @@ impl PID { // Thank you to Jaeyoon Kim for helping formulate this PID pub fn control( &mut self, - attitude: UnitQuaternion, - angular_rate: UnitQuaternion, - desired_attitude: UnitQuaternion, - ) -> (f64, f64, f64, f64) { + attitude: UnitQuaternion, + angular_rate: UnitQuaternion, + desired_attitude: UnitQuaternion, + ) -> (f32, f32, f32, f32) { let torque = self.compute_torque(attitude.scaled_axis(), angular_rate.scaled_axis()); } + + // pub fn new() -> PID { + // let config = Config::new(); + + // PID { + // integral_on: false, + // roll_kp: config.roll_kp, + // roll_ki: config.roll_ki, + // roll_kd: config.roll_kd, + // pitch_kp: config.pitch_kp, + // pitch_ki: config.pitch_ki, + // pitch_kd: config.pitch_kd, + // yaw_kp: config.yaw_kp, + // yaw_kd: config.yaw_kd, + // integral: MultiSensorData::zeros(), + // } + // } + + // pub fn correct_attitude( + // &mut self, + // dt: f32, + // current_attitude: Attitude, + // current_angular_rate: MultiSensorData, + // desired_attitude: Attitude, + // mid_level: f32, + // ) -> (f32, f32, f32, f32) { + // // println!("derivative: {:?}", current_angular_rate); + + // let proportional = current_attitude - desired_attitude; + // let derivative = current_angular_rate; + // self.integral = self.integral + proportional * dt; + + // let mut error = MultiSensorData::zeros(); + + // let ki = 0.0; + + // // x: Roll, y: Pitch PID + // error.x = proportional.x * self.roll_kp + self.integral.x * self.roll_ki + // + derivative.x * self.roll_kd; + // error.y = proportional.y * self.pitch_kp + self.integral.y * self.pitch_ki + // + derivative.y * self.pitch_kd; + + // let (mut m1, mut m2, mut m3, mut m4) = (mid_level, mid_level, mid_level, mid_level); + // m1 = (0.0 - error.x + error.y) / 2.0; + // m2 = (0.0 - error.x - error.y) / 2.0; + // m3 = (0.0 + error.x - error.y) / 2.0; + // m4 = (0.0 + error.x + error.y) / 2.0; + + // m1 += mid_level; + // m2 += mid_level; + // m3 += mid_level; + // m4 += mid_level; + + // // z: Yaw PID is added afterwards + // // Need to compute error both ways and use minimum error + // let yaw_p_1 = proportional.z; + // let mut yaw_p_2 = 0.0; + // if current_attitude.z < desired_attitude.z { + // yaw_p_2 = yaw_p_1 - 360.0; + // } else if current_attitude.z > desired_attitude.z { + // yaw_p_2 = yaw_p_1 + 360.0; + // } + // let mut yaw_p = 0.0; + // if yaw_p_1.abs() < yaw_p_2.abs() { + // yaw_p = yaw_p_1; + // } else { + // yaw_p = yaw_p_2; + // } + + // println!( + // "Yaw: {:.*}, Yaw error: {:.*}", + // 3, current_attitude.z, 3, yaw_p + // ); + + // error.z = yaw_p * self.yaw_kp + derivative.z * self.yaw_kd; + // // println!("Yaw err: {}", yaw_p); + + // m1 -= error.z; + // m2 += error.z; + // m3 -= error.z; + // m4 += error.z; + + // (m1, m2, m3, m4) + // } } diff --git a/copter/src/hardware/barometer.rs b/copter/src/hardware/barometer.rs index e63e9fb..6cc5620 100644 --- a/copter/src/hardware/barometer.rs +++ b/copter/src/hardware/barometer.rs @@ -36,7 +36,8 @@ impl BarometerThermometer { match config.hardware.barometer.name.as_ref() { "BMP180" => { logger.error("BMP180 not implemented yet."); - return Err(()); + let mock_sensor = Rc::new(RefCell::new(MockSensor::new())); + barometer = Some(mock_sensor.clone()); } "BMP280" => { logger.log("Initializing BMP280 barometer."); diff --git a/copter/src/hardware/imu.rs b/copter/src/hardware/imu.rs index b7bc863..dafb039 100644 --- a/copter/src/hardware/imu.rs +++ b/copter/src/hardware/imu.rs @@ -5,6 +5,8 @@ use std::time::Duration; use std::io; use i2cdev_lsm9ds0::*; +use i2cdev_lsm303dlhc::*; +use i2cdev_l3gd20::*; use i2csensors::{Accelerometer, Gyroscope, Magnetometer}; use i2cdev::linux::{LinuxI2CDevice, LinuxI2CError}; @@ -15,26 +17,26 @@ use na::geometry::UnitQuaternion; use typenum::U200; use num::traits::Zero; -type Matrix200x9 = Matrix>; -type Vector9 = VectorN; -type Vector200 = VectorN; +type Matrix200x9 = Matrix>; +type Vector9 = VectorN; +type Vector200 = VectorN; use configurations::{Calibrations, Config, Ellipsoid, Simple}; use logger::ModuleLogger; -const G_TO_MPSPS: f64 = 9.80665; +const G_TO_MPSPS: f32 = 9.80665; use super::mock::MockSensor; pub struct IMU { gyroscope: Rc>>, - gyroscope_offsets: Vector3, + gyroscope_offsets: Vector3, accelerometer: Rc>>, - accelerometer_offsets: Vector3, + accelerometer_offsets: Vector3, magnetometer: Rc>>, - magnetometer_offsets: Vector3, - magnetometer_rotation: Matrix3, - magnetometer_gains: Vector3, + magnetometer_offsets: Vector3, + magnetometer_rotation: Matrix3, + magnetometer_gains: Vector3, logger: ModuleLogger, // calibrations: Calibrations, } @@ -76,6 +78,21 @@ impl IMU { } } } + "L3GD20H" => { + logger.log("Initializing L3GD20H."); + match get_l3gd20h() { + Ok(device) => { + let l2gd20 = Rc::new(RefCell::new(device)); + gyroscope = Some(l2gd20.clone()); + } + Err(()) => { + logger.error( + "Couldn't initialize L3GD20H gyroscope, accelerometer, magnetometer.", + ); + return Err(()); + } + } + } _ => { logger.error("Unknown gyroscope model. Check your configuration file."); return Err(()); @@ -85,17 +102,35 @@ impl IMU { #[cfg(target_arch = "arm")] match config.hardware.accelerometer.name.as_ref() { "LSM9DS0" => {} + "LSM303DLHC" => { + logger.log("Initializing LSM303DLHC."); + match get_lsm303dlhc() { + Ok(device) => { + let reference = Rc::new(RefCell::new(device)); + accelerometer = Some(reference.clone()); + magnetometer = Some(reference.clone()); + } + Err(()) => { + logger.error( + "Couldn't initialize LSM303DLHC accelerometer, magnetometer.", + ); + return Err(()); + } + } + } _ => { - logger.error("Unknown gyroscope model. Check your configuration file."); + logger.error("Unknown acc model. Check your configuration file."); return Err(()); } + }; #[cfg(target_arch = "arm")] match config.hardware.magnetometer.name.as_ref() { "LSM9DS0" => {} + "LSM303DLHC" => {} _ => { - logger.error("Unknown gyroscope model. Check your configuration file."); + logger.error("Unknown magnometer model. Check your configuration file."); return Err(()); } }; @@ -164,13 +199,13 @@ impl IMU { Ok(imu) } - fn read_gyroscope_raw(&mut self) -> Result, ()> { + fn read_gyroscope_raw(&mut self) -> Result, ()> { match self.gyroscope.borrow_mut().angular_rate_reading() { Ok(angular_rate) => Ok(Vector3::new( // Right hand rule. LSM9DS0 is opposite for roll and yaw... - -angular_rate.x.to_radians() as f64, - angular_rate.y.to_radians() as f64, - -angular_rate.z.to_radians() as f64, + -angular_rate.x.to_radians(), + angular_rate.y.to_radians(), + -angular_rate.z.to_radians(), )), Err(_) => { self.logger.error("Couldn't read gyroscope."); @@ -179,13 +214,11 @@ impl IMU { } } - fn read_accelerometer_raw(&mut self) -> Result, ()> { + fn read_accelerometer_raw(&mut self) -> Result, ()> { match self.accelerometer.borrow_mut().acceleration_reading() { - Ok(acceleration) => Ok(Vector3::new( - acceleration.x as f64, - -acceleration.y as f64, - (acceleration.z as f64) * G_TO_MPSPS, - )), + Ok(acceleration) => { + Ok(Vector3::new(acceleration.x, -acceleration.y, acceleration.z) * G_TO_MPSPS) + } Err(_) => { self.logger.error("Couldn't read accelerometer."); return Err(()); @@ -193,13 +226,9 @@ impl IMU { } } - fn read_magnetometer_raw(&mut self) -> Result, ()> { + fn read_magnetometer_raw(&mut self) -> Result, ()> { match self.magnetometer.borrow_mut().magnetic_reading() { - Ok(magnetic) => Ok(Vector3::new( - magnetic.x as f64, - magnetic.y as f64, - magnetic.z as f64, - )), + Ok(magnetic) => Ok(Vector3::new(magnetic.x, magnetic.y, magnetic.z)), Err(_) => { self.logger.error("Couldn't read magnetometer."); return Err(()); @@ -207,19 +236,19 @@ impl IMU { } } - pub fn read_gyroscope(&mut self) -> Result, ()> { + pub fn read_gyroscope(&mut self) -> Result, ()> { match self.read_gyroscope_raw() { Ok(angular_rate_raw) => Ok(angular_rate_raw - self.gyroscope_offsets), Err(_) => Err(()), } } - pub fn read_accelerometer(&mut self) -> Result, ()> { + pub fn read_accelerometer(&mut self) -> Result, ()> { match self.read_accelerometer_raw() { Ok(acceleration_raw) => Ok(acceleration_raw - self.accelerometer_offsets), Err(_) => Err(()), } } - pub fn read_magnetometer(&mut self) -> Result, ()> { + pub fn read_magnetometer(&mut self) -> Result, ()> { match self.read_magnetometer_raw() { Ok(magnetic_reading_raw) => { let offset_corrected = magnetic_reading_raw - self.magnetometer_offsets; @@ -284,38 +313,38 @@ impl IMU { // Auxillary matrices #[rustfmt_skip] - let A_4: Matrix4 = Matrix4::new(v.data[0], v.data[3], v.data[4], v.data[6], + let A_4: Matrix4 = Matrix4::new(v.data[0], v.data[3], v.data[4], v.data[6], v.data[3], v.data[1], v.data[5], v.data[7], v.data[4], v.data[5], v.data[2], v.data[8], v.data[6], v.data[7], v.data[8], -1.0, ); #[rustfmt_skip] - let A_3: Matrix3 = Matrix3::new(v.data[0],v.data[3],v.data[4], + let A_3: Matrix3 = Matrix3::new(v.data[0],v.data[3],v.data[4], v.data[3],v.data[1],v.data[5], v.data[4],v.data[5],v.data[2], ); - let v_ghi: Vector3 = Vector3::new(v.data[6], v.data[7], v.data[8]); + let v_ghi: Vector3 = Vector3::new(v.data[6], v.data[7], v.data[8]); // Compute offsets let offsets = -1.0 * A_3.try_inverse().unwrap() * v_ghi; // More auxillary matrices #[rustfmt_skip] - let T: Matrix4 = Matrix4::new(1.0,0.0,0.0,0.0, + let T: Matrix4 = Matrix4::new(1.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0, 0.0,0.0,1.0,0.0, offsets.data[0],offsets.data[1],offsets.data[2],1.0); let B_4 = T * A_4 * T.transpose(); let b_44 = -1.0 * B_4.data[15]; - let B_3: Matrix3 = B_4.fixed_resize(0.0) / b_44; + let B_3: Matrix3 = B_4.fixed_resize(0.0) / b_44; // Compute gains and rotation let eigen_decomp = B_3.symmetric_eigen(); - let gains: Vector3 = + let gains: Vector3 = Vector3::from_fn(|r, c| (1.0 / eigen_decomp.eigenvalues.data[r]).sqrt()); - let rotation: Matrix3 = eigen_decomp.eigenvectors.try_inverse().unwrap(); + let rotation: Matrix3 = eigen_decomp.eigenvectors.try_inverse().unwrap(); calibs.magnetometer = Some(Ellipsoid::new(offsets, rotation, gains)); } @@ -411,40 +440,59 @@ fn get_lsm9ds0() -> Result, ()> { } } -// fn get_l3gd20(frequency: u32) -> L3GD20 { -// let mut gyro_settings = L3GD20GyroscopeSettings { -// DR: L3GD20GyroscopeDataRate::Hz190, -// BW: L3GD20GyroscopeBandwidth::BW1, -// power_mode: L3GD20PowerMode::Normal, -// zen: true, -// yen: true, -// xen: true, -// sensitivity: L3GD20GyroscopeFS::dps500, -// continuous_update: true, -// high_pass_filter_enabled: true, -// high_pass_filter_mode: Some(L3GD20GyroscopeHighPassFilterMode::NormalMode), -// high_pass_filter_configuration: Some(L3GD20HighPassFilterCutOffConfig::HPCF_3), -// }; +#[cfg(target_arch = "arm")] +fn get_l3gd20h() -> Result, ()> { + let mut gyro_settings = L3GD20GyroscopeSettings { + DR: L3GD20GyroscopeDataRate::Hz190, + BW: L3GD20GyroscopeBandwidth::BW1, + power_mode: L3GD20PowerMode::Normal, + zen: true, + yen: true, + xen: true, + sensitivity: L3GD20GyroscopeFS::dps500, + continuous_update: true, + high_pass_filter_enabled: true, + high_pass_filter_mode: Some(L3GD20GyroscopeHighPassFilterMode::NormalMode), + high_pass_filter_configuration: Some(L3GD20HighPassFilterCutOffConfig::HPCF_3), + }; -// if frequency <= 190 { -// gyro_settings.DR = L3GD20GyroscopeDataRate::Hz190; -// gyro_settings.BW = L3GD20GyroscopeBandwidth::BW2; -// } else if frequency <= 380 { -// gyro_settings.DR = L3GD20GyroscopeDataRate::Hz380; -// gyro_settings.BW = L3GD20GyroscopeBandwidth::BW3; -// } else { -// gyro_settings.DR = L3GD20GyroscopeDataRate::Hz760; -// gyro_settings.BW = L3GD20GyroscopeBandwidth::BW4; -// } + // Note that this gets the "h" version. + // TODO: Implement get_linux_l3gd20_i2c_device() + let gyro_device = get_linux_l3gd20h_i2c_device().unwrap(); + match L3GD20::new(gyro_device, gyro_settings) { + Ok(l3gd20) => Ok(l3gd20), + Err(e) => { + println!("{}", e); + Err(()) + }, + } +} -// let gyro_device = get_linux_l3gd20_i2c_device().unwrap(); -// match L3GD20::new(gyro_device, gyro_settings) { -// Ok(l3gd20) => l3gd20, -// Err(e) => { -// panic!("Couldn't start l3gd20"); -// } -// } -// } +#[cfg(target_arch = "arm")] +fn get_lsm303dlhc() -> Result, ()> { + let settings = LSM303DLHCSettings { + continuous_update: true, + low_power: false, + accelerometer_data_rate: LSM303DLHCAccelerometerUpdateRate::Hz100, + accelerometer_anti_alias_filter_bandwidth: LSM303DLHCAccelerometerFilterBandwidth::Hz50, + azen: true, + ayen: true, + axen: true, + accelerometer_sensitivity: LSM303DLHCAccelerometerFS::g2, + magnetometer_resolution: LSM303DLHCMagnetometerResolution::High, + magnetometer_data_rate: LSM303DLHCMagnetometerUpdateRate::Hz30, + magnetometer_low_power_mode: false, + magnetometer_mode: LSM303DLHCMagnetometerMode::ContinuousConversion, + magnetometer_sensitivity: LSM303DLHCMagnetometerFS::gauss2_5 + }; + + let (mut acc, mut mag) = get_linux_lsm303d_i2c_device().unwrap(); + + match LSM303DLHC::new(acc, mag, settings) { + Ok(device) => Ok(device), + Err(e) => Err(()), + } +} // fn get_lsm303d(frequency: u32) -> LSM303D { // let mut accel_mag_settings = LSM303DSettings { diff --git a/copter/src/hardware/mock.rs b/copter/src/hardware/mock.rs index 0991744..7153d5e 100644 --- a/copter/src/hardware/mock.rs +++ b/copter/src/hardware/mock.rs @@ -54,7 +54,7 @@ impl Magnetometer for MockSensor { impl MotorManager for MockSensor { fn arm(&mut self) {} fn terminate(&mut self) {} - fn set_powers(&mut self, powers: [f64; 4]) {} + fn set_powers(&mut self, powers: [f32; 4]) {} fn process_command(&mut self, command: MotorCommand) {} fn calibrate(&mut self) {} } diff --git a/copter/src/hardware/mod.rs b/copter/src/hardware/mod.rs index 51f7ec8..b89a5c7 100644 --- a/copter/src/hardware/mod.rs +++ b/copter/src/hardware/mod.rs @@ -30,9 +30,13 @@ use self::motors::{MotorManager, SerialMotorManager}; use self::gps::get_gps; use self::battery::{BatteryMonitor, BatteryStatus}; +// HACKS +use self::mock::MockSensor; + pub use self::motors::MotorCommand; pub use self::gps::GPSData; + const MILLISECONDS_PER_SECOND: i64 = 1000; const LOOP_FREQUENCY: i64 = 75; // 95 Hz Loop @@ -75,13 +79,14 @@ pub fn initialize_hardware() -> ( }; hardware_logger.success("IMU initialized."); - let mut motor_manager = match SerialMotorManager::new() { - Ok(motors) => motors, - Err(_) => { - hardware_logger.error("Motor initialization failed."); - panic!("Motor initialization failed."); - } - }; +// let mut motor_manager = match SerialMotorManager::new() { +// Ok(motors) => motors, +// Err(_) => { +// hardware_logger.error("Motor initialization failed."); +// panic!("Motor initialization failed."); +// } +// }; + let mut motor_manager = MockSensor::new(); hardware_logger.success("Motors initialized."); @@ -151,8 +156,8 @@ pub fn calibrate_motors() { #[derive(Debug)] pub struct PredictionReading { - pub angular_rate: Vector3, - pub acceleration: Vector3, + pub angular_rate: Vector3, + pub acceleration: Vector3, } impl Default for PredictionReading { @@ -166,9 +171,9 @@ impl Default for PredictionReading { #[derive(Debug)] pub struct UpdateReading { - pub acceleration: Vector3, - pub magnetic_reading: Option>, - pub pressure: f64, + pub acceleration: Vector3, + pub magnetic_reading: Option>, + pub pressure: f32, pub gps_information: Option, } @@ -186,7 +191,7 @@ impl Default for UpdateReading { fn hardware_loop( barometer: &mut BarometerThermometer, imu: &mut IMU, - motor_manager: &mut SerialMotorManager, + motor_manager: &mut MotorManager, gps_rx: Receiver, prediction_tx: Sender, update_tx: Sender, @@ -222,7 +227,7 @@ fn hardware_loop( }; let pressure = barometer.read_pressure(); - let mut magnetic_reading: Option> = None; + let mut magnetic_reading: Option> = None; if loop_count % 4 == 0 { magnetic_reading = Some(imu.read_magnetometer().unwrap()); } @@ -230,7 +235,7 @@ fn hardware_loop( let mut update_reading = UpdateReading { acceleration: acceleration, magnetic_reading: magnetic_reading, - pressure: pressure as f64, + pressure: pressure, gps_information: gps_information, }; diff --git a/copter/src/hardware/motors.rs b/copter/src/hardware/motors.rs index 486fc3c..8f7cf33 100644 --- a/copter/src/hardware/motors.rs +++ b/copter/src/hardware/motors.rs @@ -8,20 +8,20 @@ use std::thread::sleep; use std::time::Duration; -const MAX_VALUE: f64 = 2000.0; -const MIN_VALUE: f64 = 1000.0; +const MAX_VALUE: f32 = 2000.0; +const MIN_VALUE: f32 = 1000.0; pub enum MotorCommand { PowerDown, Arm, - SetPower(f64, f64, f64, f64), + SetPower(f32, f32, f32, f32), } pub trait MotorManager { fn arm(&mut self); fn calibrate(&mut self); fn terminate(&mut self); - fn set_powers(&mut self, powers: [f64; 4]); + fn set_powers(&mut self, powers: [f32; 4]); fn process_command(&mut self, command: MotorCommand); } @@ -84,9 +84,9 @@ impl MotorManager for SerialMotorManager { self.logger.success("Motors Off."); } - fn set_powers(&mut self, powers: [f64; 4]) { + fn set_powers(&mut self, powers: [f32; 4]) { for i in 0..self.motors.len() { - match self.device.set_pulse_length(self.motors[i], powers[i]) { + match self.device.set_pulse_length(self.motors[i], powers[i] as f32) { Ok(_) => {} Err(e) => { self.logger.error("Couldn't set motor power."); @@ -140,7 +140,7 @@ impl SerialMotorManager { impl MotorManager for SerialMotorManager { fn arm(&mut self) {} fn terminate(&mut self) {} - fn set_powers(&mut self, powers: [f64; 4]) {} + fn set_powers(&mut self, powers: [f32; 4]) {} fn process_command(&mut self, command: MotorCommand) {} fn calibrate(&mut self) {} } @@ -249,7 +249,7 @@ impl MotorManager for SerialMotorManager { // // self.motors.push(gpio_pin); // // } -// // pub fn set_powers(&mut self, mut m1: f64, mut m2: f64, mut m3: f64, mut m4: f64) { +// // pub fn set_powers(&mut self, mut m1: f32, mut m2: f32, mut m3: f32, mut m4: f32) { // // let max = m1.max(m2).max(m3).max(m4); // // let min = m1.min(m2).min(m3).min(m4); diff --git a/copter/src/main.rs b/copter/src/main.rs index 23a6c6b..91b34b2 100644 --- a/copter/src/main.rs +++ b/copter/src/main.rs @@ -13,6 +13,7 @@ extern crate i2cdev_bmp180; extern crate i2cdev_bmp280; extern crate i2cdev_l3gd20; extern crate i2cdev_lsm303d; +extern crate i2cdev_lsm303dlhc; extern crate i2cdev_lsm9ds0; extern crate i2csensors; extern crate pca9685;