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