SIMD portal intersection

portal.cpp

// A float which remains large when you modify its least significant bits
const Vec4f VEC4_LARGE_FLOAT = Vec4f::fromF32(8589934592);
const Vec4f VEC4_INDEX_MASK = Vec4f::fromI32(~1, ~1, 0, 0);
const Vec4f VEC4_INDEX_VALS = Vec4f::fromI32(0, 1, 0x50000000, 0x50000000);
const Vec4f VEC4_ZERO = Vec4f::fromF32(0.0f);


constexpr int BIG_FLOAT_INTEGER = 0x50000000;

template <size_t n, size_t i, size_t indexBits = 3> 
__m128 blendIndex(__m128 v)
{
    // n = number of lanes used
    // i = offset 
    constexpr int m = ~((1 << indexBits) - 1);
    constexpr int b = BIG_FLOAT_INTEGER;
    constexpr Vec4f maskTable[4] = { 
        Vec4f::fromI32(m,0,0,0),
        Vec4f::fromI32(m,m,0,0),
        Vec4f::fromI32(m,m,m,0),
        Vec4f::fromI32(m,m,m,m) 
    };
    constexpr Vec4f indexTable[4] = {
        Vec4f::fromI32(i+0,b,b,b),
        Vec4f::fromI32(i+0,i+1,b,b),
        Vec4f::fromI32(i+0,i+1,i+2,b),
        Vec4f::fromI32(i+0,i+1,i+2,i+3),
    };

    return _mm_or_ps(_mm_and_ps(v, maskTable[n-1]), indexTable[n-1]);
}

template <size_t indexBits = 3>
void extractMinAndIndex(Vec4f v, Vec4f &t, size_t &i)
{
    constexpr int m = (1 << indexBits) - 1;
    t = horizontalMin(v);
    i = (*reinterpret_cast<uint32_t*>(&t) & m);
}

__m128 computePortalHitTimes(const float portalX[], const float portalY[], const float portalZ[],  const Vec3x4 &rayOrigin, const Vec3x4 &rayDir)
{
    __m128 x = _mm_loadu_ps(portalX);
    __m128 y = _mm_loadu_ps(portalY);
    __m128 z = _mm_loadu_ps(portalZ);
    __m128 slope = dot4ps(rayDir.x, rayDir.y, rayDir.z, x, y, z);
    __m128 dist = dot4ps(rayOrigin.x, rayOrigin.y, rayOrigin.z, x, y, z);
    __m128 ltZero = _mm_cmplt_ps(slope, VEC4_ZERO);
    __m128 t = _mm_mul_ps(dist, _mm_rcp_ps(slope));
    __m128 tFrontFacing = _mm_blendv_ps(VEC4_LARGE_FLOAT, t, ltZero);
    return tFrontFacing;
}

__forceinline void castSphereRay(
    const float portalX[], const float portalY[], const float portalZ[], 
    const Vec3x4 &rayOrigin, const Vec3x4 &rayDir, size_t numPortals, size_t &indexMinOut, Vec4f &tMinOut)
{
    if (numPortals == 2)
    {
        __m128 tHit = computePortalHitTimes(portalX, portalY, portalZ, rayOrigin, rayDir);
        __m128 tBlended = blendIndex<2, 0, 1>(tHit);
        extractMinAndIndex<1>(tBlended, tMinOut, indexMinOut);
    }
    else if (numPortals == 3)
    {
        __m128 tHit = computePortalHitTimes(portalX, portalY, portalZ, rayOrigin, rayDir);
        __m128 tBlended = blendIndex<3, 0, 2>(tHit);
        extractMinAndIndex<2>(tBlended, tMinOut, indexMinOut);
    }
    else if (numPortals == 4)
    {
        __m128 tHit = computePortalHitTimes(portalX, portalY, portalZ, rayOrigin, rayDir);
        __m128 tBlended = blendIndex<4, 0, 2>(tHit);
        extractMinAndIndex<2>(tBlended, tMinOut, indexMinOut);
    }
    else if (numPortals == 5)
    {
        __m128 tHit0 = computePortalHitTimes(portalX+0, portalY+0, portalZ+0, rayOrigin, rayDir);
        __m128 tHit1 = computePortalHitTimes(portalX+4, portalY+4, portalZ+4, rayOrigin, rayDir);
        __m128 tBlended0 = blendIndex<4, 0, 3>(tHit0);
        __m128 tBlended1 = blendIndex<1, 4, 3>(tHit1);
        __m128 tBlended = _mm_min_ps(tBlended0, tBlended1);
        extractMinAndIndex<3>(tBlended, tMinOut, indexMinOut);
    }
    else if (numPortals == 6)
    {
        __m128 tHit0 = computePortalHitTimes(portalX+0, portalY+0, portalZ+0, rayOrigin, rayDir);
        __m128 tHit1 = computePortalHitTimes(portalX+4, portalY+4, portalZ+4, rayOrigin, rayDir);
        __m128 tBlended0 = blendIndex<4, 0, 3>(tHit0);
        __m128 tBlended1 = blendIndex<2, 4, 3>(tHit1);
        __m128 tBlended = _mm_min_ps(tBlended0, tBlended1);
        extractMinAndIndex<3>(tBlended, tMinOut, indexMinOut);
    }
    else if (numPortals == 7)
    {
        __m128 tHit0 = computePortalHitTimes(portalX+0, portalY+0, portalZ+0, rayOrigin, rayDir);
        __m128 tHit1 = computePortalHitTimes(portalX+4, portalY+4, portalZ+4, rayOrigin, rayDir);
        __m128 tBlended0 = blendIndex<4, 0, 3>(tHit0);
        __m128 tBlended1 = blendIndex<3, 4, 3>(tHit1);
        __m128 tBlended = _mm_min_ps(tBlended0, tBlended1);
        extractMinAndIndex<3>(tBlended, tMinOut, indexMinOut);
    }
    else if (numPortals == 8)
    {
        __m128 tHit0 = computePortalHitTimes(portalX+0, portalY+0, portalZ+0, rayOrigin, rayDir);
        __m128 tHit1 = computePortalHitTimes(portalX+4, portalY+4, portalZ+4, rayOrigin, rayDir);
        __m128 tBlended0 = blendIndex<4, 0, 3>(tHit0);
        __m128 tBlended1 = blendIndex<4, 4, 3>(tHit1);
        __m128 tBlended = _mm_min_ps(tBlended0, tBlended1);
        extractMinAndIndex<3>(tBlended, tMinOut, indexMinOut);
    }
    else
    {
        PANIC(0);
    }
}