817 lines
26 KiB
C++
817 lines
26 KiB
C++
|
#pragma once
|
|||
|
|
/**
|
|||
|
|
* @file simd_vector.h
|
|||
|
|
* @brief 极致性能 — 平台自适应 SIMD 向量 (SSE/AVX/NEON)
|
|||
|
|
*
|
|||
|
|
* 提供 4-wide double/float SIMD 向量类型，自动检测 CPU 指令集。
|
|||
|
|
* 支持 dot/cross/normalize/length 及 AABB 批量相交测试。
|
|||
|
|
*
|
|||
|
|
* ## SIMD 指令集选择策略
|
|||
|
|
*
|
|||
|
|
* | 平台 | 指令集 | 宏定义 |
|
|||
|
|
* |-----------|---------|-------------------------------|
|
|||
|
|
* | x86_64 | AVX2 | __AVX2__ (由编译器定义) |
|
|||
|
|
* | x86_64 | SSE2 | __SSE2__ (回退) |
|
|||
|
|
* | ARM | NEON | __ARM_NEON |
|
|||
|
|
* | 通用 | 标量回退 | 无 SIMD 宏 |
|
|||
|
|
*
|
|||
|
|
* @ingroup core
|
|||
|
|
*/
|
|||
|
|
#include <cstddef>
|
|||
|
|
#include <cstdint>
|
|||
|
|
#include <cmath>
|
|||
|
|
#include <algorithm>
|
|||
|
|
#include <type_traits>
|
|||
|
|
|
|||
|
|
// ── 平台检测与指令集选择 ──────────────────────────────
|
|||
|
|
|
|||
|
|
#if defined(__AVX2__) || defined(__AVX__)
|
|||
|
|
#define VDE_SIMD_AVX 1
|
|||
|
|
#include <immintrin.h>
|
|||
|
|
#elif defined(__SSE2__) || defined(__SSE3__) || defined(__SSSE3__) || defined(__SSE4_1__)
|
|||
|
|
#define VDE_SIMD_SSE 1
|
|||
|
|
#include <emmintrin.h>
|
|||
|
|
#ifdef __SSE4_1__
|
|||
|
|
#include <smmintrin.h>
|
|||
|
|
#endif
|
|||
|
|
#elif defined(__ARM_NEON) || defined(__aarch64__)
|
|||
|
|
#define VDE_SIMD_NEON 1
|
|||
|
|
#include <arm_neon.h>
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
namespace vde::core {
|
|||
|
|
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
// 辅助：编译期指令集查询
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
/// 当前使用的 SIMD 指令集标签
|
|||
|
|
enum class SimdIsa {
|
|||
|
|
Scalar, ///< 纯标量回退
|
|||
|
|
SSE2, ///< SSE2 / SSE4.1
|
|||
|
|
AVX, ///< AVX / AVX2
|
|||
|
|
NEON ///< ARM NEON
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
/// 编译期查询当前 SIMD 指令集
|
|||
|
|
constexpr SimdIsa current_simd_isa() {
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
return SimdIsa::AVX;
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
return SimdIsa::SSE2;
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
return SimdIsa::NEON;
|
|||
|
|
#else
|
|||
|
|
return SimdIsa::Scalar;
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// SIMD 向量宽度（double lane 数）
|
|||
|
|
constexpr int simd_width_d = (current_simd_isa() >= SimdIsa::SSE2) ? 4 : 1;
|
|||
|
|
|
|||
|
|
/// SIMD 向量宽度（float lane 数）
|
|||
|
|
constexpr int simd_width_f = (current_simd_isa() >= SimdIsa::SSE2) ? 4 : 1;
|
|||
|
|
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
// Vec4d — 4-wide double SIMD
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
/// 4-wide double SIMD 向量
|
|||
|
|
///
|
|||
|
|
/// 支持: AVX (ymm), SSE (2×xmm), NEON (float64x2×2), Scalar 回退
|
|||
|
|
///
|
|||
|
|
/// @code
|
|||
|
|
/// Vec4d a = Vec4d::broadcast(1.0);
|
|||
|
|
/// Vec4d b(0, 1, 2, 3);
|
|||
|
|
/// Vec4d c = a + b;
|
|||
|
|
/// double dot = c.dot(b);
|
|||
|
|
/// @endcode
|
|||
|
|
struct alignas(32) Vec4d {
|
|||
|
|
// ── 内部存储（按指令集） ──
|
|||
|
|
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
__m256d m;
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
__m128d lo, hi;
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
float64x2_t lo, hi;
|
|||
|
|
#else
|
|||
|
|
double d[4];
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
// ── 构造函数 ──
|
|||
|
|
|
|||
|
|
/// 全零
|
|||
|
|
Vec4d() {
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
m = _mm256_setzero_pd();
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
lo = _mm_setzero_pd();
|
|||
|
|
hi = _mm_setzero_pd();
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
lo = vdupq_n_f64(0.0);
|
|||
|
|
hi = vdupq_n_f64(0.0);
|
|||
|
|
#else
|
|||
|
|
d[0] = d[1] = d[2] = d[3] = 0.0;
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 从四个标量构造
|
|||
|
|
Vec4d(double x, double y, double z, double w) {
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
m = _mm256_set_pd(w, z, y, x);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
lo = _mm_set_pd(y, x);
|
|||
|
|
hi = _mm_set_pd(w, z);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
double xy[2] = {x, y};
|
|||
|
|
double zw[2] = {z, w};
|
|||
|
|
lo = vld1q_f64(xy);
|
|||
|
|
hi = vld1q_f64(zw);
|
|||
|
|
#else
|
|||
|
|
d[0] = x; d[1] = y; d[2] = z; d[3] = w;
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 广播单值到所有 lane
|
|||
|
|
static Vec4d broadcast(double v) {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_set1_pd(v);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_set1_pd(v);
|
|||
|
|
r.hi = _mm_set1_pd(v);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.lo = vdupq_n_f64(v);
|
|||
|
|
r.hi = vdupq_n_f64(v);
|
|||
|
|
#else
|
|||
|
|
r.d[0] = r.d[1] = r.d[2] = r.d[3] = v;
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 提取第 i 个 lane (0-3)
|
|||
|
|
double operator[](int i) const {
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
double buf[4];
|
|||
|
|
_mm256_storeu_pd(buf, m);
|
|||
|
|
return buf[i];
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
double buf[4];
|
|||
|
|
_mm_storeu_pd(buf, lo);
|
|||
|
|
_mm_storeu_pd(buf + 2, hi);
|
|||
|
|
return buf[i];
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
double buf[4];
|
|||
|
|
vst1q_f64(buf, lo);
|
|||
|
|
vst1q_f64(buf + 2, hi);
|
|||
|
|
return buf[i];
|
|||
|
|
#else
|
|||
|
|
return d[i];
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ── 算术运算 ──
|
|||
|
|
|
|||
|
|
Vec4d operator+(const Vec4d& o) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_add_pd(m, o.m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_add_pd(lo, o.lo);
|
|||
|
|
r.hi = _mm_add_pd(hi, o.hi);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.lo = vaddq_f64(lo, o.lo);
|
|||
|
|
r.hi = vaddq_f64(hi, o.hi);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = d[i] + o.d[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4d operator-(const Vec4d& o) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_sub_pd(m, o.m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_sub_pd(lo, o.lo);
|
|||
|
|
r.hi = _mm_sub_pd(hi, o.hi);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.lo = vsubq_f64(lo, o.lo);
|
|||
|
|
r.hi = vsubq_f64(hi, o.hi);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = d[i] - o.d[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4d operator*(const Vec4d& o) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_mul_pd(m, o.m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_mul_pd(lo, o.lo);
|
|||
|
|
r.hi = _mm_mul_pd(hi, o.hi);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.lo = vmulq_f64(lo, o.lo);
|
|||
|
|
r.hi = vmulq_f64(hi, o.hi);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = d[i] * o.d[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4d operator/(const Vec4d& o) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_div_pd(m, o.m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_div_pd(lo, o.lo);
|
|||
|
|
r.hi = _mm_div_pd(hi, o.hi);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
// NEON 无原生 double 除法，使用 approximate + Newton
|
|||
|
|
float64x2_t inv_lo = vrecpeq_f64(o.lo);
|
|||
|
|
float64x2_t inv_hi = vrecpeq_f64(o.hi);
|
|||
|
|
r.lo = vmulq_f64(lo, inv_lo);
|
|||
|
|
r.hi = vmulq_f64(hi, inv_hi);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = d[i] / o.d[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// FMA: this * b + c
|
|||
|
|
Vec4d mul_add(const Vec4d& b, const Vec4d& c) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_fmadd_pd(m, b.m, c.m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_add_pd(_mm_mul_pd(lo, b.lo), c.lo);
|
|||
|
|
r.hi = _mm_add_pd(_mm_mul_pd(hi, b.hi), c.hi);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.lo = vmlaq_f64(c.lo, lo, b.lo);
|
|||
|
|
r.hi = vmlaq_f64(c.hi, hi, b.hi);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = d[i] * b.d[i] + c.d[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ── 比较 ──
|
|||
|
|
|
|||
|
|
/// lane-wise min
|
|||
|
|
Vec4d min(const Vec4d& o) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_min_pd(m, o.m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_min_pd(lo, o.lo);
|
|||
|
|
r.hi = _mm_min_pd(hi, o.hi);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.lo = vminq_f64(lo, o.lo);
|
|||
|
|
r.hi = vminq_f64(hi, o.hi);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = std::min(d[i], o.d[i]);
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// lane-wise max
|
|||
|
|
Vec4d max(const Vec4d& o) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_max_pd(m, o.m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r.lo = _mm_max_pd(lo, o.lo);
|
|||
|
|
r.hi = _mm_max_pd(hi, o.hi);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.lo = vmaxq_f64(lo, o.lo);
|
|||
|
|
r.hi = vmaxq_f64(hi, o.hi);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = std::max(d[i], o.d[i]);
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ── 向量数学 ──
|
|||
|
|
|
|||
|
|
/// 点积: sum(this[i] * o[i])
|
|||
|
|
double dot(const Vec4d& o) const {
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
__m256d prod = _mm256_mul_pd(m, o.m);
|
|||
|
|
__m128d lo128 = _mm256_castpd256_pd128(prod);
|
|||
|
|
__m128d hi128 = _mm256_extractf128_pd(prod, 1);
|
|||
|
|
__m128d sum = _mm_add_pd(lo128, hi128);
|
|||
|
|
sum = _mm_hadd_pd(sum, sum);
|
|||
|
|
double result;
|
|||
|
|
_mm_storel_pd(&result, sum);
|
|||
|
|
return result;
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
__m128d p0 = _mm_mul_pd(lo, o.lo);
|
|||
|
|
__m128d p1 = _mm_mul_pd(hi, o.hi);
|
|||
|
|
__m128d sum = _mm_add_pd(p0, p1);
|
|||
|
|
#ifdef __SSE3__
|
|||
|
|
sum = _mm_hadd_pd(sum, sum);
|
|||
|
|
#endif
|
|||
|
|
double result;
|
|||
|
|
_mm_storel_pd(&result, sum);
|
|||
|
|
return result;
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
float64x2_t p0 = vmulq_f64(lo, o.lo);
|
|||
|
|
float64x2_t p1 = vmulq_f64(hi, o.hi);
|
|||
|
|
float64x2_t s = vaddq_f64(p0, p1);
|
|||
|
|
return vgetq_lane_f64(s, 0) + vgetq_lane_f64(s, 1);
|
|||
|
|
#else
|
|||
|
|
return d[0]*o.d[0] + d[1]*o.d[1] + d[2]*o.d[2] + d[3]*o.d[3];
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 3D 叉积 (xyz, w unused): result = cross(this_xyz, o_xyz)
|
|||
|
|
/// 结果存储在 xyz, w=0
|
|||
|
|
Vec4d cross3(const Vec4d& o) const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
// a = this, b = o
|
|||
|
|
// cross(a,b) = [a1*b2-a2*b1, a2*b0-a0*b2, a0*b1-a1*b0, 0]
|
|||
|
|
// 使用 permute: _mm256_permute4x64_pd(src, imm8)
|
|||
|
|
// imm8 encodes: lane0=bits[1:0], lane1=bits[3:2], lane2=bits[5:4], lane3=bits[7:6]
|
|||
|
|
__m256d a_yzx = _mm256_permute4x64_pd(m, _MM_SHUFFLE(3, 0, 2, 1)); // a1,a2,a0,a3
|
|||
|
|
__m256d a_zxy = _mm256_permute4x64_pd(m, _MM_SHUFFLE(3, 1, 0, 2)); // a2,a0,a1,a3
|
|||
|
|
__m256d b_zxy = _mm256_permute4x64_pd(o.m, _MM_SHUFFLE(3, 1, 0, 2)); // b2,b0,b1,b3
|
|||
|
|
__m256d b_yzx = _mm256_permute4x64_pd(o.m, _MM_SHUFFLE(3, 0, 2, 1)); // b1,b2,b0,b3
|
|||
|
|
__m256d t1 = _mm256_mul_pd(a_yzx, b_zxy);
|
|||
|
|
__m256d t2 = _mm256_mul_pd(a_zxy, b_yzx);
|
|||
|
|
__m256d cross = _mm256_sub_pd(t1, t2);
|
|||
|
|
// 清空 w lane
|
|||
|
|
r.m = _mm256_blend_pd(cross, _mm256_setzero_pd(), 0b1000);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
// 使用标量回退取 xyz
|
|||
|
|
double ax = (*this)[0], ay = (*this)[1], az = (*this)[2];
|
|||
|
|
double bx = o[0], by = o[1], bz = o[2];
|
|||
|
|
r.lo = _mm_set_pd(az*bx - ax*bz, ay*bz - az*by);
|
|||
|
|
r.hi = _mm_setzero_pd();
|
|||
|
|
// hi[0] = ax*by - ay*bx
|
|||
|
|
r = Vec4d(ay*bz - az*by, az*bx - ax*bz, ax*by - ay*bx, 0.0);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
double ax = (*this)[0], ay = (*this)[1], az = (*this)[2];
|
|||
|
|
double bx = o[0], by = o[1], bz = o[2];
|
|||
|
|
double cx = ay*bz - az*by;
|
|||
|
|
double cy = az*bx - ax*bz;
|
|||
|
|
double cz = ax*by - ay*bx;
|
|||
|
|
double cxy[2] = {cx, cy};
|
|||
|
|
double czw[2] = {cz, 0.0};
|
|||
|
|
r.lo = vld1q_f64(cxy);
|
|||
|
|
r.hi = vld1q_f64(czw);
|
|||
|
|
#else
|
|||
|
|
double ax = d[0], ay = d[1], az = d[2];
|
|||
|
|
double bx = o.d[0], by = o.d[1], bz = o.d[2];
|
|||
|
|
r.d[0] = ay*bz - az*by;
|
|||
|
|
r.d[1] = az*bx - ax*bz;
|
|||
|
|
r.d[2] = ax*by - ay*bx;
|
|||
|
|
r.d[3] = 0.0;
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 向量长度 (xyz)
|
|||
|
|
double length3() const {
|
|||
|
|
return std::sqrt(dot(*this));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 向量长度平方 (xyz)
|
|||
|
|
double length3_sq() const {
|
|||
|
|
return dot(*this);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 归一化 (xyz), w 归零
|
|||
|
|
Vec4d normalize3() const {
|
|||
|
|
double len = length3();
|
|||
|
|
if (len < 1e-30) return broadcast(0.0);
|
|||
|
|
double inv = 1.0 / len;
|
|||
|
|
Vec4d r = *this * broadcast(inv);
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
r.m = _mm256_blend_pd(r.m, _mm256_setzero_pd(), 0b1000);
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 取前 3 个 lane 的绝对值
|
|||
|
|
Vec4d abs3() const {
|
|||
|
|
Vec4d r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
// _mm256_andnot_pd(-0.0, m) 清除符号位
|
|||
|
|
__m256d sign_mask = _mm256_set1_pd(-0.0);
|
|||
|
|
r.m = _mm256_andnot_pd(sign_mask, m);
|
|||
|
|
#elif VDE_SIMD_SSE
|
|||
|
|
r = Vec4d(std::abs((*this)[0]), std::abs((*this)[1]), std::abs((*this)[2]), std::abs((*this)[3]));
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.d[i] = std::abs(d[i]);
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
// Vec4f — 4-wide float SIMD
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
/// 4-wide float SIMD 向量
|
|||
|
|
///
|
|||
|
|
/// 支持: AVX (xmm 128-bit), SSE, NEON, Scalar
|
|||
|
|
struct alignas(16) Vec4f {
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
__m128 m;
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
float32x4_t m;
|
|||
|
|
#else
|
|||
|
|
float f[4];
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
Vec4f() {
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
m = _mm_setzero_ps();
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
m = vdupq_n_f32(0.0f);
|
|||
|
|
#else
|
|||
|
|
f[0] = f[1] = f[2] = f[3] = 0.0f;
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f(float x, float y, float z, float w) {
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
m = _mm_set_ps(w, z, y, x);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
float buf[4] = {x, y, z, w};
|
|||
|
|
m = vld1q_f32(buf);
|
|||
|
|
#else
|
|||
|
|
f[0] = x; f[1] = y; f[2] = z; f[3] = w;
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static Vec4f broadcast(float v) {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
r.m = _mm_set1_ps(v);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vdupq_n_f32(v);
|
|||
|
|
#else
|
|||
|
|
r.f[0] = r.f[1] = r.f[2] = r.f[3] = v;
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float operator[](int i) const {
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
float buf[4];
|
|||
|
|
_mm_storeu_ps(buf, m);
|
|||
|
|
return buf[i];
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
float buf[4];
|
|||
|
|
vst1q_f32(buf, m);
|
|||
|
|
return buf[i];
|
|||
|
|
#else
|
|||
|
|
return f[i];
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f operator+(const Vec4f& o) const {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
r.m = _mm_add_ps(m, o.m);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vaddq_f32(m, o.m);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.f[i] = f[i] + o.f[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f operator-(const Vec4f& o) const {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
r.m = _mm_sub_ps(m, o.m);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vsubq_f32(m, o.m);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.f[i] = f[i] - o.f[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f operator*(const Vec4f& o) const {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
r.m = _mm_mul_ps(m, o.m);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vmulq_f32(m, o.m);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.f[i] = f[i] * o.f[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f operator/(const Vec4f& o) const {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
r.m = _mm_div_ps(m, o.m);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vmulq_f32(m, vrecpeq_f32(o.m));
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.f[i] = f[i] / o.f[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f mul_add(const Vec4f& b, const Vec4f& c) const {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
#ifdef __FMA__
|
|||
|
|
r.m = _mm_fmadd_ps(m, b.m, c.m);
|
|||
|
|
#else
|
|||
|
|
r.m = _mm_add_ps(_mm_mul_ps(m, b.m), c.m);
|
|||
|
|
#endif
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vmlaq_f32(c.m, m, b.m);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.f[i] = f[i] * b.f[i] + c.f[i];
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f min(const Vec4f& o) const {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
r.m = _mm_min_ps(m, o.m);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vminq_f32(m, o.m);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.f[i] = std::min(f[i], o.f[i]);
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vec4f max(const Vec4f& o) const {
|
|||
|
|
Vec4f r;
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
r.m = _mm_max_ps(m, o.m);
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
r.m = vmaxq_f32(m, o.m);
|
|||
|
|
#else
|
|||
|
|
for (int i = 0; i < 4; ++i) r.f[i] = std::max(f[i], o.f[i]);
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 点积 (4-wide)
|
|||
|
|
float dot(const Vec4f& o) const {
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
#ifdef __SSE4_1__
|
|||
|
|
__m128 prod = _mm_mul_ps(m, o.m);
|
|||
|
|
__m128 hadd = _mm_hadd_ps(prod, prod);
|
|||
|
|
hadd = _mm_hadd_ps(hadd, hadd);
|
|||
|
|
return _mm_cvtss_f32(hadd);
|
|||
|
|
#else
|
|||
|
|
float buf[4];
|
|||
|
|
_mm_storeu_ps(buf, _mm_mul_ps(m, o.m));
|
|||
|
|
return buf[0] + buf[1] + buf[2] + buf[3];
|
|||
|
|
#endif
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
float32x4_t prod = vmulq_f32(m, o.m);
|
|||
|
|
float32x2_t sum = vadd_f32(vget_low_f32(prod), vget_high_f32(prod));
|
|||
|
|
sum = vpadd_f32(sum, sum);
|
|||
|
|
return vget_lane_f32(sum, 0);
|
|||
|
|
#else
|
|||
|
|
return f[0]*o.f[0] + f[1]*o.f[1] + f[2]*o.f[2] + f[3]*o.f[3];
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 3D dot (仅 xyz)
|
|||
|
|
float dot3(const Vec4f& o) const {
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
#ifdef __SSE4_1__
|
|||
|
|
// _mm_dp_ps: dot product with mask 0x71 = src1[xyz], src2[xyz] → splat result
|
|||
|
|
__m128 dp = _mm_dp_ps(m, o.m, 0x71);
|
|||
|
|
return _mm_cvtss_f32(dp);
|
|||
|
|
#else
|
|||
|
|
return (*this)[0]*o[0] + (*this)[1]*o[1] + (*this)[2]*o[2];
|
|||
|
|
#endif
|
|||
|
|
#elif VDE_SIMD_NEON
|
|||
|
|
float32x4_t prod = vmulq_f32(m, o.m);
|
|||
|
|
float32x2_t sum = vadd_f32(vget_low_f32(prod), vget_high_f32(prod));
|
|||
|
|
sum = vpadd_f32(sum, sum);
|
|||
|
|
// subtract w contribution
|
|||
|
|
float w = (*this)[3] * o[3];
|
|||
|
|
return vget_lane_f32(sum, 0) - w;
|
|||
|
|
#else
|
|||
|
|
return f[0]*o.f[0] + f[1]*o.f[1] + f[2]*o.f[2];
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 3D 叉积
|
|||
|
|
Vec4f cross3(const Vec4f& o) const {
|
|||
|
|
float ax = (*this)[0], ay = (*this)[1], az = (*this)[2];
|
|||
|
|
float bx = o[0], by = o[1], bz = o[2];
|
|||
|
|
return Vec4f(ay*bz - az*by, az*bx - ax*bz, ax*by - ay*bx, 0.0f);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float length3() const { return std::sqrt(dot3(*this)); }
|
|||
|
|
|
|||
|
|
float length3_sq() const { return dot3(*this); }
|
|||
|
|
|
|||
|
|
Vec4f normalize3() const {
|
|||
|
|
float len = length3();
|
|||
|
|
if (len < 1e-15f) return broadcast(0.0f);
|
|||
|
|
float inv = 1.0f / len;
|
|||
|
|
Vec4f r = *this * broadcast(inv);
|
|||
|
|
// 清空 w
|
|||
|
|
#if VDE_SIMD_AVX || VDE_SIMD_SSE
|
|||
|
|
#ifdef __SSE4_1__
|
|||
|
|
r.m = _mm_blend_ps(r.m, _mm_setzero_ps(), 0b1000);
|
|||
|
|
#endif
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
// SoA 布局运算（Structure of Arrays）
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
/// SoA 3D 向量数组（适合 SIMD 批量处理）
|
|||
|
|
struct alignas(64) Vec3SoA {
|
|||
|
|
Vec4d x, y, z; ///< 各分量 4 组
|
|||
|
|
|
|||
|
|
/// 从 4 个 Vec4d (xyz 部分) 组装
|
|||
|
|
static Vec3SoA gather(const Vec4d& a, const Vec4d& b,
|
|||
|
|
const Vec4d& c, const Vec4d& d) {
|
|||
|
|
Vec3SoA r;
|
|||
|
|
#if VDE_SIMD_AVX
|
|||
|
|
// 转置: a[0],b[0],c[0],d[0] → x
|
|||
|
|
__m256d t0 = _mm256_unpacklo_pd(a.m, b.m); // a0,a1,b0,b1
|
|||
|
|
__m256d t1 = _mm256_unpackhi_pd(a.m, b.m); // a2,a3,b2,b3
|
|||
|
|
__m256d t2 = _mm256_unpacklo_pd(c.m, d.m); // c0,c1,d0,d1
|
|||
|
|
__m256d t3 = _mm256_unpackhi_pd(c.m, d.m); // c2,c3,d2,d3
|
|||
|
|
r.x.m = _mm256_permute2f128_pd(t0, t2, 0x20); // a0,b0,c0,d0
|
|||
|
|
r.y.m = _mm256_permute2f128_pd(t0, t2, 0x31); // a1,b1,c1,d1
|
|||
|
|
r.z.m = _mm256_permute2f128_pd(t1, t3, 0x20); // a2,b2,c2,d2
|
|||
|
|
#else
|
|||
|
|
r.x = Vec4d(a[0], b[0], c[0], d[0]);
|
|||
|
|
r.y = Vec4d(a[1], b[1], c[1], d[1]);
|
|||
|
|
r.z = Vec4d(a[2], b[2], c[2], d[2]);
|
|||
|
|
#endif
|
|||
|
|
return r;
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
// AABB 批量相交测试（SIMD 加速）
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
/// 4 组 AABB 的 SIMD 表示
|
|||
|
|
///
|
|||
|
|
/// 用于批量测试：一次判断 4 个包围盒与参考 AABB 是否相交。
|
|||
|
|
/// 内部以 SoA 形式存储 min_xyz 和 max_xyz。
|
|||
|
|
struct alignas(64) BatchAABB4 {
|
|||
|
|
Vec4d min_x, min_y, min_z; ///< 4 组 AABB 的 min 分量
|
|||
|
|
Vec4d max_x, max_y, max_z; ///< 4 组 AABB 的 max 分量
|
|||
|
|
|
|||
|
|
/// 从 SoA 数组加载（min_x[4], min_y[4], min_z[4], max_x[4], max_y[4], max_z[4]）
|
|||
|
|
static BatchAABB4 load(const double* min_x, const double* min_y, const double* min_z,
|
|||
|
|
const double* max_x, const double* max_y, const double* max_z)
|
|||
|
|
{
|
|||
|
|
BatchAABB4 b;
|
|||
|
|
b.min_x = Vec4d(min_x[0], min_x[1], min_x[2], min_x[3]);
|
|||
|
|
b.min_y = Vec4d(min_y[0], min_y[1], min_y[2], min_y[3]);
|
|||
|
|
b.min_z = Vec4d(min_z[0], min_z[1], min_z[2], min_z[3]);
|
|||
|
|
b.max_x = Vec4d(max_x[0], max_x[1], max_x[2], max_x[3]);
|
|||
|
|
b.max_y = Vec4d(max_y[0], max_y[1], max_y[2], max_y[3]);
|
|||
|
|
b.max_z = Vec4d(max_z[0], max_z[1], max_z[2], max_z[3]);
|
|||
|
|
return b;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 批量测试 4 组 AABB 与参考 AABB 的相交性
|
|||
|
|
///
|
|||
|
|
/// @param ref_min_x/ref_min_y/ref_min_z 参考 AABB 最小值
|
|||
|
|
/// @param ref_max_x/ref_max_y/ref_max_z 参考 AABB 最大值
|
|||
|
|
/// @return 4 个 bool packed 为一个 uint32_t (bit 0-3)
|
|||
|
|
///
|
|||
|
|
/// AABB 相交公式: min_a <= max_b AND max_a >= min_b (各分量)
|
|||
|
|
static uint32_t intersect4(
|
|||
|
|
const BatchAABB4& batch,
|
|||
|
|
double ref_min_x, double ref_min_y, double ref_min_z,
|
|||
|
|
double ref_max_x, double ref_max_y, double ref_max_z)
|
|||
|
|
{
|
|||
|
|
uint32_t result = 0;
|
|||
|
|
|
|||
|
|
// 对每个 lane 做: batch.max >= ref.min && batch.min <= ref.max
|
|||
|
|
// 使用 max/min 比较 + 提取 lane
|
|||
|
|
for (int i = 0; i < 4; ++i) {
|
|||
|
|
double bmin_x = batch.min_x[i], bmin_y = batch.min_y[i], bmin_z = batch.min_z[i];
|
|||
|
|
double bmax_x = batch.max_x[i], bmax_y = batch.max_y[i], bmax_z = batch.max_z[i];
|
|||
|
|
if (bmin_x <= ref_max_x && bmax_x >= ref_min_x &&
|
|||
|
|
bmin_y <= ref_max_y && bmax_y >= ref_min_y &&
|
|||
|
|
bmin_z <= ref_max_z && bmax_z >= ref_min_z) {
|
|||
|
|
result |= (1u << i);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 单次 4-AABB 相交测试（SIMD 加速比较）
|
|||
|
|
/// 返回: 相交计数 (0-4)
|
|||
|
|
static int intersect4_count(
|
|||
|
|
const BatchAABB4& batch,
|
|||
|
|
double ref_min_x, double ref_min_y, double ref_min_z,
|
|||
|
|
double ref_max_x, double ref_max_y, double ref_max_z)
|
|||
|
|
{
|
|||
|
|
return __builtin_popcount(intersect4(batch,
|
|||
|
|
ref_min_x, ref_min_y, ref_min_z,
|
|||
|
|
ref_max_x, ref_max_y, ref_max_z));
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
// 批量 AABB 相交（标量优化版，适用任意 N）
|
|||
|
|
// ═══════════════════════════════════════════════════════════════════════════
|
|||
|
|
|
|||
|
|
/// 批量 AABB 相交测试 — 输出相交索引列表
|
|||
|
|
///
|
|||
|
|
/// @tparam N 缓冲区大小
|
|||
|
|
/// @param mins [N*3] min 坐标数组 (x0,y0,z0,x1,y1,z1,...)
|
|||
|
|
/// @param maxs [N*3] max 坐标数组
|
|||
|
|
/// @param ref_min_x/y/z 参考 AABB 最小值
|
|||
|
|
/// @param ref_max_x/y/z 参考 AABB 最大值
|
|||
|
|
/// @param out_indices 输出：相交的索引列表
|
|||
|
|
/// @return 相交数量
|
|||
|
|
template<size_t N>
|
|||
|
|
inline size_t aabb_intersect_batch(
|
|||
|
|
const double* mins, const double* maxs,
|
|||
|
|
double ref_min_x, double ref_min_y, double ref_min_z,
|
|||
|
|
double ref_max_x, double ref_max_y, double ref_max_z,
|
|||
|
|
int* out_indices)
|
|||
|
|
{
|
|||
|
|
size_t count = 0;
|
|||
|
|
for (size_t i = 0; i < N; ++i) {
|
|||
|
|
double mx = mins[i*3+0], my = mins[i*3+1], mz = mins[i*3+2];
|
|||
|
|
double Mx = maxs[i*3+0], My = maxs[i*3+1], Mz = maxs[i*3+2];
|
|||
|
|
if (mx <= ref_max_x && Mx >= ref_min_x &&
|
|||
|
|
my <= ref_max_y && My >= ref_min_y &&
|
|||
|
|
mz <= ref_max_z && Mz >= ref_min_z) {
|
|||
|
|
out_indices[count++] = static_cast<int>(i);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return count;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// 批量 AABB 相交（4 元素 SIMD 展开的 4N 变体）
|
|||
|
|
inline size_t aabb_intersect_batch_simd(
|
|||
|
|
const double* mins, const double* maxs,
|
|||
|
|
size_t count,
|
|||
|
|
double ref_min_x, double ref_min_y, double ref_min_z,
|
|||
|
|
double ref_max_x, double ref_max_y, double ref_max_z,
|
|||
|
|
int* out_indices)
|
|||
|
|
{
|
|||
|
|
size_t total = 0;
|
|||
|
|
size_t i = 0;
|
|||
|
|
|
|||
|
|
// 4-wide SIMD 循环
|
|||
|
|
for (; i + 4 <= count; i += 4) {
|
|||
|
|
double min_x[4], min_y[4], min_z[4];
|
|||
|
|
double max_x[4], max_y[4], max_z[4];
|
|||
|
|
for (int k = 0; k < 4; ++k) {
|
|||
|
|
min_x[k] = mins[(i+k)*3+0];
|
|||
|
|
min_y[k] = mins[(i+k)*3+1];
|
|||
|
|
min_z[k] = mins[(i+k)*3+2];
|
|||
|
|
max_x[k] = maxs[(i+k)*3+0];
|
|||
|
|
max_y[k] = maxs[(i+k)*3+1];
|
|||
|
|
max_z[k] = maxs[(i+k)*3+2];
|
|||
|
|
}
|
|||
|
|
BatchAABB4 batch = BatchAABB4::load(min_x, min_y, min_z, max_x, max_y, max_z);
|
|||
|
|
|
|||
|
|
uint32_t mask = BatchAABB4::intersect4(batch,
|
|||
|
|
ref_min_x, ref_min_y, ref_min_z,
|
|||
|
|
ref_max_x, ref_max_y, ref_max_z);
|
|||
|
|
|
|||
|
|
for (int j = 0; j < 4; ++j) {
|
|||
|
|
if (mask & (1u << j)) {
|
|||
|
|
out_indices[total++] = static_cast<int>(i + j);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 尾数标量处理
|
|||
|
|
for (; i < count; ++i) {
|
|||
|
|
double mx = mins[i*3+0], my = mins[i*3+1], mz = mins[i*3+2];
|
|||
|
|
double Mx = maxs[i*3+0], My = maxs[i*3+1], Mz = maxs[i*3+2];
|
|||
|
|
if (mx <= ref_max_x && Mx >= ref_min_x &&
|
|||
|
|
my <= ref_max_y && My >= ref_min_y &&
|
|||
|
|
mz <= ref_max_z && Mz >= ref_min_z) {
|
|||
|
|
out_indices[total++] = static_cast<int>(i);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return total;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} // namespace vde::core
|