2026-05-10 13:36:52 -07:00
|
|
|
// SPDX-FileCopyrightText: 2025 Erin Catto
|
|
|
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
|
|
|
|
|
|
#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )
|
|
|
|
|
#define _CRT_SECURE_NO_WARNINGS
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#include "imgui.h"
|
|
|
|
|
#include "utils.h"
|
|
|
|
|
#include "sample.h"
|
2026-06-06 10:06:05 -07:00
|
|
|
#include "gfx/draw.h"
|
|
|
|
|
|
|
|
|
|
#include "box3d/box3d.h"
|
2026-05-10 13:36:52 -07:00
|
|
|
|
|
|
|
|
#include <queue>
|
|
|
|
|
#include <stdio.h>
|
|
|
|
|
#include <unordered_map>
|
|
|
|
|
#include <vector>
|
|
|
|
|
|
|
|
|
|
constexpr int colorCount = 12;
|
|
|
|
|
b3HexColor colors[colorCount] = {
|
|
|
|
|
b3_colorRoyalBlue, b3_colorPink, b3_colorLightGreen, b3_colorOrange, b3_colorYellow, b3_colorViolet,
|
|
|
|
|
b3_colorAqua, b3_colorCoral, b3_colorLightGray, b3_colorMoccasin, b3_colorLavender, b3_colorMediumOrchid,
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
struct FileConfig
|
|
|
|
|
{
|
|
|
|
|
float scale;
|
|
|
|
|
bool zUp;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
struct Proxy
|
|
|
|
|
{
|
|
|
|
|
b3AABB aabb;
|
|
|
|
|
int proxyId;
|
|
|
|
|
int queryTimeStamp;
|
|
|
|
|
int rayTimeStamp;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
struct Ray
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 origin, translation;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
|
|
|
|
|
|
#include <immintrin.h>
|
|
|
|
|
|
|
|
|
|
// Warning: this code has not been validated
|
|
|
|
|
void ClosestPointAVX2( const float* aabbMinX, const float* aabbMinY, const float* aabbMinZ, const float* aabbMaxX,
|
|
|
|
|
const float* aabbMaxY, const float* aabbMaxZ, int proxyCount, b3Vec3 point, b3Vec3* outClosestPoint,
|
|
|
|
|
float* outMinDistSq )
|
|
|
|
|
{
|
|
|
|
|
const __m256 px = _mm256_set1_ps( point.x );
|
|
|
|
|
const __m256 py = _mm256_set1_ps( point.y );
|
|
|
|
|
const __m256 pz = _mm256_set1_ps( point.z );
|
|
|
|
|
|
|
|
|
|
__m256 bestDistSq = _mm256_set1_ps( FLT_MAX );
|
|
|
|
|
__m256i bestIndex = _mm256_set1_epi32( -1 );
|
|
|
|
|
|
|
|
|
|
int j = 0;
|
|
|
|
|
for ( ; j + 8 <= proxyCount; j += 8 )
|
|
|
|
|
{
|
|
|
|
|
// Load AABB bounds for 8 proxies
|
|
|
|
|
__m256 minX = _mm256_loadu_ps( aabbMinX + j );
|
|
|
|
|
__m256 minY = _mm256_loadu_ps( aabbMinY + j );
|
|
|
|
|
__m256 minZ = _mm256_loadu_ps( aabbMinZ + j );
|
|
|
|
|
__m256 maxX = _mm256_loadu_ps( aabbMaxX + j );
|
|
|
|
|
__m256 maxY = _mm256_loadu_ps( aabbMaxY + j );
|
|
|
|
|
__m256 maxZ = _mm256_loadu_ps( aabbMaxZ + j );
|
|
|
|
|
|
|
|
|
|
// Closest point on AABB to point: clamp each coordinate
|
|
|
|
|
__m256 cx = _mm256_min_ps( _mm256_max_ps( px, minX ), maxX );
|
|
|
|
|
__m256 cy = _mm256_min_ps( _mm256_max_ps( py, minY ), maxY );
|
|
|
|
|
__m256 cz = _mm256_min_ps( _mm256_max_ps( pz, minZ ), maxZ );
|
|
|
|
|
|
|
|
|
|
// d = point - closest
|
|
|
|
|
__m256 dx = _mm256_sub_ps( px, cx );
|
|
|
|
|
__m256 dy = _mm256_sub_ps( py, cy );
|
|
|
|
|
__m256 dz = _mm256_sub_ps( pz, cz );
|
|
|
|
|
|
|
|
|
|
__m256 dx2 = _mm256_mul_ps( dx, dx );
|
|
|
|
|
__m256 dy2 = _mm256_mul_ps( dy, dy );
|
|
|
|
|
__m256 dz2 = _mm256_mul_ps( dz, dz );
|
|
|
|
|
__m256 distSq = _mm256_add_ps( _mm256_add_ps( dx2, dy2 ), dz2 );
|
|
|
|
|
|
|
|
|
|
// Compare to current best
|
|
|
|
|
__m256 mask = _mm256_cmp_ps( distSq, bestDistSq, _CMP_LT_OQ );
|
|
|
|
|
bestDistSq = _mm256_blendv_ps( bestDistSq, distSq, mask );
|
|
|
|
|
|
|
|
|
|
__m256i indices = _mm256_set_epi32( j + 7, j + 6, j + 5, j + 4, j + 3, j + 2, j + 1, j + 0 );
|
|
|
|
|
|
|
|
|
|
// Tracks the 8 best indices
|
|
|
|
|
bestIndex =
|
|
|
|
|
_mm256_castps_si256( _mm256_blendv_ps( _mm256_castsi256_ps( bestIndex ), _mm256_castsi256_ps( indices ), mask ) );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Now find the best index among the best 8.
|
|
|
|
|
alignas( 32 ) float distBuf[8];
|
|
|
|
|
alignas( 32 ) int indexBuf[8];
|
|
|
|
|
|
|
|
|
|
_mm256_store_ps( distBuf, bestDistSq );
|
|
|
|
|
_mm256_store_si256( (__m256i*)indexBuf, bestIndex );
|
|
|
|
|
|
|
|
|
|
float minDist = distBuf[0];
|
|
|
|
|
int minIdx = indexBuf[0];
|
|
|
|
|
for ( int k = 1; k < 8; ++k )
|
|
|
|
|
{
|
|
|
|
|
if ( distBuf[k] < minDist )
|
|
|
|
|
{
|
|
|
|
|
minDist = distBuf[k];
|
|
|
|
|
minIdx = indexBuf[k];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Remainder
|
|
|
|
|
for ( ; j < proxyCount; ++j )
|
|
|
|
|
{
|
|
|
|
|
float cx = b3MinFloat( b3MaxFloat( point.x, aabbMinX[j] ), aabbMaxX[j] );
|
|
|
|
|
float cy = b3MinFloat( b3MaxFloat( point.y, aabbMinY[j] ), aabbMaxY[j] );
|
|
|
|
|
float cz = b3MinFloat( b3MaxFloat( point.z, aabbMinZ[j] ), aabbMaxZ[j] );
|
|
|
|
|
|
|
|
|
|
float dx = point.x - cx;
|
|
|
|
|
float dy = point.y - cy;
|
|
|
|
|
float dz = point.z - cz;
|
|
|
|
|
float d2 = dx * dx + dy * dy + dz * dz;
|
|
|
|
|
|
|
|
|
|
if ( d2 < minDist )
|
|
|
|
|
{
|
|
|
|
|
minDist = d2;
|
|
|
|
|
minIdx = j;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
b3Vec3 cp;
|
|
|
|
|
cp.x = b3MinFloat( b3MaxFloat( point.x, aabbMinX[minIdx] ), aabbMaxX[minIdx] );
|
|
|
|
|
cp.y = b3MinFloat( b3MaxFloat( point.y, aabbMinY[minIdx] ), aabbMaxY[minIdx] );
|
|
|
|
|
cp.z = b3MinFloat( b3MaxFloat( point.z, aabbMinZ[minIdx] ), aabbMaxZ[minIdx] );
|
|
|
|
|
|
|
|
|
|
*outClosestPoint = cp;
|
|
|
|
|
*outMinDistSq = minDist;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
class TreeBenchmark : public Sample
|
|
|
|
|
{
|
|
|
|
|
public:
|
|
|
|
|
explicit TreeBenchmark( SampleContext* context )
|
|
|
|
|
: Sample( context )
|
|
|
|
|
{
|
|
|
|
|
if ( context->restart == false )
|
|
|
|
|
{
|
2026-06-18 20:51:16 -07:00
|
|
|
m_camera->SetView( 45.0f, 45.0f, 250.0f, b3Pos_zero );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_fileNames[0] = { "bounds01" };
|
|
|
|
|
m_fileNames[1] = { "bounds02" };
|
|
|
|
|
m_fileNames[2] = { "bounds03" };
|
|
|
|
|
m_config[0] = { 1.0f, false };
|
|
|
|
|
m_config[1] = { 1.0f, false };
|
|
|
|
|
m_config[2] = { 0.01f, false };
|
|
|
|
|
m_fileIndex = 0;
|
|
|
|
|
|
2026-06-18 20:51:16 -07:00
|
|
|
m_closestPoint = b3Pos_zero;
|
2026-05-10 13:36:52 -07:00
|
|
|
m_timeStamp = 1;
|
|
|
|
|
m_doOverlap = false;
|
|
|
|
|
m_doClosest = false;
|
|
|
|
|
m_doRay = false;
|
|
|
|
|
m_testIndex = 0;
|
|
|
|
|
m_rayTime = 0.0f;
|
|
|
|
|
m_overlapTime = 0.0f;
|
|
|
|
|
m_closestTime = 0.0f;
|
|
|
|
|
m_haveClosest = false;
|
|
|
|
|
m_buildTime = 0.0f;
|
|
|
|
|
m_areaRatio = 0.0f;
|
|
|
|
|
m_drawDistance = 1.0f;
|
|
|
|
|
m_loadScale = 1.0f;
|
|
|
|
|
m_height = 0;
|
|
|
|
|
m_tree = {};
|
|
|
|
|
|
|
|
|
|
m_aabbMinX = nullptr;
|
|
|
|
|
m_aabbMinY = nullptr;
|
|
|
|
|
m_aabbMinZ = nullptr;
|
|
|
|
|
m_aabbMaxX = nullptr;
|
|
|
|
|
m_aabbMaxY = nullptr;
|
|
|
|
|
m_aabbMaxZ = nullptr;
|
|
|
|
|
|
|
|
|
|
CreateTree();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
~TreeBenchmark() override
|
|
|
|
|
{
|
|
|
|
|
b3DynamicTree_Destroy( &m_tree );
|
|
|
|
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
|
_aligned_free( m_aabbMinX );
|
|
|
|
|
_aligned_free( m_aabbMinY );
|
|
|
|
|
_aligned_free( m_aabbMinZ );
|
|
|
|
|
_aligned_free( m_aabbMaxX );
|
|
|
|
|
_aligned_free( m_aabbMaxY );
|
|
|
|
|
_aligned_free( m_aabbMaxZ );
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Breath-first search to compute node depth values start with 0 at the root.
|
|
|
|
|
int ComputeDepths()
|
|
|
|
|
{
|
|
|
|
|
m_depths.resize( m_tree.nodeCapacity, 0 );
|
|
|
|
|
|
|
|
|
|
int* queue = (int*)malloc( m_tree.nodeCount * sizeof( int ) );
|
|
|
|
|
int front = 0;
|
|
|
|
|
int back = 0;
|
|
|
|
|
|
|
|
|
|
b3TreeNode* nodes = m_tree.nodes;
|
|
|
|
|
queue[0] = m_tree.root;
|
|
|
|
|
back += 1;
|
|
|
|
|
int depth = 0;
|
|
|
|
|
|
|
|
|
|
while ( back > front )
|
|
|
|
|
{
|
|
|
|
|
int index = queue[front];
|
|
|
|
|
front += 1;
|
|
|
|
|
|
|
|
|
|
b3TreeNode* node = nodes + index;
|
|
|
|
|
if ( node->parent == B3_NULL_INDEX )
|
|
|
|
|
{
|
|
|
|
|
m_depths[index] = 0;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
m_depths[index] = m_depths[node->parent] + 1;
|
|
|
|
|
depth = b3MaxInt( depth, m_depths[index] );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( ( node->flags & b3_leafNode ) == 0 )
|
|
|
|
|
{
|
|
|
|
|
B3_ASSERT( back < m_tree.nodeCount - 1 );
|
|
|
|
|
queue[back++] = node->children.child1;
|
|
|
|
|
queue[back++] = node->children.child2;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
free( queue );
|
|
|
|
|
|
|
|
|
|
return depth;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void CreateTree()
|
|
|
|
|
{
|
|
|
|
|
if ( m_tree.nodeCapacity > 0 )
|
|
|
|
|
{
|
|
|
|
|
b3DynamicTree_Destroy( &m_tree );
|
|
|
|
|
m_tree = {};
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_proxies.clear();
|
|
|
|
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
|
_aligned_free( m_aabbMinX );
|
|
|
|
|
_aligned_free( m_aabbMinY );
|
|
|
|
|
_aligned_free( m_aabbMinZ );
|
|
|
|
|
_aligned_free( m_aabbMaxX );
|
|
|
|
|
_aligned_free( m_aabbMaxY );
|
|
|
|
|
_aligned_free( m_aabbMaxZ );
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
m_tree = b3DynamicTree_Create( 512 );
|
|
|
|
|
|
|
|
|
|
char filePath[64] = {};
|
|
|
|
|
snprintf( filePath, 64, "data/trees/%s.txt", m_fileNames[m_fileIndex] );
|
|
|
|
|
float scale = m_config[m_fileIndex].scale;
|
|
|
|
|
bool zUp = m_config[m_fileIndex].zUp;
|
|
|
|
|
|
|
|
|
|
FILE* file = fopen( filePath, "r" );
|
|
|
|
|
if ( file == nullptr )
|
|
|
|
|
{
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
constexpr int bufferSize = 512;
|
|
|
|
|
char buffer[bufferSize];
|
|
|
|
|
|
|
|
|
|
int maxArea = 0.0f;
|
|
|
|
|
int maxAreaIndex = -1;
|
|
|
|
|
uint64_t key = 0;
|
|
|
|
|
|
|
|
|
|
while ( fgets( buffer, sizeof( buffer ), file ) )
|
|
|
|
|
{
|
|
|
|
|
char* trim = buffer;
|
|
|
|
|
while ( *trim == ' ' || *trim == '\t' )
|
|
|
|
|
{
|
|
|
|
|
++trim;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( *trim == '\0' || *trim == '\n' || *trim == '#' )
|
|
|
|
|
{
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
Proxy rec = {};
|
|
|
|
|
float b1, b2, b3, b4, b5, b6;
|
|
|
|
|
int parsed = sscanf( buffer, "%f %f %f %f %f %f", &b1, &b2, &b3, &b4, &b5, &b6 );
|
|
|
|
|
|
|
|
|
|
if ( parsed != 6 )
|
|
|
|
|
{
|
|
|
|
|
fprintf( stderr, "Skipping malformed line: %s", buffer );
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( zUp )
|
|
|
|
|
{
|
|
|
|
|
rec.aabb.lowerBound.x = scale * b1;
|
|
|
|
|
rec.aabb.lowerBound.y = scale * b3;
|
|
|
|
|
rec.aabb.lowerBound.z = scale * b2;
|
|
|
|
|
rec.aabb.upperBound.x = scale * b4;
|
|
|
|
|
rec.aabb.upperBound.y = scale * b6;
|
|
|
|
|
rec.aabb.upperBound.z = scale * b5;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
rec.aabb.lowerBound.x = scale * b1;
|
|
|
|
|
rec.aabb.lowerBound.y = scale * b2;
|
|
|
|
|
rec.aabb.lowerBound.z = scale * b3;
|
|
|
|
|
rec.aabb.upperBound.x = scale * b4;
|
|
|
|
|
rec.aabb.upperBound.y = scale * b5;
|
|
|
|
|
rec.aabb.upperBound.z = scale * b6;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float area = b3AABB_Area( rec.aabb );
|
|
|
|
|
if ( area > maxArea )
|
|
|
|
|
{
|
|
|
|
|
maxArea = area;
|
|
|
|
|
maxAreaIndex = (int)m_proxies.size();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_proxies[key] = rec;
|
|
|
|
|
key += 1;
|
|
|
|
|
|
|
|
|
|
// if ( m_proxies.size() == 320 )
|
|
|
|
|
//{
|
|
|
|
|
// break;
|
|
|
|
|
// }
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
|
int proxyCount = m_proxies.size();
|
|
|
|
|
m_aabbMinX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMinY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMinZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMaxX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMaxY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMaxZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
|
|
|
|
|
for ( int i = 0; i < proxyCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 lb = m_proxies[i].aabb.lowerBound;
|
|
|
|
|
b3Vec3 ub = m_proxies[i].aabb.upperBound;
|
|
|
|
|
|
|
|
|
|
m_aabbMinX[i] = lb.x;
|
|
|
|
|
m_aabbMinY[i] = lb.y;
|
|
|
|
|
m_aabbMinZ[i] = lb.z;
|
|
|
|
|
m_aabbMaxX[i] = ub.x;
|
|
|
|
|
m_aabbMaxY[i] = ub.y;
|
|
|
|
|
m_aabbMaxZ[i] = ub.z;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
printf( "max index = %d\n", maxAreaIndex );
|
|
|
|
|
|
|
|
|
|
// constexpr int n = m_isDebug ? 1000 : INT_MAX;
|
|
|
|
|
|
|
|
|
|
uint64_t ticks = b3GetTicks();
|
|
|
|
|
int count = (int)m_proxies.size();
|
|
|
|
|
for ( int i = 0; i < count; ++i )
|
|
|
|
|
{
|
|
|
|
|
// Arbitrary category bits to ensure correctness
|
|
|
|
|
m_proxies[i].proxyId = b3DynamicTree_CreateProxy( &m_tree, m_proxies[i].aabb, 1, i );
|
|
|
|
|
m_proxies[i].queryTimeStamp = 0;
|
|
|
|
|
m_proxies[i].rayTimeStamp = 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_buildTime = b3GetMilliseconds( ticks );
|
|
|
|
|
b3DynamicTree_Validate( &m_tree );
|
|
|
|
|
m_areaRatio = b3DynamicTree_GetAreaRatio( &m_tree );
|
|
|
|
|
|
|
|
|
|
m_height = ComputeDepths();
|
|
|
|
|
m_drawLevel = -1;
|
|
|
|
|
|
|
|
|
|
Generate();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void LoadTree()
|
|
|
|
|
{
|
|
|
|
|
if ( m_tree.nodeCapacity > 0 )
|
|
|
|
|
{
|
|
|
|
|
b3DynamicTree_Destroy( &m_tree );
|
|
|
|
|
m_tree = {};
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
m_proxies.clear();
|
|
|
|
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
|
_aligned_free( m_aabbMinX );
|
|
|
|
|
_aligned_free( m_aabbMinY );
|
|
|
|
|
_aligned_free( m_aabbMinZ );
|
|
|
|
|
_aligned_free( m_aabbMaxX );
|
|
|
|
|
_aligned_free( m_aabbMaxY );
|
|
|
|
|
_aligned_free( m_aabbMaxZ );
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
{
|
|
|
|
|
//bool zUp = true;
|
|
|
|
|
m_tree = b3DynamicTree_Load( m_saveFileName, m_loadScale );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
constexpr int bufferSize = 512;
|
|
|
|
|
char buffer[bufferSize];
|
|
|
|
|
|
|
|
|
|
int maxArea = 0.0f;
|
|
|
|
|
int maxAreaIndex = -1;
|
|
|
|
|
|
|
|
|
|
for ( int i = 0; i < m_tree.nodeCapacity; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3TreeNode* node = m_tree.nodes + i;
|
|
|
|
|
|
|
|
|
|
if ( ( node->flags & b3_leafNode ) == 0 )
|
|
|
|
|
{
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
assert( node->flags & b3_allocatedNode );
|
|
|
|
|
|
|
|
|
|
Proxy proxy = {};
|
|
|
|
|
proxy.aabb = node->aabb;
|
|
|
|
|
|
|
|
|
|
float area = b3AABB_Area( proxy.aabb );
|
|
|
|
|
if ( area > maxArea )
|
|
|
|
|
{
|
|
|
|
|
maxArea = area;
|
|
|
|
|
maxAreaIndex = (int)m_proxies.size();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
proxy.proxyId = i;
|
|
|
|
|
m_proxies[node->userData] = proxy;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
|
int proxyCount = m_proxies.size();
|
|
|
|
|
m_aabbMinX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMinY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMinZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMaxX = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMaxY = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
m_aabbMaxZ = (float*)_aligned_malloc( proxyCount * sizeof( float ), 32 );
|
|
|
|
|
|
|
|
|
|
for ( int i = 0; i < proxyCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 lb = m_proxies[i].aabb.lowerBound;
|
|
|
|
|
b3Vec3 ub = m_proxies[i].aabb.upperBound;
|
|
|
|
|
|
|
|
|
|
m_aabbMinX[i] = lb.x;
|
|
|
|
|
m_aabbMinY[i] = lb.y;
|
|
|
|
|
m_aabbMinZ[i] = lb.z;
|
|
|
|
|
m_aabbMaxX[i] = ub.x;
|
|
|
|
|
m_aabbMaxY[i] = ub.y;
|
|
|
|
|
m_aabbMaxZ[i] = ub.z;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
printf( "max index = %d\n", maxAreaIndex );
|
|
|
|
|
|
|
|
|
|
// constexpr int n = m_isDebug ? 1000 : INT_MAX;
|
|
|
|
|
|
|
|
|
|
m_buildTime = 0.0f;
|
|
|
|
|
b3DynamicTree_Validate( &m_tree );
|
|
|
|
|
m_areaRatio = b3DynamicTree_GetAreaRatio( &m_tree );
|
|
|
|
|
|
|
|
|
|
m_height = ComputeDepths();
|
|
|
|
|
m_drawLevel = -1;
|
|
|
|
|
|
|
|
|
|
Generate();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static float RayCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context )
|
|
|
|
|
{
|
|
|
|
|
TreeBenchmark* sample = static_cast<TreeBenchmark*>( context );
|
|
|
|
|
Proxy& proxy = sample->m_proxies[userData];
|
|
|
|
|
proxy.rayTimeStamp = sample->m_timeStamp;
|
|
|
|
|
return 1.0f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static bool QueryCallback( int proxyId, uint64_t userData, void* context )
|
|
|
|
|
{
|
|
|
|
|
TreeBenchmark* sample = static_cast<TreeBenchmark*>( context );
|
|
|
|
|
Proxy& proxy = sample->m_proxies[userData];
|
|
|
|
|
proxy.queryTimeStamp = sample->m_timeStamp;
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static float ClosetPointCallback( float minDistanceSquared, int proxyId, uint64_t userData, void* context )
|
|
|
|
|
{
|
|
|
|
|
(void)proxyId;
|
|
|
|
|
|
|
|
|
|
TreeBenchmark* sample = static_cast<TreeBenchmark*>( context );
|
|
|
|
|
int testIndex = sample->m_testIndex;
|
|
|
|
|
Proxy& proxy = sample->m_proxies[userData];
|
|
|
|
|
proxy.queryTimeStamp = sample->m_timeStamp;
|
|
|
|
|
b3Vec3 queryPoint = sample->m_closestPointQueries[testIndex].center;
|
|
|
|
|
b3Vec3 closestPoint = b3ClosestPointToAABB( queryPoint, proxy.aabb );
|
|
|
|
|
float distanceSquared = b3DistanceSquared( queryPoint, closestPoint );
|
|
|
|
|
if ( distanceSquared < minDistanceSquared )
|
|
|
|
|
{
|
2026-06-18 20:51:16 -07:00
|
|
|
sample->m_closestPoint = b3ToPos( closestPoint );
|
2026-05-10 13:36:52 -07:00
|
|
|
sample->m_haveClosest = true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return distanceSquared;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Generate()
|
|
|
|
|
{
|
|
|
|
|
srand( 42 );
|
|
|
|
|
|
|
|
|
|
b3AABB bounds = m_tree.nodes[m_tree.root].aabb;
|
|
|
|
|
b3Vec3 extents = b3AABB_Extents( bounds );
|
|
|
|
|
float radius = ( extents.x + extents.y + extents.z ) / 3.0f;
|
|
|
|
|
|
|
|
|
|
for ( int i = 0; i < m_testCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
m_rays[i].origin = RandomVec3( bounds.lowerBound, bounds.upperBound );
|
|
|
|
|
|
|
|
|
|
b3Vec3 end = RandomVec3( bounds.lowerBound, bounds.upperBound );
|
|
|
|
|
m_rays[i].translation = end - m_rays[i].origin;
|
|
|
|
|
|
|
|
|
|
float s = RandomFloatRange( 0.01f, 0.2f );
|
|
|
|
|
b3Vec3 c = RandomVec3( bounds.lowerBound, bounds.upperBound );
|
|
|
|
|
b3Vec3 p1 = c - s * extents;
|
|
|
|
|
b3Vec3 p2 = c + s * extents;
|
|
|
|
|
|
|
|
|
|
m_overlapQueries[i].lowerBound = b3Min( p1, p2 );
|
|
|
|
|
m_overlapQueries[i].upperBound = b3Max( p1, p2 );
|
|
|
|
|
|
|
|
|
|
m_closestPointQueries[i] = {
|
|
|
|
|
.center = c,
|
|
|
|
|
.radius = s * radius,
|
|
|
|
|
};
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Profile()
|
|
|
|
|
{
|
|
|
|
|
uint64_t ticks = b3GetTicks();
|
|
|
|
|
for ( int i = 0; i < m_testCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
Ray ray = m_rays[i];
|
|
|
|
|
b3RayCastInput input = { ray.origin, ray.translation, 1.0f };
|
|
|
|
|
|
|
|
|
|
b3DynamicTree_RayCast( &m_tree, &input, B3_DEFAULT_MASK_BITS, false, RayCallback, this );
|
|
|
|
|
}
|
|
|
|
|
m_rayTime = b3GetMillisecondsAndReset( &ticks );
|
|
|
|
|
|
|
|
|
|
for ( int i = 0; i < m_testCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3DynamicTree_Query( &m_tree, m_overlapQueries[i], B3_DEFAULT_MASK_BITS, false, QueryCallback, this );
|
|
|
|
|
}
|
|
|
|
|
m_overlapTime = b3GetMilliseconds( ticks );
|
|
|
|
|
|
|
|
|
|
#if 0 && defined( _WIN32 )
|
|
|
|
|
// brute force avx2
|
|
|
|
|
int proxyCount = (int)m_proxies.size();
|
|
|
|
|
for ( int i = 0; i < m_testCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 point = m_closestPointQueries[i].center;
|
|
|
|
|
float minDistSq;
|
|
|
|
|
b3Vec3 closest;
|
|
|
|
|
|
|
|
|
|
ClosestPointAVX2( m_aabbMinX, m_aabbMinY, m_aabbMinZ, m_aabbMaxX, m_aabbMaxY, m_aabbMaxZ, proxyCount, point, &closest,
|
|
|
|
|
&minDistSq );
|
|
|
|
|
|
|
|
|
|
m_closestPoint = closest;
|
|
|
|
|
m_haveClosest = true;
|
|
|
|
|
}
|
|
|
|
|
#elif 0
|
|
|
|
|
// Brute force
|
|
|
|
|
int proxyCount = (int)m_proxies.size();
|
|
|
|
|
for ( int i = 0; i < m_testCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 point = m_closestPointQueries[i].center;
|
|
|
|
|
float minDistanceSquared = FLT_MAX;
|
|
|
|
|
m_haveClosest = false;
|
|
|
|
|
for ( int j = 0; j < proxyCount; ++j )
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 closestPoint = b3ClosestPointToAABB( point, m_proxies[j].aabb );
|
|
|
|
|
float d = b3DistanceSquared( point, closestPoint );
|
|
|
|
|
if ( d < minDistanceSquared )
|
|
|
|
|
{
|
|
|
|
|
m_closestPoint = closestPoint;
|
|
|
|
|
m_haveClosest = true;
|
|
|
|
|
minDistanceSquared = d;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#else
|
|
|
|
|
for ( int i = 0; i < m_testCount; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 point = m_closestPointQueries[i].center;
|
|
|
|
|
float radius = m_closestPointQueries[i].radius;
|
|
|
|
|
float distanceSquared = radius * radius;
|
2026-06-18 20:51:16 -07:00
|
|
|
m_closestPoint = b3ToPos( point );
|
2026-05-10 13:36:52 -07:00
|
|
|
m_haveClosest = false;
|
|
|
|
|
b3DynamicTree_QueryClosest( &m_tree, point, B3_DEFAULT_MASK_BITS, false, ClosetPointCallback, this,
|
|
|
|
|
&distanceSquared );
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
m_closestTime = b3GetMilliseconds( ticks );
|
|
|
|
|
}
|
|
|
|
|
|
2026-06-06 10:06:05 -07:00
|
|
|
bool HasSolverControls() const override
|
|
|
|
|
{
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool DrawControls() override
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
DrawTextLine( "leaves = %d, height = %d, area = %g", m_tree.proxyCount, m_height, m_areaRatio );
|
|
|
|
|
DrawTextLine( "build time = %g ms", m_buildTime );
|
|
|
|
|
DrawTextLine( "total: ray = %g ms, overlap = %g ms, closest = %g ms", m_rayTime, m_overlapTime, m_closestTime );
|
|
|
|
|
|
|
|
|
|
float s = 1000.0f / m_testCount;
|
|
|
|
|
DrawTextLine( "ave: ray = %g us, overlap = %g us, closest = %g us", s * m_rayTime, s * m_overlapTime, s * m_closestTime );
|
|
|
|
|
|
|
|
|
|
if ( ImGui::Combo( "File", &m_fileIndex, m_fileNames, m_fileCount ) )
|
|
|
|
|
{
|
|
|
|
|
CreateTree();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( ImGui::Button( "Top Down" ) )
|
|
|
|
|
{
|
|
|
|
|
uint64_t ticks = b3GetTicks();
|
|
|
|
|
b3DynamicTree_Rebuild( &m_tree, true );
|
|
|
|
|
m_buildTime = b3GetMilliseconds( ticks );
|
|
|
|
|
m_areaRatio = b3DynamicTree_GetAreaRatio( &m_tree );
|
|
|
|
|
m_height = ComputeDepths();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ImGui::Separator();
|
|
|
|
|
|
|
|
|
|
ImGui::Checkbox( "Ray Cast", &m_doRay );
|
|
|
|
|
ImGui::Checkbox( "Overlap", &m_doOverlap );
|
|
|
|
|
ImGui::Checkbox( "Closet Point", &m_doClosest );
|
|
|
|
|
|
|
|
|
|
if ( ImGui::Button( "Profile" ) )
|
|
|
|
|
{
|
|
|
|
|
Profile();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ImGui::SliderInt( "Test", &m_testIndex, 0, m_testCount - 1 );
|
|
|
|
|
ImGui::SliderInt( "Level", &m_drawLevel, -1, m_height );
|
|
|
|
|
ImGui::SliderFloat( "Kilometers", &m_drawDistance, 0.5f, 20.0f, "%.1f" );
|
|
|
|
|
|
|
|
|
|
if ( ImGui::Button( "Save" ) )
|
|
|
|
|
{
|
|
|
|
|
b3DynamicTree_Save( &m_tree, m_saveFileName );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( ImGui::Button( "Load" ) )
|
|
|
|
|
{
|
|
|
|
|
LoadTree();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ImGui::SliderFloat( "Load Scale", &m_loadScale, 0.01f, 1.0f );
|
|
|
|
|
|
2026-06-06 10:06:05 -07:00
|
|
|
return true;
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Render() override
|
|
|
|
|
{
|
|
|
|
|
Sample::Render();
|
|
|
|
|
|
2026-06-18 20:51:16 -07:00
|
|
|
DrawAxes( b3WorldTransform_identity, 2.0f );
|
2026-05-10 13:36:52 -07:00
|
|
|
|
2026-06-18 20:51:16 -07:00
|
|
|
b3Pos cp = m_camera->m_worldEye;
|
2026-05-10 13:36:52 -07:00
|
|
|
b3TreeNode* nodes = m_tree.nodes;
|
|
|
|
|
float distSquared = m_drawDistance * m_drawDistance * 1000.0f * 1000.0f;
|
|
|
|
|
|
|
|
|
|
if ( m_drawLevel >= 0 )
|
|
|
|
|
{
|
|
|
|
|
constexpr int colorCount = 20;
|
|
|
|
|
b3HexColor colors[colorCount] = {
|
|
|
|
|
b3_colorAliceBlue, b3_colorAntiqueWhite, b3_colorAqua, b3_colorAquamarine, b3_colorAzure,
|
|
|
|
|
b3_colorBeige, b3_colorBisque, b3_colorBlanchedAlmond, b3_colorBlue, b3_colorBlueViolet,
|
|
|
|
|
b3_colorBrown, b3_colorBurlywood, b3_colorCadetBlue, b3_colorChartreuse, b3_colorChocolate,
|
|
|
|
|
b3_colorCoral, b3_colorCornflowerBlue, b3_colorCornsilk, b3_colorCrimson, b3_colorCyan,
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
int capacity = m_tree.nodeCapacity;
|
|
|
|
|
for ( int i = 0; i < capacity; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3TreeNode* node = nodes + i;
|
|
|
|
|
if ( m_depths[i] != m_drawLevel || ( node->flags & b3_allocatedNode ) == 0 )
|
|
|
|
|
{
|
|
|
|
|
// skip internal nodes
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
b3Vec3 c = b3AABB_Center( node->aabb );
|
2026-06-18 20:51:16 -07:00
|
|
|
if ( m_drawLevel < 10 || b3LengthSquared( b3SubPos( cp, b3ToPos( c ) ) ) < distSquared )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
2026-06-06 10:06:05 -07:00
|
|
|
DrawBounds( node->aabb, 0.0f, MakeColor( colors[m_drawLevel % colorCount] ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
uint16_t requiredFlags = b3_allocatedNode | b3_leafNode;
|
|
|
|
|
for ( int i = 0; i < m_tree.nodeCapacity; ++i )
|
|
|
|
|
{
|
|
|
|
|
b3TreeNode* node = nodes + i;
|
|
|
|
|
|
|
|
|
|
if ( node->flags != requiredFlags )
|
|
|
|
|
{
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
b3Vec3 c = b3AABB_Center( node->aabb );
|
2026-06-18 20:51:16 -07:00
|
|
|
if ( b3LengthSquared( b3SubPos( cp, b3ToPos( c ) ) ) > distSquared )
|
2026-05-10 13:36:52 -07:00
|
|
|
{
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
Proxy& proxy = m_proxies[node->userData];
|
|
|
|
|
if ( proxy.queryTimeStamp == m_timeStamp || proxy.rayTimeStamp == m_timeStamp )
|
|
|
|
|
{
|
2026-06-06 10:06:05 -07:00
|
|
|
DrawBounds( node->aabb, 0.0f, MakeColor( b3_colorLightGray ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
// float extension = 0.01f * node->height;
|
|
|
|
|
float extension = 0.0f;
|
2026-06-06 10:06:05 -07:00
|
|
|
DrawBounds( node->aabb, extension, MakeColor( b3_colorLightBlue ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( m_doRay )
|
|
|
|
|
{
|
|
|
|
|
Ray ray = m_rays[m_testIndex];
|
2026-06-18 20:51:16 -07:00
|
|
|
DrawLine( b3ToPos( ray.origin ), b3ToPos( ray.origin + ray.translation ), MakeColor( b3_colorRed ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( m_doOverlap )
|
|
|
|
|
{
|
2026-06-06 10:06:05 -07:00
|
|
|
DrawBounds( m_overlapQueries[m_testIndex], 0.0f, MakeColor( b3_colorRed ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( m_doClosest )
|
|
|
|
|
{
|
2026-06-18 20:51:16 -07:00
|
|
|
DrawSolidSphere( b3WorldTransform_identity, m_closestPointQueries[m_testIndex], MakeColor( b3_colorCyan ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
if ( m_haveClosest )
|
|
|
|
|
{
|
2026-06-06 10:06:05 -07:00
|
|
|
DrawPoint( m_closestPoint, 15.0f, MakeColor( b3_colorOrange ) );
|
2026-05-10 13:36:52 -07:00
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Step() override
|
|
|
|
|
{
|
|
|
|
|
Sample::Step();
|
|
|
|
|
++m_timeStamp;
|
|
|
|
|
|
|
|
|
|
if ( m_doRay )
|
|
|
|
|
{
|
|
|
|
|
Ray ray = m_rays[m_testIndex];
|
|
|
|
|
b3RayCastInput input = { ray.origin, ray.translation, 1.0f };
|
|
|
|
|
|
|
|
|
|
b3DynamicTree_RayCast( &m_tree, &input, B3_DEFAULT_MASK_BITS, false, RayCallback, this );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( m_doOverlap )
|
|
|
|
|
{
|
|
|
|
|
b3DynamicTree_Query( &m_tree, m_overlapQueries[m_testIndex], B3_DEFAULT_MASK_BITS, false, QueryCallback, this );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if ( m_doClosest )
|
|
|
|
|
{
|
|
|
|
|
b3Vec3 point = m_closestPointQueries[m_testIndex].center;
|
|
|
|
|
float radius = m_closestPointQueries[m_testIndex].radius;
|
|
|
|
|
float distanceSquared = radius * radius;
|
2026-06-18 20:51:16 -07:00
|
|
|
m_closestPoint = b3ToPos( point );
|
2026-05-10 13:36:52 -07:00
|
|
|
m_haveClosest = false;
|
|
|
|
|
b3DynamicTree_QueryClosest( &m_tree, point, B3_DEFAULT_MASK_BITS, false, ClosetPointCallback, this,
|
|
|
|
|
&distanceSquared );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// constexpr int n = m_isDebug ? 0 : 100;
|
|
|
|
|
// for ( int i = 0; i < n; ++i )
|
|
|
|
|
//{
|
|
|
|
|
// Profile();
|
|
|
|
|
// }
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static Sample* Create( SampleContext* context )
|
|
|
|
|
{
|
|
|
|
|
return new TreeBenchmark( context );
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static constexpr int m_fileCount = 3;
|
|
|
|
|
static constexpr int m_testCount = 1024;
|
|
|
|
|
static constexpr const char* m_saveFileName = "tree.dt";
|
|
|
|
|
|
|
|
|
|
const char* m_fileNames[m_fileCount];
|
|
|
|
|
FileConfig m_config[m_fileCount];
|
|
|
|
|
int m_fileIndex;
|
|
|
|
|
|
|
|
|
|
float* m_aabbMinX;
|
|
|
|
|
float* m_aabbMinY;
|
|
|
|
|
float* m_aabbMinZ;
|
|
|
|
|
float* m_aabbMaxX;
|
|
|
|
|
float* m_aabbMaxY;
|
|
|
|
|
float* m_aabbMaxZ;
|
|
|
|
|
|
|
|
|
|
std::unordered_map<uint64_t, Proxy> m_proxies;
|
|
|
|
|
std::vector<uint16_t> m_depths;
|
|
|
|
|
b3DynamicTree m_tree;
|
|
|
|
|
Ray m_rays[m_testCount];
|
|
|
|
|
b3AABB m_overlapQueries[m_testCount];
|
|
|
|
|
b3Sphere m_closestPointQueries[m_testCount];
|
2026-06-18 20:51:16 -07:00
|
|
|
b3Pos m_closestPoint;
|
2026-05-10 13:36:52 -07:00
|
|
|
float m_areaRatio;
|
|
|
|
|
float m_rayTime;
|
|
|
|
|
float m_overlapTime;
|
|
|
|
|
float m_closestTime;
|
|
|
|
|
float m_buildTime;
|
|
|
|
|
float m_drawDistance;
|
|
|
|
|
float m_loadScale;
|
|
|
|
|
int m_testIndex;
|
|
|
|
|
int m_timeStamp;
|
|
|
|
|
int m_drawLevel;
|
|
|
|
|
int m_height;
|
|
|
|
|
bool m_haveClosest;
|
|
|
|
|
bool m_doOverlap;
|
|
|
|
|
bool m_doClosest;
|
|
|
|
|
bool m_doRay;
|
|
|
|
|
};
|
|
|
|
|
|
2026-06-06 10:06:05 -07:00
|
|
|
static int sampleTree = RegisterSample( "Tree", "Benchmark", TreeBenchmark::Create );
|