#include #include enum { MAX_CHAINS = 64, PARTICLES_PER_CHAIN = 16, SPHERES_PER_CHAIN = 8, MAX_PARTICLES = (MAX_CHAINS * PARTICLES_PER_CHAIN), MAX_SPHERES = (MAX_CHAINS * SPHERES_PER_CHAIN), CONSTRAINTS_PER_CHAIN = PARTICLES_PER_CHAIN, MAX_REST_LENGTHS = (MAX_CHAINS * CONSTRAINTS_PER_CHAIN), MAX_ITERATIONS = 4, }; struct chain_cfg { float damping; float stiffness; float stretch_factor; float max_strain; float friction; float collision_radius; float linear_inertia; // [0, 1], 1.0f = physically correct linear motion float angular_inertia; // [0, 1], 1.0f = physically correct angular rotation float centrifugal_inertia; // [0, 1], 1.0f = physically correct centrifugal force } __attribute__((aligned(32))); struct chain_sim { unsigned short free_idx_cnt; unsigned short free_idx[MAX_CHAINS]; struct chain_cfg chain_configs[MAX_CHAINS] __attribute__((aligned(32))); // Chain global forces (world space gravity and wind) float gravity_x[MAX_CHAINS] __attribute__((aligned(32))); float gravity_y[MAX_CHAINS] __attribute__((aligned(32))); float gravity_z[MAX_CHAINS] __attribute__((aligned(32))); float wind_x[MAX_CHAINS] __attribute__((aligned(32))); float wind_y[MAX_CHAINS] __attribute__((aligned(32))); float wind_z[MAX_CHAINS] __attribute__((aligned(32))); // Chain transformations (world space) float chain_pos_x[MAX_CHAINS] __attribute__((aligned(32))); float chain_pos_y[MAX_CHAINS] __attribute__((aligned(32))); float chain_pos_z[MAX_CHAINS] __attribute__((aligned(32))); float chain_quat_x[MAX_CHAINS] __attribute__((aligned(32))); float chain_quat_y[MAX_CHAINS] __attribute__((aligned(32))); float chain_quat_z[MAX_CHAINS] __attribute__((aligned(32))); float chain_quat_w[MAX_CHAINS] __attribute__((aligned(32))); float prev_chain_pos_x[MAX_CHAINS] __attribute__((aligned(32))); float prev_chain_pos_y[MAX_CHAINS] __attribute__((aligned(32))); float prev_chain_pos_z[MAX_CHAINS] __attribute__((aligned(32))); float prev_chain_quat_x[MAX_CHAINS] __attribute__((aligned(32))); float prev_chain_quat_y[MAX_CHAINS] __attribute__((aligned(32))); float prev_chain_quat_z[MAX_CHAINS] __attribute__((aligned(32))); float prev_chain_quat_w[MAX_CHAINS] __attribute__((aligned(32))); // Particle positions (model space) float p_pos_x[MAX_PARTICLES] __attribute__((aligned(32))); float p_pos_y[MAX_PARTICLES] __attribute__((aligned(32))); float p_pos_z[MAX_PARTICLES] __attribute__((aligned(32))); float inv_mass[MAX_PARTICLES] __attribute__((aligned(32))); float prev_p_pos_x[MAX_PARTICLES] __attribute__((aligned(32))); float prev_p_pos_y[MAX_PARTICLES] __attribute__((aligned(32))); float prev_p_pos_z[MAX_PARTICLES] __attribute__((aligned(32))); float rest_lengths[MAX_REST_LENGTHS] __attribute__((aligned(32))); // Sphere positions (model space) float sphere_x[MAX_SPHERES] __attribute__((aligned(32))); float sphere_y[MAX_SPHERES] __attribute__((aligned(32))); float sphere_z[MAX_SPHERES] __attribute__((aligned(32))); float sphere_radius[MAX_SPHERES] __attribute__((aligned(32))); // Rest positions (model space) float rest_pos_x[MAX_PARTICLES] __attribute__((aligned(32))); float rest_pos_y[MAX_PARTICLES] __attribute__((aligned(32))); float rest_pos_z[MAX_PARTICLES] __attribute__((aligned(32))); float motion_radius[MAX_PARTICLES] __attribute__((aligned(32))); }; void simulate_chains(struct chain_sim *cs, float dt) { // Common SIMD constants const __m256 dt_vec = _mm256_set1_ps(dt); const __m256 dt2_vec = _mm256_set1_ps(dt * dt); const __m256 one_vec = _mm256_set1_ps(1.0f); const __m256 neg_one_vec = _mm256_set1_ps(-1.0f); const __m256 eps_vec = _mm256_set1_ps(1e-6f); const __m256 inv_mass_clamp_min = _mm256_set1_ps(0.0f); const __m256 inv_mass_clamp_max = _mm256_set1_ps(1e6f); const __m256 zero_vec = _mm256_setzero_ps(); const __m256 inv_dt_vec = _mm256_set1_ps(1.0f / dt); const __m256 two_vec = _mm256_set1_ps(2.0f); const __m256 inertia_clamp_min = _mm256_set1_ps(0.0f); const __m256 inertia_clamp_max = _mm256_set1_ps(1.0f); const __m256 abs_mask_ps = _mm256_castsi256_ps(_mm256_set1_epi32(0x7FFFFFFF)); // Local arrays for PBD solver float px_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); float py_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); float pz_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); float pm_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); // Initialize dummy elements px_local[PARTICLES_PER_CHAIN] = 0.0f; py_local[PARTICLES_PER_CHAIN] = 0.0f; pz_local[PARTICLES_PER_CHAIN] = 0.0f; pm_local[PARTICLES_PER_CHAIN] = 0.0f; // Stack arrays for precomputed data float wind_local_x[MAX_CHAINS] __attribute__((aligned(32))); float wind_local_y[MAX_CHAINS] __attribute__((aligned(32))); float wind_local_z[MAX_CHAINS] __attribute__((aligned(32))); float gravity_local_x[MAX_CHAINS] __attribute__((aligned(32))); float gravity_local_y[MAX_CHAINS] __attribute__((aligned(32))); float gravity_local_z[MAX_CHAINS] __attribute__((aligned(32))); float vel_x[MAX_CHAINS] __attribute__((aligned(32))); float vel_y[MAX_CHAINS] __attribute__((aligned(32))); float vel_z[MAX_CHAINS] __attribute__((aligned(32))); float ang_vel_x[MAX_CHAINS] __attribute__((aligned(32))); float ang_vel_y[MAX_CHAINS] __attribute__((aligned(32))); float ang_vel_z[MAX_CHAINS] __attribute__((aligned(32))); // Local arrays for PBD solver float px_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); float py_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); float pz_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); float pm_local[PARTICLES_PER_CHAIN + 1] __attribute__((aligned(32))); // Initialize dummy elements px_local[PARTICLES_PER_CHAIN] = 0.0f; py_local[PARTICLES_PER_CHAIN] = 0.0f; pz_local[PARTICLES_PER_CHAIN] = 0.0f; pm_local[PARTICLES_PER_CHAIN] = 0.0f; // Stack arrays for precomputed data float wind_local_x[MAX_CHAINS] __attribute__((aligned(32))); float wind_local_y[MAX_CHAINS] __attribute__((aligned(32))); float wind_local_z[MAX_CHAINS] __attribute__((aligned(32))); float gravity_local_x[MAX_CHAINS] __attribute__((aligned(32))); float gravity_local_y[MAX_CHAINS] __attribute__((aligned(32))); float gravity_local_z[MAX_CHAINS] __attribute__((aligned(32))); float vel_x[MAX_CHAINS] __attribute__((aligned(32))); float vel_y[MAX_CHAINS] __attribute__((aligned(32))); float vel_z[MAX_CHAINS] __attribute__((aligned(32))); float ang_vel_x[MAX_CHAINS] __attribute__((aligned(32))); float ang_vel_y[MAX_CHAINS] __attribute__((aligned(32))); float ang_vel_z[MAX_CHAINS] __attribute__((aligned(32))); // Step 0: Precomputation (SIMD, 8 chains at once) for (int c = 0; c < MAX_CHAINS; c += 8) { // Load chain quaternions __m256 qx = _mm256_load_ps(&cs->chain_quat_x[c]); __m256 qy = _mm256_load_ps(&cs->chain_quat_y[c]); __m256 qz = _mm256_load_ps(&cs->chain_quat_z[c]); __m256 qw = _mm256_load_ps(&cs->chain_quat_w[c]); // Compute local-space wind { __m256 vx = _mm256_load_ps(&cs->wind_x[c]); __m256 vy = _mm256_load_ps(&cs->wind_y[c]); __m256 vz = _mm256_load_ps(&cs->wind_z[c]); // Create q_conjugate components (qw remains, qx, qy, qz are negated) __m256 cqx = _mm256_sub_ps(zero_vec, qx); // -qx __m256 cqy = _mm256_sub_ps(zero_vec, qy); // -qy __m256 cqz = _mm256_sub_ps(zero_vec, qz); // -qz // Step 1: t = q_conj * v_world_as_quat // t = (qw, -qx, -qy, -qz) * (0, vx, vy, vz) // tw = -(-qx)*vx - (-qy)*vy - (-qz)*vz = qx*vx + qy*vy + qz*vz // tx = qw*vx + (-qy)*vz - (-qz)*vy = qw*vx - qy*vz + qz*vy // ty = qw*vy + (-qz)*vx - (-qx)*vz = qw*vy - qz*vx + qx*vz // tz = qw*vz + (-qx)*vy - (-qy)*vx = qw*vz - qx*vy + qy*vx __m256 tx = _mm256_fmadd_ps(cqy, vz, _mm256_mul_ps(qw, vx)); // qw*vx + (-qy)*vz tx = _mm256_fnmadd_ps(cqz, vy, tx);// qw*vx - qy*vz - (-qz)*vy = qw*vx - qy*vz + qz*vy __m256 ty = _mm256_fmadd_ps(cqz, vx, _mm256_mul_ps(qw, vy)); // qw*vy + (-qz)*vx ty = _mm256_fnmadd_ps(cqx, vz, ty); // qw*vy - qz*vx - (-qx)*vz = qw*vy - qz*vx + qx*vz __m256 tz = _mm256_fmadd_ps(cqx, vy, _mm256_mul_ps(qw, vz)); // qw*vz + (-qx)*vy tz = _mm256_fnmadd_ps(cqy, vx, tz); // qw*vz - qx*vy - (-qy)*vx = qw*vz - qx*vy + qy*vx __m256 tw = _mm256_fnmadd_ps(cqx, vx, zero_vec); // 0 - (-qx)*vx = qx*vx tw = _mm256_fnmadd_ps(cqy, vy, tw); // qx*vx - (-qy)*vy = qx*vx + qy*vy tw = _mm256_fnmadd_ps(cqz, vz, tw); // qx*vx + qy*vy - (-qz)*vz = qx*vx + qy*vy + qz*vz // Step 2: result_vec = (t * q)_vec // result = (tw, tx, ty, tz) * (qw, qx, qy, qz) // result_x = tw*qx + tx*qw + ty*qz - tz*qy // result_y = tw*qy + ty*qw + tz*qx - tx*qz // result_z = tw*qz + ty*qw + tx*qy - ty*qx __m256 result_x = _mm256_fmadd_ps(ty, qz, _mm256_mul_ps(tw, qx)); result_x = _mm256_fnmadd_ps(tz, qy, _mm256_fmadd_ps(tx, qw, result_x)); __m256 result_y = _mm256_fmadd_ps(tz, qx, _mm256_mul_ps(tw, qy)); result_y = _mm256_fnmadd_ps(tx, qz, _mm256_fmadd_ps(ty, qw, result_y)); __m256 result_z = _mm256_fmadd_ps(tx, qy, _mm256_mul_ps(tw, qz)); result_z = _mm256_fnmadd_ps(ty, qx, _mm256_fmadd_ps(tz, qw, result_z)); _mm256_store_ps(&wind_local_x[c], result_x); _mm256_store_ps(&wind_local_y[c], result_y); _mm256_store_ps(&wind_local_z[c], result_z); } // Compute local-space gravity { __m256 vx = _mm256_load_ps(&cs->gravity_x[c]); __m256 vy = _mm256_load_ps(&cs->gravity_y[c]); __m256 vz = _mm256_load_ps(&cs->gravity_z[c]); // Create q_conjugate components (qw remains, qx, qy, qz are negated) __m256 cqx = _mm256_sub_ps(zero_vec, qx); // -qx __m256 cqy = _mm256_sub_ps(zero_vec, qy); // -qy __m256 cqz = _mm256_sub_ps(zero_vec, qz); // -qz // Step 1: t = q_conj * v_world_as_quat // t = (qw, -qx, -qy, -qz) * (0, vx, vy, vz) // tw = -(-qx)*vx - (-qy)*vy - (-qz)*vz = qx*vx + qy*vy + qz*vz // tx = qw*vx + (-qy)*vz - (-qz)*vy = qw*vx - qy*vz + qz*vy // ty = qw*vy + (-qz)*vx - (-qx)*vz = qw*vy - qz*vx + qx*vz // tz = qw*vz + (-qx)*vy - (-qy)*vx = qw*vz - qx*vy + qy*vx __m256 tx = _mm256_fmadd_ps(cqy, vz, _mm256_mul_ps(qw, vx)); // qw*vx + (-qy)*vz tx = _mm256_fnmadd_ps(cqz, vy, tx);// qw*vx - qy*vz - (-qz)*vy = qw*vx - qy*vz + qz*vy __m256 ty = _mm256_fmadd_ps(cqz, vx, _mm256_mul_ps(qw, vy)); // qw*vy + (-qz)*vx ty = _mm256_fnmadd_ps(cqx, vz, ty); // qw*vy - qz*vx - (-qx)*vz = qw*vy - qz*vx + qx*vz __m256 tz = _mm256_fmadd_ps(cqx, vy, _mm256_mul_ps(qw, vz)); // qw*vz + (-qx)*vy tz = _mm256_fnmadd_ps(cqy, vx, tz); // qw*vz - qx*vy - (-qy)*vx = qw*vz - qx*vy + qy*vx __m256 tw = _mm256_fnmadd_ps(cqx, vx, zero_vec); // 0 - (-qx)*vx = qx*vx tw = _mm256_fnmadd_ps(cqy, vy, tw); // qx*vx - (-qy)*vy = qx*vx + qy*vy tw = _mm256_fnmadd_ps(cqz, vz, tw); // qx*vx + qy*vy - (-qz)*vz = qx*vx + qy*vy + qz*vz // Step 2: result_vec = (t * q)_vec // result = (tw, tx, ty, tz) * (qw, qx, qy, qz) // result_x = tw*qx + tx*qw + ty*qz - tz*qy // result_y = tw*qy + ty*qw + tz*qx - tx*qz // result_z = tw*qz + ty*qw + tx*qy - ty*qx __m256 result_x = _mm256_fmadd_ps(ty, qz, _mm256_mul_ps(tw, qx)); result_x = _mm256_fnmadd_ps(tz, qy, _mm256_fmadd_ps(tx, qw, result_x)); __m256 result_y = _mm256_fmadd_ps(tz, qx, _mm256_mul_ps(tw, qy)); result_y = _mm256_fnmadd_ps(tx, qz, _mm256_fmadd_ps(ty, qw, result_y)); __m256 result_z = _mm256_fmadd_ps(tx, qy, _mm256_mul_ps(tw, qz)); result_z = _mm256_fnmadd_ps(ty, qx, _mm256_fmadd_ps(tz, qw, result_z)); _mm256_store_ps(&gravity_local_x[c], result_x); _mm256_store_ps(&gravity_local_y[c], result_y); _mm256_store_ps(&gravity_local_z[c], result_z); } // Compute linear velocity { __m256 curr_x = _mm256_load_ps(&cs->chain_pos_x[c]); __m256 curr_y = _mm256_load_ps(&cs->chain_pos_y[c]); __m256 curr_z = _mm256_load_ps(&cs->chain_pos_z[c]); __m256 prev_x = _mm256_load_ps(&cs->prev_chain_pos_x[c]); __m256 prev_y = _mm256_load_ps(&cs->prev_chain_pos_y[c]); __m256 prev_z = _mm256_load_ps(&cs->prev_chain_pos_z[c]); __m256 vel_x_vec = _mm256_mul_ps(_mm256_sub_ps(curr_x, prev_x), inv_dt_vec); __m256 vel_y_vec = _mm256_mul_ps(_mm256_sub_ps(curr_y, prev_y), inv_dt_vec); __m256 vel_z_vec = _mm256_mul_ps(_mm256_sub_ps(curr_z, prev_z), inv_dt_vec); _mm256_store_ps(&vel_x[c], vel_x_vec); _mm256_store_ps(&vel_y[c], vel_y_vec); _mm256_store_ps(&vel_z[c], vel_z_vec); } // Compute angular velocity { __m256 qx = _mm256_load_ps(&cs->chain_quat_x[c]); __m256 qy = _mm256_load_ps(&cs->chain_quat_y[c]); __m256 qz = _mm256_load_ps(&cs->chain_quat_z[c]); __m256 qw = _mm256_load_ps(&cs->chain_quat_w[c]); __m256 prev_qx = _mm256_load_ps(&cs->prev_chain_quat_x[c]); __m256 prev_qy = _mm256_load_ps(&cs->prev_chain_quat_y[c]); __m256 prev_qz = _mm256_load_ps(&cs->prev_chain_quat_z[c]); __m256 prev_qw = _mm256_load_ps(&cs->prev_chain_quat_w[c]); // Step 1: Compute delta quaternion (to - from) __m256 delta_qx = _mm256_sub_ps(qx, prev_qx); __m256 delta_qy = _mm256_sub_ps(qy, prev_qy); __m256 delta_qz = _mm256_sub_ps(qz, prev_qz); __m256 delta_qw = _mm256_sub_ps(qw, prev_qw); // Step 2: Compute conjugate of previous quaternion (qw, -qx, -qy, -qz) __m256 conj_qx = _mm256_sub_ps(zero_vec, prev_qx); __m256 conj_qy = _mm256_sub_ps(zero_vec, prev_qy); __m256 conj_qz = _mm256_sub_ps(zero_vec, prev_qz); __m256 conj_qw = prev_qw; // Step 3: Compute orientation delta = deltaQuat * conj(prev_quat) // Quaternion multiplication: (w1w2 - v1·v2, w1v2 + w2v1 + v1×v2) // Result: orientationDelta = (delta_qw, delta_qx, delta_qy, delta_qz) * (conj_qw, conj_qx, conj_qy, conj_qz) __m256 orient_qw = _mm256_fnmadd_ps(delta_qx, conj_qx, _mm256_mul_ps(delta_qw, conj_qw)); orient_qw = _mm256_fnmadd_ps(delta_qy, conj_qy, orient_qw); orient_qw = _mm256_fnmadd_ps(delta_qz, conj_qz, orient_qw); __m256 cross_x = _mm256_sub_ps(_mm256_mul_ps(delta_qy, conj_qz), _mm256_mul_ps(delta_qz, conj_qy)); __m256 cross_y = _mm256_sub_ps(_mm256_mul_ps(delta_qz, conj_qx), _mm256_mul_ps(delta_qx, conj_qz)); __m256 cross_z = _mm256_sub_ps(_mm256_mul_ps(delta_qx, conj_qy), _mm256_mul_ps(delta_qy, conj_qx)); __m256 orient_qx = _mm256_fmadd_ps(delta_qw, conj_qx, _mm256_mul_ps(delta_qx, conj_qw)); orient_qx = _mm256_add_ps(orient_qx, cross_x); __m256 orient_qy = _mm256_fmadd_ps(delta_qw, conj_qy, _mm256_mul_ps(delta_qy, conj_qw)); orient_qy = _mm256_add_ps(orient_qy, cross_y); __m256 orient_qz = _mm256_fmadd_ps(delta_qw, conj_qz, _mm256_mul_ps(delta_qz, conj_qw)); orient_qz = _mm256_add_ps(orient_qz, cross_z); // Step 4: Compute dot product (to, from) for shortest arc check __m256 dot = _mm256_fmadd_ps(qx, prev_qx, _mm256_mul_ps(qw, prev_qw)); dot = _mm256_fmadd_ps(qy, prev_qy, dot); dot = _mm256_fmadd_ps(qz, prev_qz, dot); // Step 5: Negate orientation delta if dot < 0 (shortest arc adjustment) __m256 negate_mask = _mm256_cmp_ps(dot, zero_vec, _CMP_LT_OQ); __m256 negate_sign = _mm256_or_ps(_mm256_and_ps(negate_mask, neg_one_vec), _mm256_andnot_ps(negate_mask, one_vec)); orient_qx = _mm256_mul_ps(orient_qx, negate_sign); orient_qy = _mm256_mul_ps(orient_qy, negate_sign); orient_qz = _mm256_mul_ps(orient_qz, negate_sign); // Step 6: Compute angular velocity: (orient_qx, orient_qy, orient_qz) * (1 / (2 * dt)) __m256 scale = _mm256_div_ps(one_vec, _mm256_mul_ps(two_vec, dt_vec)); __m256 ang_vel_x_vec = _mm256_mul_ps(orient_qx, scale); __m256 ang_vel_y_vec = _mm256_mul_ps(orient_qy, scale); __m256 ang_vel_z_vec = _mm256_mul_ps(orient_qz, scale); // Store results _mm256_store_ps(&ang_vel_x[c], ang_vel_x_vec); _mm256_store_ps(&ang_vel_y[c], ang_vel_y_vec); _mm256_store_ps(&ang_vel_z[c], ang_vel_z_vec); } } // Step 1: Apply Chain Inertia (Linear, Angular, and Centrifugal) for (int c = 0; c < MAX_CHAINS; c++) { int base_idx = c * PARTICLES_PER_CHAIN; struct chain_cfg *cfg = &cs->chain_configs[c]; // Load precomputed velocities and inertia parameters __m256 vel_x_vec = _mm256_set1_ps(vel_x[c]); __m256 vel_y_vec = _mm256_set1_ps(vel_y[c]); __m256 vel_z_vec = _mm256_set1_ps(vel_z[c]); __m256 ang_vel_x_vec = _mm256_set1_ps(ang_vel_x[c]); __m256 ang_vel_y_vec = _mm256_set1_ps(ang_vel_y[c]); __m256 ang_vel_z_vec = _mm256_set1_ps(ang_vel_z[c]); __m256 linear_inertia_vec = _mm256_set1_ps(cfg->linear_inertia); __m256 angular_inertia_vec = _mm256_set1_ps(cfg->angular_inertia); __m256 centrifugal_inertia_vec = _mm256_set1_ps(cfg->centrifugal_inertia); // Clamp inertia parameters to [0, 1] linear_inertia_vec = _mm256_max_ps(_mm256_min_ps(linear_inertia_vec, inertia_clamp_max), inertia_clamp_min); angular_inertia_vec = _mm256_max_ps(_mm256_min_ps(angular_inertia_vec, inertia_clamp_max), inertia_clamp_min); centrifugal_inertia_vec = _mm256_max_ps(_mm256_min_ps(centrifugal_inertia_vec, inertia_clamp_max), inertia_clamp_min); for (int i = 0; i < PARTICLES_PER_CHAIN; i += 8) { __m256 px = _mm256_load_ps(&cs->p_pos_x[base_idx + i]); __m256 py = _mm256_load_ps(&cs->p_pos_y[base_idx + i]); __m256 pz = _mm256_load_ps(&cs->p_pos_z[base_idx + i]); __m256 p_inv_mass = _mm256_load_ps(&cs->inv_mass[base_idx + i]); p_inv_mass = _mm256_max_ps(_mm256_min_ps(p_inv_mass, inv_mass_clamp_max), inv_mass_clamp_min); // Linear inertia: v * dt * linear_inertia __m256 lin_delta_x = _mm256_mul_ps(_mm256_mul_ps(vel_x_vec, dt_vec), linear_inertia_vec); __m256 lin_delta_y = _mm256_mul_ps(_mm256_mul_ps(vel_y_vec, dt_vec), linear_inertia_vec); __m256 lin_delta_z = _mm256_mul_ps(_mm256_mul_ps(vel_z_vec, dt_vec), linear_inertia_vec); // Angular inertia: (ω × r) * dt * angular_inertia __m256 ang_delta_x = _mm256_sub_ps(_mm256_mul_ps(ang_vel_y_vec, pz), _mm256_mul_ps(ang_vel_z_vec, py)); __m256 ang_delta_y = _mm256_sub_ps(_mm256_mul_ps(ang_vel_z_vec, px), _mm256_mul_ps(ang_vel_x_vec, pz)); __m256 ang_delta_z = _mm256_sub_ps(_mm256_mul_ps(ang_vel_x_vec, py), _mm256_mul_ps(ang_vel_y_vec, px)); ang_delta_x = _mm256_mul_ps(_mm256_mul_ps(ang_delta_x, dt_vec), angular_inertia_vec); ang_delta_y = _mm256_mul_ps(_mm256_mul_ps(ang_delta_y, dt_vec), angular_inertia_vec); ang_delta_z = _mm256_mul_ps(_mm256_mul_ps(ang_delta_z, dt_vec), angular_inertia_vec); // Centrifugal inertia: (ω × (ω × r)) * dt^2 * centrifugal_inertia __m256 cross1_x = _mm256_sub_ps(_mm256_mul_ps(ang_vel_y_vec, pz), _mm256_mul_ps(ang_vel_z_vec, py)); __m256 cross1_y = _mm256_sub_ps(_mm256_mul_ps(ang_vel_z_vec, px), _mm256_mul_ps(ang_vel_x_vec, pz)); __m256 cross1_z = _mm256_sub_ps(_mm256_mul_ps(ang_vel_x_vec, py), _mm256_mul_ps(ang_vel_y_vec, px)); __m256 cross2_x = _mm256_sub_ps(_mm256_mul_ps(ang_vel_y_vec, cross1_z), _mm256_mul_ps(ang_vel_z_vec, cross1_y)); __m256 cross2_y = _mm256_sub_ps(_mm256_mul_ps(ang_vel_z_vec, cross1_x), _mm256_mul_ps(ang_vel_x_vec, cross1_z)); __m256 cross2_z = _mm256_sub_ps(_mm256_mul_ps(ang_vel_x_vec, cross1_y), _mm256_mul_ps(ang_vel_y_vec, cross1_x)); // Calculate displacement: (ω × (ω × r)) * dt^2 __m256 base_cent_delta_x = _mm256_mul_ps(cross2_x, dt2_vec); __m256 base_cent_delta_y = _mm256_mul_ps(cross2_y, dt2_vec); __m256 base_cent_delta_z = _mm256_mul_ps(cross2_z, dt2_vec); // Apply the centrifugal_inertia factor __m256 cent_delta_x = _mm256_mul_ps(base_cent_delta_x, centrifugal_inertia_vec); __m256 cent_delta_y = _mm256_mul_ps(base_cent_delta_y, centrifugal_inertia_vec); __m256 cent_delta_z = _mm256_mul_ps(base_cent_delta_z, centrifugal_inertia_vec); // Total delta __m256 delta_x = _mm256_add_ps(_mm256_add_ps(lin_delta_x, ang_delta_x), cent_delta_x); __m256 delta_y = _mm256_add_ps(_mm256_add_ps(lin_delta_y, ang_delta_y), cent_delta_y); __m256 delta_z = _mm256_add_ps(_mm256_add_ps(lin_delta_z, ang_delta_z), cent_delta_z); // Update positions _mm256_store_ps(&cs->p_pos_x[base_idx + i], _mm256_add_ps(px, delta_x)); _mm256_store_ps(&cs->p_pos_y[base_idx + i], _mm256_add_ps(py, delta_y)); _mm256_store_ps(&cs->p_pos_z[base_idx + i], _mm256_add_ps(pz, delta_z)); } // Update previous transformation cs->prev_chain_pos_x[c] = cs->chain_pos_x[c]; cs->prev_chain_pos_y[c] = cs->chain_pos_y[c]; cs->prev_chain_pos_z[c] = cs->chain_pos_z[c]; cs->prev_chain_quat_x[c] = cs->chain_quat_x[c]; cs->prev_chain_quat_y[c] = cs->chain_quat_y[c]; cs->prev_chain_quat_z[c] = cs->chain_quat_z[c]; cs->prev_chain_quat_w[c] = cs->chain_quat_w[c]; } // Step 2: Verlet Integration for (int c = 0; c < MAX_CHAINS; c++) { int base_idx = c * PARTICLES_PER_CHAIN; struct chain_cfg *cfg = &cs->chain_configs[c]; __m256 gravity_x_vec = _mm256_set1_ps(gravity_local_x[c]); __m256 gravity_y_vec = _mm256_set1_ps(gravity_local_y[c]); __m256 gravity_z_vec = _mm256_set1_ps(gravity_local_z[c]); __m256 wind_x_vec = _mm256_set1_ps(wind_local_x[c]); __m256 wind_y_vec = _mm256_set1_ps(wind_local_y[c]); __m256 wind_z_vec = _mm256_set1_ps(wind_local_z[c]); __m256 damping_vec = _mm256_set1_ps(cfg->damping); for (int i = 0; i < PARTICLES_PER_CHAIN; i += 8) { __m256 p_curr_x = _mm256_load_ps(&cs->p_pos_x[base_idx + i]); __m256 p_curr_y = _mm256_load_ps(&cs->p_pos_y[base_idx + i]); __m256 p_curr_z = _mm256_load_ps(&cs->p_pos_z[base_idx + i]); __m256 p_inv_mass = _mm256_load_ps(&cs->inv_mass[base_idx + i]); p_inv_mass = _mm256_max_ps(_mm256_min_ps(p_inv_mass, inv_mass_clamp_max), inv_mass_clamp_min); __m256 p_prev_x = _mm256_load_ps(&cs->prev_p_pos_x[base_idx + i]); __m256 p_prev_y = _mm256_load_ps(&cs->prev_p_pos_y[base_idx + i]); __m256 p_prev_z = _mm256_load_ps(&cs->prev_p_pos_z[base_idx + i]); __m256 force_x = _mm256_add_ps(gravity_x_vec, wind_x_vec); __m256 force_y = _mm256_add_ps(gravity_y_vec, wind_y_vec); __m256 force_z = _mm256_add_ps(gravity_z_vec, wind_z_vec); __m256 acc_x = _mm256_mul_ps(force_x, p_inv_mass); __m256 acc_y = _mm256_mul_ps(force_y, p_inv_mass); __m256 acc_z = _mm256_mul_ps(force_z, p_inv_mass); __m256 vel_x = _mm256_sub_ps(p_curr_x, p_prev_x); __m256 vel_y = _mm256_sub_ps(p_curr_y, p_prev_y); __m256 vel_z = _mm256_sub_ps(p_curr_z, p_prev_z); __m256 damped_vel_x = _mm256_mul_ps(vel_x, damping_vec); __m256 damped_vel_y = _mm256_mul_ps(vel_y, damping_vec); __m256 damped_vel_z = _mm256_mul_ps(vel_z, damping_vec); __m256 term_accel_x = _mm256_mul_ps(acc_x, dt2_vec); __m256 term_accel_y = _mm256_mul_ps(acc_y, dt2_vec); __m256 term_accel_z = _mm256_mul_ps(acc_z, dt2_vec); __m256 new_p_x = _mm256_add_ps(p_curr_x, _mm256_add_ps(damped_vel_x, term_accel_x)); __m256 new_p_y = _mm256_add_ps(p_curr_y, _mm256_add_ps(damped_vel_y, term_accel_y)); __m256 new_p_z = _mm256_add_ps(p_curr_z, _mm256_add_ps(damped_vel_z, term_accel_z)); _mm256_store_ps(&cs->p_pos_x[base_idx + i], new_p_x); _mm256_store_ps(&cs->p_pos_y[base_idx + i], new_p_y); _mm256_store_ps(&cs->p_pos_z[base_idx + i], new_p_z); _mm256_store_ps(&cs->prev_p_pos_x[base_idx + i], p_curr_x); _mm256_store_ps(&cs->prev_p_pos_y[base_idx + i], p_curr_y); _mm256_store_ps(&cs->prev_p_pos_z[base_idx + i], p_curr_z); } } // Step 3: Distance Constraints for (int c = 0; c < MAX_CHAINS; c++) { int p_base = c * PARTICLES_PER_CHAIN; int r_base = c * CONSTRAINTS_PER_CHAIN; struct chain_cfg *cfg = &cs->chain_configs[c]; __m256 stiffness_vec = _mm256_set1_ps(cfg->stiffness); __m256 stretch_factor_vec = _mm256_set1_ps(cfg->stretch_factor); __m256 max_strain_vec_abs = _mm256_set1_ps(cfg->max_strain); __m256 neg_max_strain_vec_abs = _mm256_mul_ps(max_strain_vec_abs, neg_one_vec); for (int i = 0; i < PARTICLES_PER_CHAIN; i += 8) { __m256 px = _mm256_load_ps(&cs->p_pos_x[p_base + i]); __m256 py = _mm256_load_ps(&cs->p_pos_y[p_base + i]); __m256 pz = _mm256_load_ps(&cs->p_pos_z[p_base + i]); __m256 pm = _mm256_load_ps(&cs->inv_mass[p_base + i]); pm = _mm256_max_ps(_mm256_min_ps(pm, inv_mass_clamp_max), inv_mass_clamp_min); _mm256_store_ps(&px_local[i], px); _mm256_store_ps(&py_local[i], py); _mm256_store_ps(&pz_local[i], pz); _mm256_store_ps(&pm_local[i], pm); } for (int iter = 0; iter < MAX_ITERATIONS; iter++) { // First block (i=0 to 7) { __m256 p0x = _mm256_load_ps(&px_local[0]); __m256 p0y = _mm256_load_ps(&py_local[0]); __m256 p0z = _mm256_load_ps(&pz_local[0]); __m256 p0m = _mm256_load_ps(&pm_local[0]); __m256 p1x = _mm256_loadu_ps(&px_local[1]); __m256 p1y = _mm256_loadu_ps(&py_local[1]); __m256 p1z = _mm256_loadu_ps(&pz_local[1]); __m256 p1m = _mm256_loadu_ps(&pm_local[1]); __m256 rest_len_vec = _mm256_load_ps(&cs->rest_lengths[r_base]); __m256 dx = _mm256_sub_ps(p1x, p0x); __m256 dy = _mm256_sub_ps(p1y, p0y); __m256 dz = _mm256_sub_ps(p1z, p0z); __m256 dist_sq = _mm256_mul_ps(dx, dx); dist_sq = _mm256_fmadd_ps(dy, dy, dist_sq); dist_sq = _mm256_fmadd_ps(dz, dz, dist_sq); __m256 inv_dist = _mm256_rsqrt_ps(_mm256_max_ps(dist_sq, eps_vec)); __m256 w_sum = _mm256_add_ps(p0m, p1m); __m256 w_sum_clamped = _mm256_max_ps(w_sum, eps_vec); __m256 rcp_w_sum = _mm256_rcp_ps(w_sum_clamped); __m256 dist = _mm256_rcp_ps(inv_dist); __m256 diff = _mm256_sub_ps(dist, rest_len_vec); __m256 max_rest_len_eps = _mm256_max_ps(rest_len_vec, eps_vec); __m256 strain = _mm256_mul_ps(diff, _mm256_rcp_ps(max_rest_len_eps)); __m256 strain_clamped = _mm256_max_ps(neg_max_strain_vec_abs, _mm256_min_ps(strain, max_strain_vec_abs)); __m256 dynamic_stiffness_denom = _mm256_add_ps(one_vec, _mm256_and_ps(strain_clamped, abs_mask_ps)); __m256 dynamic_stiffness = _mm256_mul_ps(stiffness_vec, _mm256_rcp_ps(dynamic_stiffness_denom)); __m256 corr_scalar_part = _mm256_mul_ps(dynamic_stiffness, stretch_factor_vec); corr_scalar_part = _mm256_mul_ps(corr_scalar_part, rcp_w_sum); __m256 corr_magnitude = _mm256_mul_ps(diff, corr_scalar_part); __m256 apply_corr_mask = _mm256_cmp_ps(dist_sq, eps_vec, _CMP_GT_OQ); __m256 delta_unit_x = _mm256_mul_ps(dx, inv_dist); __m256 delta_unit_y = _mm256_mul_ps(dy, inv_dist); __m256 delta_unit_z = _mm256_mul_ps(dz, inv_dist); __m256 delta_x = _mm256_mul_ps(delta_unit_x, corr_magnitude); __m256 delta_y = _mm256_mul_ps(delta_unit_y, corr_magnitude); __m256 delta_z = _mm256_mul_ps(delta_unit_z, corr_magnitude); delta_x = _mm256_and_ps(delta_x, apply_corr_mask); delta_y = _mm256_and_ps(delta_y, apply_corr_mask); delta_z = _mm256_and_ps(delta_z, apply_corr_mask); _mm256_store_ps(&px_local[0], _mm256_fmadd_ps(delta_x, p0m, p0x)); _mm256_store_ps(&py_local[0], _mm256_fmadd_ps(delta_y, p0m, p0y)); _mm256_store_ps(&pz_local[0], _mm256_fmadd_ps(delta_z, p0m, p0z)); _mm256_store_ps(&px_local[1], _mm256_fnmadd_ps(delta_x, p1m, p1x)); _mm256_store_ps(&py_local[1], _mm256_fnmadd_ps(delta_y, p1m, p1y)); _mm256_store_ps(&pz_local[1], _mm256_fnmadd_ps(delta_z, p1m, p1z)); } // Second block (i=8 to 14) { __m256 p0x = _mm256_load_ps(&px_local[8]); __m256 p0y = _mm256_load_ps(&py_local[8]); __m256 p0z = _mm256_load_ps(&pz_local[8]); __m256 p0m = _mm256_load_ps(&pm_local[8]); __m256 p1x = _mm256_loadu_ps(&px_local[9]); __m256 p1y = _mm256_loadu_ps(&py_local[9]); __m256 p1z = _mm256_loadu_ps(&pz_local[9]); __m256 p1m = _mm256_loadu_ps(&pm_local[9]); __m256 rest_len_vec = _mm256_load_ps(&cs->rest_lengths[r_base + 8]); __m256 dx = _mm256_sub_ps(p1x, p0x); __m256 dy = _mm256_sub_ps(p1y, p0y); __m256 dz = _mm256_sub_ps(p1z, p0z); __m256 dist_sq = _mm256_mul_ps(dx, dx); dist_sq = _mm256_fmadd_ps(dy, dy, dist_sq); dist_sq = _mm256_fmadd_ps(dz, dz, dist_sq); __m256 inv_dist = _mm256_rsqrt_ps(_mm256_max_ps(dist_sq, eps_vec)); __m256 w_sum = _mm256_add_ps(p0m, p1m); __m256 w_sum_clamped = _mm256_max_ps(w_sum, eps_vec); __m256 rcp_w_sum = _mm256_rcp_ps(w_sum_clamped); __m256 dist = _mm256_rcp_ps(inv_dist); __m256 diff = _mm256_sub_ps(dist, rest_len_vec); __m256 max_rest_len_eps = _mm256_max_ps(rest_len_vec, eps_vec); __m256 strain = _mm256_mul_ps(diff, _mm256_rcp_ps(max_rest_len_eps)); __m256 strain_clamped = _mm256_max_ps(neg_max_strain_vec_abs, _mm256_min_ps(strain, max_strain_vec_abs)); __m256 dynamic_stiffness_denom = _mm256_add_ps(one_vec, _mm256_and_ps(strain_clamped, abs_mask_ps)); __m256 dynamic_stiffness = _mm256_mul_ps(stiffness_vec, _mm256_rcp_ps(dynamic_stiffness_denom)); __m256 corr_scalar_part = _mm256_mul_ps(dynamic_stiffness, stretch_factor_vec); corr_scalar_part = _mm256_mul_ps(corr_scalar_part, rcp_w_sum); __m256 corr_magnitude = _mm256_mul_ps(diff, corr_scalar_part); __m256 apply_corr_mask = _mm256_cmp_ps(dist_sq, eps_vec, _CMP_GT_OQ); __attribute__((aligned(32))) float valid_mask_array[8] = { -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 0.0f }; __m256 valid_mask = _mm256_load_ps(valid_mask_array); apply_corr_mask = _mm256_and_ps(apply_corr_mask, valid_mask); __m256 delta_unit_x = _mm256_mul_ps(dx, inv_dist); __m256 delta_unit_y = _mm256_mul_ps(dy, inv_dist); __m256 delta_unit_z = _mm256_mul_ps(dz, inv_dist); __m256 delta_x = _mm256_mul_ps(delta_unit_x, corr_magnitude); __m256 delta_y = _mm256_mul_ps(delta_unit_y, corr_magnitude); __m256 delta_z = _mm256_mul_ps(delta_unit_z, corr_magnitude); delta_x = _mm256_and_ps(delta_x, apply_corr_mask); delta_y = _mm256_and_ps(delta_y, apply_corr_mask); delta_z = _mm256_and_ps(delta_z, apply_corr_mask); _mm256_store_ps(&px_local[8], _mm256_fmadd_ps(delta_x, p0m, p0x)); _mm256_store_ps(&py_local[8], _mm256_fmadd_ps(delta_y, p0m, p0y)); _mm256_store_ps(&pz_local[8], _mm256_fmadd_ps(delta_z, p0m, p0z)); _mm256_storeu_ps(&px_local[9], _mm256_fnmadd_ps(delta_x, p1m, p1x)); _mm256_storeu_ps(&py_local[9], _mm256_fnmadd_ps(delta_y, p1m, p1y)); _mm256_storeu_ps(&pz_local[9], _mm256_fnmadd_ps(delta_z, p1m, p1z)); } } for (int i = 0; i < PARTICLES_PER_CHAIN; i += 8) { __m256 px = _mm256_load_ps(&px_local[i]); __m256 py = _mm256_load_ps(&py_local[i]); __m256 pz = _mm256_load_ps(&pz_local[i]); _mm256_store_ps(&cs->p_pos_x[p_base + i], px); _mm256_store_ps(&cs->p_pos_y[p_base + i], py); _mm256_store_ps(&cs->p_pos_z[p_base + i], pz); } } // Step 4: Sphere Collisions with Friction for (int c = 0; c < MAX_CHAINS; c++) { int p_base = c * PARTICLES_PER_CHAIN; int s_base = c * SPHERES_PER_CHAIN; struct chain_cfg *cfg = &cs->chain_configs[c]; __m256 friction_vec = _mm256_set1_ps(cfg->friction); for (int s = 0; s < SPHERES_PER_CHAIN; s++) { __m256 sphere_x_vec = _mm256_set1_ps(cs->sphere_x[s_base + s]); __m256 sphere_y_vec = _mm256_set1_ps(cs->sphere_y[s_base + s]); __m256 sphere_z_vec = _mm256_set1_ps(cs->sphere_z[s_base + s]); __m256 sphere_r_vec = _mm256_set1_ps(cs->sphere_radius[s_base + s]); for (int i = 0; i < PARTICLES_PER_CHAIN; i += 8) { __m256 p_curr_x = _mm256_load_ps(&cs->p_pos_x[p_base + i]); __m256 p_curr_y = _mm256_load_ps(&cs->p_pos_y[p_base + i]); __m256 p_curr_z = _mm256_load_ps(&cs->p_pos_z[p_base + i]); __m256 p_prev_x = _mm256_load_ps(&cs->prev_p_pos_x[p_base + i]); __m256 p_prev_y = _mm256_load_ps(&cs->prev_p_pos_y[p_base + i]); __m256 p_prev_z = _mm256_load_ps(&cs->prev_p_pos_z[p_base + i]); __m256 p_inv_mass = _mm256_load_ps(&cs->inv_mass[p_base + i]); p_inv_mass = _mm256_max_ps(_mm256_min_ps(p_inv_mass, inv_mass_clamp_max), inv_mass_clamp_min); __m256 dx = _mm256_sub_ps(p_curr_x, sphere_x_vec); __m256 dy = _mm256_sub_ps(p_curr_y, sphere_y_vec); __m256 dz = _mm256_sub_ps(p_curr_z, sphere_z_vec); __m256 dist_sq = _mm256_mul_ps(dx, dx); dist_sq = _mm256_fmadd_ps(dy, dy, dist_sq); dist_sq = _mm256_fmadd_ps(dz, dz, dist_sq); __m256 inv_dist = _mm256_rsqrt_ps(_mm256_max_ps(dist_sq, eps_vec)); __m256 dist = _mm256_rcp_ps(inv_dist); __m256 penetration = _mm256_sub_ps(sphere_r_vec, dist); __m256 collision_mask = _mm256_cmp_ps(penetration, zero_vec, _CMP_GT_OQ); __m256 normal_x = _mm256_mul_ps(dx, inv_dist); __m256 normal_y = _mm256_mul_ps(dy, inv_dist); __m256 normal_z = _mm256_mul_ps(dz, inv_dist); __m256 normal_corr_x = _mm256_mul_ps(normal_x, penetration); __m256 normal_corr_y = _mm256_mul_ps(normal_y, penetration); __m256 normal_corr_z = _mm256_mul_ps(normal_z, penetration); __m256 vel_x = _mm256_sub_ps(p_curr_x, p_prev_x); __m256 vel_y = _mm256_sub_ps(p_curr_y, p_prev_y); __m256 vel_z = _mm256_sub_ps(p_curr_z, p_prev_z); __m256 vel_dot_normal = _mm256_mul_ps(vel_x, normal_x); vel_dot_normal = _mm256_fmadd_ps(vel_y, normal_y, vel_dot_normal); vel_dot_normal = _mm256_fmadd_ps(vel_z, normal_z, vel_dot_normal); __m256 tangent_x = _mm256_sub_ps(vel_x, _mm256_mul_ps(vel_dot_normal, normal_x)); __m256 tangent_y = _mm256_sub_ps(vel_y, _mm256_mul_ps(vel_dot_normal, normal_y)); __m256 tangent_z = _mm256_sub_ps(vel_z, _mm256_mul_ps(vel_dot_normal, normal_z)); __m256 tangent_mag_sq = _mm256_mul_ps(tangent_x, tangent_x); tangent_mag_sq = _mm256_fmadd_ps(tangent_y, tangent_y, tangent_mag_sq); tangent_mag_sq = _mm256_fmadd_ps(tangent_z, tangent_z, tangent_mag_sq); __m256 inv_tangent_mag = _mm256_rsqrt_ps(_mm256_max_ps(tangent_mag_sq, eps_vec)); __m256 friction_mag = _mm256_mul_ps(penetration, friction_vec); __m256 friction_x = _mm256_mul_ps(_mm256_mul_ps(tangent_x, inv_tangent_mag), friction_mag); __m256 friction_y = _mm256_mul_ps(_mm256_mul_ps(tangent_y, inv_tangent_mag), friction_mag); __m256 friction_z = _mm256_mul_ps(_mm256_mul_ps(tangent_z, inv_tangent_mag), friction_mag); __m256 total_corr_x = _mm256_sub_ps(normal_corr_x, friction_x); __m256 total_corr_y = _mm256_sub_ps(normal_corr_y, friction_y); __m256 total_corr_z = _mm256_sub_ps(normal_corr_z, friction_z); total_corr_x = _mm256_and_ps(total_corr_x, collision_mask); total_corr_y = _mm256_and_ps(total_corr_y, collision_mask); total_corr_z = _mm256_and_ps(total_corr_z, collision_mask); total_corr_x = _mm256_mul_ps(total_corr_x, p_inv_mass); total_corr_y = _mm256_mul_ps(total_corr_y, p_inv_mass); total_corr_z = _mm256_mul_ps(total_corr_z, p_inv_mass); __m256 new_p_x = _mm256_add_ps(p_curr_x, total_corr_x); __m256 new_p_y = _mm256_add_ps(p_curr_y, total_corr_y); __m256 new_p_z = _mm256_add_ps(p_curr_z, total_corr_z); _mm256_store_ps(&cs->p_pos_x[p_base + i], new_p_x); _mm256_store_ps(&cs->p_pos_y[p_base + i], new_p_y); _mm256_store_ps(&cs->p_pos_z[p_base + i], new_p_z); } } } // Step 5: Motion Constraints for (int c = 0; c < MAX_CHAINS; c++) { int p_base = c * PARTICLES_PER_CHAIN; for (int i = 0; i < PARTICLES_PER_CHAIN; i += 8) { __m256 px = _mm256_load_ps(&cs->p_pos_x[p_base + i]); __m256 py = _mm256_load_ps(&cs->p_pos_y[p_base + i]); __m256 pz = _mm256_load_ps(&cs->p_pos_z[p_base + i]); __m256 pm = _mm256_load_ps(&cs->inv_mass[p_base + i]); pm = _mm256_max_ps(_mm256_min_ps(pm, inv_mass_clamp_max), inv_mass_clamp_min); __m256 rx = _mm256_load_ps(&cs->rest_pos_x[p_base + i]); __m256 ry = _mm256_load_ps(&cs->rest_pos_y[p_base + i]); __m256 rz = _mm256_load_ps(&cs->rest_pos_z[p_base + i]); __m256 radius_vec = _mm256_load_ps(&cs->motion_radius[p_base + i]); __m256 dx = _mm256_sub_ps(px, rx); __m256 dy = _mm256_sub_ps(py, ry); __m256 dz = _mm256_sub_ps(pz, rz); __m256 dist_sq = _mm256_mul_ps(dx, dx); dist_sq = _mm256_fmadd_ps(dy, dy, dist_sq); dist_sq = _mm256_fmadd_ps(dz, dz, dist_sq); __m256 inv_dist = _mm256_rsqrt_ps(_mm256_max_ps(dist_sq, eps_vec)); __m256 dist = _mm256_rcp_ps(inv_dist); __m256 penetration = _mm256_sub_ps(dist, radius_vec); __m256 mask = _mm256_cmp_ps(penetration, zero_vec, _CMP_GT_OQ); __m256 corr_factor = _mm256_mul_ps(penetration, inv_dist); corr_factor = _mm256_and_ps(corr_factor, mask); __m256 delta_x = _mm256_mul_ps(dx, corr_factor); __m256 delta_y = _mm256_mul_ps(dy, corr_factor); __m256 delta_z = _mm256_mul_ps(dz, corr_factor); delta_x = _mm256_mul_ps(delta_x, pm); delta_y = _mm256_mul_ps(delta_y, pm); delta_z = _mm256_mul_ps(delta_z, pm); _mm256_store_ps(&cs->p_pos_x[p_base + i], _mm256_sub_ps(px, delta_x)); _mm256_store_ps(&cs->p_pos_y[p_base + i], _mm256_sub_ps(py, delta_y)); _mm256_store_ps(&cs->p_pos_z[p_base + i], _mm256_sub_ps(pz, delta_z)); } } }