added comments to fastest version, maybe skim them

variants/variant_float_inlining_SoA_AVX512_polyexp_moreavx_ilp_2_rsqrt_morersqrt.cpp

@@ -161,12 +161,18 @@ Pedestrian *init_pedestrians(int n_p) {
 Board init_board_without_obstacles() { return Board(BOARD_LENGTH, BOARD_WIDTH, 0, nullptr); }
 
 static inline __m512 _avx512_exp_ps(__m512 x) {
+  // define own inlined intrinsic function for approximating the exponent function
+  // e^x = 2^n * e^r --> x = n * ln(2) + r
+
   const __m512 log2e = _mm512_set1_ps(1.4426950408889634f);
   __m512 nf = _mm512_roundscale_ps(_mm512_mul_ps(x, log2e), _MM_FROUND_TO_NEAREST_INT | _MM_FROUND_NO_EXC);
 
   const __m512 ln2 = _mm512_set1_ps(0.6931471805599453f);
+  // r = x - n * ln(2)
   __m512 r = _mm512_fnmadd_ps(nf, ln2, x);
 
+  // approximation of e^r using taylor polynomial
+  // cNUM --> coefficients
   const __m512 c5 = _mm512_set1_ps(1.0f / 120.0f);
   const __m512 c4 = _mm512_set1_ps(1.0f / 24.0f);
   const __m512 c3 = _mm512_set1_ps(1.0f / 6.0f);
@@ -174,6 +180,7 @@ static inline __m512 _avx512_exp_ps(__m512 x) {
   const __m512 c1 = _mm512_set1_ps(1.0f);
   const __m512 c0 = _mm512_set1_ps(1.0f);
 
+  // apply coefficients
   __m512 p = c5;
   p = _mm512_fmadd_ps(p, r, c4);
   p = _mm512_fmadd_ps(p, r, c3);
@@ -181,11 +188,13 @@ static inline __m512 _avx512_exp_ps(__m512 x) {
   p = _mm512_fmadd_ps(p, r, c1);
   p = _mm512_fmadd_ps(p, r, c0);
 
+  // use bit shifting for 2^n and bias because float have 23 bit mantissa
   __m512i ni = _mm512_cvtps_epi32(nf);
   __m512i biased = _mm512_add_epi32(ni, _mm512_set1_epi32(127));
   __m512i scaled = _mm512_slli_epi32(biased, 23);
   __m512 pow2n = _mm512_castsi512_ps(scaled);
 
+  // e^x
   return _mm512_mul_ps(p, pow2n);
 }
 
@@ -226,126 +235,195 @@ int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, c
 
   for (int t = 0; t < n_iter; t++) {
     for (int i = 0; i < n_p; i++) {
+      // set the vector for the current pedestrian
+
+      // get desired velocity
       float desired_velocity_x = e_x[i] * desired_v0[i];
       float desired_velocity_y = e_y[i] * desired_v0[i];
 
+      // velocity change accumulator
       float f_alpha_dt_x = (desired_velocity_x - curr_vel_x[i]) * dt_times_tau_alpha_inv;
       float f_alpha_dt_y = (desired_velocity_y - curr_vel_y[i]) * dt_times_tau_alpha_inv;
 
+      // pos
       const __m512 pix = _mm512_set1_ps(pos_x[i]);
       const __m512 piy = _mm512_set1_ps(pos_y[i]);
+      // desired direction
       const __m512 eix = _mm512_set1_ps(e_x[i]);
       const __m512 eiy = _mm512_set1_ps(e_y[i]);
+      // broadcast index used by mask to prevent situation pedestrian[i] == pedestrian[j], see m_neq_i_0 and m_neq_i_1
       const __m512 v_i = _mm512_set1_ps((float)i);
 
+      // accumulators --> 16 pedestrians per accumulator
       __m512 acc_x_0 = _mm512_setzero_ps();
       __m512 acc_y_0 = _mm512_setzero_ps();
       __m512 acc_x_1 = _mm512_setzero_ps();
       __m512 acc_y_1 = _mm512_setzero_ps();
 
+      // force between pedestrians
+      // loop unroll factor 32 with 2 vectors
+      // --> each vector has 16 lanes
       for (int j = 0; j < n_p_pad; j += 32) {
+        
+        // [j, j+1, j+2, ..., j+15] and [j+16, j+17, j+18, ..., j+31]
         __m512 j_idx_0 = _mm512_add_ps(_mm512_set1_ps((float)j), lane_off);
         __m512 j_idx_1 = _mm512_add_ps(_mm512_set1_ps((float)j + 16), lane_off);
 
+        // lane is active if j != i
+        // [1,1,1,0,1,1,1,...] 0 --> i == j, 1 --> i != j
         __mmask16 m_neq_i_0 = _mm512_cmp_ps_mask(j_idx_0, v_i, _CMP_NEQ_OQ);
         __mmask16 m_neq_i_1 = _mm512_cmp_ps_mask(j_idx_1, v_i, _CMP_NEQ_OQ);
         
+        // lane is active if j < n_p --> valid range
+        // [1,1,1,0,1,1,1,...] 0 --> j >= n_p, 1 --> j < n_p
         __mmask16 m_in_range_0 = _mm512_cmp_ps_mask(j_idx_0, v_np, _CMP_LT_OQ);
         __mmask16 m_in_range_1 = _mm512_cmp_ps_mask(j_idx_1, v_np, _CMP_LT_OQ);
 
+        // bitwise comparison if not valid --> 0
+        // finally check if lane is valid or not --> prevents i == j, and
+        // j >= n_p because of loop unrolling --> we can therefore omit a remainings for loop
         __mmask16 active_0 = m_neq_i_0 & m_in_range_0;
         __mmask16 active_1 = m_neq_i_1 & m_in_range_1;
 
+        // ---
+
+        // load the positions of the other pedestrians x value
+        // [pos[j], pos[j+1], ..., pos[j+15]] and [pos[16], pos[j+17], ..., pos[j+31]] for x and y
         __m512 pjx_0 = _mm512_loadu_ps(&pos_x[j]);
         __m512 pjx_1 = _mm512_loadu_ps(&pos_x[j + 16]);
-
         __m512 pjy_0 = _mm512_loadu_ps(&pos_y[j]);
         __m512 pjy_1 = _mm512_loadu_ps(&pos_y[j + 16]);
 
+        // ---
+
+        // step vector of j
+        // [(v_j * delta_t * e_j).x, (v_j+1 * delta_t * e_j+1).x, ...]
         __m512 sbe_jx_0 = _mm512_loadu_ps(&sb_e_x[j]);
         __m512 sbe_jx_1 = _mm512_loadu_ps(&sb_e_x[j + 16]);
-
         __m512 sbe_jy_0 = _mm512_loadu_ps(&sb_e_y[j]);
         __m512 sbe_jy_1 = _mm512_loadu_ps(&sb_e_y[j + 16]);
 
+        // ---
+
+        // squared step length
+        // [j, j+1, ...]
         __m512 sb_jsq_0 = _mm512_loadu_ps(&sb_sq[j]);
         __m512 sb_jsq_1 = _mm512_loadu_ps(&sb_sq[j + 16]);
 
+        // ---
+
+        // distance vector
+        // [j, j+1, ...]
         __m512 r_ab_x_0 = _mm512_sub_ps(pix, pjx_0);
         __m512 r_ab_x_1 = _mm512_sub_ps(pix, pjx_1);
-
         __m512 r_ab_y_0 = _mm512_sub_ps(piy, pjy_0);
         __m512 r_ab_y_1 = _mm512_sub_ps(piy, pjy_1);
 
+        // |r_ab|^2 needed for rsqrt to prevent 0 values --> x^2 + y^2
         __m512 r_ab_n2_0 = _mm512_fmadd_ps(r_ab_x_0, r_ab_x_0, _mm512_mul_ps(r_ab_y_0, r_ab_y_0));
         __m512 r_ab_n2_1 = _mm512_fmadd_ps(r_ab_x_1, r_ab_x_1, _mm512_mul_ps(r_ab_y_1, r_ab_y_1));
 
+        // prevent cases where distance = 0 (=pedestrian on top of each other)
+        // [dist_i_j, dist_i_j+1, ...]
         r_ab_n2_0 = _mm512_max_ps(r_ab_n2_0, v_eps_sq);
         r_ab_n2_1 = _mm512_max_ps(r_ab_n2_1, v_eps_sq);
         
+        // ---
+        
+        // 1 / |r_ab| --> 14 bit rsqrt approx. rsqrt --> 1 / x^2 --> rsqrt(x^2) = 1/x
+        // [1 / dist_i_j, 1 / dist_i_j+1, ...]
         __m512 inv_r_ab_n_0 = _mm512_rsqrt14_ps(r_ab_n2_0);
         __m512 inv_r_ab_n_1 = _mm512_rsqrt14_ps(r_ab_n2_1);
 
+        // r_ab^2 * 1 / r_ab = |r_ab| --> normed
+        // [r_i_j, r_i_j+1, ...]
         __m512 r_ab_n_0 = _mm512_mul_ps(r_ab_n2_0, inv_r_ab_n_0);
         __m512 r_ab_n_1 = _mm512_mul_ps(r_ab_n2_1, inv_r_ab_n_1);
 
+        // r_ab - step[j]
+        // j next pos
         __m512 r_ab2_x_0 = _mm512_sub_ps(r_ab_x_0, sbe_jx_0);
         __m512 r_ab2_x_1 = _mm512_sub_ps(r_ab_x_1, sbe_jx_1);
-
         __m512 r_ab2_y_0 = _mm512_sub_ps(r_ab_y_0, sbe_jy_0);
         __m512 r_ab2_y_1 = _mm512_sub_ps(r_ab_y_1, sbe_jy_1);
 
+        // |r_ab_2| --> needed for rsqrt trick (r_ab - step)
         __m512 r_ab2_n2_0 = _mm512_fmadd_ps(r_ab2_x_0, r_ab2_x_0, _mm512_mul_ps(r_ab2_y_0, r_ab2_y_0));
         __m512 r_ab2_n2_1 = _mm512_fmadd_ps(r_ab2_x_1, r_ab2_x_1, _mm512_mul_ps(r_ab2_y_1, r_ab2_y_1));
 
+        // prevent 0 case --> infinity
         r_ab2_n2_0 = _mm512_max_ps(r_ab2_n2_0, v_eps_sq);
         r_ab2_n2_1 = _mm512_max_ps(r_ab2_n2_1, v_eps_sq);
 
+        // same as before use rsqrt with 14bit approx
+        // [1 / |r_ab_2|, 1 / |r_ab_2|, ...]
         __m512 inv_r_ab2_n_0 = _mm512_rsqrt14_ps(r_ab2_n2_0);
         __m512 inv_r_ab2_n_1 = _mm512_rsqrt14_ps(r_ab2_n2_1);
 
+        // |r_ab_2_second_distance_vector| * 1 / |r_ab_2_first_distance_vector|
         __m512 r_ab2_n_0 = _mm512_mul_ps(r_ab2_n2_0, inv_r_ab2_n_0);
         __m512 r_ab2_n_1 = _mm512_mul_ps(r_ab2_n2_1, inv_r_ab2_n_1);
 
+        // r_sum = |r_ab| + |r_ab - step[j]| --> [w_0, w_1, w_2, ..., w_15] and [w_16, ..., w_31]
         __m512 r_sum_0 = _mm512_add_ps(r_ab_n_0, r_ab2_n_0);
         __m512 r_sum_1 = _mm512_add_ps(r_ab_n_1, r_ab2_n_1);
 
+        // 2_b^2 = r_sum^2 - step[j]^2
         __m512 two_b_sq_0 = _mm512_fmsub_ps(r_sum_0, r_sum_0, sb_jsq_0);
         __m512 two_b_sq_1 = _mm512_fmsub_ps(r_sum_1, r_sum_1, sb_jsq_1);
 
+        // prevent 0 case
         two_b_sq_0 = _mm512_max_ps(two_b_sq_0, v_eps_sq);
         two_b_sq_1 = _mm512_max_ps(two_b_sq_1, v_eps_sq);
 
+        // 2_b
+        // 1 / 2_b^(2*0.5) --> 1 / 2_b
         __m512 inv_two_b_0 = _mm512_rsqrt14_ps(two_b_sq_0);
         __m512 inv_two_b_1 = _mm512_rsqrt14_ps(two_b_sq_1);
 
+        // 2_b = 2_b^2 * 1 / 2_b
         __m512 two_b_0 = _mm512_mul_ps(two_b_sq_0, inv_two_b_0);
         __m512 two_b_1 = _mm512_mul_ps(two_b_sq_1, inv_two_b_1);
 
+        // ---
+
+        // two_b * -1 / (2*sigma) --> exp
         __m512 e_arg_0 = _mm512_mul_ps(two_b_0, v_neg_2s_inv);
         __m512 e_arg_1 = _mm512_mul_ps(two_b_1, v_neg_2s_inv);
 
+        // exp(x)
+        // when x is small just assume constant value --> faster exp
         e_arg_0 = _mm512_max_ps(e_arg_0, v_exp_min);
         e_arg_1 = _mm512_max_ps(e_arg_1, v_exp_min);
 
+        // custom fast exponent intrinsic using polynomial approximations and some precomputed constants
         __m512 e_val_0 = _avx512_exp_ps(e_arg_0);
         __m512 e_val_1 = _avx512_exp_ps(e_arg_1);
 
+        // [−(V0/σ) * e^(−b/σ) * w/(4b), ...] --> scalar
         __m512 numer_inv_two_b_0 = _mm512_mul_ps(_mm512_mul_ps(r_sum_0, v_neg_v0_div_2s), _mm512_mul_ps(e_val_0, inv_two_b_0));
         __m512 numer_inv_two_b_1 = _mm512_mul_ps(_mm512_mul_ps(r_sum_1, v_neg_v0_div_2s), _mm512_mul_ps(e_val_1, inv_two_b_1));
 
+        // scalar * 1 / |r_ab|
         __m512 sm_r_ab_0 = _mm512_mul_ps(numer_inv_two_b_0, inv_r_ab_n_0);
         __m512 sm_r_ab_1 = _mm512_mul_ps(numer_inv_two_b_1, inv_r_ab_n_1);
 
+        // scalar * 1 / |r_ab_2|
         __m512 sm_r_ab2_0 = _mm512_mul_ps(numer_inv_two_b_0, inv_r_ab2_n_0);
         __m512 sm_r_ab2_1 = _mm512_mul_ps(numer_inv_two_b_1, inv_r_ab2_n_1);
 
+        // summed force
         __m512 f_x_0 = _mm512_fmadd_ps(r_ab2_x_0, sm_r_ab2_0, _mm512_mul_ps(r_ab_x_0, sm_r_ab_0));
         __m512 f_x_1 = _mm512_fmadd_ps(r_ab2_x_1, sm_r_ab2_1, _mm512_mul_ps(r_ab_x_1, sm_r_ab_1));
-
         __m512 f_y_0 = _mm512_fmadd_ps(r_ab2_y_0, sm_r_ab2_0, _mm512_mul_ps(r_ab_y_0, sm_r_ab_0));
         __m512 f_y_1 = _mm512_fmadd_ps(r_ab2_y_1, sm_r_ab2_1, _mm512_mul_ps(r_ab_y_1, sm_r_ab_1));
 
+        // using masking to perform --> field of view formula
+        // if (e_dot_f >= 0.0 || e_dot_f_sq <= rhs)
+        //    return dt_neg;
+        // else
+        //    return c_neg_times_dt;
+        // 
         __m512 f_norm_sq_0 = _mm512_fmadd_ps(f_x_0, f_x_0, _mm512_mul_ps(f_y_0, f_y_0));
         __m512 f_norm_sq_1 = _mm512_fmadd_ps(f_x_1, f_x_1, _mm512_mul_ps(f_y_1, f_y_1));
 
@@ -358,34 +436,45 @@ int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, c
         __m512 e_dot_f_sq_0 = _mm512_mul_ps(e_dot_f_0, e_dot_f_0);
         __m512 e_dot_f_sq_1 = _mm512_mul_ps(e_dot_f_1, e_dot_f_1);
 
+        // e_i * f < 0 --> behind
         __mmask16 c1_0 = _mm512_cmp_ps_mask(e_dot_f_0, v_zero, _CMP_LT_OQ);
         __mmask16 c1_1 = _mm512_cmp_ps_mask(e_dot_f_1, v_zero, _CMP_LT_OQ);
 
+        // (e_i * f)^2 > cos(phi) --> out of view
         __mmask16 c2_0 = _mm512_cmp_ps_mask(e_dot_f_sq_0, rhs_0, _CMP_GT_OQ);
         __mmask16 c2_1 = _mm512_cmp_ps_mask(e_dot_f_sq_1, rhs_1, _CMP_GT_OQ);
 
+        // get the case (in field of view or not)
+        // 1 = reduced weight, 0 = regular
         __mmask16 cond_0 = c1_0 & c2_0;
         __mmask16 cond_1 = c1_1 & c2_1;
 
+        // apply the correct weight
         __m512 w_val_0 = _mm512_mask_blend_ps(cond_0, v_dt_neg, v_c_neg_dt);
         __m512 w_val_1 = _mm512_mask_blend_ps(cond_1, v_dt_neg, v_c_neg_dt);
 
+        // take care of i == j case by zeroing it out
+        // see active_0 and active_1 at the beginning of this loop
+        // [f.x or 0, ...]
         __m512 f_x_safe_0 = _mm512_maskz_mov_ps(active_0, f_x_0);
         __m512 f_x_safe_1 = _mm512_maskz_mov_ps(active_1, f_x_1);
-
         __m512 f_y_safe_0 = _mm512_maskz_mov_ps(active_0, f_y_0);
         __m512 f_y_safe_1 = _mm512_maskz_mov_ps(active_1, f_y_1);
 
+        // accumulate force with weight
+        // force_P_total += weight * force_P_other
         acc_x_0 = _mm512_fmadd_ps(f_x_safe_0, w_val_0, acc_x_0);
         acc_x_1 = _mm512_fmadd_ps(f_x_safe_1, w_val_1, acc_x_1);
-
         acc_y_0 = _mm512_fmadd_ps(f_y_safe_0, w_val_0, acc_y_0);
         acc_y_1 = _mm512_fmadd_ps(f_y_safe_1, w_val_1, acc_y_1);
       }
 
+      // scalar force accumulator
       f_alpha_dt_x += _mm512_reduce_add_ps(_mm512_add_ps(acc_x_0, acc_x_1));
       f_alpha_dt_y += _mm512_reduce_add_ps(_mm512_add_ps(acc_y_0, acc_y_1));
 
+      // force between pedestrian and obstacles
+      // no optimizations, because force between pedestrian is the bottleneck of the computation
       for (int k = 0; k < board.n_obstacles; k++) {
         float v1_x = pos_x[i] - board.obstacles[k].start_x;
         float v1_y = pos_y[i] - board.obstacles[k].start_y;
@@ -426,8 +515,10 @@ int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, c
       w_y[i] += f_alpha_dt_y;
     }
 
+    // update each pedestrian
     for (int i = 0; i < n_p_pad; i += 16) {
-
+      // load 16 pedestrians per iteration with their data
+      // SoA in vectors
       __m512 w_x_i_vec = _mm512_loadu_ps(&w_x[i]);
       __m512 w_y_i_vec = _mm512_loadu_ps(&w_y[i]);
       __m512 v_max_i_vec = _mm512_loadu_ps(&v_max[i]);
@@ -450,6 +541,7 @@ int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, c
       pos_x_i_vec = _mm512_fmadd_ps(curr_vel_x_i_vec, dt_vec, pos_x_i_vec);
       pos_y_i_vec = _mm512_fmadd_ps(curr_vel_y_i_vec, dt_vec, pos_y_i_vec);
 
+      // differentiate whether pedestrian came from left or right using masking
       __mmask16 cond_1_left = _mm512_cmpeq_epi32_mask(color_i_vec, _mm512_setzero_si512());
       __mmask16 cond_1_right = _mm512_cmp_ps_mask(pos_x_i_vec, _mm512_set1_ps(BOARD_LENGTH - 1.5f), _CMP_GT_OQ);
       __mmask16 cond_1 = cond_1_left & cond_1_right;
@@ -487,6 +579,7 @@ int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, c
       __m512 sb_sq_i_vec = _mm512_mul_ps(vel_norm_sq, _mm512_set1_ps(delta_t_sq));
       __m512 vel_norm_dt = _mm512_sqrt_ps(sb_sq_i_vec);
 
+      // write the data
       _mm512_storeu_ps(&curr_vel_x[i], curr_vel_x_i_vec);
       _mm512_storeu_ps(&curr_vel_y[i], curr_vel_y_i_vec);
       _mm512_storeu_ps(&w_x[i], w_x_i_vec);