SIMD based hull collision inspired by example code from @cairnc in #54 <img width="1676" height="1332" alt="image" src="https://github.com/user-attachments/assets/58b137bd-bf7b-4c99-863c-64341b973a3d" /> - Fixed #78 - Fixed #85 and fixed CI to error on UB. - Updated benchmarks - Added AMD scalar benchmarks - Added Neon benchmarks - More determinism unit tests - Sample leak detection - Added b3Body_AllowFastRotation - Added b3Body_IsFastRotationAllowed - Bumped b3HullData version - Bumped b3CompoundData version
2415 lines
64 KiB
C
2415 lines
64 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 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 = b3NonZeroHash( b3Hash( B3_HASH_INIT, (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];
|
|
}
|
|
}
|