SIMD hull collision (#93)
SIMD based hull collision inspired by example code from @cairnc in #54 <img width="1676" height="1332" alt="image" src="https://github.com/user-attachments/assets/58b137bd-bf7b-4c99-863c-64341b973a3d" /> - Fixed #78 - Fixed #85 and fixed CI to error on UB. - Updated benchmarks - Added AMD scalar benchmarks - Added Neon benchmarks - More determinism unit tests - Sample leak detection - Added b3Body_AllowFastRotation - Added b3Body_IsFastRotationAllowed - Bumped b3HullData version - Bumped b3CompoundData version
This commit is contained in:
parent
d421e45c82
commit
c37cfe4cd7
84 changed files with 3898 additions and 1151 deletions
|
|
@ -9,6 +9,10 @@ b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount )
|
|||
{
|
||||
B3_ASSERT( elementSize >= (int)sizeof( void* ) );
|
||||
|
||||
// The free list stores a pointer inside each element, so the stride must keep every element aligned.
|
||||
// Blocks come from b3Alloc, so a stride that is a multiple of B3_ALIGNMENT aligns all elements.
|
||||
elementSize = ( ( elementSize - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
|
||||
|
||||
b3BlockAllocator allocator = { 0 };
|
||||
|
||||
b3Array_Create( allocator.blocks );
|
||||
|
|
@ -81,6 +85,7 @@ void b3FreeElement( b3BlockAllocator* allocator, void* element )
|
|||
B3_ASSERT( allocator != NULL );
|
||||
B3_ASSERT( element != NULL );
|
||||
B3_ASSERT( allocator->allocationCount > 0 );
|
||||
B3_ASSERT( ( (uintptr_t)element & ( B3_ALIGNMENT - 1 ) ) == 0 );
|
||||
|
||||
allocator->allocationCount -= 1;
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ typedef struct b3BlockAllocator
|
|||
int allocationCount;
|
||||
} b3BlockAllocator;
|
||||
|
||||
// Element must be large enough to hold a pointer
|
||||
// Element must be large enough to hold a pointer. The element size is rounded up to B3_ALIGNMENT.
|
||||
b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount );
|
||||
void b3DestroyBlockAllocator( b3BlockAllocator* allocator );
|
||||
|
||||
|
|
|
|||
33
src/body.c
33
src/body.c
|
|
@ -253,8 +253,6 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def )
|
|||
bodyState->angularVelocity = def->angularVelocity;
|
||||
bodyState->deltaRotation = b3Quat_identity;
|
||||
bodyState->flags = bodySim->flags;
|
||||
|
||||
bodySim->maxAngularVelocity = b3Length( def->angularVelocity ) + 5.0f;
|
||||
}
|
||||
|
||||
if ( bodyId == world->bodies.count )
|
||||
|
|
@ -2305,6 +2303,37 @@ bool b3Body_IsBullet( b3BodyId bodyId )
|
|||
return ( body->flags & b3_isBullet ) != 0;
|
||||
}
|
||||
|
||||
void b3Body_AllowFastRotation(b3BodyId bodyId, bool flag)
|
||||
{
|
||||
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
||||
if ( world == NULL )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
B3_REC( world, BodyAllowFastRotation, bodyId, flag );
|
||||
|
||||
uint32_t newFlag = flag ? b3_allowFastRotation : 0;
|
||||
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
if ( ( body->flags & b3_allowFastRotation ) == newFlag )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
body->flags &= ~b3_allowFastRotation;
|
||||
body->flags |= newFlag;
|
||||
|
||||
b3SyncBodyFlags( world, body );
|
||||
}
|
||||
|
||||
bool b3Body_IsFastRotationAllowed(b3BodyId bodyId)
|
||||
{
|
||||
b3World* world = b3GetWorld( bodyId.world0 );
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
return ( body->flags & b3_allowFastRotation ) != 0;
|
||||
}
|
||||
|
||||
void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag )
|
||||
{
|
||||
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
||||
|
|
|
|||
|
|
@ -208,7 +208,6 @@ typedef struct b3BodySim
|
|||
|
||||
float minExtent;
|
||||
b3Vec3 maxExtent;
|
||||
float maxAngularVelocity;
|
||||
float linearDamping;
|
||||
float angularDamping;
|
||||
float gravityScale;
|
||||
|
|
|
|||
|
|
@ -276,7 +276,7 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
int sphereCount = def->sphereCount;
|
||||
b3CompoundSphere* sphereInstances = b3AllocZeroed( sphereCount * sizeof( b3CompoundSphere ) );
|
||||
|
||||
// Determine material capacity
|
||||
// Determine material capacity.
|
||||
int materialCapacity = convexCount;
|
||||
for ( int i = 0; i < def->meshCount; ++i )
|
||||
{
|
||||
|
|
@ -296,10 +296,10 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
const b3CompoundCapsuleDef* capsuleDef = def->capsules + i;
|
||||
capsuleInstances[i].capsule = capsuleDef->capsule;
|
||||
|
||||
// Look for an existing material
|
||||
// Look for an existing material.
|
||||
b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &capsuleDef->material, materialCount );
|
||||
|
||||
// Get the shared material index
|
||||
// Get the shared material index.
|
||||
int materialIndex = materialItr.data->val;
|
||||
capsuleInstances[i].materialIndex = materialIndex;
|
||||
|
||||
|
|
@ -333,10 +333,10 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex );
|
||||
childIndex += 1;
|
||||
|
||||
// Look for an existing material
|
||||
// Look for an existing material.
|
||||
b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &hullDef->material, materialCount );
|
||||
|
||||
// Get the shared material index
|
||||
// Get the shared material index.
|
||||
int materialIndex = materialItr.data->val;
|
||||
hullInstances[i].materialIndex = materialIndex;
|
||||
|
||||
|
|
@ -395,11 +395,11 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
|
||||
for ( int j = 0; j < meshDef->materialCount; ++j )
|
||||
{
|
||||
// Look for an existing material
|
||||
// Look for an existing material.
|
||||
b3MaterialMap_itr materialItr =
|
||||
b3MaterialMap_get_or_insert( &materialMap, &meshDef->materials[j], materialCount );
|
||||
|
||||
// Get the shared material index
|
||||
// Get the shared material index.
|
||||
int materialIndex = materialItr.data->val;
|
||||
meshInstances[i].materialIndices[j] = materialIndex;
|
||||
|
||||
|
|
@ -411,17 +411,17 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
}
|
||||
}
|
||||
|
||||
// Look for an existing matching mesh
|
||||
// Look for an existing matching mesh.
|
||||
b3MeshMap_itr itr = b3MeshMap_get_or_insert( &meshMap, meshData, sharedMeshCount );
|
||||
|
||||
// Get the shared mesh index
|
||||
// Get the shared mesh index.
|
||||
int sharedMeshIndex = itr.data->val;
|
||||
|
||||
// Create mesh instance
|
||||
// Create mesh instance.
|
||||
meshInstances[i].transform = def->meshes[i].transform;
|
||||
meshInstances[i].scale = def->meshes[i].scale;
|
||||
|
||||
// The offset isn't known yet, so store the index of the shared mesh
|
||||
// The offset isn't known yet, so store the index of the shared mesh.
|
||||
meshInstances[i].meshOffset = sharedMeshIndex;
|
||||
|
||||
// Is this a new mesh?
|
||||
|
|
@ -443,10 +443,10 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
const b3CompoundSphereDef* sphereDef = def->spheres + i;
|
||||
sphereInstances[i].sphere = sphereDef->sphere;
|
||||
|
||||
// Look for an existing material
|
||||
// Look for an existing material.
|
||||
b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &sphereDef->material, materialCount );
|
||||
|
||||
// Get the shared material index
|
||||
// Get the shared material index.
|
||||
int materialIndex = materialItr.data->val;
|
||||
sphereInstances[i].materialIndex = materialIndex;
|
||||
|
||||
|
|
@ -467,58 +467,55 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def )
|
|||
|
||||
b3DynamicTree_Rebuild( &tree, true );
|
||||
|
||||
int byteCount = sizeof( b3CompoundData );
|
||||
|
||||
// Tree nodes - todo 64 byte alignment
|
||||
int nodeOffset = byteCount;
|
||||
byteCount += tree.nodeCapacity * sizeof( b3TreeNode );
|
||||
|
||||
int materialOffset = byteCount;
|
||||
byteCount += materialCount * sizeof( b3SurfaceMaterial );
|
||||
|
||||
int capsuleOffset = byteCount;
|
||||
byteCount += def->capsuleCount * sizeof( b3CompoundCapsule );
|
||||
// Tree nodes
|
||||
size_t byteCount = b3AlignUp8( sizeof( b3CompoundData ) );
|
||||
int nodeOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( tree.nodeCapacity * sizeof( b3TreeNode ) );
|
||||
int materialOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( materialCount * sizeof( b3SurfaceMaterial ) );
|
||||
int capsuleOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( def->capsuleCount * sizeof( b3CompoundCapsule ) );
|
||||
|
||||
// Hull data layout has another level of indirection to allow for tight data packing
|
||||
// 1. hull instance array : hull count array of b3HullInstance with individual hull transforms and offsets
|
||||
// 2. heterogeneous array of shared hull data : each shared hull can have a different byte count, so direct indexing is not
|
||||
// possible
|
||||
int hullArrayOffset = byteCount;
|
||||
int hullArrayOffset = (int)byteCount;
|
||||
|
||||
// Array of hull instances
|
||||
byteCount += hullCount * sizeof( b3HullInstance );
|
||||
byteCount += b3AlignUp8( hullCount * sizeof( b3HullInstance ) );
|
||||
|
||||
// Packed shared hull blobs
|
||||
for ( int i = 0; i < sharedHullCount; ++i )
|
||||
{
|
||||
sharedHulls[i].hullOffset = byteCount;
|
||||
byteCount += sharedHulls[i].hull->byteCount;
|
||||
sharedHulls[i].hullOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( sharedHulls[i].hull->byteCount );
|
||||
}
|
||||
|
||||
// Mesh data layout has another level of indirection to allow for tight data packing
|
||||
// 1. mesh instance array : mesh count array of b3MeshInstance with individual mesh transform, scale, and offset
|
||||
// 2. heterogeneous array of shared mesh data : each shared mesh can have a different byte count, so direct indexing is not
|
||||
// possible
|
||||
int meshArrayOffset = byteCount;
|
||||
int meshArrayOffset = (int)byteCount;
|
||||
|
||||
// Array of mesh instances
|
||||
byteCount += meshCount * sizeof( b3MeshInstance );
|
||||
byteCount += b3AlignUp8( meshCount * sizeof( b3MeshInstance ) );
|
||||
|
||||
// Packed shared mesh blobs
|
||||
for ( int i = 0; i < sharedMeshCount; ++i )
|
||||
{
|
||||
sharedMeshes[i].meshOffset = byteCount;
|
||||
byteCount += sharedMeshes[i].meshData->byteCount;
|
||||
sharedMeshes[i].meshOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( sharedMeshes[i].meshData->byteCount );
|
||||
}
|
||||
|
||||
int sphereOffset = byteCount;
|
||||
byteCount += def->sphereCount * sizeof( b3CompoundSphere );
|
||||
int sphereOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( def->sphereCount * sizeof( b3CompoundSphere ) );
|
||||
|
||||
b3CompoundData* compound = b3Alloc( byteCount );
|
||||
memset( compound, 0, byteCount );
|
||||
|
||||
compound->version = B3_COMPOUND_VERSION;
|
||||
compound->byteCount = byteCount;
|
||||
compound->byteCount = (int)byteCount;
|
||||
compound->nodeOffset = nodeOffset;
|
||||
memcpy( &compound->tree, &tree, sizeof( b3DynamicTree ) );
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
#include "math_internal.h"
|
||||
#include "physics_world.h"
|
||||
#include "platform.h"
|
||||
#include "simd.h"
|
||||
#include "solver_set.h"
|
||||
|
||||
#if B3_ENABLE_VALIDATION
|
||||
|
|
@ -785,30 +786,6 @@ void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int work
|
|||
taskContext->hasHitEvents = hasHitEvents;
|
||||
}
|
||||
|
||||
#if defined( B3_SIMD_NEON )
|
||||
|
||||
#include <arm_neon.h>
|
||||
|
||||
// wide float holds 4 numbers
|
||||
typedef float32x4_t b3FloatW;
|
||||
|
||||
#elif defined( B3_SIMD_SSE2 )
|
||||
|
||||
#include <emmintrin.h>
|
||||
|
||||
// wide float holds 4 numbers
|
||||
typedef __m128 b3FloatW;
|
||||
|
||||
#else
|
||||
|
||||
// scalar math
|
||||
typedef struct b3FloatW
|
||||
{
|
||||
float x, y, z, w;
|
||||
} b3FloatW;
|
||||
|
||||
#endif
|
||||
|
||||
// Wide vec2
|
||||
typedef struct b3Vec2W
|
||||
{
|
||||
|
|
@ -840,372 +817,6 @@ typedef struct b3SymMatrix3W
|
|||
b3FloatW cxx, cxy, cxz, cyy, cyz, czz;
|
||||
} b3SymMatrix3W;
|
||||
|
||||
#if defined( B3_SIMD_NEON )
|
||||
|
||||
static inline b3FloatW b3ZeroW( void )
|
||||
{
|
||||
return vdupq_n_f32( 0.0f );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SplatW( float scalar )
|
||||
{
|
||||
return vdupq_n_f32( scalar );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
return vnegq_f32( a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SetW( float a, float b, float c, float d )
|
||||
{
|
||||
float32_t array[4] = { a, b, c, d };
|
||||
return vld1q_f32( array );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vaddq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vsubq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vmulq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vdivq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SqrtW( b3FloatW a )
|
||||
{
|
||||
return vsqrtq_f32( a );
|
||||
}
|
||||
|
||||
// Cannot use real FMA because it doesn't match the non-SIMD path
|
||||
static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
{
|
||||
return vaddq_f32( a, vmulq_f32( b, c ) );
|
||||
}
|
||||
|
||||
// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
//{
|
||||
// return vsubq_f32( a, vmulq_f32( b, c ) );
|
||||
// }
|
||||
|
||||
static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vminq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vmaxq_f32( a, b );
|
||||
}
|
||||
|
||||
// clamp a to [-b, b]
|
||||
static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW nb = b3NegW( b );
|
||||
b3FloatW c = b3MaxW( nb, a );
|
||||
return b3MinW( c, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vcgtq_f32( a, b ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vceqq_f32( a, b ) );
|
||||
}
|
||||
|
||||
static inline bool b3AllZeroW( b3FloatW a )
|
||||
{
|
||||
// Create a zero vector for comparison
|
||||
b3FloatW zero = vdupq_n_f32( 0.0f );
|
||||
|
||||
// Compare the input vector with zero
|
||||
uint32x4_t cmp_result = vceqq_f32( a, zero );
|
||||
|
||||
// Check if all comparison results are non-zero using vminvq
|
||||
#ifdef __ARM_FEATURE_SVE
|
||||
// ARM v8.2+ has horizontal minimum instruction
|
||||
return vminvq_u32( cmp_result ) != 0;
|
||||
#else
|
||||
// For older ARM architectures, we need to manually check all lanes
|
||||
return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 &&
|
||||
vgetq_lane_u32( cmp_result, 3 ) != 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
// component-wise returns mask ? b : a
|
||||
static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask )
|
||||
{
|
||||
uint32x4_t mask32 = vreinterpretq_u32_f32( mask );
|
||||
return vbslq_f32( mask32, b, a );
|
||||
}
|
||||
|
||||
#elif defined( B3_SIMD_SSE2 )
|
||||
|
||||
static inline b3FloatW b3ZeroW( void )
|
||||
{
|
||||
return _mm_setzero_ps();
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SplatW( float scalar )
|
||||
{
|
||||
return _mm_set1_ps( scalar );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
// Create a mask with the sign bit set for each element
|
||||
__m128 mask = _mm_set1_ps( -0.0f );
|
||||
|
||||
// XOR the input with the mask to negate each element
|
||||
return _mm_xor_ps( a, mask );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SetW( float a, float b, float c, float d )
|
||||
{
|
||||
return _mm_setr_ps( a, b, c, d );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_add_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_sub_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_mul_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_div_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SqrtW( b3FloatW a )
|
||||
{
|
||||
return _mm_sqrt_ps( a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
{
|
||||
return _mm_add_ps( a, _mm_mul_ps( b, c ) );
|
||||
}
|
||||
|
||||
// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
//{
|
||||
// return _mm_sub_ps( a, _mm_mul_ps( b, c ) );
|
||||
// }
|
||||
|
||||
static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_min_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_max_ps( a, b );
|
||||
}
|
||||
|
||||
// clamp a to [-b, b]
|
||||
static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW nb = b3NegW( b );
|
||||
b3FloatW c = b3MaxW( nb, a );
|
||||
return b3MinW( c, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_or_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_cmpgt_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_cmpeq_ps( a, b );
|
||||
}
|
||||
|
||||
static inline bool b3AllZeroW( b3FloatW a )
|
||||
{
|
||||
// Compare each element with zero
|
||||
b3FloatW zero = _mm_setzero_ps();
|
||||
b3FloatW cmp = _mm_cmpeq_ps( a, zero );
|
||||
|
||||
// Create a mask from the comparison results
|
||||
int mask = _mm_movemask_ps( cmp );
|
||||
|
||||
// If all elements are zero, the mask will be 0xF (1111 in binary)
|
||||
return mask == 0xF;
|
||||
}
|
||||
|
||||
// component-wise returns mask ? b : a
|
||||
static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask )
|
||||
{
|
||||
return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) );
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
static inline b3FloatW b3ZeroW( void )
|
||||
{
|
||||
return (b3FloatW){ 0.0f, 0.0f, 0.0f, 0.0f };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SplatW( float scalar )
|
||||
{
|
||||
return (b3FloatW){ scalar, scalar, scalar, scalar };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
return (b3FloatW){ -a.x, -a.y, -a.z, -a.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SqrtW( b3FloatW a )
|
||||
{
|
||||
return (b3FloatW){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
{
|
||||
return (b3FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w };
|
||||
}
|
||||
|
||||
// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
//{
|
||||
// return { a.x - b.x * c.x, a.y - b.y * c.y, a.z - b.z * c.z, a.w - b.w * c.w };
|
||||
// }
|
||||
|
||||
// static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b )
|
||||
//{
|
||||
// b3FloatW r;
|
||||
// r.x = a.x <= b.x ? a.x : b.x;
|
||||
// r.y = a.y <= b.y ? a.y : b.y;
|
||||
// r.z = a.z <= b.z ? a.z : b.z;
|
||||
// r.w = a.w <= b.w ? a.w : b.w;
|
||||
// return r;
|
||||
// }
|
||||
|
||||
static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x >= b.x ? a.x : b.x;
|
||||
r.y = a.y >= b.y ? a.y : b.y;
|
||||
r.z = a.z >= b.z ? a.z : b.z;
|
||||
r.w = a.w >= b.w ? a.w : b.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
// clamp a to [-b, b]
|
||||
static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x <= b.x ? a.x : b.x;
|
||||
r.y = a.y <= b.y ? a.y : b.y;
|
||||
r.z = a.z <= b.z ? a.z : b.z;
|
||||
r.w = a.w <= b.w ? a.w : b.w;
|
||||
r.x = r.x <= -b.x ? -b.x : r.x;
|
||||
r.y = r.y <= -b.y ? -b.y : r.y;
|
||||
r.z = r.z <= -b.z ? -b.z : r.z;
|
||||
r.w = r.w <= -b.w ? -b.w : r.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f;
|
||||
r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f;
|
||||
r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f;
|
||||
r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x > b.x ? 1.0f : 0.0f;
|
||||
r.y = a.y > b.y ? 1.0f : 0.0f;
|
||||
r.z = a.z > b.z ? 1.0f : 0.0f;
|
||||
r.w = a.w > b.w ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x == b.x ? 1.0f : 0.0f;
|
||||
r.y = a.y == b.y ? 1.0f : 0.0f;
|
||||
r.z = a.z == b.z ? 1.0f : 0.0f;
|
||||
r.w = a.w == b.w ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline bool b3AllZeroW( b3FloatW a )
|
||||
{
|
||||
return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f;
|
||||
}
|
||||
|
||||
// component-wise returns mask ? b : a
|
||||
static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = mask.x != 0.0f ? b.x : a.x;
|
||||
r.y = mask.y != 0.0f ? b.y : a.y;
|
||||
r.z = mask.z != 0.0f ? b.z : a.z;
|
||||
r.w = mask.w != 0.0f ? b.w : a.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// s * a
|
||||
static inline b3Vec3W b3MulSVW( b3FloatW s, b3Vec3W a )
|
||||
{
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -1382,7 +1382,7 @@ b3HeightFieldData* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bo
|
|||
}
|
||||
|
||||
b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency,
|
||||
bool makeHoles )
|
||||
bool makeHoles )
|
||||
{
|
||||
int heightCount = rowCount * columnCount;
|
||||
float* heights = (float*)b3Alloc( heightCount * sizeof( float ) );
|
||||
|
|
@ -1390,14 +1390,18 @@ b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, fl
|
|||
float omegaZ = 2.0f * B3_PI * rowFrequency;
|
||||
float omegaX = 2.0f * B3_PI * columnFrequency;
|
||||
|
||||
b3CosSin cs = { 0 };
|
||||
|
||||
for ( int i = 0; i < rowCount; ++i )
|
||||
{
|
||||
float rowHeight = sinf( omegaZ * i );
|
||||
cs = b3ComputeCosSin( omegaZ * i );
|
||||
float rowHeight = cs.sine;
|
||||
|
||||
for ( int j = 0; j < columnCount; ++j )
|
||||
{
|
||||
int k = i * columnCount + j;
|
||||
float columnHeight = sinf( omegaX * j );
|
||||
cs = b3ComputeCosSin( omegaX * j );
|
||||
float columnHeight = cs.sine;
|
||||
heights[k] = rowHeight * columnHeight;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
187
src/hull.c
187
src/hull.c
|
|
@ -1421,7 +1421,8 @@ static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int
|
|||
}
|
||||
|
||||
b3HullBuilder_ComputeTolerance( b, pointCount, shiftedPoints );
|
||||
if ( !b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints ) )
|
||||
bool haveInitialHull = b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints );
|
||||
if ( haveInitialHull == false )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
|
@ -1607,6 +1608,24 @@ static b3HullHalfEdge* b3GetHullEdgesWrite( b3HullData* hull )
|
|||
return (b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset );
|
||||
}
|
||||
|
||||
static float* b3GetHullSoaVerticesWrite( b3HullData* hull )
|
||||
{
|
||||
if ( hull->soaVertexOffset == 0 )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return (float*)( (intptr_t)hull + hull->soaVertexOffset );
|
||||
}
|
||||
|
||||
static float* b3GetHullSoaNormalsWrite( b3HullData* hull )
|
||||
{
|
||||
if ( hull->soaNormalOffset == 0 )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return (float*)( (intptr_t)hull + hull->soaNormalOffset );
|
||||
}
|
||||
|
||||
int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction )
|
||||
{
|
||||
int bestIndex = B3_NULL_INDEX;
|
||||
|
|
@ -2028,7 +2047,7 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
}
|
||||
|
||||
b3Vec3 origin = points[0];
|
||||
int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_HULL_LIMIT );
|
||||
int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_MAX_HULL_VERTICES );
|
||||
|
||||
// Single allocation for all working memory.
|
||||
b3HullWorkSizes sizes = b3ComputeHullWorkSizes( pointCount, clampedMaxCount );
|
||||
|
|
@ -2040,29 +2059,30 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
b3Vec3* shiftedPoints = (b3Vec3*)( work + sizes.offsetShiftedPoints );
|
||||
|
||||
bool ok = b3HullBuilder_Construct( &builder, points, pointCount, clampedMaxCount, origin, shiftedPoints );
|
||||
if ( !ok )
|
||||
if ( ok == false )
|
||||
{
|
||||
b3Free( work, sizes.totalBytes );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( builder.finalVertexCount >= B3_HULL_LIMIT )
|
||||
if ( builder.finalVertexCount >= B3_MAX_HULL_VERTICES )
|
||||
{
|
||||
b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_HULL_LIMIT );
|
||||
b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_MAX_HULL_VERTICES );
|
||||
b3Free( work, sizes.totalBytes );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( builder.finalFaceCount >= B3_HULL_LIMIT )
|
||||
if ( builder.finalFaceCount >= B3_MAX_HULL_FACES )
|
||||
{
|
||||
b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_HULL_LIMIT );
|
||||
b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_MAX_HULL_FACES );
|
||||
b3Free( work, sizes.totalBytes );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( builder.finalHalfEdgeCount >= B3_HULL_LIMIT )
|
||||
int maxHalfEdgeCount = 2 * B3_MAX_HULL_EDGES;
|
||||
if ( builder.finalHalfEdgeCount >= maxHalfEdgeCount )
|
||||
{
|
||||
b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, B3_HULL_LIMIT );
|
||||
b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, maxHalfEdgeCount );
|
||||
b3Free( work, sizes.totalBytes );
|
||||
return NULL;
|
||||
}
|
||||
|
|
@ -2112,6 +2132,9 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
while ( edge != face->edge );
|
||||
}
|
||||
|
||||
int soaVertexCount = ( vertexCount + 3 ) & ~3;
|
||||
int soaNormalCount = ( faceCount + 3 ) & ~3;
|
||||
|
||||
// Allocate the hull. Arrays hang off the end.
|
||||
size_t byteCount = b3AlignUp8( sizeof( b3HullData ) );
|
||||
int vertexOffset = (int)byteCount;
|
||||
|
|
@ -2120,10 +2143,14 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3Vec3 ) );
|
||||
int edgeOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( edgeCount * (int)sizeof( b3HullHalfEdge ) );
|
||||
int faceOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) );
|
||||
int planeOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( faceCount * (int)sizeof( b3Plane ) );
|
||||
int faceOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) );
|
||||
int soaVertexOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( 3 * soaVertexCount * (int)sizeof( float ) );
|
||||
int soaNormalOffset = (int)byteCount;
|
||||
byteCount += b3AlignUp8( 3 * soaNormalCount * (int)sizeof( float ) );
|
||||
|
||||
b3HullData* hull = b3Alloc( byteCount );
|
||||
memset( hull, 0, byteCount );
|
||||
|
|
@ -2132,8 +2159,10 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
hull->vertexOffset = vertexOffset;
|
||||
hull->pointOffset = pointOffset;
|
||||
hull->edgeOffset = edgeOffset;
|
||||
hull->faceOffset = faceOffset;
|
||||
hull->planeOffset = planeOffset;
|
||||
hull->faceOffset = faceOffset;
|
||||
hull->soaVertexOffset = soaVertexOffset;
|
||||
hull->soaNormalOffset = soaNormalOffset;
|
||||
|
||||
hull->vertexCount = vertexCount;
|
||||
hull->edgeCount = edgeCount;
|
||||
|
|
@ -2146,11 +2175,29 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
b3HullFace* faces = (b3HullFace*)( (intptr_t)hull + hull->faceOffset );
|
||||
b3Vec3* finalPoints = b3GetHullPointsWrite( hull );
|
||||
b3Plane* planes = b3GetHullPlanesWrite( hull );
|
||||
float* soaVertices = b3GetHullSoaVerticesWrite( hull );
|
||||
float* soaNormals = b3GetHullSoaNormalsWrite( hull );
|
||||
|
||||
float* vx = soaVertices;
|
||||
float* vy = vx + soaVertexCount;
|
||||
float* vz = vy + soaVertexCount;
|
||||
|
||||
for ( int index = 0; index < vertexCount; ++index )
|
||||
{
|
||||
vertices[index].edge = 0;
|
||||
finalPoints[index] = tempVertices[index]->position;
|
||||
b3Vec3 p = tempVertices[index]->position;
|
||||
finalPoints[index] = p;
|
||||
|
||||
vx[index] = p.x;
|
||||
vy[index] = p.y;
|
||||
vz[index] = p.z;
|
||||
}
|
||||
|
||||
for ( int index = vertexCount; index < soaVertexCount; ++index )
|
||||
{
|
||||
vx[index] = vx[0];
|
||||
vy[index] = vy[0];
|
||||
vz[index] = vz[0];
|
||||
}
|
||||
|
||||
for ( int index = 0; index < edgeCount; ++index )
|
||||
|
|
@ -2169,6 +2216,10 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
vertices[edge->origin->finalIndex].edge = (uint8_t)index;
|
||||
}
|
||||
|
||||
float* nx = soaNormals;
|
||||
float* ny = nx + soaNormalCount;
|
||||
float* nz = ny + soaNormalCount;
|
||||
|
||||
for ( int index = 0; index < faceCount; ++index )
|
||||
{
|
||||
const b3QHFace* face = tempFaces[index];
|
||||
|
|
@ -2176,6 +2227,18 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
|
||||
faces[index].edge = (uint8_t)face->edge->finalIndex;
|
||||
planes[index] = face->plane;
|
||||
|
||||
b3Vec3 n = face->plane.normal;
|
||||
nx[index] = n.x;
|
||||
ny[index] = n.y;
|
||||
nz[index] = n.z;
|
||||
}
|
||||
|
||||
for ( int index = faceCount; index < soaNormalCount; ++index )
|
||||
{
|
||||
nx[index] = 0.0f;
|
||||
ny[index] = 0.0f;
|
||||
nz[index] = 0.0f;
|
||||
}
|
||||
|
||||
// All builder pointers are dead from here on.
|
||||
|
|
@ -2238,8 +2301,8 @@ bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 )
|
|||
|
||||
// Hull identity covers every byte, so the structs carry explicit padding. These lock
|
||||
// the layout, re-audit padding if a size changes.
|
||||
_Static_assert( sizeof( b3HullData ) == 136, "unexpected hull data size" );
|
||||
_Static_assert( sizeof( b3BoxHull ) == 440, "unexpected box hull size" );
|
||||
_Static_assert( sizeof( b3HullData ) == 144, "unexpected hull data size" );
|
||||
_Static_assert( sizeof( b3BoxHull ) == 648, "unexpected box hull size" );
|
||||
|
||||
#define NAME b3HullMap
|
||||
#define KEY_TY const b3HullData*
|
||||
|
|
@ -2269,7 +2332,7 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra
|
|||
return NULL;
|
||||
}
|
||||
|
||||
b3HullData* hull = (b3HullData*)b3Alloc( original->byteCount );
|
||||
b3HullData* hull = b3Alloc( original->byteCount );
|
||||
memcpy( hull, original, original->byteCount );
|
||||
|
||||
b3Vec3 safeScale = b3SafeScale( scale );
|
||||
|
|
@ -2339,12 +2402,32 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra
|
|||
b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q );
|
||||
b3Vec3* points = b3GetHullPointsWrite( hull );
|
||||
|
||||
int soaVertexCount = ( vertexCount + 3 ) & ~3;
|
||||
float* vx = b3GetHullSoaVerticesWrite( hull );
|
||||
float* vy = vx + soaVertexCount;
|
||||
float* vz = vy + soaVertexCount;
|
||||
|
||||
for ( int i = 0; i < vertexCount; ++i )
|
||||
{
|
||||
points[i] = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p );
|
||||
b3Vec3 p = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p );
|
||||
points[i] = p;
|
||||
vx[i] = p.x;
|
||||
vy[i] = p.y;
|
||||
vz[i] = p.z;
|
||||
}
|
||||
|
||||
for ( int i = vertexCount; i < soaVertexCount; ++i )
|
||||
{
|
||||
vx[i] = vx[0];
|
||||
vy[i] = vy[0];
|
||||
vz[i] = vz[0];
|
||||
}
|
||||
|
||||
b3Plane* planes = b3GetHullPlanesWrite( hull );
|
||||
int soaNormalCount = ( faceCount + 3 ) & ~3;
|
||||
float* nx = b3GetHullSoaNormalsWrite( hull );
|
||||
float* ny = nx + soaNormalCount;
|
||||
float* nz = ny + soaNormalCount;
|
||||
|
||||
for ( int i = 0; i < faceCount; ++i )
|
||||
{
|
||||
|
|
@ -2390,6 +2473,17 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra
|
|||
normal = b3MulSV( 1.0f / area, normal );
|
||||
|
||||
planes[i] = b3MakePlaneFromNormalAndPoint( normal, centroid );
|
||||
|
||||
nx[i] = normal.x;
|
||||
ny[i] = normal.y;
|
||||
nz[i] = normal.z;
|
||||
}
|
||||
|
||||
for ( int i = faceCount; i < soaNormalCount; ++i )
|
||||
{
|
||||
nx[i] = 0.0f;
|
||||
ny[i] = 0.0f;
|
||||
nz[i] = 0.0f;
|
||||
}
|
||||
|
||||
b3UpdateHullBounds( hull );
|
||||
|
|
@ -2641,8 +2735,10 @@ static const b3BoxHull s_boxHull = {
|
|||
.vertexOffset = offsetof( b3BoxHull, boxVertices ),
|
||||
.pointOffset = offsetof( b3BoxHull, boxPoints ),
|
||||
.edgeOffset = offsetof( b3BoxHull, boxEdges ),
|
||||
.faceOffset = offsetof( b3BoxHull, boxFaces ),
|
||||
.planeOffset = offsetof( b3BoxHull, boxPlanes ),
|
||||
.faceOffset = offsetof( b3BoxHull, boxFaces ),
|
||||
.soaVertexOffset = offsetof( b3BoxHull, vx ),
|
||||
.soaNormalOffset = offsetof( b3BoxHull, nx ),
|
||||
},
|
||||
.boxVertices =
|
||||
{
|
||||
|
|
@ -2710,6 +2806,61 @@ b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform tr
|
|||
boxHull.boxPoints[6] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, -h.z } );
|
||||
boxHull.boxPoints[7] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, -h.z } );
|
||||
|
||||
// SOA
|
||||
boxHull.vx[0] = boxHull.boxPoints[0].x;
|
||||
boxHull.vx[1] = boxHull.boxPoints[1].x;
|
||||
boxHull.vx[2] = boxHull.boxPoints[2].x;
|
||||
boxHull.vx[3] = boxHull.boxPoints[3].x;
|
||||
boxHull.vx[4] = boxHull.boxPoints[4].x;
|
||||
boxHull.vx[5] = boxHull.boxPoints[5].x;
|
||||
boxHull.vx[6] = boxHull.boxPoints[6].x;
|
||||
boxHull.vx[7] = boxHull.boxPoints[7].x;
|
||||
|
||||
boxHull.vy[0] = boxHull.boxPoints[0].y;
|
||||
boxHull.vy[1] = boxHull.boxPoints[1].y;
|
||||
boxHull.vy[2] = boxHull.boxPoints[2].y;
|
||||
boxHull.vy[3] = boxHull.boxPoints[3].y;
|
||||
boxHull.vy[4] = boxHull.boxPoints[4].y;
|
||||
boxHull.vy[5] = boxHull.boxPoints[5].y;
|
||||
boxHull.vy[6] = boxHull.boxPoints[6].y;
|
||||
boxHull.vy[7] = boxHull.boxPoints[7].y;
|
||||
|
||||
boxHull.vz[0] = boxHull.boxPoints[0].z;
|
||||
boxHull.vz[1] = boxHull.boxPoints[1].z;
|
||||
boxHull.vz[2] = boxHull.boxPoints[2].z;
|
||||
boxHull.vz[3] = boxHull.boxPoints[3].z;
|
||||
boxHull.vz[4] = boxHull.boxPoints[4].z;
|
||||
boxHull.vz[5] = boxHull.boxPoints[5].z;
|
||||
boxHull.vz[6] = boxHull.boxPoints[6].z;
|
||||
boxHull.vz[7] = boxHull.boxPoints[7].z;
|
||||
|
||||
boxHull.nx[0] = boxHull.boxPlanes[0].normal.x;
|
||||
boxHull.nx[1] = boxHull.boxPlanes[1].normal.x;
|
||||
boxHull.nx[2] = boxHull.boxPlanes[2].normal.x;
|
||||
boxHull.nx[3] = boxHull.boxPlanes[3].normal.x;
|
||||
boxHull.nx[4] = boxHull.boxPlanes[4].normal.x;
|
||||
boxHull.nx[5] = boxHull.boxPlanes[5].normal.x;
|
||||
boxHull.nx[6] = 0.0f;
|
||||
boxHull.nx[7] = 0.0f;
|
||||
|
||||
boxHull.ny[0] = boxHull.boxPlanes[0].normal.y;
|
||||
boxHull.ny[1] = boxHull.boxPlanes[1].normal.y;
|
||||
boxHull.ny[2] = boxHull.boxPlanes[2].normal.y;
|
||||
boxHull.ny[3] = boxHull.boxPlanes[3].normal.y;
|
||||
boxHull.ny[4] = boxHull.boxPlanes[4].normal.y;
|
||||
boxHull.ny[5] = boxHull.boxPlanes[5].normal.y;
|
||||
boxHull.ny[6] = 0.0f;
|
||||
boxHull.ny[7] = 0.0f;
|
||||
|
||||
boxHull.nz[0] = boxHull.boxPlanes[0].normal.z;
|
||||
boxHull.nz[1] = boxHull.boxPlanes[1].normal.z;
|
||||
boxHull.nz[2] = boxHull.boxPlanes[2].normal.z;
|
||||
boxHull.nz[3] = boxHull.boxPlanes[3].normal.z;
|
||||
boxHull.nz[4] = boxHull.boxPlanes[4].normal.z;
|
||||
boxHull.nz[5] = boxHull.boxPlanes[5].normal.z;
|
||||
boxHull.nz[6] = 0.0f;
|
||||
boxHull.nz[7] = 0.0f;
|
||||
|
||||
boxHull.base.hash = 0;
|
||||
boxHull.base.hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)&boxHull, sizeof( b3BoxHull ) ) );
|
||||
|
||||
|
|
|
|||
|
|
@ -23,6 +23,15 @@ typedef struct b3EdgeQuery
|
|||
int indexB;
|
||||
} b3EdgeQuery;
|
||||
|
||||
typedef struct b3AxisQuery
|
||||
{
|
||||
b3Vec3 normal;
|
||||
float separation;
|
||||
int indexA;
|
||||
int indexB;
|
||||
b3SeparatingFeature type;
|
||||
} b3AxisQuery;
|
||||
|
||||
typedef struct b3ClipVertex
|
||||
{
|
||||
b3Vec3 position;
|
||||
|
|
@ -43,6 +52,8 @@ b3FeaturePair b3FlipPair( b3FeaturePair pair );
|
|||
|
||||
int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane );
|
||||
|
||||
b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride );
|
||||
|
||||
#if B3_ENABLE_VALIDATION
|
||||
bool b3ValidatePolygon( b3ClipVertex* polygon, int count );
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@ b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max );
|
|||
int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis );
|
||||
int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis );
|
||||
|
||||
// Align up to 8 byte alignment.
|
||||
static inline size_t b3AlignUp8( size_t x )
|
||||
{
|
||||
return ( x + 7u ) & ~(size_t)7u;
|
||||
|
|
|
|||
|
|
@ -1322,14 +1322,17 @@ b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amp
|
|||
float omegaX = 2.0f * B3_PI * columnFrequency * cellWidth;
|
||||
|
||||
float x = -0.5f * xWidth;
|
||||
b3CosSin cs = { 0 };
|
||||
for ( int ix = 0; ix <= xCount; ++ix )
|
||||
{
|
||||
float rowHeight = sinf( omegaX * ix );
|
||||
cs = b3ComputeCosSin( omegaX * ix );
|
||||
float rowHeight = cs.sine;
|
||||
|
||||
float z = -0.5f * zWidth;
|
||||
for ( int iz = 0; iz <= zCount; ++iz )
|
||||
{
|
||||
float columnHeight = sinf( omegaZ * iz );
|
||||
cs = b3ComputeCosSin( omegaZ * iz );
|
||||
float columnHeight = cs.sine;
|
||||
|
||||
float y = amplitude * rowHeight * columnHeight;
|
||||
vertices.data[index] = (b3Vec3){ x, y, z };
|
||||
|
|
@ -1483,7 +1486,7 @@ b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges )
|
|||
return b3CreateMesh( &def, NULL, 0 );
|
||||
}
|
||||
|
||||
b3MeshData* b3CreateHollowBoxMesh(b3Vec3 center, b3Vec3 extent)
|
||||
b3MeshData* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent )
|
||||
{
|
||||
float x = extent.x;
|
||||
float y = extent.y;
|
||||
|
|
|
|||
|
|
@ -77,7 +77,8 @@ void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCou
|
|||
// Target multiple blocks per worker to reduce thread stalls.
|
||||
// block size grows once items exceed maxBlockCount * minRange
|
||||
// so the block count stays bounded and per-block sync overhead stays low.
|
||||
int blocksPerWorker = 4;
|
||||
// Benchmarking shows 32 is optimal for the convex pile benchmark and others.
|
||||
int blocksPerWorker = 32;
|
||||
int maxBlockCount = blocksPerWorker * workerCount;
|
||||
|
||||
int blockSize;
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@ B3_REC_OP( 0x36, BodySetMotionLocks, RET_NONE, ARG( BODYID, body ) ARG( LOCKS, l
|
|||
B3_REC_OP( 0x37, BodySetBullet, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
|
||||
B3_REC_OP( 0x38, BodyEnableContactRecycling, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
|
||||
B3_REC_OP( 0x39, BodyEnableHitEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
|
||||
B3_REC_OP( 0x3A, BodyAllowFastRotation, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
|
||||
|
||||
// Shape create/destroy
|
||||
B3_REC_OP( 0x40, CreateSphereShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( SPHERE, sphere ) )
|
||||
|
|
|
|||
|
|
@ -957,6 +957,11 @@ static void b3RecDispatch_BodySetBullet( const b3RecArgs_BodySetBullet* a, b3Rec
|
|||
b3Body_SetBullet( b3RecMakeBodyId( rdr, a->body ), a->flag );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_BodyAllowFastRotation( const b3RecArgs_BodyAllowFastRotation* a, b3RecReader* rdr )
|
||||
{
|
||||
b3Body_AllowFastRotation( b3RecMakeBodyId( rdr, a->body ), a->flag );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_BodyEnableContactRecycling( const b3RecArgs_BodyEnableContactRecycling* a, b3RecReader* rdr )
|
||||
{
|
||||
b3Body_EnableContactRecycling( b3RecMakeBodyId( rdr, a->body ), a->flag );
|
||||
|
|
|
|||
568
src/simd.h
568
src/simd.h
|
|
@ -7,10 +7,35 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
|
||||
#if defined( B3_SIMD_SSE2 )
|
||||
#if defined( B3_SIMD_NEON )
|
||||
|
||||
#include <arm_neon.h>
|
||||
|
||||
// wide float holds 4 numbers
|
||||
typedef float32x4_t b3FloatW;
|
||||
|
||||
#elif defined( B3_SIMD_SSE2 )
|
||||
|
||||
#include <emmintrin.h>
|
||||
|
||||
// wide float holds 4 numbers
|
||||
typedef __m128 b3FloatW;
|
||||
|
||||
#else
|
||||
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
// scalar math
|
||||
typedef struct b3FloatW
|
||||
{
|
||||
float x, y, z, w;
|
||||
} b3FloatW;
|
||||
|
||||
#endif
|
||||
|
||||
#if defined( B3_SIMD_SSE2 )
|
||||
|
||||
// wide float holds 4 numbers
|
||||
typedef __m128 b3V32;
|
||||
|
||||
|
|
@ -355,3 +380,544 @@ static inline bool b3TestBoundsRayOverlap( b3V32 nodeMin, b3V32 nodeMax, b3V32 r
|
|||
|
||||
bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 );
|
||||
float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 );
|
||||
|
||||
#if defined( B3_SIMD_NEON )
|
||||
|
||||
static inline b3FloatW b3ZeroW( void )
|
||||
{
|
||||
return vdupq_n_f32( 0.0f );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SplatW( float scalar )
|
||||
{
|
||||
return vdupq_n_f32( scalar );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SetW( float a, float b, float c, float d )
|
||||
{
|
||||
float32_t array[4] = { a, b, c, d };
|
||||
return vld1q_f32( array );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3LoadW( const float* data )
|
||||
{
|
||||
return vld1q_f32( data );
|
||||
}
|
||||
|
||||
static inline void b3StoreW( float* data, b3FloatW a )
|
||||
{
|
||||
vst1q_f32( data, a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
return vnegq_f32( a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vaddq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vsubq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vmulq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vdivq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SqrtW( b3FloatW a )
|
||||
{
|
||||
return vsqrtq_f32( a );
|
||||
}
|
||||
|
||||
// Cannot use real FMA because it doesn't match the non-SIMD path
|
||||
static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
{
|
||||
return vaddq_f32( a, vmulq_f32( b, c ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vminq_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vmaxq_f32( a, b );
|
||||
}
|
||||
|
||||
// clamp a to [-b, b]
|
||||
static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW nb = b3NegW( b );
|
||||
b3FloatW c = b3MaxW( nb, a );
|
||||
return b3MinW( c, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AndW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vcgtq_f32( a, b ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vcltq_f32( a, b ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vreinterpretq_f32_u32( vceqq_f32( a, b ) );
|
||||
}
|
||||
|
||||
static inline bool b3AllZeroW( b3FloatW a )
|
||||
{
|
||||
// Create a zero vector for comparison
|
||||
b3FloatW zero = vdupq_n_f32( 0.0f );
|
||||
|
||||
// Compare the input vector with zero
|
||||
uint32x4_t cmp_result = vceqq_f32( a, zero );
|
||||
|
||||
// Check if all comparison results are non-zero using vminvq
|
||||
#ifdef __ARM_FEATURE_SVE
|
||||
// ARM v8.2+ has horizontal minimum instruction
|
||||
return vminvq_u32( cmp_result ) != 0;
|
||||
#else
|
||||
// For older ARM architectures, we need to manually check all lanes
|
||||
return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 &&
|
||||
vgetq_lane_u32( cmp_result, 3 ) != 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
// _mm_movemask_ps equivalent, compatible with ARM v7.
|
||||
static inline bool b3AnyTrueW( b3FloatW mask )
|
||||
{
|
||||
uint32x4_t m = vreinterpretq_u32_f32( mask );
|
||||
uint32x2_t p = vorr_u32( vget_low_u32( m ), vget_high_u32( m ) );
|
||||
return ( vget_lane_u32( p, 0 ) | vget_lane_u32( p, 1 ) ) != 0;
|
||||
}
|
||||
|
||||
// component-wise returns mask ? b : a
|
||||
static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask )
|
||||
{
|
||||
uint32x4_t mask32 = vreinterpretq_u32_f32( mask );
|
||||
return vbslq_f32( mask32, b, a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz )
|
||||
{
|
||||
return vaddq_f32( vmulq_f32( ax, bx ), vaddq_f32( vmulq_f32( ay, by ), vmulq_f32( az, bz ) ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount )
|
||||
{
|
||||
uint32_t mask = ( 1u << bitCount ) - 1;
|
||||
const int32_t lanes[4] = { 0, 1, 2, 3 };
|
||||
int32x4_t index = vaddq_s32( vdupq_n_s32( baseIndex ), vld1q_s32( lanes ) );
|
||||
uint32x4_t clearLow = vdupq_n_u32( ~mask );
|
||||
uint32x4_t bits = vorrq_u32( vandq_u32( vreinterpretq_u32_f32( value ), clearLow ), vreinterpretq_u32_s32( index ) );
|
||||
return vreinterpretq_f32_u32( bits );
|
||||
}
|
||||
|
||||
static inline int b3MinIndexW( b3FloatW a, int bitCount )
|
||||
{
|
||||
float32x2_t m = vmin_f32( vget_low_f32( a ), vget_high_f32( a ) );
|
||||
m = vpmin_f32( m, m );
|
||||
uint32_t bits = vget_lane_u32( vreinterpret_u32_f32( m ), 0 );
|
||||
return (int)( bits & ( ( 1u << bitCount ) - 1 ) );
|
||||
}
|
||||
|
||||
#elif defined( B3_SIMD_SSE2 )
|
||||
|
||||
static inline b3FloatW b3ZeroW( void )
|
||||
{
|
||||
return _mm_setzero_ps();
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SplatW( float scalar )
|
||||
{
|
||||
return _mm_set1_ps( scalar );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SetW( float a, float b, float c, float d )
|
||||
{
|
||||
return _mm_setr_ps( a, b, c, d );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3LoadW( const float* data )
|
||||
{
|
||||
return _mm_loadu_ps( data );
|
||||
}
|
||||
|
||||
static inline void b3StoreW( float* data, b3FloatW a )
|
||||
{
|
||||
_mm_storeu_ps( data, a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
// Create a mask with the sign bit set for each element
|
||||
__m128 mask = _mm_set1_ps( -0.0f );
|
||||
|
||||
// XOR the input with the mask to negate each element
|
||||
return _mm_xor_ps( a, mask );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_add_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_sub_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_mul_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_div_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SqrtW( b3FloatW a )
|
||||
{
|
||||
return _mm_sqrt_ps( a );
|
||||
}
|
||||
|
||||
// a + b * c
|
||||
static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
{
|
||||
return _mm_add_ps( a, _mm_mul_ps( b, c ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_min_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_max_ps( a, b );
|
||||
}
|
||||
|
||||
// Horizontal min over a 4-lane vector (result broadcast to all lanes).
|
||||
static inline b3FloatW b3HorizontalMinW( b3FloatW v )
|
||||
{
|
||||
v = _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );
|
||||
return _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );
|
||||
}
|
||||
|
||||
// clamp a to [-b, b]
|
||||
static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW nb = b3NegW( b );
|
||||
b3FloatW c = b3MaxW( nb, a );
|
||||
return b3MinW( c, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AndW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_and_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_or_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_cmpgt_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_cmplt_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_cmpeq_ps( a, b );
|
||||
}
|
||||
|
||||
static inline bool b3AllZeroW( b3FloatW a )
|
||||
{
|
||||
// Compare each element with zero
|
||||
b3FloatW zero = _mm_setzero_ps();
|
||||
b3FloatW cmp = _mm_cmpeq_ps( a, zero );
|
||||
|
||||
// Create a mask from the comparison results
|
||||
int mask = _mm_movemask_ps( cmp );
|
||||
|
||||
// If all elements are zero, the mask will be 0xF (1111 in binary)
|
||||
return mask == 0xF;
|
||||
}
|
||||
|
||||
static inline bool b3AnyTrueW( b3FloatW mask )
|
||||
{
|
||||
return _mm_movemask_ps( mask ) != 0;
|
||||
}
|
||||
|
||||
// component-wise returns mask ? b : a
|
||||
static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask )
|
||||
{
|
||||
return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz )
|
||||
{
|
||||
return _mm_add_ps( _mm_mul_ps( ax, bx ), _mm_add_ps( _mm_mul_ps( ay, by ), _mm_mul_ps( az, bz ) ) );
|
||||
}
|
||||
|
||||
// Replace the low bitCount mantissa bits of each lane with baseIndex + lane. The value must be
|
||||
// positive so the embedded index sorts with the value, and ties fall to the lower index.
|
||||
static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount )
|
||||
{
|
||||
int mask = ( 1 << bitCount ) - 1;
|
||||
__m128i index = _mm_add_epi32( _mm_set1_epi32( baseIndex ), _mm_setr_epi32( 0, 1, 2, 3 ) );
|
||||
__m128 clearLow = _mm_castsi128_ps( _mm_set1_epi32( ~mask ) );
|
||||
return _mm_or_ps( _mm_and_ps( value, clearLow ), _mm_castsi128_ps( index ) );
|
||||
}
|
||||
|
||||
// Recovers the index embedded by b3EmbedIndexW from the lane holding the minimum.
|
||||
static inline int b3MinIndexW( b3FloatW a, int bitCount )
|
||||
{
|
||||
a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );
|
||||
a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );
|
||||
return _mm_cvtsi128_si32( _mm_castps_si128( a ) ) & ( ( 1 << bitCount ) - 1 );
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
static inline b3FloatW b3ZeroW( void )
|
||||
{
|
||||
return (b3FloatW){ 0.0f, 0.0f, 0.0f, 0.0f };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SplatW( float scalar )
|
||||
{
|
||||
return (b3FloatW){ scalar, scalar, scalar, scalar };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SetW( float a, float b, float c, float d )
|
||||
{
|
||||
return (b3FloatW){ a, b, c, d };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3LoadW( const float* data )
|
||||
{
|
||||
return (b3FloatW){ data[0], data[1], data[2], data[3] };
|
||||
}
|
||||
|
||||
static inline void b3StoreW( float* data, b3FloatW a )
|
||||
{
|
||||
data[0] = a.x;
|
||||
data[1] = a.y;
|
||||
data[2] = a.z;
|
||||
data[3] = a.w;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
return (b3FloatW){ -a.x, -a.y, -a.z, -a.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3SqrtW( b3FloatW a )
|
||||
{
|
||||
return (b3FloatW){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c )
|
||||
{
|
||||
return (b3FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x <= b.x ? a.x : b.x;
|
||||
r.y = a.y <= b.y ? a.y : b.y;
|
||||
r.z = a.z <= b.z ? a.z : b.z;
|
||||
r.w = a.w <= b.w ? a.w : b.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x >= b.x ? a.x : b.x;
|
||||
r.y = a.y >= b.y ? a.y : b.y;
|
||||
r.z = a.z >= b.z ? a.z : b.z;
|
||||
r.w = a.w >= b.w ? a.w : b.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
// clamp a to [-b, b]
|
||||
static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x <= b.x ? a.x : b.x;
|
||||
r.y = a.y <= b.y ? a.y : b.y;
|
||||
r.z = a.z <= b.z ? a.z : b.z;
|
||||
r.w = a.w <= b.w ? a.w : b.w;
|
||||
r.x = r.x <= -b.x ? -b.x : r.x;
|
||||
r.y = r.y <= -b.y ? -b.y : r.y;
|
||||
r.z = r.z <= -b.z ? -b.z : r.z;
|
||||
r.w = r.w <= -b.w ? -b.w : r.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
// Logical operations on the scalar path are 0/1 float values. Not bit-wise like SIMD.
|
||||
|
||||
static inline b3FloatW b3AndW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x != 0.0f && b.x != 0.0f ? 1.0f : 0.0f;
|
||||
r.y = a.y != 0.0f && b.y != 0.0f ? 1.0f : 0.0f;
|
||||
r.z = a.z != 0.0f && b.z != 0.0f ? 1.0f : 0.0f;
|
||||
r.w = a.w != 0.0f && b.w != 0.0f ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f;
|
||||
r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f;
|
||||
r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f;
|
||||
r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x > b.x ? 1.0f : 0.0f;
|
||||
r.y = a.y > b.y ? 1.0f : 0.0f;
|
||||
r.z = a.z > b.z ? 1.0f : 0.0f;
|
||||
r.w = a.w > b.w ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x < b.x ? 1.0f : 0.0f;
|
||||
r.y = a.y < b.y ? 1.0f : 0.0f;
|
||||
r.z = a.z < b.z ? 1.0f : 0.0f;
|
||||
r.w = a.w < b.w ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = a.x == b.x ? 1.0f : 0.0f;
|
||||
r.y = a.y == b.y ? 1.0f : 0.0f;
|
||||
r.z = a.z == b.z ? 1.0f : 0.0f;
|
||||
r.w = a.w == b.w ? 1.0f : 0.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline bool b3AllZeroW( b3FloatW a )
|
||||
{
|
||||
return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f;
|
||||
}
|
||||
|
||||
static inline bool b3AnyTrueW( b3FloatW mask )
|
||||
{
|
||||
return mask.x != 0.0f || mask.y != 0.0f || mask.z != 0.0f || mask.w != 0.0f;
|
||||
}
|
||||
|
||||
// component-wise returns mask ? b : a
|
||||
static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = mask.x != 0.0f ? b.x : a.x;
|
||||
r.y = mask.y != 0.0f ? b.y : a.y;
|
||||
r.z = mask.z != 0.0f ? b.z : a.z;
|
||||
r.w = mask.w != 0.0f ? b.w : a.w;
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz )
|
||||
{
|
||||
b3FloatW r;
|
||||
r.x = ax.x * bx.x + ( ay.x * by.x + az.x * bz.x );
|
||||
r.y = ax.y * bx.y + ( ay.y * by.y + az.y * bz.y );
|
||||
r.z = ax.z * bx.z + ( ay.z * by.z + az.z * bz.z );
|
||||
r.w = ax.w * bx.w + ( ay.w * by.w + az.w * bz.w );
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount )
|
||||
{
|
||||
uint32_t mask = ( 1u << bitCount ) - 1;
|
||||
float lanes[4] = { value.x, value.y, value.z, value.w };
|
||||
|
||||
for ( int i = 0; i < 4; ++i )
|
||||
{
|
||||
uint32_t bits;
|
||||
memcpy( &bits, lanes + i, sizeof( bits ) );
|
||||
bits = ( bits & ~mask ) | (uint32_t)( baseIndex + i );
|
||||
memcpy( lanes + i, &bits, sizeof( bits ) );
|
||||
}
|
||||
|
||||
return (b3FloatW){ lanes[0], lanes[1], lanes[2], lanes[3] };
|
||||
}
|
||||
|
||||
static inline int b3MinIndexW( b3FloatW a, int bitCount )
|
||||
{
|
||||
float m = a.x;
|
||||
m = a.y < m ? a.y : m;
|
||||
m = a.z < m ? a.z : m;
|
||||
m = a.w < m ? a.w : m;
|
||||
|
||||
uint32_t bits;
|
||||
memcpy( &bits, &m, sizeof( bits ) );
|
||||
return (int)( bits & ( ( 1u << bitCount ) - 1 ) );
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -125,7 +125,7 @@ static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* conte
|
|||
float i12 = inertiaLocal.cz.y;
|
||||
float i22 = inertiaLocal.cz.z;
|
||||
|
||||
for ( int gyroIteration = 0; gyroIteration < 1; ++gyroIteration )
|
||||
for ( int gyroIteration = 0; gyroIteration < B3_GYROSCOPIC_ITERATIONS; ++gyroIteration )
|
||||
{
|
||||
float w1 = omega2.x;
|
||||
float w2 = omega2.y;
|
||||
|
|
|
|||
Loading…
Reference in a new issue