310 lines
9.7 KiB
C
310 lines
9.7 KiB
C
// SPDX-FileCopyrightText: 2026 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
// Dirk Gregorius contributed portions of this code
|
|
|
|
#include "simd.h"
|
|
|
|
#include "platform.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
static b3AtomicInt b3_simdWidth;
|
|
|
|
#if defined( B3_SIMD_HAS_WIDTH_8 )
|
|
|
|
#if defined( _MSC_VER )
|
|
#include <intrin.h>
|
|
#endif
|
|
|
|
// This code enables runtime dispatch to AVX2.
|
|
// Inspired by https://github.com/simdjson/simdjson/blob/master/src/internal/isadetection.h
|
|
|
|
static void b3CpuId( unsigned int leaf, unsigned int subLeaf, unsigned int registers[4] )
|
|
{
|
|
#if defined( _MSC_VER )
|
|
int info[4];
|
|
__cpuidex( info, (int)leaf, (int)subLeaf );
|
|
registers[0] = (unsigned int)info[0];
|
|
registers[1] = (unsigned int)info[1];
|
|
registers[2] = (unsigned int)info[2];
|
|
registers[3] = (unsigned int)info[3];
|
|
#else
|
|
// clang cpuid.h can give the rbx swap register the same register as the leaf input, clobbering rbx
|
|
__asm__ volatile( "cpuid"
|
|
: "=a"( registers[0] ), "=b"( registers[1] ), "=c"( registers[2] ), "=d"( registers[3] )
|
|
: "a"( leaf ), "c"( subLeaf ) );
|
|
#endif
|
|
}
|
|
|
|
static uint64_t b3GetExtendedControlRegister( void )
|
|
{
|
|
#if defined( _MSC_VER ) && !defined( __clang__ )
|
|
return _xgetbv( 0 );
|
|
#else
|
|
uint32_t low, high;
|
|
__asm__ volatile( "xgetbv" : "=a"( low ), "=d"( high ) : "c"( 0 ) );
|
|
return ( (uint64_t)high << 32 ) | low;
|
|
#endif
|
|
}
|
|
|
|
static bool b3HasAVX2( void )
|
|
{
|
|
unsigned int registers[4];
|
|
b3CpuId( 0, 0, registers );
|
|
if ( registers[0] < 7 )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
b3CpuId( 1, 0, registers );
|
|
unsigned int osxsaveAndAvx = ( 1u << 27 ) | ( 1u << 28 );
|
|
if ( ( registers[2] & osxsaveAndAvx ) != osxsaveAndAvx )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if ( ( b3GetExtendedControlRegister() & 6 ) != 6 )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
b3CpuId( 7, 0, registers );
|
|
return ( registers[1] & ( 1u << 5 ) ) != 0;
|
|
}
|
|
|
|
#endif
|
|
|
|
static int b3DetectSIMDWidth( void )
|
|
{
|
|
#if defined( B3_SIMD_HAS_WIDTH_8 )
|
|
return b3HasAVX2() ? 8 : 4;
|
|
#else
|
|
return 4;
|
|
#endif
|
|
}
|
|
|
|
int b3GetSIMDWidth( void )
|
|
{
|
|
int width = b3AtomicLoadInt( &b3_simdWidth );
|
|
if ( width == 0 )
|
|
{
|
|
width = b3DetectSIMDWidth();
|
|
b3AtomicStoreInt( &b3_simdWidth, width );
|
|
}
|
|
|
|
return width;
|
|
}
|
|
|
|
bool b3IsAVX2Available( void )
|
|
{
|
|
return b3DetectSIMDWidth() == 8;
|
|
}
|
|
|
|
void b3SetSIMDWidth( int width )
|
|
{
|
|
B3_ASSERT( width == 0 || width == 4 || width == 8 );
|
|
if ( width == 8 && b3DetectSIMDWidth() != 8 )
|
|
{
|
|
width = 4;
|
|
}
|
|
|
|
b3AtomicStoreInt( &b3_simdWidth, width );
|
|
}
|
|
|
|
#if defined( B3_SIMD_SSE2 )
|
|
|
|
#define B3_TRANSPOSE3( C1, C2, C3 ) \
|
|
{ \
|
|
b3V32 T1 = _mm_unpacklo_ps( ( C1 ), ( C2 ) ); \
|
|
b3V32 T2 = _mm_unpackhi_ps( ( C1 ), ( C2 ) ); \
|
|
( C1 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 0, 0, 1, 0 ) ); \
|
|
( C2 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 1, 1, 3, 2 ) ); \
|
|
( C3 ) = _mm_shuffle_ps( ( T2 ), ( C3 ), _MM_SHUFFLE( 2, 2, 1, 0 ) ); \
|
|
}
|
|
|
|
static inline b3V32 b3SplatXV( b3V32 v )
|
|
{
|
|
return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 0, 0, 0, 0 ) );
|
|
}
|
|
|
|
static inline b3V32 b3SplatYV( b3V32 v )
|
|
{
|
|
return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 1, 1, 1 ) );
|
|
}
|
|
|
|
static inline b3V32 b3SplatZV( b3V32 v )
|
|
{
|
|
return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 2, 2, 2 ) );
|
|
}
|
|
|
|
static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b )
|
|
{
|
|
b3V32 v = _mm_cmpge_ps( a, b );
|
|
return ( _mm_movemask_ps( v ) & 0x07 ) != 0;
|
|
}
|
|
|
|
static inline b3V32 b3Dot3V( b3V32 a, b3V32 b )
|
|
{
|
|
b3V32 m = _mm_mul_ps( a, b );
|
|
b3V32 x = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 0, 0, 0, 0 ) );
|
|
b3V32 y = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 1, 1, 1, 1 ) );
|
|
b3V32 z = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 2, 2, 2, 2 ) );
|
|
|
|
return _mm_add_ps( _mm_add_ps( x, y ), z );
|
|
}
|
|
|
|
#else
|
|
|
|
#define B3_TRANSPOSE3( C1, C2, C3 ) \
|
|
{ \
|
|
float temp1 = C1.y; \
|
|
float temp2 = C1.z; \
|
|
float temp3 = C2.z; \
|
|
\
|
|
C1.y = C2.x; \
|
|
C1.z = C3.x; \
|
|
C2.z = C3.y; \
|
|
\
|
|
C2.x = temp1; \
|
|
C3.x = temp2; \
|
|
C3.y = temp3; \
|
|
}
|
|
|
|
static inline b3V32 b3SplatXV( b3V32 a )
|
|
{
|
|
return B3_LITERAL( b3V32 ){ a.x, a.x, a.x };
|
|
}
|
|
|
|
static inline b3V32 b3SplatYV( b3V32 a )
|
|
{
|
|
return B3_LITERAL( b3V32 ){ a.y, a.y, a.y };
|
|
}
|
|
|
|
static inline b3V32 b3SplatZV( b3V32 a )
|
|
{
|
|
return B3_LITERAL( b3V32 ){ a.z, a.z, a.z };
|
|
}
|
|
|
|
static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b )
|
|
{
|
|
return a.x >= b.x || a.y >= b.y || a.z >= b.z;
|
|
}
|
|
|
|
static inline b3V32 b3Dot3V( b3V32 a, b3V32 b )
|
|
{
|
|
float d = a.x * b.x + a.y * b.y + a.z * b.z;
|
|
return B3_LITERAL( b3V32 ){ d, d, d };
|
|
}
|
|
|
|
#endif
|
|
|
|
bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 )
|
|
{
|
|
b3V32 two = b3SplatV( 2.0f );
|
|
|
|
// Setup triangle
|
|
vertex1 = b3SubV( vertex1, nodeCenter );
|
|
vertex2 = b3SubV( vertex2, nodeCenter );
|
|
vertex3 = b3SubV( vertex3, nodeCenter );
|
|
|
|
// Face separation
|
|
b3V32 triangleMin = b3MinV( vertex1, b3MinV( vertex2, vertex3 ) );
|
|
b3V32 triangleMax = b3MaxV( vertex1, b3MaxV( vertex2, vertex3 ) );
|
|
|
|
b3V32 separation1 = b3SubV( triangleMin, nodeExtent );
|
|
b3V32 separation2 = b3AddV( triangleMax, nodeExtent );
|
|
|
|
b3V32 faceSeparation = b3MaxV( separation1, b3NegV( separation2 ) );
|
|
if ( b3AnyGreater3V( faceSeparation, b3_zeroV ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// SAT: Face separation
|
|
b3V32 edge1 = b3SubV( vertex2, vertex1 );
|
|
b3V32 edge2 = b3SubV( vertex3, vertex2 );
|
|
b3V32 edge3 = b3SubV( vertex1, vertex3 );
|
|
|
|
b3V32 normal = b3CrossV( edge1, edge2 );
|
|
|
|
b3V32 triangleSeparation = b3SubV( b3AbsV( b3Dot3V( normal, vertex1 ) ), b3Dot3V( b3AbsV( normal ), nodeExtent ) );
|
|
if ( b3AnyGreater3V( triangleSeparation, b3_zeroV ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// SAT: Edge separation
|
|
b3V32 edgeSeparation1 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge1, b3AddV( vertex1, vertex3 ) ) ), b3AbsV( b3CrossV( edge1, edge3 ) ) ),
|
|
b3MulV( two, b3ModifiedCrossV( b3AbsV( edge1 ), nodeExtent ) ) );
|
|
if ( b3AnyGreater3V( edgeSeparation1, b3_zeroV ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
b3V32 edgeSeparation2 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge2, b3AddV( vertex1, vertex2 ) ) ), b3AbsV( b3CrossV( edge2, edge1 ) ) ),
|
|
b3MulV( two, b3ModifiedCrossV( b3AbsV( edge2 ), nodeExtent ) ) );
|
|
if ( b3AnyGreater3V( edgeSeparation2, b3_zeroV ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
b3V32 edgeSeparation3 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge3, b3AddV( vertex2, vertex3 ) ) ), b3AbsV( b3CrossV( edge3, edge2 ) ) ),
|
|
b3MulV( two, b3ModifiedCrossV( b3AbsV( edge3 ), nodeExtent ) ) );
|
|
if ( b3AnyGreater3V( edgeSeparation3, b3_zeroV ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 )
|
|
{
|
|
// Test if ray intersects this triangle sharing same calculations for each triangle
|
|
{
|
|
b3V32 edge1 = b3SubV( vertex3, vertex2 );
|
|
b3V32 edge2 = b3SubV( vertex1, vertex3 );
|
|
b3V32 edge3 = b3SubV( vertex2, vertex1 );
|
|
|
|
b3V32 midPoint1 = b3MulV( b3_halfV, b3AddV( vertex2, vertex3 ) );
|
|
b3V32 midPoint2 = b3MulV( b3_halfV, b3AddV( vertex3, vertex1 ) );
|
|
b3V32 midPoint3 = b3MulV( b3_halfV, b3AddV( vertex1, vertex2 ) );
|
|
|
|
b3V32 normal1 = b3CrossV( edge1, b3SubV( midPoint1, rayStart ) );
|
|
b3V32 normal2 = b3CrossV( edge2, b3SubV( midPoint2, rayStart ) );
|
|
b3V32 normal3 = b3CrossV( edge3, b3SubV( midPoint3, rayStart ) );
|
|
B3_TRANSPOSE3( normal1, normal2, normal3 );
|
|
|
|
b3V32 rayDeltaX = b3SplatXV( rayDelta );
|
|
b3V32 rayDeltaY = b3SplatYV( rayDelta );
|
|
b3V32 rayDeltaZ = b3SplatZV( rayDelta );
|
|
|
|
b3V32 volumes = b3AddV( b3AddV( b3MulV( normal1, rayDeltaX ), b3MulV( normal2, rayDeltaY ) ), b3MulV( normal3, rayDeltaZ ) );
|
|
if ( b3AnyLess3V( volumes, b3_zeroV ) )
|
|
{
|
|
return 1.0f;
|
|
}
|
|
}
|
|
|
|
// Compute intersection with triangle plane
|
|
b3V32 edge1 = b3SubV( vertex2, vertex1 );
|
|
b3V32 edge2 = b3SubV( vertex3, vertex1 );
|
|
b3V32 normal = b3CrossV( edge1, edge2 );
|
|
|
|
b3V32 denominator = b3Dot3V( normal, rayDelta );
|
|
if ( b3AnyGreaterEq3V( denominator, b3_zeroV ) )
|
|
{
|
|
return 1.0f;
|
|
}
|
|
|
|
b3V32 lambda = b3DivV( b3Dot3V( normal, b3SubV( vertex1, rayStart ) ), denominator );
|
|
if ( b3AnyLessEq3V( lambda, b3_zeroV ) )
|
|
{
|
|
return 1.0f;
|
|
}
|
|
|
|
lambda = b3MinV( lambda, b3_oneV );
|
|
return b3GetXV( lambda );
|
|
}
|