Box3D-0.1.0.0: upstream/src/mesh.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 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 ) )
{
return false;
}
if ( !b3AABB_Contains( nodeBounds, bounds2 ) )
{
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
// 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;
b3VertexMap vertexMap;
float cellSize;
float tolerance;
} b3SpatialHash;
static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, float tolerance )
{
h->vertices = vertices;
h->vertexCount = vertexCount;
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 b3SpatialHash_Destroy( 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 );
b3Vec3 vertex = h->vertices[currentIndex];
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 = h->vertices[existingIndex];
// 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;
} b3WeldData;
static int b3WeldVertices( b3WeldData* data, float tolerance )
{
int vertexCount = data->vertexCount;
int uniqueCount = 0;
// Create spatial hash and find duplicates
b3SpatialHash spatialHash;
b3SpatialHash_Create( &spatialHash, data->srcVertices, vertexCount, 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] = data->srcVertices[i];
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( srcIndex < vertexCount );
data->dstIndices[i] = vertexMapping.data[srcIndex];
}
b3SpatialHash_Destroy( &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;
typedef struct b3Bucket
{
int count;
b3AABB bounds;
} 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 = { primitives[0].center, primitives[0].center };
for ( int i = 1; i < count; ++i )
{
bounds = b3AABB_AddPoint( bounds, primitives[i].center );
}
// 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;
for ( int axis = 0; axis < 3; ++axis )
{
b3Vec3 extent = b3AABB_Extents( bounds );
if ( b3GetByIndex( extent, axis ) < B3_LINEAR_SLOP )
{
continue;
}
// Initialize buckets
b3Bucket buckets[B3_BIN_COUNT];
for ( int i = 0; i < B3_BIN_COUNT; ++i )
{
buckets[i].count = 0;
buckets[i].bounds = B3_BOUNDS3_EMPTY;
}
// Fill buckets
float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) /
( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) );
for ( int i = 0; i < count; ++i )
{
b3Vec3 center = primitives[i].center;
int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) );
B3_ASSERT( 0 <= index && index < B3_BIN_COUNT );
buckets[index].count++;
buckets[index].bounds = b3AABB_Union( buckets[index].bounds, primitives[i].aabb );
}
// Evaluate splits
for ( int i = 0; i < B3_BIN_COUNT - 1; ++i )
{
int leftCount = 0;
b3AABB leftBounds = B3_BOUNDS3_EMPTY;
for ( int k = 0; k <= i; ++k )
{
leftCount += buckets[k].count;
leftBounds = b3AABB_Union( leftBounds, buckets[k].bounds );
}
int rightCount = 0;
b3AABB rightBounds = B3_BOUNDS3_EMPTY;
for ( int k = i + 1; k < B3_BIN_COUNT; ++k )
{
rightCount += buckets[k].count;
rightBounds = b3AABB_Union( rightBounds, buckets[k].bounds );
}
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 = B3_BOUNDS3_EMPTY;
for ( int i = 0; i < splitIndex; ++i )
{
leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb );
}
b3AABB rightBounds = B3_BOUNDS3_EMPTY;
for ( int i = splitIndex; i < count; ++i )
{
rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb );
}
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 );
b3Vec3 lowerBound = primitives[0].center;
b3Vec3 upperBound = primitives[0].center;
for ( int i = 1; i < count; ++i )
{
lowerBound = b3Min( lowerBound, primitives[i].center );
upperBound = b3Max( upperBound, primitives[i].center );
}
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;
}
b3AABB leftBounds = B3_BOUNDS3_EMPTY;
for ( int i = 0; i < i1; ++i )
{
leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb );
}
b3AABB rightBounds = B3_BOUNDS3_EMPTY;
for ( int i = i1; i < count; ++i )
{
rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb );
}
split.index = i1;
split.leftBounds = leftBounds;
split.rightBounds = rightBounds;
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 = B3_BOUNDS3_EMPTY;
for ( int i = 0; i < count; ++i )
{
bounds = b3AABB_Union( bounds, primitives[i].aabb );
}
// 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 = B3_BOUNDS3_EMPTY;
for ( int i = 0; i < count; ++i )
{
aabb = b3AABB_Union( aabb, primitives[i].aabb );
}
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 NAME b3EdgeMap
#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"
#if 0
// todo this is for testing other hash tables
struct b3TestHash
{
size_t operator()( uint64_t key ) const
{
key ^= key >> 23;
key *= 0x2127599bf4325c37ull;
key ^= key >> 47;
return (size_t)key;
}
};
#endif
// Results for MeshCreationBenchmark
// eastl::hash_map : 4.9703 ms and 5.1445ms with FastHash function
// std::unordered_map : 4.8755 ms and 4.4780ms with FastHash function
// verstable : 3.3968 ms with default FastHash
// no edge identification : 1.7396 ms
static void b3IdentifyEdges( b3MeshData* mesh )
{
b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh );
const b3Vec3* vertices = b3GetMeshVertices( mesh );
uint8_t* flags = b3GetMeshFlagsWrite( mesh );
int triangleCount = mesh->triangleCount;
int edgeCount = 3 * triangleCount;
b3MeshEdge* edges = B3_ALLOC( b3MeshEdge, edgeCount );
b3Vec3* normals = B3_ALLOC( b3Vec3, triangleCount );
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 );
}
b3EdgeMap map;
b3EdgeMap_init( &map );
b3EdgeMap_reserve( &map, edgeCount );
uint64_t key = (uint64_t)edges[0].vertex1 << 32 | (uint64_t)edges[0].vertex2;
b3EdgeMap_insert( &map, key, 0 );
// Find unique edges and assign adjacency
for ( int i = 1; i < edgeCount; ++i )
{
b3MeshEdge* edge = edges + i;
key = (uint64_t)edge->vertex1 << 32 | (uint64_t)edge->vertex2;
b3EdgeMap_itr itr = b3EdgeMap_get( &map, key );
if ( b3EdgeMap_is_end( itr ) )
{
b3EdgeMap_insert( &map, key, i );
}
else
{
int otherIndex = itr.data->val;
B3_ASSERT( otherIndex < i );
b3MeshEdge* base = edges + otherIndex;
if ( base->triangleCount == 1 )
{
base->triangle2 = edge->triangle1;
base->triangleEdgeIndex2 = edge->triangleEdgeIndex1;
}
base->triangleCount += 1;
}
}
b3EdgeMap_cleanup( &map );
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];
float cos5Deg = 0.9962f;
float signedVolume = b3SignedVolume( v1, v2, v3, p );
b3Vec3 n1 = normals[edge->triangle1];
b3Vec3 n2 = normals[edge->triangle2];
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];
}
}
B3_FREE( normals, b3Vec3, triangleCount );
B3_FREE( edges, b3MeshEdge, edgeCount );
}
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 );
}
// 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->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;
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,
};
vertices.count = b3WeldVertices( &data, def->weldTolerance );
vertexCount = vertices.count;
B3_ASSERT( vertexCount <= def->vertexCount );
}
else
{
b3Array_Append( vertices, def->vertices, vertexCount );
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;
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];
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 )
{
degenerateCount += 1;
if ( degenerateTriangleIndices != NULL && degenerateCount < degenerateCapacity )
{
degenerateTriangleIndices[degenerateCount - 1] = index;
}
}
continue;
}
surfaceArea += area;
b3AABB box = {
b3Min( vertex1, b3Min( vertex2, vertex3 ) ),
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 );
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 ) );
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;
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];
triangles[index].index1 = indices.data[3 * primitive.triangleIndex + 0];
triangles[index].index2 = indices.data[3 * primitive.triangleIndex + 1];
triangles[index].index3 = indices.data[3 * primitive.triangleIndex + 2];
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 );
return NULL;
}
if ( def->identifyEdges )
{
b3IdentifyEdges( mesh );
}
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 clockwise = 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 ( clockwise )
{
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
}
else
{
vertex2 = b3Mul( meshScale, vertices[triangle.index2] );
vertex3 = b3Mul( meshScale, vertices[triangle.index3] );
}
// 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( ¢er.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 clockwise = 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 ray with triangle in scaled space
b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] );
b3Vec3 vertex2, vertex3;
// The CPU should predict this branch
if ( clockwise )
{
vertex2 = b3Mul( meshScale, vertices[triangle.index3] );
vertex3 = b3Mul( meshScale, vertices[triangle.index2] );
}
else
{
vertex2 = b3Mul( meshScale, vertices[triangle.index2] );
vertex3 = b3Mul( meshScale, vertices[triangle.index3] );
}
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 triangle-ray overlap in scaled space
if ( b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ) )
{
// 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;
}
b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex )
{
B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount );
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data );
const uint8_t* flags = b3GetMeshFlags( mesh->data );
const b3Vec3* vertices = b3GetMeshVertices( mesh->data );
b3Triangle result;
b3MeshTriangle triangle = triangles[triangleIndex];
uint8_t triangleFlags = flags[triangleIndex];
b3Vec3 scale = mesh->scale;
result.vertices[0] = b3Mul( scale, vertices[triangle.index1] );
result.i1 = triangle.index1;
if ( scale.x * scale.y * scale.z < 0.0f )
{
result.vertices[1] = b3Mul( scale, vertices[triangle.index3] );
result.vertices[2] = b3Mul( scale, vertices[triangle.index2] );
result.i2 = triangle.index3;
result.i3 = triangle.index2;
// mesh is inverted, so concave edges are now convex
result.flags = 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0;
}
else
{
result.vertices[1] = b3Mul( scale, vertices[triangle.index2] );
result.vertices[2] = b3Mul( scale, vertices[triangle.index3] );
result.i2 = triangle.index2;
result.i3 = triangle.index3;
result.flags = triangleFlags;
}
return result;
}
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;
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;
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 );
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];
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. Winding order doesn't matter.
// todo implement one-sided collision?
b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ),
b3Mul( meshScale, vertex3 ) };
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 )
{
// todo SAT
}
else if ( distanceOutput.distance <= mover->radius )
{
b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance };
planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA };
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 clockwise = 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 );
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 );
// Perform triangle overlap test in unscaled space. Winding order doesn't matter.
// 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 ( clockwise )
{
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
{
// Recurse
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetRightChild( node );
node = b3GetLeftChild( node );
continue;
}
}
if ( count == 0 )
{
break;
}
node = stack[--count];
}
}