box3d/src/mesh.c
Erin Catto 38f2ae1e19
Reduce ghosts (#212)
Full ghost collision handling for sphere, capsule, and hull versus a
single mesh or heightfield. This requires mesh to be built with edge
identification.

Optimized mesh builder.

Removed B3_MAX_MESH_CONTACT_TRIANGLES. You will still hit performance
problems for dense meshes. Fixes #184
2026-11-10 21:53:10 -08:00

3453 lines
95 KiB
C

// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
// Dirk Gregorius contributed portions of this code
#include "container.h"
#include "math_internal.h"
#include "shape.h"
#include "simd.h"
#include "box3d/collision.h"
#include "box3d/constants.h"
#include <stdint.h>
b3DeclareArray( b3VertexNode );
b3DeclareArray( b3MeshNode );
b3DeclareArray( b3MeshTriangle );
b3DeclareArray( b3Vec3 );
b3DeclareArray( b3Primitive );
b3DeclareArrayNative( uint8_t );
#define B3_BIN_COUNT 8
#define B3_DESIRED_TRIANGLES_PER_LEAF 4
#define B3_LEAF_NODE 3
#define B3_MAXIMUM_TRIANGLES_PER_LEAF 8
#define B3_MESH_STACK_SIZE 256
static bool b3IsLeaf( const b3MeshNode* node )
{
return node->data.asLeaf.type == B3_LEAF_NODE;
}
static b3MeshNode* b3GetMeshNodesWrite( b3MeshData* mesh )
{
if ( mesh->nodeOffset == 0 )
{
return NULL;
}
return (b3MeshNode*)( (intptr_t)mesh + mesh->nodeOffset );
}
static b3MeshNode* b3GetLeftChildWrite( b3MeshNode* node )
{
// The left child follows its parent.
B3_ASSERT( !b3IsLeaf( node ) );
return node + 1;
}
static const b3MeshNode* b3GetLeftChild( const b3MeshNode* node )
{
// The left child follows its parent.
B3_ASSERT( !b3IsLeaf( node ) );
return node + 1;
}
static b3MeshNode* b3GetRightChildWrite( b3MeshNode* node )
{
// We store the offset of the right child relative to its parent
B3_ASSERT( !b3IsLeaf( node ) );
return node + node->data.asNode.childOffset;
}
static const b3MeshNode* b3GetRightChild( const b3MeshNode* node )
{
// We store the offset of the right child relative to its parent
B3_ASSERT( !b3IsLeaf( node ) );
return node + node->data.asNode.childOffset;
}
static const b3MeshNode* b3GetRoot( const b3MeshData* mesh )
{
// The first node is the root
return b3GetMeshNodes( mesh );
}
static b3MeshNode* b3GetRootWrite( b3MeshData* mesh )
{
// The first node is the root
return b3GetMeshNodesWrite( mesh );
}
static b3MeshTriangle* b3GetMeshTrianglesWrite( b3MeshData* mesh )
{
if ( mesh->triangleOffset == 0 )
{
return NULL;
}
return (b3MeshTriangle*)( (intptr_t)mesh + mesh->triangleOffset );
}
static b3Vec3* b3GetMeshVerticesWrite( b3MeshData* mesh )
{
if ( mesh->vertexOffset == 0 )
{
return NULL;
}
return (b3Vec3*)( (intptr_t)mesh + mesh->vertexOffset );
}
// static b3Vec3 b3GetVertex( b3MeshData& mesh, int vertexIndex )
//{
// B3_ASSERT( 0 <= vertexIndex && vertexIndex < mesh.vertexCount );
// b3Vec3* vertices = b3GetMeshVertices( &mesh );
// return vertices[vertexIndex];
// }
static uint8_t* b3GetMeshMaterialIndicesWrite( b3MeshData* mesh )
{
if ( mesh->materialOffset == 0 )
{
return NULL;
}
return (uint8_t*)( (intptr_t)mesh + mesh->materialOffset );
}
static uint8_t* b3GetMeshFlagsWrite( b3MeshData* mesh )
{
if ( mesh->flagsOffset == 0 )
{
return NULL;
}
return (uint8_t*)( (intptr_t)mesh + mesh->flagsOffset );
}
static int8_t* b3GetMeshEdgeAnglesWrite( b3MeshData* mesh )
{
if ( mesh->edgeAngleOffset == 0 )
{
return NULL;
}
return (int8_t*)( (intptr_t)mesh + mesh->edgeAngleOffset );
}
static uint8_t* b3GetMeshVertexFlagsWrite( b3MeshData* mesh )
{
if ( mesh->vertexFlagOffset == 0 )
{
return NULL;
}
return (uint8_t*)( (intptr_t)mesh + mesh->vertexFlagOffset );
}
static int b3GetNodeHeight( const b3MeshNode* node )
{
if ( b3IsLeaf( node ) )
{
return 0;
}
const b3MeshNode* leftChild = b3GetLeftChild( node );
int leftHeight = b3GetNodeHeight( leftChild );
const b3MeshNode* rightChild = b3GetRightChild( node );
int rightHeight = b3GetNodeHeight( rightChild );
return 1 + b3MaxInt( leftHeight, rightHeight );
}
int b3GetHeight( const b3MeshData* mesh )
{
const b3MeshNode* root = b3GetRoot( mesh );
if ( root == NULL )
{
return 0;
}
return b3GetNodeHeight( root );
}
#if B3_ENABLE_VALIDATION == 1
static bool b3IsDegenerate( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, float minArea )
{
b3Vec3 normal = b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) );
float lengthSq = b3LengthSquared( normal );
return lengthSq < minArea * minArea;
}
static bool b3IsNonDegenerate( const b3MeshData* mesh, float minArea )
{
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh );
const b3Vec3* vertices = b3GetMeshVertices( mesh );
// Check triangles
for ( int index = 0; index < mesh->triangleCount; ++index )
{
// Index range
b3MeshTriangle triangle = triangles[index];
if ( triangle.index1 >= mesh->vertexCount )
{
return false;
}
if ( triangle.index2 >= mesh->vertexCount )
{
return false;
}
if ( triangle.index3 >= mesh->vertexCount )
{
return false;
}
// Degenerate topology
if ( triangle.index1 == triangle.index2 )
{
return false;
}
if ( triangle.index1 == triangle.index3 )
{
return false;
}
if ( triangle.index2 == triangle.index3 )
{
return false;
}
// Degenerate geometry
b3Vec3 vertex1 = vertices[triangle.index1];
b3Vec3 vertex2 = vertices[triangle.index2];
b3Vec3 vertex3 = vertices[triangle.index3];
if ( b3IsDegenerate( vertex1, vertex2, vertex3, minArea ) )
{
return false;
}
}
return true;
}
static inline b3AABB b3GetNodeAABB( const b3MeshNode* node )
{
return (b3AABB){
node->lowerBound,
node->upperBound,
};
}
static bool b3IsConsistent( const b3MeshData* mesh )
{
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh );
const b3Vec3* vertices = b3GetMeshVertices( mesh );
// Check nodes
int count = 0;
const b3MeshNode* stack[64];
stack[count++] = b3GetRoot( mesh );
while ( count > 0 )
{
const b3MeshNode* node = stack[--count];
b3AABB nodeBounds = b3GetNodeAABB( node );
if ( b3IsLeaf( node ) == false )
{
const b3MeshNode* child1 = b3GetLeftChild( node );
b3AABB bounds1 = b3GetNodeAABB( child1 );
const b3MeshNode* child2 = b3GetRightChild( node );
b3AABB bounds2 = b3GetNodeAABB( child2 );
if ( b3AABB_Contains( nodeBounds, bounds1 ) == false )
{
return false;
}
if ( b3AABB_Contains( nodeBounds, bounds2 ) == false )
{
return false;
}
stack[count++] = child2;
stack[count++] = child1;
}
else
{
b3AABB triangleBounds = B3_BOUNDS3_EMPTY;
for ( uint32_t index = 0; index < node->data.asLeaf.triangleCount; ++index )
{
int triangleIndex = node->triangleOffset + index;
B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->triangleCount );
b3MeshTriangle triangle = triangles[triangleIndex];
b3AABB vertexBounds = B3_BOUNDS3_EMPTY;
vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index1] );
vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index2] );
vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index3] );
triangleBounds = b3AABB_Union( triangleBounds, vertexBounds );
}
if ( !b3AABB_Contains( nodeBounds, triangleBounds ) )
{
return false;
}
}
}
return true;
}
bool b3IsValidMesh( const b3MeshData* meshData )
{
if ( meshData == NULL )
{
return false;
}
if ( meshData->version != B3_MESH_VERSION )
{
return false;
}
if ( meshData->byteCount < (int)sizeof( b3MeshData ) )
{
return false;
}
return b3IsConsistent( meshData );
}
#else
bool b3IsValidMesh( const b3MeshData* meshData )
{
if ( meshData == NULL )
{
return false;
}
if ( meshData->version != B3_MESH_VERSION )
{
return false;
}
if ( meshData->byteCount < (int)sizeof( b3MeshData ) )
{
return false;
}
return true;
}
#endif
static inline b3Vec3 b3GetStridedVertex( const b3Vec3* vertices, int index, size_t stride )
{
return *(const b3Vec3*)( (const uint8_t*)vertices + index * stride );
}
// Node for a vertex linked list
typedef struct b3VertexNode
{
int32_t vertexIndex;
int nextNodeIndex;
} b3VertexNode;
#define NAME b3VertexMap
#define KEY_TY uint64_t
#define VAL_TY int
#define HASH_FN vt_hash_integer
#define CMPR_FN vt_cmpr_integer
#define MALLOC_FN b3Alloc
#define FREE_FN b3Free
#include "verstable.h"
typedef struct b3SpatialHash
{
b3Array( b3VertexNode ) nodes;
const b3Vec3* vertices;
int vertexCount;
size_t stride;
b3VertexMap vertexMap;
float cellSize;
float tolerance;
} b3SpatialHash;
static void b3CreateSpatialHash( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, size_t stride,
float tolerance )
{
h->vertices = vertices;
h->vertexCount = vertexCount;
h->stride = stride;
h->tolerance = tolerance;
h->cellSize = 2.0f * tolerance;
b3Array_CreateN( h->nodes, vertexCount );
b3VertexMap_init( &h->vertexMap );
b3VertexMap_reserve( &h->vertexMap, vertexCount );
B3_ASSERT( h->cellSize > 0.0f );
}
static void b3DestroySpatialHash( b3SpatialHash* h )
{
b3VertexMap_cleanup( &h->vertexMap );
b3Array_Destroy( h->nodes );
}
// Welding works by bucketing nearby vertices into identical keys in a hash table.
// Bucketing is done manually with an array.
static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentIndex )
{
B3_ASSERT( currentIndex < h->vertexCount );
size_t stride = h->stride;
b3Vec3 vertex = b3GetStridedVertex( h->vertices, currentIndex, stride );
float cellSize = h->cellSize;
float tolerance = h->tolerance;
// Get the grid coordinates for the current vertex
int32_t baseX = (int32_t)( floorf( vertex.x / cellSize ) );
int32_t baseY = (int32_t)( floorf( vertex.y / cellSize ) );
int32_t baseZ = (int32_t)( floorf( vertex.z / cellSize ) );
// Check the current cell and all 26 neighboring cells (3x3x3 - 1)
for ( int dx = -1; dx <= 1; ++dx )
{
for ( int dy = -1; dy <= 1; ++dy )
{
for ( int dz = -1; dz <= 1; ++dz )
{
int32_t x = baseX + dx;
int32_t y = baseY + dy;
int32_t z = baseZ + dz;
// Compute hash for this neighboring cell (this is the key in the map)
uint64_t key = 0;
key ^= (uint64_t)( x ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 );
key ^= (uint64_t)( y ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 );
key ^= (uint64_t)( z ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 );
b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, key );
if ( b3VertexMap_is_end( it ) == false )
{
// Check all vertices in this key
int nodeIndex = it.data->val;
while ( nodeIndex != B3_NULL_INDEX )
{
b3VertexNode node = h->nodes.data[nodeIndex];
int32_t existingIndex = node.vertexIndex;
B3_ASSERT( existingIndex < currentIndex );
B3_ASSERT( existingIndex < h->vertexCount );
b3Vec3 other = b3GetStridedVertex( h->vertices, existingIndex, stride );
// IsEqual inlined: check if vertices are within tolerance
if ( fabsf( vertex.x - other.x ) <= tolerance && fabsf( vertex.y - other.y ) <= tolerance &&
fabsf( vertex.z - other.z ) <= tolerance )
{
// Found duplicate
return existingIndex;
}
nodeIndex = node.nextNodeIndex;
}
}
}
}
}
// No duplicate found, add to hash table
uint64_t currentKey = 0;
currentKey ^= (uint64_t)( baseX ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 );
currentKey ^= (uint64_t)( baseY ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 );
currentKey ^= (uint64_t)( baseZ ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 );
b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, currentKey );
if ( b3VertexMap_is_end( it ) == false )
{
int nodeIndex = it.data->val;
b3VertexNode node = {
.vertexIndex = currentIndex,
.nextNodeIndex = nodeIndex,
};
it.data->val = h->nodes.count;
b3Array_Push( h->nodes, node );
}
else
{
b3VertexNode node = {
.vertexIndex = currentIndex,
.nextNodeIndex = B3_NULL_INDEX,
};
b3VertexMap_insert( &h->vertexMap, currentKey, h->nodes.count );
b3Array_Push( h->nodes, node );
}
// Not welded
return B3_NULL_INDEX;
}
typedef struct b3WeldData
{
const b3Vec3* srcVertices;
const int32_t* srcIndices;
b3Vec3* dstVertices;
int32_t* dstIndices;
int vertexCount;
int indexCount;
size_t stride;
} b3WeldData;
static int b3WeldVertices( b3WeldData* data, float tolerance )
{
int vertexCount = data->vertexCount;
int uniqueCount = 0;
size_t stride = data->stride;
// Create spatial hash and find duplicates
b3SpatialHash spatialHash;
b3CreateSpatialHash( &spatialHash, data->srcVertices, vertexCount, stride, tolerance );
b3Array( int ) vertexMapping = { 0 };
b3Array_Resize( vertexMapping, vertexCount );
for ( int i = 0; i < vertexCount; ++i )
{
int32_t duplicateIndex = b3SpatialHash_FindDuplicate( &spatialHash, i );
if ( duplicateIndex == B3_NULL_INDEX )
{
// New unique vertex
vertexMapping.data[i] = uniqueCount;
data->dstVertices[uniqueCount] = b3GetStridedVertex( data->srcVertices, i, stride );
uniqueCount += 1;
}
else
{
// Found duplicate, map to existing vertex
vertexMapping.data[i] = vertexMapping.data[duplicateIndex];
}
}
// Update indices to reference the new vertex array
int indexCount = data->indexCount;
for ( int i = 0; i < indexCount; ++i )
{
int srcIndex = data->srcIndices[i];
B3_ASSERT( 0 <= srcIndex && srcIndex < vertexCount );
data->dstIndices[i] = vertexMapping.data[srcIndex];
}
b3DestroySpatialHash( &spatialHash );
b3Array_Destroy( vertexMapping );
return uniqueCount;
}
static inline void b3StoreLeaf( b3MeshNode* node, const b3AABB* aabb, int triangleCount, int triangleOffset )
{
node->data.asLeaf.type = B3_LEAF_NODE;
node->data.asLeaf.triangleCount = triangleCount;
node->triangleOffset = triangleOffset;
node->lowerBound = aabb->lowerBound;
node->upperBound = aabb->upperBound;
}
typedef struct b3Primitive
{
b3AABB aabb;
b3Vec3 center;
int triangleIndex;
} b3Primitive;
_Static_assert( offsetof( b3Primitive, aabb.upperBound ) == 3 * sizeof( float ) &&
offsetof( b3Primitive, center ) == 6 * sizeof( float ),
"primitive floats must be contiguous" );
static inline b3FloatW4 b3PrimitiveMinW4( b3FloatW4 a, b3FloatW4 b )
{
#if defined( B3_SIMD_SSE2 )
return b3MinW4( a, b );
#else
return b3BlendW4( b, a, b3LessThanW4( a, b ) );
#endif
}
static inline b3FloatW4 b3PrimitiveMaxW4( b3FloatW4 a, b3FloatW4 b )
{
#if defined( B3_SIMD_SSE2 )
return b3MaxW4( a, b );
#else
return b3BlendW4( b, a, b3GreaterThanW4( a, b ) );
#endif
}
static inline b3Vec3 b3PrimitiveLanes( b3FloatW4 v, int first )
{
float lanes[4];
b3StoreW4( lanes, v );
return (b3Vec3){ lanes[first], lanes[first + 1], lanes[first + 2] };
}
static b3AABB b3PrimitiveBounds( const b3Primitive* primitives, int count )
{
b3FloatW4 lower = b3SplatW4( FLT_MAX );
b3FloatW4 upper = b3SplatW4( -FLT_MAX );
for ( int i = 0; i < count; ++i )
{
const float* p = &primitives[i].aabb.lowerBound.x;
lower = b3PrimitiveMinW4( lower, b3LoadW4( p ) );
upper = b3PrimitiveMaxW4( upper, b3LoadW4( p + 2 ) );
}
return (b3AABB){ b3PrimitiveLanes( lower, 0 ), b3PrimitiveLanes( upper, 1 ) };
}
static b3AABB b3PrimitiveCenterBounds( const b3Primitive* primitives, int count )
{
b3FloatW4 lower = b3LoadW4( &primitives[0].aabb.lowerBound.x + 5 );
b3FloatW4 upper = lower;
for ( int i = 1; i < count; ++i )
{
b3FloatW4 center = b3LoadW4( &primitives[i].aabb.lowerBound.x + 5 );
lower = b3PrimitiveMinW4( lower, center );
upper = b3PrimitiveMaxW4( upper, center );
}
return (b3AABB){ b3PrimitiveLanes( lower, 1 ), b3PrimitiveLanes( upper, 1 ) };
}
typedef struct b3Bucket
{
b3FloatW4 lower;
b3FloatW4 upper;
int count;
} b3Bucket;
typedef struct b3Split
{
b3AABB leftBounds;
b3AABB rightBounds;
int axis;
int index;
} b3Split;
static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives )
{
b3Split split;
split.axis = -1;
split.index = -1;
// Compute bounds of primitive centroids and choose split axis
b3AABB bounds = b3PrimitiveCenterBounds( primitives, count );
b3FloatW4 emptyLower = b3SplatW4( FLT_MAX );
b3FloatW4 emptyUpper = b3SplatW4( -FLT_MAX );
// Compute costs for splitting after each bucket and keep track of best split
// This is a small O(n^2) loop. This can be further optimized, but it is already
// very fast and is kept for simplicity right now.
int bestBucket = -1;
float bestCost = FLT_MAX;
b3Vec3 extent = b3AABB_Extents( bounds );
bool binAxis[3];
float factors[3];
b3Bucket axisBuckets[3][B3_BIN_COUNT];
for ( int axis = 0; axis < 3; ++axis )
{
binAxis[axis] = b3GetByIndex( extent, axis ) >= B3_LINEAR_SLOP;
factors[axis] = binAxis[axis] ? B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) /
( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) )
: 0.0f;
for ( int i = 0; i < B3_BIN_COUNT; ++i )
{
axisBuckets[axis][i].lower = emptyLower;
axisBuckets[axis][i].upper = emptyUpper;
axisBuckets[axis][i].count = 0;
}
}
for ( int i = 0; i < count; ++i )
{
b3Vec3 center = primitives[i].center;
int indexX = (int)( factors[0] * ( center.x - bounds.lowerBound.x ) );
int indexY = (int)( factors[1] * ( center.y - bounds.lowerBound.y ) );
int indexZ = (int)( factors[2] * ( center.z - bounds.lowerBound.z ) );
B3_ASSERT( 0 <= indexX && indexX < B3_BIN_COUNT );
B3_ASSERT( 0 <= indexY && indexY < B3_BIN_COUNT );
B3_ASSERT( 0 <= indexZ && indexZ < B3_BIN_COUNT );
const float* p = &primitives[i].aabb.lowerBound.x;
b3FloatW4 lower = b3LoadW4( p );
b3FloatW4 upper = b3LoadW4( p + 2 );
b3Bucket* bucketX = axisBuckets[0] + indexX;
bucketX->count += 1;
bucketX->lower = b3PrimitiveMinW4( bucketX->lower, lower );
bucketX->upper = b3PrimitiveMaxW4( bucketX->upper, upper );
b3Bucket* bucketY = axisBuckets[1] + indexY;
bucketY->count += 1;
bucketY->lower = b3PrimitiveMinW4( bucketY->lower, lower );
bucketY->upper = b3PrimitiveMaxW4( bucketY->upper, upper );
b3Bucket* bucketZ = axisBuckets[2] + indexZ;
bucketZ->count += 1;
bucketZ->lower = b3PrimitiveMinW4( bucketZ->lower, lower );
bucketZ->upper = b3PrimitiveMaxW4( bucketZ->upper, upper );
}
for ( int axis = 0; axis < 3; ++axis )
{
if ( binAxis[axis] == false )
{
continue;
}
b3Bucket* buckets = axisBuckets[axis];
// Evaluate splits
for ( int i = 0; i < B3_BIN_COUNT - 1; ++i )
{
int leftCount = 0;
b3FloatW4 leftLower = emptyLower;
b3FloatW4 leftUpper = emptyUpper;
for ( int k = 0; k <= i; ++k )
{
leftCount += buckets[k].count;
leftLower = b3PrimitiveMinW4( leftLower, buckets[k].lower );
leftUpper = b3PrimitiveMaxW4( leftUpper, buckets[k].upper );
}
int rightCount = 0;
b3FloatW4 rightLower = emptyLower;
b3FloatW4 rightUpper = emptyUpper;
for ( int k = i + 1; k < B3_BIN_COUNT; ++k )
{
rightCount += buckets[k].count;
rightLower = b3PrimitiveMinW4( rightLower, buckets[k].lower );
rightUpper = b3PrimitiveMaxW4( rightUpper, buckets[k].upper );
}
b3AABB leftBounds = { b3PrimitiveLanes( leftLower, 0 ), b3PrimitiveLanes( leftUpper, 1 ) };
b3AABB rightBounds = { b3PrimitiveLanes( rightLower, 0 ), b3PrimitiveLanes( rightUpper, 1 ) };
B3_ASSERT( leftCount + rightCount == count );
if ( leftCount > 0 && rightCount > 0 )
{
float cost = leftCount * b3AABB_Area( leftBounds ) + rightCount * b3AABB_Area( rightBounds );
if ( cost < bestCost )
{
bestBucket = i;
bestCost = cost;
split.axis = axis;
split.index = leftCount;
split.leftBounds = leftBounds;
split.rightBounds = rightBounds;
}
}
}
}
// Partition
if ( bestBucket >= 0 )
{
int axis = split.axis;
float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) /
( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) );
int splitIndex = 0;
for ( int i = 0; i < count; ++i )
{
b3Vec3 center = primitives[i].center;
int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) );
if ( index <= bestBucket )
{
b3Primitive temp = primitives[i];
primitives[i] = primitives[splitIndex];
primitives[splitIndex] = temp;
splitIndex++;
}
}
B3_ASSERT( splitIndex == split.index );
}
return split;
}
static b3Split b3SplitHalf( int count, b3Primitive* primitives )
{
// Split in the middle
int splitIndex = count / 2;
b3AABB leftBounds = b3PrimitiveBounds( primitives, splitIndex );
b3AABB rightBounds = b3PrimitiveBounds( primitives + splitIndex, count - splitIndex );
b3AABB bounds = b3AABB_Union( leftBounds, rightBounds );
int axis = b3MajorAxis( b3AABB_Extents( bounds ) );
b3Split split;
split.axis = axis;
split.index = splitIndex;
split.leftBounds = leftBounds;
split.rightBounds = rightBounds;
return split;
}
static b3Split b3SplitMedian( int count, b3Primitive* primitives )
{
B3_ASSERT( count > 2 );
b3AABB centerBounds = b3PrimitiveCenterBounds( primitives, count );
b3Vec3 lowerBound = centerBounds.lowerBound;
b3Vec3 upperBound = centerBounds.upperBound;
b3Vec3 d = b3Sub( upperBound, lowerBound );
b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) );
b3Split split = { 0 };
split.index = -1;
// Partition longest axis using the Hoare partition scheme
// https://en.wikipedia.org/wiki/Quicksort
// https://nicholasvadivelu.com/2021/01/11/array-partition/
int i1 = 0, i2 = count;
if ( d.x >= d.y && d.x >= d.z )
{
split.axis = 0;
float pivot = c.x;
while ( i1 < i2 )
{
while ( i1 < i2 && primitives[i1].center.x < pivot )
{
i1 += 1;
};
while ( i1 < i2 && primitives[i2 - 1].center.x >= pivot )
{
i2 -= 1;
};
if ( i1 < i2 )
{
// Swap primitives
b3Primitive temp = primitives[i1];
primitives[i1] = primitives[i2 - 1];
primitives[i2 - 1] = temp;
i1 += 1;
i2 -= 1;
}
}
}
else if ( d.y >= d.z )
{
split.axis = 1;
float pivot = c.y;
while ( i1 < i2 )
{
while ( i1 < i2 && primitives[i1].center.y < pivot )
{
i1 += 1;
};
while ( i1 < i2 && primitives[i2 - 1].center.y >= pivot )
{
i2 -= 1;
};
if ( i1 < i2 )
{
// Swap primitives
b3Primitive temp = primitives[i1];
primitives[i1] = primitives[i2 - 1];
primitives[i2 - 1] = temp;
i1 += 1;
i2 -= 1;
}
}
}
else
{
split.axis = 2;
float pivot = c.z;
while ( i1 < i2 )
{
while ( i1 < i2 && primitives[i1].center.z < pivot )
{
i1 += 1;
};
while ( i1 < i2 && primitives[i2 - 1].center.z >= pivot )
{
i2 -= 1;
};
if ( i1 < i2 )
{
// Swap primitives
b3Primitive temp = primitives[i1];
primitives[i1] = primitives[i2 - 1];
primitives[i2 - 1] = temp;
i1 += 1;
i2 -= 1;
}
}
}
B3_ASSERT( i1 == i2 );
B3_ASSERT( 0 <= i1 && i1 < count );
if ( i1 == 0 || i1 == count - 1 )
{
// failed to split
i1 = count / 2;
}
split.index = i1;
split.leftBounds = b3PrimitiveBounds( primitives, i1 );
split.rightBounds = b3PrimitiveBounds( primitives + i1, count - i1 );
return split;
}
#if B3_ENABLE_VALIDATION == 1
static bool b3ValidateSplit( int count, b3Primitive* primitives, const b3Split* split )
{
if ( split->axis < 0 )
{
return false;
}
for ( int i = 0; i < split->index; ++i )
{
if ( !b3AABB_Contains( split->leftBounds, primitives[i].aabb ) )
{
return false;
}
}
for ( int i = split->index; i < count; ++i )
{
if ( !b3AABB_Contains( split->rightBounds, primitives[i].aabb ) )
{
return false;
}
}
return true;
}
#endif
static int b3BuildRecursive( b3Array( b3MeshNode ) * nodes, int count, b3Primitive* primitives, b3Primitive* base,
bool useMedianSplit, int* height )
{
if ( count > B3_DESIRED_TRIANGLES_PER_LEAF )
{
// Try to split the input set using the SAH
b3Split split;
if ( useMedianSplit )
{
split = b3SplitMedian( count, primitives );
}
else
{
split = b3SplitBinnedSah( count, primitives );
}
if ( split.axis < 0 )
{
if ( count > B3_MAXIMUM_TRIANGLES_PER_LEAF )
{
// Re-split. This is a less optimal split and can create more false positives!
split = b3SplitHalf( count, primitives );
}
else
{
b3AABB bounds = b3PrimitiveBounds( primitives, count );
// We have only a few triangles left. Create a leaf.
int index = b3Array_AddIndex( *nodes );
b3StoreLeaf( &nodes->data[index], &bounds, count, (int)( primitives - base ) );
return index;
}
}
B3_VALIDATE( b3ValidateSplit( count, primitives, &split ) );
// Allocate node and recurse
int index = b3Array_AddIndex( *nodes );
int heightLeft = 0, heightRight = 0;
int leftIndex = b3BuildRecursive( nodes, split.index, primitives, base, useMedianSplit, &heightLeft );
int rightIndex = b3BuildRecursive( nodes, count - split.index, primitives + split.index, base, useMedianSplit,
&heightRight );
*height = b3MaxInt( heightLeft, heightRight ) + 1;
B3_UNUSED( leftIndex );
B3_ASSERT( leftIndex - index == 1 && rightIndex - index > 1 );
b3AABB aabb = b3AABB_Union( split.leftBounds, split.rightBounds );
b3MeshNode* node = b3Array_Get( *nodes, index );
node->data.asNode.axis = split.axis;
node->data.asNode.childOffset = rightIndex - index;
node->lowerBound = aabb.lowerBound;
node->upperBound = aabb.upperBound;
// Zero so mesh->hash is deterministic
node->triangleOffset = 0;
return index;
}
b3AABB aabb = b3PrimitiveBounds( primitives, count );
int index = b3Array_AddIndex( *nodes );
b3StoreLeaf( &nodes->data[index], &aabb, count, (int)( primitives - base ) );
*height = 1;
return index;
}
static bool b3SortMeshTriangles( b3MeshData* mesh )
{
b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh );
uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh );
// Sort triangles in depth-first-order
int offset = 0;
b3Array( b3MeshTriangle ) tempTriangles;
b3Array_CreateN( tempTriangles, mesh->triangleCount );
b3Array( uint8_t ) tempMaterialIndices;
b3Array_CreateN( tempMaterialIndices, mesh->triangleCount );
int count = 0;
b3MeshNode* stack[B3_MESH_STACK_SIZE];
stack[count++] = b3GetRootWrite( mesh );
while ( count > 0 )
{
b3MeshNode* node = stack[--count];
if ( b3IsLeaf( node ) == false )
{
if ( count >= B3_MESH_STACK_SIZE - 2 )
{
return false;
}
stack[count++] = b3GetRightChildWrite( node );
stack[count++] = b3GetLeftChildWrite( node );
}
else
{
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
for ( int triangle = 0; triangle < triangleCount; ++triangle )
{
int index = triangleOffset + triangle;
b3Array_Push( tempTriangles, triangles[index] );
b3Array_Push( tempMaterialIndices, materialIndices[index] );
}
node->triangleOffset = offset;
offset += triangleCount;
}
}
B3_ASSERT( offset == tempTriangles.count );
B3_ASSERT( tempTriangles.count == mesh->triangleCount );
B3_ASSERT( tempMaterialIndices.count == mesh->triangleCount );
// Copy sorted triangle array back to tree
memcpy( triangles, tempTriangles.data, mesh->triangleCount * sizeof( b3MeshTriangle ) );
memcpy( materialIndices, tempMaterialIndices.data, mesh->triangleCount * sizeof( uint8_t ) );
b3Array_Destroy( tempTriangles );
b3Array_Destroy( tempMaterialIndices );
return true;
}
typedef struct
{
int vertex1;
int vertex2;
int triangle1;
int triangle2;
uint16_t triangleCount;
// The index of an edge within the parent triangle: 0, 1, or 2. 0xFF is unset
uint8_t triangleEdgeIndex1;
uint8_t triangleEdgeIndex2;
} b3MeshEdge;
#define B3_EDGE_ANGLE_STEPS 127.0f
// Lookup table for 8-bit compressed triangle neighbor angles.
static const b3CosSin s_quantizedCosSin[255] = {
{ 1.0f, 0.0f },
{ 0.9999235f, 0.012368159f },
{ 0.99969405f, 0.024734428f },
{ 0.9993117f, 0.03709691f },
{ 0.99877644f, 0.04945372f },
{ 0.99808836f, 0.061802965f },
{ 0.99724764f, 0.074142754f },
{ 0.9962543f, 0.0864712f },
{ 0.99510866f, 0.09878642f },
{ 0.9938107f, 0.111086525f },
{ 0.9923608f, 0.12336964f },
{ 0.990759f, 0.13563387f },
{ 0.9890057f, 0.14787737f },
{ 0.9871011f, 0.16009824f },
{ 0.9850455f, 0.17229462f },
{ 0.98283917f, 0.18446463f },
{ 0.98048246f, 0.19660644f },
{ 0.97797585f, 0.20871817f },
{ 0.97531956f, 0.22079796f },
{ 0.9725141f, 0.23284398f },
{ 0.96955985f, 0.24485438f },
{ 0.9664573f, 0.25682732f },
{ 0.9632069f, 0.26876098f },
{ 0.9598091f, 0.2806535f },
{ 0.95626456f, 0.29250312f },
{ 0.9525737f, 0.304308f },
{ 0.9487371f, 0.3160663f },
{ 0.9447554f, 0.32777625f },
{ 0.9406291f, 0.33943605f },
{ 0.936359f, 0.35104394f },
{ 0.9319456f, 0.36259815f },
{ 0.9273896f, 0.37409684f },
{ 0.9226918f, 0.38553834f },
{ 0.9178528f, 0.39692086f },
{ 0.91287345f, 0.40824264f },
{ 0.9077544f, 0.419502f },
{ 0.9024965f, 0.43069714f },
{ 0.89710057f, 0.44182643f },
{ 0.89156735f, 0.4528881f },
{ 0.88589776f, 0.4638805f },
{ 0.8800927f, 0.47480196f },
{ 0.8741529f, 0.48565078f },
{ 0.8680794f, 0.4964253f },
{ 0.8618732f, 0.5071239f },
{ 0.8555351f, 0.51774484f },
{ 0.8490661f, 0.52828664f },
{ 0.8424672f, 0.5387476f },
{ 0.83573943f, 0.5491262f },
{ 0.8288838f, 0.55942076f },
{ 0.82190144f, 0.5696297f },
{ 0.8147933f, 0.57975155f },
{ 0.8075605f, 0.58978474f },
{ 0.8002042f, 0.59972763f },
{ 0.79272544f, 0.60957885f },
{ 0.78512543f, 0.6193367f },
{ 0.7774054f, 0.62899995f },
{ 0.7695663f, 0.6385669f },
{ 0.76160955f, 0.6480362f },
{ 0.7535363f, 0.65740633f },
{ 0.74534774f, 0.66667587f },
{ 0.73704517f, 0.6758435f },
{ 0.7286299f, 0.6849077f },
{ 0.7201031f, 0.6938671f },
{ 0.71146613f, 0.70272034f },
{ 0.70272034f, 0.71146613f },
{ 0.6938671f, 0.7201031f },
{ 0.6849077f, 0.7286299f },
{ 0.6758435f, 0.73704517f },
{ 0.66667587f, 0.74534774f },
{ 0.65740633f, 0.7535363f },
{ 0.6480362f, 0.76160955f },
{ 0.6385669f, 0.7695663f },
{ 0.62899995f, 0.7774054f },
{ 0.6193367f, 0.78512543f },
{ 0.60957885f, 0.79272544f },
{ 0.59972763f, 0.8002042f },
{ 0.58978474f, 0.8075605f },
{ 0.57975155f, 0.8147933f },
{ 0.5696297f, 0.82190144f },
{ 0.55942076f, 0.8288838f },
{ 0.5491262f, 0.83573943f },
{ 0.5387476f, 0.8424672f },
{ 0.52828664f, 0.8490661f },
{ 0.51774484f, 0.8555351f },
{ 0.5071239f, 0.8618732f },
{ 0.4964253f, 0.8680794f },
{ 0.48565078f, 0.8741529f },
{ 0.47480196f, 0.8800927f },
{ 0.4638805f, 0.88589776f },
{ 0.4528881f, 0.89156735f },
{ 0.44182643f, 0.89710057f },
{ 0.43069714f, 0.9024965f },
{ 0.419502f, 0.9077544f },
{ 0.40824264f, 0.91287345f },
{ 0.39692086f, 0.9178528f },
{ 0.38553834f, 0.9226918f },
{ 0.37409684f, 0.9273896f },
{ 0.36259815f, 0.9319456f },
{ 0.35104394f, 0.936359f },
{ 0.33943605f, 0.9406291f },
{ 0.32777625f, 0.9447554f },
{ 0.3160663f, 0.9487371f },
{ 0.304308f, 0.9525737f },
{ 0.29250312f, 0.95626456f },
{ 0.2806535f, 0.9598091f },
{ 0.26876098f, 0.9632069f },
{ 0.25682732f, 0.9664573f },
{ 0.24485438f, 0.96955985f },
{ 0.23284398f, 0.9725141f },
{ 0.22079796f, 0.97531956f },
{ 0.20871817f, 0.97797585f },
{ 0.19660644f, 0.98048246f },
{ 0.18446463f, 0.98283917f },
{ 0.17229462f, 0.9850455f },
{ 0.16009824f, 0.9871011f },
{ 0.14787737f, 0.9890057f },
{ 0.13563387f, 0.990759f },
{ 0.12336964f, 0.9923608f },
{ 0.111086525f, 0.9938107f },
{ 0.09878642f, 0.99510866f },
{ 0.0864712f, 0.9962543f },
{ 0.074142754f, 0.99724764f },
{ 0.061802965f, 0.99808836f },
{ 0.04945372f, 0.99877644f },
{ 0.03709691f, 0.9993117f },
{ 0.024734428f, 0.99969405f },
{ 0.012368159f, 0.9999235f },
{ 0.0f, 1.0f },
{ -0.012368159f, 0.9999235f },
{ -0.024734428f, 0.99969405f },
{ -0.03709691f, 0.9993117f },
{ -0.04945372f, 0.99877644f },
{ -0.061802965f, 0.99808836f },
{ -0.074142754f, 0.99724764f },
{ -0.0864712f, 0.9962543f },
{ -0.09878642f, 0.99510866f },
{ -0.111086525f, 0.9938107f },
{ -0.12336964f, 0.9923608f },
{ -0.13563387f, 0.990759f },
{ -0.14787737f, 0.9890057f },
{ -0.16009824f, 0.9871011f },
{ -0.17229462f, 0.9850455f },
{ -0.18446463f, 0.98283917f },
{ -0.19660644f, 0.98048246f },
{ -0.20871817f, 0.97797585f },
{ -0.22079796f, 0.97531956f },
{ -0.23284398f, 0.9725141f },
{ -0.24485438f, 0.96955985f },
{ -0.25682732f, 0.9664573f },
{ -0.26876098f, 0.9632069f },
{ -0.2806535f, 0.9598091f },
{ -0.29250312f, 0.95626456f },
{ -0.304308f, 0.9525737f },
{ -0.3160663f, 0.9487371f },
{ -0.32777625f, 0.9447554f },
{ -0.33943605f, 0.9406291f },
{ -0.35104394f, 0.936359f },
{ -0.36259815f, 0.9319456f },
{ -0.37409684f, 0.9273896f },
{ -0.38553834f, 0.9226918f },
{ -0.39692086f, 0.9178528f },
{ -0.40824264f, 0.91287345f },
{ -0.419502f, 0.9077544f },
{ -0.43069714f, 0.9024965f },
{ -0.44182643f, 0.89710057f },
{ -0.4528881f, 0.89156735f },
{ -0.4638805f, 0.88589776f },
{ -0.47480196f, 0.8800927f },
{ -0.48565078f, 0.8741529f },
{ -0.4964253f, 0.8680794f },
{ -0.5071239f, 0.8618732f },
{ -0.51774484f, 0.8555351f },
{ -0.52828664f, 0.8490661f },
{ -0.5387476f, 0.8424672f },
{ -0.5491262f, 0.83573943f },
{ -0.55942076f, 0.8288838f },
{ -0.5696297f, 0.82190144f },
{ -0.57975155f, 0.8147933f },
{ -0.58978474f, 0.8075605f },
{ -0.59972763f, 0.8002042f },
{ -0.60957885f, 0.79272544f },
{ -0.6193367f, 0.78512543f },
{ -0.62899995f, 0.7774054f },
{ -0.6385669f, 0.7695663f },
{ -0.6480362f, 0.76160955f },
{ -0.65740633f, 0.7535363f },
{ -0.66667587f, 0.74534774f },
{ -0.6758435f, 0.73704517f },
{ -0.6849077f, 0.7286299f },
{ -0.6938671f, 0.7201031f },
{ -0.70272034f, 0.71146613f },
{ -0.71146613f, 0.70272034f },
{ -0.7201031f, 0.6938671f },
{ -0.7286299f, 0.6849077f },
{ -0.73704517f, 0.6758435f },
{ -0.74534774f, 0.66667587f },
{ -0.7535363f, 0.65740633f },
{ -0.76160955f, 0.6480362f },
{ -0.7695663f, 0.6385669f },
{ -0.7774054f, 0.62899995f },
{ -0.78512543f, 0.6193367f },
{ -0.79272544f, 0.60957885f },
{ -0.8002042f, 0.59972763f },
{ -0.8075605f, 0.58978474f },
{ -0.8147933f, 0.57975155f },
{ -0.82190144f, 0.5696297f },
{ -0.8288838f, 0.55942076f },
{ -0.83573943f, 0.5491262f },
{ -0.8424672f, 0.5387476f },
{ -0.8490661f, 0.52828664f },
{ -0.8555351f, 0.51774484f },
{ -0.8618732f, 0.5071239f },
{ -0.8680794f, 0.4964253f },
{ -0.8741529f, 0.48565078f },
{ -0.8800927f, 0.47480196f },
{ -0.88589776f, 0.4638805f },
{ -0.89156735f, 0.4528881f },
{ -0.89710057f, 0.44182643f },
{ -0.9024965f, 0.43069714f },
{ -0.9077544f, 0.419502f },
{ -0.91287345f, 0.40824264f },
{ -0.9178528f, 0.39692086f },
{ -0.9226918f, 0.38553834f },
{ -0.9273896f, 0.37409684f },
{ -0.9319456f, 0.36259815f },
{ -0.936359f, 0.35104394f },
{ -0.9406291f, 0.33943605f },
{ -0.9447554f, 0.32777625f },
{ -0.9487371f, 0.3160663f },
{ -0.9525737f, 0.304308f },
{ -0.95626456f, 0.29250312f },
{ -0.9598091f, 0.2806535f },
{ -0.9632069f, 0.26876098f },
{ -0.9664573f, 0.25682732f },
{ -0.96955985f, 0.24485438f },
{ -0.9725141f, 0.23284398f },
{ -0.97531956f, 0.22079796f },
{ -0.97797585f, 0.20871817f },
{ -0.98048246f, 0.19660644f },
{ -0.98283917f, 0.18446463f },
{ -0.9850455f, 0.17229462f },
{ -0.9871011f, 0.16009824f },
{ -0.9890057f, 0.14787737f },
{ -0.990759f, 0.13563387f },
{ -0.9923608f, 0.12336964f },
{ -0.9938107f, 0.111086525f },
{ -0.99510866f, 0.09878642f },
{ -0.9962543f, 0.0864712f },
{ -0.99724764f, 0.074142754f },
{ -0.99808836f, 0.061802965f },
{ -0.99877644f, 0.04945372f },
{ -0.9993117f, 0.03709691f },
{ -0.99969405f, 0.024734428f },
{ -0.9999235f, 0.012368159f },
{ -1.0f, 0.0f },
};
static b3CosSin b3GetQuantizedCosSin( int code )
{
B3_ASSERT( -254 <= code && code <= 254 );
int index = b3MinInt( code < 0 ? -code : code, 254 );
b3CosSin cs = s_quantizedCosSin[index];
if ( code < 0 )
{
cs.sine = -cs.sine;
}
return cs;
}
int8_t b3QuantizeEdgeAngle( float radians )
{
float scaled = radians * ( B3_EDGE_ANGLE_STEPS / B3_PI );
int quantized = scaled >= 0.0f ? (int)( scaled + 0.5f ) : -(int)( 0.5f - scaled );
return (int8_t)b3ClampInt( quantized, -127, 127 );
}
// Ring of mesh-space edge directions in 16-bit octahedral encoding.
typedef struct b3VertexRing
{
// Number of boundary edges with 90 degree cliffs.
uint16_t cliffCount;
} b3VertexRing;
typedef struct b3VertexRingEntry
{
// Octohedral encoded edge direction.
uint16_t code;
} b3VertexRingEntry;
static b3Vec3 b3FoldOctahedral( float x, float y )
{
return (b3Vec3){
( 1.0f - b3AbsFloat( y ) ) * ( x >= 0.0f ? 1.0f : -1.0f ),
( 1.0f - b3AbsFloat( x ) ) * ( y >= 0.0f ? 1.0f : -1.0f ),
0.0f,
};
}
static b3Vec3 b3DecodeRingDirection( uint16_t code )
{
float x = (float)( code & 0xFF ) * ( 2.0f / 255.0f ) - 1.0f;
float y = (float)( code >> 8 ) * ( 2.0f / 255.0f ) - 1.0f;
float z = 1.0f - b3AbsFloat( x ) - b3AbsFloat( y );
if ( z < 0.0f )
{
b3Vec3 folded = b3FoldOctahedral( x, y );
x = folded.x;
y = folded.y;
}
return b3Normalize( (b3Vec3){ x, y, z } );
}
typedef struct b3RingEncoder
{
float* x;
float* y;
float* z;
b3VertexRingEntry* entries;
int count;
int capacity;
} b3RingEncoder;
static void b3PushRingEntry( b3RingEncoder* encoder, b3Vec3 direction )
{
int index = encoder->count;
B3_ASSERT( index < encoder->capacity );
encoder->x[index] = direction.x;
encoder->y[index] = direction.y;
encoder->z[index] = direction.z;
encoder->count = index + 1;
}
static void b3EncodeRingEntries( b3RingEncoder* encoder )
{
float* xs = encoder->x;
float* ys = encoder->y;
float* zs = encoder->z;
int count = encoder->count;
int paddedCount = ( count + 3 ) & ~3;
B3_ASSERT( paddedCount <= encoder->capacity );
for ( int i = count; i < paddedCount; ++i )
{
xs[i] = 0.0f;
ys[i] = 0.0f;
zs[i] = 1.0f;
}
for ( int i = 0; i < count; i += 4 )
{
b3FloatW4 x = b3LoadW4( xs + i );
b3FloatW4 y = b3LoadW4( ys + i );
b3FloatW4 z = b3LoadW4( zs + i );
b3FloatW4 zero = b3ZeroW4();
b3FloatW4 one = b3SplatW4( 1.0f );
b3FloatW4 minusOne = b3SplatW4( -1.0f );
b3FloatW4 sum = b3AddW4( b3AddW4( b3AbsW4( x ), b3AbsW4( y ) ), b3AbsW4( z ) );
b3FloatW4 ox = b3DivW4( x, sum );
b3FloatW4 oy = b3DivW4( y, sum );
b3FloatW4 fx = b3MulW4( b3SubW4( one, b3AbsW4( oy ) ), b3BlendW4( one, minusOne, b3LessThanW4( ox, zero ) ) );
b3FloatW4 fy = b3MulW4( b3SubW4( one, b3AbsW4( ox ) ), b3BlendW4( one, minusOne, b3LessThanW4( oy, zero ) ) );
b3FloatW4 below = b3LessThanW4( z, zero );
ox = b3BlendW4( ox, fx, below );
oy = b3BlendW4( oy, fy, below );
b3FloatW4 scale = b3SplatW4( 127.5f );
b3FloatW4 half = b3SplatW4( 0.5f );
b3StoreW4( xs + i, b3AddW4( b3MulW4( scale, b3AddW4( ox, one ) ), half ) );
b3StoreW4( ys + i, b3AddW4( b3MulW4( scale, b3AddW4( oy, one ) ), half ) );
}
for ( int i = 0; i < count; ++i )
{
int u = b3ClampInt( (int)xs[i], 0, 255 );
int v = b3ClampInt( (int)ys[i], 0, 255 );
encoder->entries[i].code = (uint16_t)( u | ( v << 8 ) );
}
}
bool b3GetMeshNeighborNormal( const b3Mesh* mesh, int triangleIndex, int edgeIndex, b3Vec3* normal )
{
const b3MeshData* data = mesh->data;
B3_ASSERT( 0 <= triangleIndex && triangleIndex < data->triangleCount );
B3_ASSERT( 0 <= edgeIndex && edgeIndex < 3 );
const int8_t* edgeAngles = b3GetMeshEdgeAngles( data );
if ( edgeAngles == NULL )
{
return false;
}
b3Vec3 scale = mesh->scale;
if ( scale.x * scale.y * scale.z < 0.0f )
{
// Reflected mesh.
edgeIndex = 2 - edgeIndex;
}
int8_t quantized = edgeAngles[3 * triangleIndex + edgeIndex];
if ( quantized == B3_BOUNDARY_EDGE_ANGLE )
{
return false;
}
const b3MeshTriangle* triangles = b3GetMeshTriangles( data );
const b3Vec3* vertices = b3GetMeshVertices( data );
b3MeshTriangle triangle = triangles[triangleIndex];
int indices[3] = { triangle.index1, triangle.index2, triangle.index3 };
b3Vec3 a = vertices[indices[edgeIndex]];
b3Vec3 b = vertices[indices[( edgeIndex + 1 ) % 3]];
b3Vec3 c = vertices[indices[( edgeIndex + 2 ) % 3]];
b3Vec3 n = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) );
b3Vec3 outward = b3Cross( b3Normalize( b3Sub( b, a ) ), n );
b3CosSin cs = b3GetQuantizedCosSin( 2 * quantized );
b3Vec3 unscaled = b3Add( b3MulSV( cs.cosine, n ), b3MulSV( cs.sine, outward ) );
b3Vec3 absScale = b3Abs( scale );
if ( absScale.y == absScale.x && absScale.z == absScale.x )
{
// Shortcut for uniform scale. Common case.
*normal = (b3Vec3){
scale.x < 0.0f ? -unscaled.x : unscaled.x,
scale.y < 0.0f ? -unscaled.y : unscaled.y,
scale.z < 0.0f ? -unscaled.z : unscaled.z,
};
return true;
}
// Handle non-uniform scale.
b3Vec3 invScale = { 1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z };
*normal = b3Normalize( b3Mul( invScale, unscaled ) );
return true;
}
static int b3GetMeshVertexIndex( const b3Mesh* mesh, b3MeshTriangle triangle, int vertexIndex )
{
b3Vec3 scale = mesh->scale;
if ( scale.x * scale.y * scale.z < 0.0f )
{
// Handle reflection.
vertexIndex = ( 3 - vertexIndex ) % 3;
}
int indices[3] = { triangle.index1, triangle.index2, triangle.index3 };
return indices[vertexIndex];
}
int b3GetMeshVertexFlag( const b3Mesh* mesh, int triangleIndex, int vertexIndex )
{
const b3MeshData* data = mesh->data;
B3_ASSERT( 0 <= triangleIndex && triangleIndex < data->triangleCount );
B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 );
const uint8_t* vertexFlags = b3GetMeshVertexFlags( data );
if ( vertexFlags == NULL )
{
return b3_flatVertex;
}
b3MeshTriangle triangle = b3GetMeshTriangles( data )[triangleIndex];
int index = b3GetMeshVertexIndex( mesh, triangle, vertexIndex );
return vertexFlags[index];
}
// Is the candidate manifold normal in vertex Voronoi region?
bool b3IsInMeshVertexRing( const b3Mesh* mesh, int triangleIndex, int vertexIndex, b3Vec3 normal, float tolerance )
{
const b3MeshData* data = mesh->data;
B3_ASSERT( 0 <= triangleIndex && triangleIndex < data->triangleCount );
B3_ASSERT( 0 <= vertexIndex && vertexIndex < 3 );
if ( data->vertexRingOffset == 0 )
{
return true;
}
const b3MeshTriangle* triangles = b3GetMeshTriangles( data );
b3MeshTriangle triangle = triangles[triangleIndex];
int vertex = b3GetMeshVertexIndex( mesh, triangle, vertexIndex );
// Account for the mesh scale, including reflection. The cliff scale is separate
// to maintain the 90 degree angle that all cliffs use.
b3Vec3 scale = mesh->scale;
b3Vec3 edgeScale, cliffScale;
b3Vec3 absScale = b3Abs( scale );
bool uniform = absScale.y == absScale.x && absScale.z == absScale.x;
if ( uniform )
{
edgeScale =
(b3Vec3){ scale.x < 0.0f ? -1.0f : 1.0f, scale.y < 0.0f ? -1.0f : 1.0f, scale.z < 0.0f ? -1.0f : 1.0f };
cliffScale = edgeScale;
}
else
{
edgeScale = scale;
cliffScale = (b3Vec3){ 1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z };
}
const int* starts = (const int*)( (intptr_t)data + data->vertexRingOffset );
int byteCount = starts[vertex + 1] - starts[vertex];
if ( byteCount == 0 )
{
// Flat vertex
const b3Vec3* vertices = b3GetMeshVertices( data );
b3Vec3 faceNormal =
b3MakeNormalFromPoints( vertices[triangle.index1], vertices[triangle.index2], vertices[triangle.index3] );
faceNormal = b3Normalize( b3Mul( cliffScale, faceNormal ) );
float cosAngle = b3Dot( normal, faceNormal );
return cosAngle > 0.0f && cosAngle * cosAngle >= 1.0f - tolerance * tolerance;
}
const b3VertexRing* ring = (const b3VertexRing*)( (intptr_t)data + starts[vertex] );
const b3VertexRingEntry* entries = (const b3VertexRingEntry*)( ring + 1 );
int entryCount = ( byteCount - (int)sizeof( b3VertexRing ) ) / (int)sizeof( b3VertexRingEntry );
int edgeCount = entryCount - ring->cliffCount;
b3Vec3 edgeNormal = b3Mul( edgeScale, normal );
b3Vec3 cliffNormal = b3Mul( cliffScale, normal );
for ( int i = 0; i < entryCount; ++i )
{
bool isCliff = i >= edgeCount;
b3Vec3 direction = b3DecodeRingDirection( entries[i].code );
float lean = b3Dot( isCliff ? cliffNormal : edgeNormal, direction );
float limit = tolerance;
if ( uniform == false )
{
limit *= b3Length( b3Mul( isCliff ? cliffScale : edgeScale, direction ) );
}
if ( lean > limit )
{
return false;
}
}
return true;
}
static void b3IdentifyEdges( b3MeshData* mesh, b3Vec3* normals )
{
b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh );
const b3Vec3* vertices = b3GetMeshVertices( mesh );
uint8_t* flags = b3GetMeshFlagsWrite( mesh );
int8_t* edgeAngles = b3GetMeshEdgeAnglesWrite( mesh );
B3_ASSERT( edgeAngles != NULL );
int triangleCount = mesh->triangleCount;
int edgeCount = 3 * triangleCount;
for ( int i = 0; i < edgeCount; ++i )
{
edgeAngles[i] = B3_BOUNDARY_EDGE_ANGLE;
}
b3MeshEdge* edges = B3_ALLOC( b3MeshEdge, edgeCount );
for ( int i = 0; i < triangleCount; ++i )
{
b3MeshTriangle* triangle = triangles + i;
int i1 = triangle->index1;
int i2 = triangle->index2;
int i3 = triangle->index3;
edges[3 * i + 0].vertex1 = b3MinInt( i1, i2 );
edges[3 * i + 0].vertex2 = b3MaxInt( i1, i2 );
edges[3 * i + 0].triangle1 = i;
edges[3 * i + 0].triangle2 = B3_NULL_INDEX;
edges[3 * i + 0].triangleEdgeIndex1 = 0;
edges[3 * i + 0].triangleEdgeIndex2 = 0xFF;
edges[3 * i + 0].triangleCount = 1;
edges[3 * i + 1].vertex1 = b3MinInt( i2, i3 );
edges[3 * i + 1].vertex2 = b3MaxInt( i2, i3 );
edges[3 * i + 1].triangle1 = i;
edges[3 * i + 1].triangle2 = B3_NULL_INDEX;
edges[3 * i + 1].triangleEdgeIndex1 = 1;
edges[3 * i + 1].triangleEdgeIndex2 = 0xFF;
edges[3 * i + 1].triangleCount = 1;
edges[3 * i + 2].vertex1 = b3MinInt( i3, i1 );
edges[3 * i + 2].vertex2 = b3MaxInt( i3, i1 );
edges[3 * i + 2].triangle1 = i;
edges[3 * i + 2].triangle2 = B3_NULL_INDEX;
edges[3 * i + 2].triangleEdgeIndex1 = 2;
edges[3 * i + 2].triangleEdgeIndex2 = 0xFF;
edges[3 * i + 2].triangleCount = 1;
b3Vec3 v1 = vertices[i1];
b3Vec3 v2 = vertices[i2];
b3Vec3 v3 = vertices[i3];
b3Vec3 e1 = b3Sub( v2, v1 );
b3Vec3 e2 = b3Sub( v3, v1 );
b3Vec3 n = b3Cross( e1, e2 );
normals[i] = b3Normalize( n );
}
// Counting sort for edges. Faster than a hashtable.
int vertexCount = mesh->vertexCount;
int* edgeStarts = B3_ALLOC( int, ( vertexCount + 1 ) );
int* sortedEdges = B3_ALLOC( int, edgeCount );
int* stamps = B3_ALLOC( int, vertexCount );
int* firstEdges = B3_ALLOC( int, vertexCount );
for ( int i = 0; i <= vertexCount; ++i )
{
edgeStarts[i] = 0;
}
for ( int i = 0; i < vertexCount; ++i )
{
stamps[i] = B3_NULL_INDEX;
}
for ( int i = 0; i < edgeCount; ++i )
{
B3_ASSERT( 0 <= edges[i].vertex1 && edges[i].vertex2 < vertexCount );
edgeStarts[edges[i].vertex1 + 1] += 1;
}
for ( int i = 0; i < vertexCount; ++i )
{
edgeStarts[i + 1] += edgeStarts[i];
}
for ( int i = 0; i < edgeCount; ++i )
{
int vertex1 = edges[i].vertex1;
sortedEdges[edgeStarts[vertex1]] = i;
edgeStarts[vertex1] += 1;
}
// Find unique edges and assign adjacency
int begin = 0;
for ( int vertex1 = 0; vertex1 < vertexCount; ++vertex1 )
{
int end = edgeStarts[vertex1];
for ( int j = begin; j < end; ++j )
{
int i = sortedEdges[j];
b3MeshEdge* edge = edges + i;
int vertex2 = edge->vertex2;
// Establish twin.
if ( stamps[vertex2] != vertex1 )
{
stamps[vertex2] = vertex1;
firstEdges[vertex2] = i;
continue;
}
int otherIndex = firstEdges[vertex2];
B3_ASSERT( otherIndex < i );
b3MeshEdge* base = edges + otherIndex;
if ( base->triangleCount == 1 )
{
base->triangle2 = edge->triangle1;
base->triangleEdgeIndex2 = edge->triangleEdgeIndex1;
}
base->triangleCount += 1;
}
begin = end;
}
B3_FREE( firstEdges, int, vertexCount );
B3_FREE( stamps, int, vertexCount );
B3_FREE( sortedEdges, int, edgeCount );
B3_FREE( edgeStarts, int, ( vertexCount + 1 ) );
int angleCapacity = ( edgeCount / 2 + 3 ) & ~3;
float* angleYs = B3_ALLOC( float, angleCapacity );
float* angleXs = B3_ALLOC( float, angleCapacity );
float* angleLengthSqs = B3_ALLOC( float, angleCapacity );
int* angleSlots = B3_ALLOC( int, 2 * angleCapacity );
int angleCount = 0;
for ( int i = 0; i < edgeCount; ++i )
{
b3MeshEdge* edge = edges + i;
if ( edge->triangleCount != 2 )
{
continue;
}
B3_ASSERT( edge->triangleEdgeIndex1 < 3 );
B3_ASSERT( edge->triangleEdgeIndex2 < 3 );
b3MeshTriangle* triangle1 = triangles + edge->triangle1;
b3MeshTriangle* triangle2 = triangles + edge->triangle2;
uint8_t* flag1 = flags + edge->triangle1;
uint8_t* flag2 = flags + edge->triangle2;
int j1 = triangle2->index1;
int j2 = triangle2->index2;
int j3 = triangle2->index3;
int opposite = B3_NULL_INDEX;
switch ( edge->triangleEdgeIndex2 )
{
case 0:
opposite = j3;
break;
case 1:
opposite = j1;
break;
case 2:
opposite = j2;
break;
default:
B3_ASSERT( false );
}
int i1 = triangle1->index1;
int i2 = triangle1->index2;
int i3 = triangle1->index3;
b3Vec3 v1 = vertices[i1];
b3Vec3 v2 = vertices[i2];
b3Vec3 v3 = vertices[i3];
b3Vec3 p = vertices[opposite];
b3Vec3 n1 = normals[edge->triangle1];
b3Vec3 n2 = normals[edge->triangle2];
// Quantize the angles between neighboring normals.
int indices1[3] = { i1, i2, i3 };
int indices2[3] = { j1, j2, j3 };
int k1 = edge->triangleEdgeIndex1;
int k2 = edge->triangleEdgeIndex2;
if ( indices1[k1] == indices2[( k2 + 1 ) % 3] )
{
b3Vec3 a = vertices[indices1[k1]];
b3Vec3 b = vertices[indices1[( k1 + 1 ) % 3]];
b3Vec3 edgeVector = b3Sub( b, a );
b3Vec3 outward = b3Cross( edgeVector, n1 );
B3_ASSERT( angleCount < angleCapacity );
angleYs[angleCount] = b3Dot( n2, outward );
angleXs[angleCount] = b3Dot( n2, n1 );
angleLengthSqs[angleCount] = b3Dot( edgeVector, edgeVector );
angleSlots[2 * angleCount + 0] = 3 * edge->triangle1 + k1;
angleSlots[2 * angleCount + 1] = 3 * edge->triangle2 + k2;
angleCount += 1;
}
float cos5Deg = 0.9962f;
float signedVolume = b3SignedVolume( v1, v2, v3, p );
float cosAngle = b3Dot( n1, n2 );
if ( signedVolume > 0.0f || cosAngle > cos5Deg )
{
int edgeFlags[3] = { b3_concaveEdge1, b3_concaveEdge2, b3_concaveEdge3 };
*flag1 |= edgeFlags[edge->triangleEdgeIndex1];
*flag2 |= edgeFlags[edge->triangleEdgeIndex2];
}
if ( signedVolume < 0.0f || cosAngle > cos5Deg )
{
int edgeFlags[3] = { b3_inverseConcaveEdge1, b3_inverseConcaveEdge2, b3_inverseConcaveEdge3 };
*flag1 |= edgeFlags[edge->triangleEdgeIndex1];
*flag2 |= edgeFlags[edge->triangleEdgeIndex2];
}
}
int paddedAngleCount = ( angleCount + 3 ) & ~3;
B3_ASSERT( paddedAngleCount <= angleCapacity );
for ( int i = angleCount; i < paddedAngleCount; ++i )
{
angleYs[i] = 0.0f;
angleXs[i] = 0.0f;
angleLengthSqs[i] = 0.0f;
}
for ( int i = 0; i < angleCount; i += 4 )
{
b3FloatW4 x = b3MulW4( b3LoadW4( angleXs + i ), b3SqrtW4( b3LoadW4( angleLengthSqs + i ) ) );
b3FloatW4 angle = b3Atan2W4( b3LoadW4( angleYs + i ), x );
b3StoreW4( angleYs + i, angle );
}
for ( int i = 0; i < angleCount; ++i )
{
int8_t quantized = b3QuantizeEdgeAngle( angleYs[i] );
edgeAngles[angleSlots[2 * i + 0]] = quantized;
edgeAngles[angleSlots[2 * i + 1]] = quantized;
}
B3_FREE( angleSlots, int, 2 * angleCapacity );
B3_FREE( angleLengthSqs, float, angleCapacity );
B3_FREE( angleXs, float, angleCapacity );
B3_FREE( angleYs, float, angleCapacity );
uint8_t* vertexFlags = b3GetMeshVertexFlagsWrite( mesh );
B3_ASSERT( vertexFlags != NULL );
for ( int i = 0; i < mesh->vertexCount; ++i )
{
vertexFlags[i] = b3_flatVertex;
}
// Vertex flags for ghost collision handling.
for ( int i = 0; i < triangleCount; ++i )
{
int indices[3] = { triangles[i].index1, triangles[i].index2, triangles[i].index3 };
for ( int k = 0; k < 3; ++k )
{
int8_t angle = edgeAngles[3 * i + k];
int a = indices[k];
int b = indices[k < 2 ? k + 1 : 0];
// Edge not flat?
if ( angle != 0 )
{
vertexFlags[a] = (uint8_t)( vertexFlags[a] & ~b3_flatVertex );
vertexFlags[b] = (uint8_t)( vertexFlags[b] & ~b3_flatVertex );
}
// Edge concave?
if ( angle < 0 && angle != B3_BOUNDARY_EDGE_ANGLE )
{
vertexFlags[a] |= b3_concaveVertex;
vertexFlags[b] |= b3_concaveVertex;
}
}
}
B3_FREE( edges, b3MeshEdge, edgeCount );
}
// Push the ring entry directions of a vertex to the encoder. Returns the cliff count.
static int b3BuildVertexRing( const b3MeshData* mesh, const b3Vec3* normals, const int* fan, int fanCount, int vertex,
int* stamps, b3RingEncoder* encoder )
{
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh );
const b3Vec3* vertices = b3GetMeshVertices( mesh );
const int8_t* edgeAngles = b3GetMeshEdgeAngles( mesh );
for ( int i = 0; i < fanCount; ++i )
{
b3MeshTriangle triangle = triangles[fan[i]];
int indices[3] = { triangle.index1, triangle.index2, triangle.index3 };
int k = indices[0] == vertex ? 0 : ( indices[1] == vertex ? 1 : 2 );
// The other vertex indices on this triangle that are not the input vertex index.
int others[2] = { indices[k < 2 ? k + 1 : 0], indices[k > 0 ? k - 1 : 2] };
for ( int j = 0; j < 2; ++j )
{
int other = others[j];
if ( stamps[other] == vertex )
{
continue;
}
stamps[other] = vertex;
// This encodes every edge. The intersection of all planes perpendicular to the edge axis builds
// the Voronoi region. That process can identify edges that don't contribute to the Voronoi region.
// That culling could save storage but is skipped for simplicity.
b3PushRingEntry( encoder, b3Sub( vertices[other], vertices[vertex] ) );
}
}
int cliffCount = 0;
for ( int i = 0; i < fanCount; ++i )
{
int triangleIndex = fan[i];
b3MeshTriangle triangle = triangles[triangleIndex];
int indices[3] = { triangle.index1, triangle.index2, triangle.index3 };
int k = indices[0] == vertex ? 0 : ( indices[1] == vertex ? 1 : 2 );
int edgeIndices[2] = { k, k > 0 ? k - 1 : 2 };
for ( int j = 0; j < 2; ++j )
{
if ( edgeAngles[3 * triangleIndex + edgeIndices[j]] != B3_BOUNDARY_EDGE_ANGLE )
{
continue;
}
// The cliff is a 90 degree drop.
b3PushRingEntry( encoder, b3Neg( normals[triangleIndex] ) );
cliffCount += 1;
}
}
return cliffCount;
}
// Build a neighbor angle ring for each vertex. Used to mitigate ghost collisions with minimal tolerances
// and fudge factors.
static b3MeshData* b3AppendVertexRings( b3MeshData* mesh, const b3Vec3* normals )
{
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh );
const uint8_t* vertexFlags = b3GetMeshVertexFlags( mesh );
B3_ASSERT( vertexFlags != NULL );
int vertexCount = mesh->vertexCount;
int triangleCount = mesh->triangleCount;
// Triangle fan counts per vertex via counting sort and prefix sum.
int* fanStarts = B3_ALLOC( int, ( vertexCount + 1 ) );
// Sub-arrays of triangle fans for each vertex.
int* fans = B3_ALLOC( int, ( 3 * triangleCount ) );
// Cursors for walking fan sub-arrays.
int* cursors = B3_ALLOC( int, vertexCount );
// Edge stamps to avoid visiting the same edge twice when
// iterating triangles.
int* stamps = B3_ALLOC( int, vertexCount );
// Byte offsets for each sub-array of ring entries per vertex.
int* ringStarts = B3_ALLOC( int, ( vertexCount + 1 ) );
// Counting sort
memset( fanStarts, 0, ( vertexCount + 1 ) * sizeof( int ) );
for ( int i = 0; i < triangleCount; ++i )
{
fanStarts[triangles[i].index1 + 1] += 1;
fanStarts[triangles[i].index2 + 1] += 1;
fanStarts[triangles[i].index3 + 1] += 1;
}
for ( int i = 0; i < vertexCount; ++i )
{
fanStarts[i + 1] += fanStarts[i];
}
memcpy( cursors, fanStarts, vertexCount * sizeof( int ) );
for ( int i = 0; i < triangleCount; ++i )
{
fans[cursors[triangles[i].index1]++] = i;
fans[cursors[triangles[i].index2]++] = i;
fans[cursors[triangles[i].index3]++] = i;
}
for ( int i = 0; i < vertexCount; ++i )
{
stamps[i] = B3_NULL_INDEX;
}
// Hey dude, I got an offset to the array of offsets.
size_t startsOffset = mesh->byteCount;
// After the array of offsets, the ring sub-arrays begin.
size_t ringOffset = startsOffset + b3AlignUp8( ( vertexCount + 1 ) * sizeof( int ) );
// Upper bound on entries: two edges per fan triangle and two cliffs per boundary edge.
const int8_t* edgeAngles = b3GetMeshEdgeAngles( mesh );
int entryBound = 0;
for ( int i = 0; i < vertexCount; ++i )
{
if ( ( vertexFlags[i] & b3_flatVertex ) == 0 )
{
entryBound += 2 * ( fanStarts[i + 1] - fanStarts[i] );
}
}
for ( int i = 0; i < 3 * triangleCount; ++i )
{
if ( edgeAngles[i] == B3_BOUNDARY_EDGE_ANGLE )
{
entryBound += 2;
}
}
b3RingEncoder encoder = { 0 };
encoder.capacity = ( entryBound + 3 ) & ~3;
encoder.x = B3_ALLOC( float, encoder.capacity );
encoder.y = B3_ALLOC( float, encoder.capacity );
encoder.z = B3_ALLOC( float, encoder.capacity );
encoder.entries = B3_ALLOC( b3VertexRingEntry, encoder.capacity );
int* entryStarts = B3_ALLOC( int, ( vertexCount + 1 ) );
int* cliffCounts = B3_ALLOC( int, vertexCount );
// The vertex ring offset for the current vertex.
size_t offset = ringOffset;
for ( int i = 0; i < vertexCount; ++i )
{
ringStarts[i] = (int)offset;
entryStarts[i] = encoder.count;
cliffCounts[i] = 0;
if ( vertexFlags[i] & b3_flatVertex )
{
// Flat vertices don't store ring data.
continue;
}
const int* fan = fans + fanStarts[i];
int fanCount = fanStarts[i + 1] - fanStarts[i];
cliffCounts[i] = b3BuildVertexRing( mesh, normals, fan, fanCount, i, stamps, &encoder );
int entryCount = encoder.count - entryStarts[i];
offset += sizeof( b3VertexRing ) + entryCount * sizeof( b3VertexRingEntry );
}
ringStarts[vertexCount] = (int)offset;
entryStarts[vertexCount] = encoder.count;
b3EncodeRingEntries( &encoder );
// We now have the required space, grow the blob.
size_t byteCount = ringOffset + b3AlignUp8( offset - ringOffset );
b3MeshData* result = b3Alloc( byteCount );
memset( result, 0, byteCount );
memcpy( result, mesh, mesh->byteCount );
b3Free( mesh, mesh->byteCount );
result->byteCount = (int)byteCount;
result->vertexRingOffset = (int)startsOffset;
// Copy the ring sub-array offsets per vertex. The vertexCount + 1 entry is the end offset.
memcpy( (uint8_t*)result + startsOffset, ringStarts, ( vertexCount + 1 ) * sizeof( int ) );
for ( int i = 0; i < vertexCount; ++i )
{
if ( ringStarts[i + 1] == ringStarts[i] )
{
continue;
}
b3VertexRing* ring = (b3VertexRing*)( (uint8_t*)result + ringStarts[i] );
B3_ASSERT( cliffCounts[i] <= UINT16_MAX );
ring->cliffCount = (uint16_t)cliffCounts[i];
int entryCount = entryStarts[i + 1] - entryStarts[i];
memcpy( ring + 1, encoder.entries + entryStarts[i], entryCount * sizeof( b3VertexRingEntry ) );
}
B3_FREE( cliffCounts, int, vertexCount );
B3_FREE( entryStarts, int, ( vertexCount + 1 ) );
B3_FREE( encoder.entries, b3VertexRingEntry, encoder.capacity );
B3_FREE( encoder.z, float, encoder.capacity );
B3_FREE( encoder.y, float, encoder.capacity );
B3_FREE( encoder.x, float, encoder.capacity );
B3_FREE( ringStarts, int, ( vertexCount + 1 ) );
B3_FREE( stamps, int, vertexCount );
B3_FREE( cursors, int, vertexCount );
B3_FREE( fans, int, ( 3 * triangleCount ) );
B3_FREE( fanStarts, int, ( vertexCount + 1 ) );
return result;
}
b3MeshData* b3CreateGridMesh( int xCount, int zCount, float cellWidth, int materialCount, bool identifyEdges )
{
B3_ASSERT( 0 <= materialCount && materialCount <= UINT8_MAX );
// Create vertices
int vertexCount = ( xCount + 1 ) * ( zCount + 1 );
b3Array( b3Vec3 ) vertices = { 0 };
b3Array_Resize( vertices, vertexCount );
int index = 0;
float xWidth = cellWidth * xCount;
float zWidth = cellWidth * zCount;
float x = -0.5f * xWidth;
for ( int ix = 0; ix <= xCount; ++ix )
{
float z = -0.5f * zWidth;
for ( int iz = 0; iz <= zCount; ++iz )
{
vertices.data[index] = (b3Vec3){ x, 0.0f, z };
z += cellWidth;
index += 1;
}
x += cellWidth;
}
B3_ASSERT( index == vertexCount );
// Triangles
int triangleCount = 2 * xCount * zCount;
b3Array( int ) indices = { 0 };
b3Array_Resize( indices, 3 * triangleCount );
b3Array( uint8_t ) materialIndices = { 0 };
b3Array_Resize( materialIndices, triangleCount );
int materialIndex = 0;
index = 0;
for ( int ix = 0; ix < xCount; ++ix )
{
for ( int iz = 0; iz < zCount; ++iz )
{
int index1 = iz + ( zCount + 1 ) * ix;
int index2 = index1 + 1;
int index3 = index2 + ( zCount + 1 );
int index4 = index3 - 1;
B3_ASSERT( index1 < vertexCount );
B3_ASSERT( index2 < vertexCount );
B3_ASSERT( index3 < vertexCount );
B3_ASSERT( index4 < vertexCount );
indices.data[index + 0] = index1;
indices.data[index + 1] = index2;
indices.data[index + 2] = index3;
indices.data[index + 3] = index3;
indices.data[index + 4] = index4;
indices.data[index + 5] = index1;
if ( materialCount > 0 )
{
materialIndices.data[2 * materialIndex + 0] = (uint8_t)( materialIndex % materialCount );
materialIndices.data[2 * materialIndex + 1] = (uint8_t)( materialIndex % materialCount );
}
materialIndex += 1;
index += 6;
}
}
B3_ASSERT( index == 3 * triangleCount );
b3MeshDef def = { 0 };
def.vertexCount = vertices.count;
def.vertices = vertices.data;
def.triangleCount = indices.count / 3;
def.indices = indices.data;
def.materialIndices = materialCount > 0 ? materialIndices.data : NULL;
def.useMedianSplit = true;
def.identifyEdges = identifyEdges;
b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 );
b3Array_Destroy( indices );
b3Array_Destroy( vertices );
b3Array_Destroy( materialIndices );
return meshData;
}
b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amplitude, float rowFrequency,
float columnFrequency )
{
// Create vertices
int vertexCount = ( xCount + 1 ) * ( zCount + 1 );
b3Array( b3Vec3 ) vertices = { 0 };
b3Array_Resize( vertices, vertexCount );
int index = 0;
float xWidth = cellWidth * xCount;
float zWidth = cellWidth * zCount;
float omegaZ = 2.0f * B3_PI * rowFrequency * cellWidth;
float omegaX = 2.0f * B3_PI * columnFrequency * cellWidth;
float x = -0.5f * xWidth;
b3CosSin cs = { 0 };
for ( int ix = 0; ix <= xCount; ++ix )
{
cs = b3ComputeCosSin( omegaX * ix );
float rowHeight = cs.sine;
float z = -0.5f * zWidth;
for ( int iz = 0; iz <= zCount; ++iz )
{
cs = b3ComputeCosSin( omegaZ * iz );
float columnHeight = cs.sine;
float y = amplitude * rowHeight * columnHeight;
vertices.data[index] = (b3Vec3){ x, y, z };
z += cellWidth;
index += 1;
}
x += cellWidth;
}
B3_ASSERT( index == vertexCount );
// Triangles
int triangleCount = 2 * xCount * zCount;
b3Array( int ) indices = { 0 };
b3Array_Resize( indices, 3 * triangleCount );
index = 0;
for ( int ix = 0; ix < xCount; ++ix )
{
for ( int iz = 0; iz < zCount; ++iz )
{
int index1 = iz + ( zCount + 1 ) * ix;
int index2 = index1 + 1;
int index3 = index2 + ( zCount + 1 );
int index4 = index3 - 1;
B3_ASSERT( index1 < vertexCount );
B3_ASSERT( index2 < vertexCount );
B3_ASSERT( index3 < vertexCount );
B3_ASSERT( index4 < vertexCount );
indices.data[index + 0] = index1;
indices.data[index + 1] = index2;
indices.data[index + 2] = index3;
indices.data[index + 3] = index3;
indices.data[index + 4] = index4;
indices.data[index + 5] = index1;
index += 6;
}
}
B3_ASSERT( index == 3 * triangleCount );
b3MeshDef def = { 0 };
def.vertexCount = vertices.count;
def.vertices = vertices.data;
def.triangleCount = indices.count / 3;
def.indices = indices.data;
def.useMedianSplit = true;
def.identifyEdges = true;
b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 );
b3Array_Destroy( indices );
b3Array_Destroy( vertices );
return meshData;
}
b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness )
{
// Create vertices
b3Array( b3Vec3 ) vertices = { 0 };
for ( int radialIndex = 0; radialIndex < radialResolution; radialIndex++ )
{
for ( int tubularIndex = 0; tubularIndex < tubularResolution; tubularIndex++ )
{
float u = (float)tubularIndex / tubularResolution * B3_TWO_PI;
float v = (float)radialIndex / radialResolution * B3_TWO_PI;
float x = ( radius + thickness * b3Cos( v ) ) * b3Cos( u );
float y = ( radius + thickness * b3Cos( v ) ) * b3Sin( u );
float z = thickness * b3Sin( v );
b3Vec3 vertex = { x, y, z };
b3Array_Push( vertices, vertex );
}
}
// Triangles
b3Array( int ) indices = { 0 };
for ( int radialIndex1 = 0; radialIndex1 < radialResolution; radialIndex1++ )
{
int radialIndex2 = ( radialIndex1 + 1 ) % radialResolution;
for ( int tubularIndex1 = 0; tubularIndex1 < tubularResolution; tubularIndex1++ )
{
int tubularIndex2 = ( tubularIndex1 + 1 ) % tubularResolution;
int index1 = radialIndex1 * tubularResolution + tubularIndex1;
int index2 = radialIndex1 * tubularResolution + tubularIndex2;
int index3 = radialIndex2 * tubularResolution + tubularIndex2;
int index4 = radialIndex2 * tubularResolution + tubularIndex1;
b3Array_Push( indices, index1 );
b3Array_Push( indices, index2 );
b3Array_Push( indices, index3 );
b3Array_Push( indices, index3 );
b3Array_Push( indices, index4 );
b3Array_Push( indices, index1 );
}
}
b3MeshDef def = { 0 };
def.vertexCount = vertices.count;
def.vertices = vertices.data;
def.triangleCount = indices.count / 3;
def.indices = indices.data;
def.useMedianSplit = false;
def.identifyEdges = true;
b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 );
b3Array_Destroy( vertices );
b3Array_Destroy( indices );
return meshData;
}
b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges )
{
float x = extent.x;
float y = extent.y;
float z = extent.z;
b3Vec3 vertices[] = {
{ x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z },
{ x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z },
};
for ( int i = 0; i < 8; ++i )
{
vertices[i] = b3Add( vertices[i], center );
}
int indices[] = {
0, 1, 3, 1, 2, 3, // front
0, 4, 1, 1, 4, 5, // top
0, 3, 7, 4, 0, 7, // right
4, 7, 5, 6, 5, 7, // back
1, 5, 2, 6, 2, 5, // left
3, 2, 7, 6, 7, 2, // bottom
};
b3MeshDef def = { 0 };
def.vertexCount = 8;
def.vertices = vertices;
def.triangleCount = 12;
def.indices = indices;
def.useMedianSplit = false;
def.identifyEdges = identifyEdges;
return b3CreateMesh( &def, NULL, 0 );
}
b3MeshData* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent )
{
float x = extent.x;
float y = extent.y;
float z = extent.z;
b3Vec3 vertices[] = {
{ x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z },
{ x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z },
};
for ( int i = 0; i < 8; ++i )
{
vertices[i] = b3Add( vertices[i], center );
}
int indices[] = {
3, 1, 0, 3, 2, 1, // front
1, 4, 0, 5, 4, 1, // top
7, 3, 0, 7, 0, 4, // right
5, 7, 4, 7, 5, 6, // back
2, 5, 1, 5, 2, 6, // left
7, 2, 3, 2, 7, 6, // bottom
};
b3MeshDef def = { 0 };
def.vertexCount = 8;
def.vertices = vertices;
def.triangleCount = 12;
def.indices = indices;
def.useMedianSplit = false;
def.identifyEdges = true;
return b3CreateMesh( &def, NULL, 0 );
}
b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth )
{
float hb = 0.5f * bottomWidth;
float ht = 0.5f * topWidth;
float hy = 0.5f * height;
b3Vec3 vertices[] = {
{ ht, hy, ht }, { -ht, hy, ht }, { -hb, -hy, hb }, { hb, -hy, hb },
{ ht, hy, -ht }, { -ht, hy, -ht }, { -hb, -hy, -hb }, { hb, -hy, -hb },
};
for ( int i = 0; i < 8; ++i )
{
vertices[i] = b3Add( vertices[i], center );
}
int indices[] = {
0, 1, 3, 1, 2, 3, // front
0, 4, 1, 1, 4, 5, // top
0, 3, 7, 4, 0, 7, // right
4, 7, 5, 6, 5, 7, // back
1, 5, 2, 6, 2, 5, // left
3, 2, 7, 6, 7, 2, // bottom
};
b3MeshDef def = { 0 };
def.vertexCount = 8;
def.vertices = vertices;
def.triangleCount = 12;
def.indices = indices;
def.useMedianSplit = true;
def.identifyEdges = true;
return b3CreateMesh( &def, NULL, 0 );
}
static void b3CopyVerticesWithStride( b3Vec3* dst, const b3Vec3* src, int count, size_t stride )
{
if ( stride == sizeof( b3Vec3 ) )
{
memcpy( dst, src, count * sizeof( b3Vec3 ) );
return;
}
for ( int i = 0; i < count; ++i )
{
dst[i] = *(const b3Vec3*)( (const uint8_t*)src + i * stride );
}
}
// Stride larger than this likely indicates stride is uninitialized memory.
#define B3_MAX_STRIDE 4096
// todo this should fail if the mesh has a height greater than B3_MESH_STACK_SIZE
b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity )
{
if ( def->stride != 0 && ( def->stride < sizeof( b3Vec3 ) || B3_MAX_STRIDE < def->stride ) )
{
return NULL;
}
if ( ( def->stride & 3 ) != 0 )
{
return NULL;
}
if ( def->vertexCount < 3 || def->vertices == NULL || def->triangleCount <= 0 || def->indices == NULL )
{
return NULL;
}
int triangleCount = def->triangleCount;
if ( triangleCount == 0 )
{
return NULL;
}
int vertexCount = def->vertexCount;
size_t stride = def->stride == 0 ? sizeof( b3Vec3 ) : def->stride;
b3AABB meshBounds = B3_BOUNDS3_EMPTY;
// Clone indices and vertices to support welding
b3Array( int ) indices;
b3Array_CreateN( indices, 3 * triangleCount );
b3Array( b3Vec3 ) vertices;
b3Array_CreateN( vertices, vertexCount );
if ( def->weldVertices && def->weldTolerance > 0.0f )
{
b3Array_Resize( vertices, vertexCount );
b3Array_Resize( indices, 3 * triangleCount );
b3WeldData data = {
.srcVertices = def->vertices,
.srcIndices = def->indices,
.dstVertices = vertices.data,
.dstIndices = indices.data,
.vertexCount = vertexCount,
.indexCount = 3 * triangleCount,
.stride = stride,
};
vertices.count = b3WeldVertices( &data, def->weldTolerance );
vertexCount = vertices.count;
B3_ASSERT( vertexCount <= def->vertexCount );
}
else
{
vertices.count = vertexCount;
b3CopyVerticesWithStride( vertices.data, def->vertices, vertexCount, stride );
b3Array_Append( indices, def->indices, 3 * triangleCount );
}
b3Array( b3Primitive ) primitives;
b3Array_CreateN( primitives, triangleCount );
int degenerateCount = 0;
float minArea = 0.01f * B3_LINEAR_SLOP * B3_LINEAR_SLOP;
float surfaceArea = 0.0f;
int materialCount = 1;
bool clockWise = def->clockWiseWinding;
for ( int index = 0; index < triangleCount; ++index )
{
int index1 = indices.data[3 * index + 0];
int index2 = indices.data[3 * index + 1];
int index3 = indices.data[3 * index + 2];
B3_ASSERT( 0 <= index1 && index1 < vertexCount );
B3_ASSERT( 0 <= index2 && index2 < vertexCount );
B3_ASSERT( 0 <= index3 && index3 < vertexCount );
b3Vec3 vertex1 = vertices.data[index1];
b3Vec3 vertex2 = vertices.data[index2];
b3Vec3 vertex3 = vertices.data[index3];
b3Vec3 normal = b3Cross( b3Sub( vertex2, vertex1 ), b3Sub( vertex3, vertex1 ) );
float area = 0.5f * b3Length( normal );
if ( area < minArea )
{
// b3Log( "degenerate: %d %d %d\n", index1, index2, index3 );
if ( index1 != index2 && index1 != index3 && index2 != index3 )
{
if ( degenerateTriangleIndices != NULL && degenerateCount < degenerateCapacity )
{
degenerateTriangleIndices[degenerateCount] = index;
}
degenerateCount += 1;
}
continue;
}
surfaceArea += area;
b3AABB box = {
.lowerBound = b3Min( vertex1, b3Min( vertex2, vertex3 ) ),
.upperBound = b3Max( vertex1, b3Max( vertex2, vertex3 ) ),
};
b3Vec3 center = b3AABB_Center( box );
b3Primitive primitive = {
.aabb = box,
.center = center,
.triangleIndex = index,
};
b3Array_Push( primitives, primitive );
if ( def->materialIndices != NULL )
{
materialCount = b3MaxInt( materialCount, def->materialIndices[index] + 1 );
}
meshBounds = b3AABB_Union( meshBounds, box );
}
// Update triangle count due to degenerates being skipped
triangleCount = primitives.count;
if ( b3IsSaneAABB( meshBounds ) == false )
{
b3Array_Destroy( primitives );
b3Array_Destroy( indices );
b3Array_Destroy( vertices );
return NULL;
}
// Build the tree (this reorders the builder triangles)
b3Array( b3MeshNode ) tempNodes;
b3Array_CreateN( tempNodes, 2 * triangleCount - 1 );
int treeHeight = 0;
b3BuildRecursive( &tempNodes, triangleCount, primitives.data, primitives.data, def->useMedianSplit, &treeHeight );
// Allocate the mesh
size_t byteCount = b3AlignUp8( sizeof( b3MeshData ) );
int nodeOffset = (int)byteCount;
byteCount += b3AlignUp8( tempNodes.count * sizeof( b3MeshNode ) );
int vertexOffset = (int)byteCount;
byteCount += b3AlignUp8( vertexCount * sizeof( b3Vec3 ) );
int triangleOffset = (int)byteCount;
byteCount += b3AlignUp8( triangleCount * sizeof( b3MeshTriangle ) );
int materialIndicesOffset = (int)byteCount;
byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) );
int flagsOffset = (int)byteCount;
byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) );
int edgeAngleOffset = 0;
int vertexFlagOffset = 0;
if ( def->identifyEdges )
{
edgeAngleOffset = (int)byteCount;
byteCount += b3AlignUp8( 3 * triangleCount * sizeof( int8_t ) );
vertexFlagOffset = (int)byteCount;
byteCount += b3AlignUp8( vertexCount * sizeof( uint8_t ) );
}
b3MeshData* mesh = b3Alloc( byteCount );
// zero initialize for determinism
memset( mesh, 0, byteCount );
mesh->version = B3_MESH_VERSION;
mesh->byteCount = (int)byteCount;
mesh->bounds = meshBounds;
mesh->surfaceArea = surfaceArea;
mesh->nodeCount = tempNodes.count;
mesh->treeHeight = treeHeight;
mesh->vertexCount = vertexCount;
mesh->triangleCount = triangleCount;
mesh->degenerateCount = degenerateCount;
mesh->nodeOffset = nodeOffset;
mesh->vertexOffset = vertexOffset;
mesh->triangleOffset = triangleOffset;
mesh->materialOffset = materialIndicesOffset;
mesh->materialCount = materialCount;
mesh->flagsOffset = flagsOffset;
mesh->edgeAngleOffset = edgeAngleOffset;
mesh->vertexFlagOffset = vertexFlagOffset;
b3MeshNode* nodes = b3GetMeshNodesWrite( mesh );
b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh );
b3Vec3* meshVertices = b3GetMeshVerticesWrite( mesh );
uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh );
uint8_t* flags = b3GetMeshFlagsWrite( mesh );
memcpy( nodes, tempNodes.data, tempNodes.count * sizeof( b3MeshNode ) );
memcpy( meshVertices, vertices.data, vertexCount * sizeof( b3Vec3 ) );
for ( int index = 0; index < triangleCount; ++index )
{
b3Primitive primitive = primitives.data[index];
int i1 = 3 * primitive.triangleIndex + 0;
int i2 = 3 * primitive.triangleIndex + 1;
int i3 = 3 * primitive.triangleIndex + 2;
if ( clockWise )
{
B3_SWAP( i2, i3 );
}
triangles[index].index1 = indices.data[i1];
triangles[index].index2 = indices.data[i2];
triangles[index].index3 = indices.data[i3];
flags[index] = 0;
// Copy material indices if they exist. Otherwise the material indices are all zeroes.
if ( def->materialIndices != NULL )
{
uint8_t materialIndex = def->materialIndices[primitive.triangleIndex];
materialIndices[index] = materialIndex;
}
}
// Sort triangle in DFS order. Casts and volume queries will return sorted arrays.
// This also sorts material indices, but not the materials.
// This can fail if the BVH height is too large.
bool success = b3SortMeshTriangles( mesh );
if ( success == false )
{
b3Array_Destroy( tempNodes );
b3Array_Destroy( primitives );
b3Array_Destroy( indices );
b3Array_Destroy( vertices );
b3Free( mesh, byteCount );
return NULL;
}
if ( def->identifyEdges )
{
int meshTriangleCount = mesh->triangleCount;
b3Vec3* normals = B3_ALLOC( b3Vec3, meshTriangleCount );
b3IdentifyEdges( mesh, normals );
mesh = b3AppendVertexRings( mesh, normals );
B3_FREE( normals, b3Vec3, meshTriangleCount );
}
B3_VALIDATE( b3IsNonDegenerate( mesh, minArea ) );
B3_VALIDATE( b3IsConsistent( mesh ) );
b3Array_Destroy( tempNodes );
b3Array_Destroy( primitives );
b3Array_Destroy( indices );
b3Array_Destroy( vertices );
mesh->hash = 0;
mesh->hash = b3Hash64NonZero( (uint8_t*)mesh, mesh->byteCount );
return mesh;
}
void b3DestroyMesh( b3MeshData* mesh )
{
b3Free( mesh, mesh->byteCount );
}
bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy )
{
B3_ASSERT( proxy->count > 0 );
b3SimplexCache cache = { 0 };
b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS];
b3ShapeProxy localProxy = b3MakeLocalProxy( proxy, shapeTransform, buffer );
b3AABB aabb = b3ComputeProxyAABB( &localProxy );
b3Vec3 meshScale = shape->scale;
// Scale may have reflection so min/max may become invalid when unscaled
b3V32 scale = b3LoadV( &meshScale.x );
b3V32 invScale = b3DivV( b3_oneV, scale );
b3V32 temp1 = b3MulV( invScale, b3LoadV( &aabb.lowerBound.x ) );
b3V32 temp2 = b3MulV( invScale, b3LoadV( &aabb.upperBound.x ) );
b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 );
b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 );
b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) );
b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter );
b3DistanceInput input;
input.proxyB = localProxy;
input.transform = b3Transform_identity;
input.useRadii = true;
int count = 0;
const b3MeshNode* stack[B3_MESH_STACK_SIZE];
const b3MeshNode* node = b3GetRoot( shape->data );
const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data );
const b3Vec3* vertices = b3GetMeshVertices( shape->data );
while ( true )
{
// Test node overlap in unscaled space
b3V32 nodeMin = b3LoadV( &node->lowerBound.x );
b3V32 nodeMax = b3LoadV( &node->upperBound.x );
if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) )
{
if ( b3IsLeaf( node ) )
{
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
for ( int index = 0; index < triangleCount; ++index )
{
int triangleIndex = triangleOffset + index;
b3MeshTriangle triangle = triangles[triangleIndex];
b3Vec3 vertex1 = vertices[triangle.index1];
b3Vec3 vertex2 = vertices[triangle.index2];
b3Vec3 vertex3 = vertices[triangle.index3];
b3V32 v1 = b3LoadV( &vertex1.x );
b3V32 v2 = b3LoadV( &vertex2.x );
b3V32 v3 = b3LoadV( &vertex3.x );
// Bounding box overlap test in unscaled space
if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) )
{
// Shape-triangle overlap test in scaled space. Winding order doesn't matter.
b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ),
b3Mul( meshScale, vertex3 ) };
input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
// reset the cache
cache.count = 0;
// get distance between triangle and query shape
b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 );
float tolerance = 0.1f * B3_LINEAR_SLOP;
if ( output.distance < tolerance )
{
// overlap detected
return true;
}
}
}
}
else
{
// Recurse
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetRightChild( node );
node = b3GetLeftChild( node );
continue;
}
}
if ( count == 0 )
{
break;
}
node = stack[--count];
}
return false;
}
b3AABB b3ComputeMeshAABB( const b3MeshData* shape, b3Transform transform, b3Vec3 scale )
{
b3Vec3 scaledLower = b3Mul( scale, shape->bounds.lowerBound );
b3Vec3 scaledUpper = b3Mul( scale, shape->bounds.upperBound );
b3AABB bounds = { b3Min( scaledLower, scaledUpper ), b3Max( scaledLower, scaledUpper ) };
return b3AABB_Transform( transform, bounds );
}
b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input )
{
const b3MeshData* data = mesh->data;
b3Vec3 meshScale = mesh->scale;
b3CastOutput bestOutput = { 0 };
bestOutput.fraction = input->maxFraction;
bestOutput.triangleIndex = B3_NULL_INDEX;
b3V32 lambda = b3SplatV( input->maxFraction );
b3V32 rayStart = b3LoadV( &input->origin.x );
b3V32 rayDelta = b3LoadV( &input->translation.x );
b3V32 scale = b3LoadV( &meshScale.x );
b3V32 invScale = b3DivV( b3_oneV, scale );
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
// Use the inverse scaled ray for traversal of the BVH
b3V32 invScaledRayStart = b3MulV( invScale, rayStart );
b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta );
b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) );
b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd );
b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd );
int count = 0;
const b3MeshNode* stack[B3_MESH_STACK_SIZE];
const b3MeshNode* node = b3GetRoot( data );
const b3MeshTriangle* triangles = b3GetMeshTriangles( data );
const b3Vec3* vertices = b3GetMeshVertices( data );
const uint8_t* materialIndices = b3GetMeshMaterialIndices( data );
while ( true )
{
// Test node/ray overlap using SAT
b3V32 nodeMin = b3LoadV( &node->lowerBound.x );
b3V32 nodeMax = b3LoadV( &node->upperBound.x );
if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) &&
b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) )
{
// SAT: The node and ray overlap - process leaf node or recurse
if ( b3IsLeaf( node ) )
{
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
for ( int index = 0; index < triangleCount; ++index )
{
int triangleIndex = triangleOffset + index;
b3MeshTriangle triangle = triangles[triangleIndex];
// Collide ray with triangle in scaled space
b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] );
b3Vec3 vertex2, vertex3;
// The CPU should predict this branch
if ( ccw )
{
vertex2 = b3Mul( meshScale, vertices[triangle.index2] );
vertex3 = b3Mul( meshScale, vertices[triangle.index3] );
}
else
{
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
}
// Collide ray with triangle in scaled space
b3V32 v1 = b3LoadV( &vertex1.x );
b3V32 v2 = b3LoadV( &vertex2.x );
b3V32 v3 = b3LoadV( &vertex3.x );
float alpha = b3IntersectRayTriangle( rayStart, rayDelta, v1, v2, v3 );
B3_ASSERT( 0 <= alpha && alpha <= 1.0f );
if ( alpha < bestOutput.fraction )
{
b3Vec3 edge1 = b3Sub( vertex2, vertex1 );
b3Vec3 edge2 = b3Sub( vertex3, vertex1 );
bestOutput.normal = b3Normalize( b3Cross( edge1, edge2 ) );
bestOutput.point = b3Add( input->origin, b3MulSV( alpha, input->translation ) );
bestOutput.fraction = alpha;
bestOutput.triangleIndex = triangleIndex;
bestOutput.materialIndex = materialIndices[triangleIndex];
bestOutput.hit = true;
// Update ray bounds in unscaled space
lambda = b3SplatV( alpha );
invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) );
invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd );
invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd );
}
}
}
else
{
// Determine traversal order (front -> back) and recurse
int axis = node->data.asNode.axis;
if ( b3GetV( invScaledRayDelta, axis ) > 0.0f )
{
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetRightChild( node );
node = b3GetLeftChild( node );
}
else
{
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetLeftChild( node );
node = b3GetRightChild( node );
}
continue;
}
}
if ( count == 0 )
{
break;
}
node = stack[--count];
}
return bestOutput;
}
b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input )
{
const b3MeshData* data = mesh->data;
b3Vec3 meshScale = mesh->scale;
b3CastOutput bestOutput = { 0 };
bestOutput.fraction = input->maxFraction;
bestOutput.triangleIndex = B3_NULL_INDEX;
b3V32 lambda = b3SplatV( input->maxFraction );
b3AABB shapeBounds = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius );
b3Vec3 center = b3AABB_Center( shapeBounds );
b3Vec3 extents = b3AABB_Extents( shapeBounds );
b3V32 shapeExtent = b3LoadV( &extents.x );
b3V32 rayStart = b3LoadV( &center.x );
b3V32 rayDelta = b3LoadV( &input->translation.x );
b3V32 rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) );
b3V32 rayMin = b3MinV( rayStart, rayEnd );
b3V32 rayMax = b3MaxV( rayStart, rayEnd );
b3V32 scale = b3LoadV( &meshScale.x );
b3V32 invScale = b3DivV( b3_oneV, scale );
b3V32 absInvScale = b3AbsV( invScale );
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
// Use the inverse scaled shape cast for traversal of the BVH
b3V32 invScaledRayStart = b3MulV( invScale, rayStart );
b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta );
b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) );
b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd );
b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd );
b3V32 invScaledShapeExtent = b3MulV( absInvScale, shapeExtent );
int count = 0;
const b3MeshNode* stack[B3_MESH_STACK_SIZE];
const b3MeshNode* node = b3GetRoot( data );
const b3MeshTriangle* triangles = b3GetMeshTriangles( data );
const b3Vec3* vertices = b3GetMeshVertices( data );
const uint8_t* materialIndices = b3GetMeshMaterialIndices( data );
while ( true )
{
// Test node/ray overlap using SAT in unscaled space
b3V32 nodeMin = b3SubV( b3LoadV( &node->lowerBound.x ), invScaledShapeExtent );
b3V32 nodeMax = b3AddV( b3LoadV( &node->upperBound.x ), invScaledShapeExtent );
if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) &&
b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) )
{
// SAT: The node and ray overlap - process leaf node or recurse
if ( b3IsLeaf( node ) )
{
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
for ( int index = 0; index < triangleCount; ++index )
{
int triangleIndex = triangleOffset + index;
b3MeshTriangle triangle = triangles[triangleIndex];
// Collide shape with triangle in scaled space
b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] );
b3Vec3 vertex2, vertex3;
// The CPU should predict this branch
if ( ccw )
{
vertex2 = b3Mul( meshScale, vertices[triangle.index2] );
vertex3 = b3Mul( meshScale, vertices[triangle.index3] );
}
else
{
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
}
b3V32 v1 = b3LoadV( &vertex1.x );
b3V32 v2 = b3LoadV( &vertex2.x );
b3V32 v3 = b3LoadV( &vertex3.x );
b3V32 triangleMin = b3SubV( b3MinV( v1, b3MinV( v2, v3 ) ), shapeExtent );
b3V32 triangleMax = b3AddV( b3MaxV( v1, b3MaxV( v2, v3 ) ), shapeExtent );
// Test extended triangle vs ray bounds overlap in scaled space. Skip edge tests.
bool overlap = b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax );
if ( overlap == false )
{
continue;
}
// This test is in scaled space. vertices and center are scaled.
float signedVolume = b3SignedVolume( vertex1, vertex2, vertex3, center );
if ( signedVolume < 0.0f )
{
// Backside
continue;
}
// Collide shape with triangle in scaled space.
b3Vec3 origin = vertex1;
b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) };
b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity };
b3ShapeCastPairInput pairInput;
pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
pairInput.proxyB = input->proxy;
pairInput.transform = shiftedOrigin;
pairInput.maxFraction = bestOutput.fraction;
pairInput.translationB = input->translation;
pairInput.canEncroach = input->canEncroach;
b3CastOutput pairOutput = b3ShapeCast( &pairInput );
if ( pairOutput.hit )
{
pairOutput.point = b3Add( pairOutput.point, origin );
bestOutput = pairOutput;
bestOutput.triangleIndex = triangleIndex;
bestOutput.materialIndex = materialIndices[triangleIndex];
// Update ray bounds in scaled space
lambda = b3SplatV( pairOutput.fraction );
rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) );
rayMin = b3MinV( rayStart, rayEnd );
rayMax = b3MaxV( rayStart, rayEnd );
// Ray bounds in unscaled space
invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) );
invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd );
invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd );
}
}
}
else
{
// Determine traversal order (front -> back) and recurse
int axis = node->data.asNode.axis;
if ( b3GetV( invScaledRayDelta, axis ) > 0.0f )
{
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetRightChild( node );
node = b3GetLeftChild( node );
}
else
{
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetLeftChild( node );
node = b3GetRightChild( node );
}
continue;
}
}
if ( count == 0 )
{
break;
}
node = stack[--count];
}
return bestOutput;
}
int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover )
{
if ( capacity == 0 )
{
return 0;
}
b3DistanceInput distanceInput = { 0 };
distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f };
distanceInput.transform = b3Transform_identity;
distanceInput.useRadii = false;
b3SimplexCache cache = { 0 };
float radius = mover->radius;
b3Vec3 center = b3Lerp( mover->center1, mover->center2, 0.5f );
b3V32 center1 = b3LoadV( &mover->center1.x );
b3V32 center2 = b3LoadV( &mover->center2.x );
b3V32 r = b3SplatV( radius );
b3V32 boundsMin = b3SubV( b3MinV( center1, center2 ), r );
b3V32 boundsMax = b3AddV( b3MaxV( center1, center2 ), r );
// Scale may have reflection so min/max may become invalid when unscaled
b3Vec3 meshScale = shape->scale;
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
b3V32 scale = b3LoadV( &meshScale.x );
b3V32 invScale = b3DivV( b3_oneV, scale );
b3V32 temp1 = b3MulV( invScale, boundsMin );
b3V32 temp2 = b3MulV( invScale, boundsMax );
b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 );
b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 );
b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) );
b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter );
int count = 0;
const b3MeshNode* stack[B3_MESH_STACK_SIZE];
const b3MeshNode* node = b3GetRoot( shape->data );
const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data );
const b3Vec3* vertices = b3GetMeshVertices( shape->data );
const uint8_t* materialIndices = b3GetMeshMaterialIndices( shape->data );
int planeCount = 0;
while ( planeCount < capacity )
{
// Test node overlap in unscaled space
b3V32 nodeMin = b3LoadV( &node->lowerBound.x );
b3V32 nodeMax = b3LoadV( &node->upperBound.x );
if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) )
{
if ( b3IsLeaf( node ) )
{
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
for ( int index = 0; index < triangleCount; ++index )
{
int triangleIndex = triangleOffset + index;
b3MeshTriangle triangle = triangles[triangleIndex];
b3Vec3 vertex1 = vertices[triangle.index1];
b3Vec3 vertex2 = vertices[triangle.index2];
b3Vec3 vertex3 = vertices[triangle.index3];
if ( ccw == false )
{
B3_SWAP( vertex2, vertex3 );
}
b3V32 v1 = b3LoadV( &vertex1.x );
b3V32 v2 = b3LoadV( &vertex2.x );
b3V32 v3 = b3LoadV( &vertex3.x );
// Test triangle bounds overlap in unscaled space
if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) )
{
// Compute shape distance in scaled space.
b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ),
b3Mul( meshScale, vertex3 ) };
// Vertices and center are in scaled space.
float signedVolume =
b3SignedVolume( triangleVertices[0], triangleVertices[1], triangleVertices[2], center );
if ( signedVolume < 0.0f )
{
// Backside
continue;
}
distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f };
// reset the cache
cache.count = 0;
// get distance between triangle and query shape
b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 );
if ( distanceOutput.distance == 0.0f )
{
// deep overlap
}
else if ( distanceOutput.distance <= mover->radius )
{
b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance };
planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0,
materialIndices[triangleIndex] };
planeCount += 1;
if ( planeCount == capacity )
{
return planeCount;
}
}
}
}
}
else
{
// Recurse
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetRightChild( node );
node = b3GetLeftChild( node );
continue;
}
}
if ( count == 0 )
{
break;
}
node = stack[--count];
}
return planeCount;
}
void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* context )
{
b3Vec3 meshScale = mesh->scale;
bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f;
// Scale may have reflection so min/max may become invalid when unscaled
b3V32 scale = b3LoadV( &meshScale.x );
b3V32 invScale = b3DivV( b3_oneV, scale );
b3V32 temp1 = b3MulV( invScale, b3LoadV( &bounds.lowerBound.x ) );
b3V32 temp2 = b3MulV( invScale, b3LoadV( &bounds.upperBound.x ) );
b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 );
b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 );
b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) );
b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter );
const b3MeshData* data = mesh->data;
int count = 0;
const b3MeshNode* stack[B3_MESH_STACK_SIZE];
const b3MeshNode* node = b3GetRoot( data );
const b3MeshTriangle* triangles = b3GetMeshTriangles( data );
const b3Vec3* vertices = b3GetMeshVertices( data );
// Test node overlap in unscaled space
if ( b3TestBoundsOverlap( b3LoadV( &node->lowerBound.x ), b3LoadV( &node->upperBound.x ), invScaledBoundsMin,
invScaledBoundsMax ) == false )
{
return;
}
while ( true )
{
if ( b3IsLeaf( node ) )
{
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
for ( int index = 0; index < triangleCount; ++index )
{
int triangleIndex = triangleOffset + index;
b3MeshTriangle triangle = triangles[triangleIndex];
b3Vec3 vertex1 = vertices[triangle.index1];
b3Vec3 vertex2 = vertices[triangle.index2];
b3Vec3 vertex3 = vertices[triangle.index3];
b3V32 v1 = b3LoadV( &vertex1.x );
b3V32 v2 = b3LoadV( &vertex2.x );
b3V32 v3 = b3LoadV( &vertex3.x );
// Perform triangle overlap test in unscaled space.
// todo it is possible that some margins are getting scaled
if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) )
{
b3Vec3 a = b3Mul( meshScale, vertex1 );
b3Vec3 b, c;
if ( ccw )
{
b = b3Mul( meshScale, vertex2 );
c = b3Mul( meshScale, vertex3 );
}
else
{
b = b3Mul( meshScale, vertex3 );
c = b3Mul( meshScale, vertex2 );
}
bool result = fcn( a, b, c, triangleIndex, context );
if ( result == false )
{
return;
}
}
}
}
else
{
const b3MeshNode* leftChild = b3GetLeftChild( node );
const b3MeshNode* rightChild = b3GetRightChild( node );
bool overlapLeft =
b3TestBoundsOverlap( b3LoadV( &leftChild->lowerBound.x ), b3LoadV( &leftChild->upperBound.x ),
invScaledBoundsMin, invScaledBoundsMax );
bool overlapRight =
b3TestBoundsOverlap( b3LoadV( &rightChild->lowerBound.x ), b3LoadV( &rightChild->upperBound.x ),
invScaledBoundsMin, invScaledBoundsMax );
if ( overlapLeft )
{
if ( overlapRight )
{
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = rightChild;
}
node = leftChild;
continue;
}
if ( overlapRight )
{
node = rightChild;
continue;
}
}
if ( count == 0 )
{
break;
}
node = stack[--count];
}
}