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
836 lines
26 KiB
C
836 lines
26 KiB
C
// SPDX-FileCopyrightText: 2026 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "test_macros.h"
|
|
|
|
// b3GetHeightFieldTriangle and b3IntersectRayTriangle are internal
|
|
#include "shape.h"
|
|
#include "simd.h"
|
|
|
|
#include "box3d/collision.h"
|
|
#include "box3d/math_functions.h"
|
|
|
|
#include <float.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
|
|
static int HeightFieldCreate( void )
|
|
{
|
|
b3Vec3 scale = { 1.0f, 1.0f, 1.0f };
|
|
b3HeightFieldData* hf = b3CreateGrid( 4, 4, scale, false );
|
|
|
|
ENSURE( hf->rowCount == 4 );
|
|
ENSURE( hf->columnCount == 4 );
|
|
ENSURE( hf->clockwise == false );
|
|
ENSURE( hf->version == B3_HEIGHT_FIELD_VERSION );
|
|
|
|
ENSURE_SMALL( hf->aabb.lowerBound.x, FLT_EPSILON );
|
|
ENSURE_SMALL( hf->aabb.lowerBound.y, FLT_EPSILON );
|
|
ENSURE_SMALL( hf->aabb.lowerBound.z, FLT_EPSILON );
|
|
|
|
ENSURE_SMALL( hf->aabb.upperBound.x - 3.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( hf->aabb.upperBound.y, FLT_EPSILON );
|
|
ENSURE_SMALL( hf->aabb.upperBound.z - 3.0f, FLT_EPSILON );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
static int HeightFieldTriangleIndex( void )
|
|
{
|
|
// Asymmetric grid (rows != cols) catches off-by-one errors between
|
|
// vertex stride (columnCount) and cell stride (columnCount - 1).
|
|
int rowCount = 4;
|
|
int columnCount = 5;
|
|
b3Vec3 scale = { 1.0f, 1.0f, 1.0f };
|
|
b3HeightFieldData* hf = b3CreateGrid( rowCount, columnCount, scale, false );
|
|
|
|
int triangleCount = 2 * ( rowCount - 1 ) * ( columnCount - 1 );
|
|
|
|
for ( int triangleIndex = 0; triangleIndex < triangleCount; ++triangleIndex )
|
|
{
|
|
int quadIndex = triangleIndex >> 1;
|
|
int sub = triangleIndex & 1;
|
|
int row = quadIndex / ( columnCount - 1 );
|
|
int column = quadIndex - row * ( columnCount - 1 );
|
|
|
|
int index11 = row * columnCount + column;
|
|
int index12 = index11 + 1;
|
|
int index21 = ( row + 1 ) * columnCount + column;
|
|
int index22 = index21 + 1;
|
|
|
|
b3Triangle t = b3GetHeightFieldTriangle( hf, triangleIndex );
|
|
|
|
if ( sub == 0 )
|
|
{
|
|
// Triangle 0 (CCW): {11, 21, 12}
|
|
ENSURE( t.i1 == index11 );
|
|
ENSURE( t.i2 == index21 );
|
|
ENSURE( t.i3 == index12 );
|
|
}
|
|
else
|
|
{
|
|
// Triangle 1 (CCW): {22, 12, 21}
|
|
ENSURE( t.i1 == index22 );
|
|
ENSURE( t.i2 == index12 );
|
|
ENSURE( t.i3 == index21 );
|
|
}
|
|
}
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
static int HeightFieldWinding( void )
|
|
{
|
|
// Build the same flat 3x3 field with CCW and CW winding. The cross-product
|
|
// normal of triangle 0 must flip sign accordingly.
|
|
float heights[9] = { 0 };
|
|
uint8_t materials[4] = { 0, 0, 0, 0 };
|
|
|
|
b3HeightFieldDef def = { 0 };
|
|
def.heights = heights;
|
|
def.materialIndices = materials;
|
|
def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f };
|
|
def.countX = 3;
|
|
def.countZ = 3;
|
|
def.globalMinimumHeight = -1.0f;
|
|
def.globalMaximumHeight = 1.0f;
|
|
|
|
def.clockwiseWinding = false;
|
|
b3HeightFieldData* ccw = b3CreateHeightField( &def );
|
|
|
|
def.clockwiseWinding = true;
|
|
b3HeightFieldData* cw = b3CreateHeightField( &def );
|
|
|
|
b3Triangle ta = b3GetHeightFieldTriangle( ccw, 0 );
|
|
b3Triangle tb = b3GetHeightFieldTriangle( cw, 0 );
|
|
|
|
b3Vec3 na =
|
|
b3Normalize( b3Cross( b3Sub( ta.vertices[1], ta.vertices[0] ), b3Sub( ta.vertices[2], ta.vertices[0] ) ) );
|
|
b3Vec3 nb =
|
|
b3Normalize( b3Cross( b3Sub( tb.vertices[1], tb.vertices[0] ), b3Sub( tb.vertices[2], tb.vertices[0] ) ) );
|
|
|
|
ENSURE_SMALL( na.x, FLT_EPSILON );
|
|
ENSURE_SMALL( na.y - 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( na.z, FLT_EPSILON );
|
|
|
|
ENSURE_SMALL( nb.x, FLT_EPSILON );
|
|
ENSURE_SMALL( nb.y + 1.0f, FLT_EPSILON );
|
|
ENSURE_SMALL( nb.z, FLT_EPSILON );
|
|
|
|
b3DestroyHeightField( ccw );
|
|
b3DestroyHeightField( cw );
|
|
return 0;
|
|
}
|
|
|
|
static int RayCastFlatField( void )
|
|
{
|
|
// Build a flat 4x4 field with a tight quantization range so the recovered
|
|
// surface stays within ~1e-5 of y=0 (b3CreateGrid uses -256..256 which
|
|
// blows the 1/UINT16_MAX quantum up to ~4e-3 in y).
|
|
float heights[16] = { 0 };
|
|
uint8_t materials[9] = { 0 };
|
|
|
|
b3HeightFieldDef def = { 0 };
|
|
def.heights = heights;
|
|
def.materialIndices = materials;
|
|
def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f };
|
|
def.countX = 4;
|
|
def.countZ = 4;
|
|
def.globalMinimumHeight = -1.0f;
|
|
def.globalMaximumHeight = 1.0f;
|
|
def.clockwiseWinding = false;
|
|
|
|
b3HeightFieldData* hf = b3CreateHeightField( &def );
|
|
|
|
// Origin sits clearly inside triangle 0 of cell (1, 1). Off the cell
|
|
// diagonal x+z = 3. The translation overshoots the surface so the hit
|
|
// fraction is strictly less than maxFraction.
|
|
b3RayCastInput input = { 0 };
|
|
input.origin = (b3Vec3){ 1.25f, 10.0f, 1.25f };
|
|
input.translation = (b3Vec3){ 0.0f, -20.0f, 0.0f };
|
|
input.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3RayCastHeightField( hf, &input );
|
|
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.5f, 1e-5f );
|
|
ENSURE_SMALL( out.normal.x, 1e-5f );
|
|
ENSURE_SMALL( out.normal.y - 1.0f, 1e-5f );
|
|
ENSURE_SMALL( out.normal.z, 1e-5f );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
static int OverlapAtSurface( void )
|
|
{
|
|
b3Vec3 scale = { 1.0f, 1.0f, 1.0f };
|
|
b3HeightFieldData* hf = b3CreateGrid( 4, 4, scale, false );
|
|
|
|
// Sphere center 1.0 above the surface, radius 0.5. Clear gap.
|
|
b3Vec3 above = { 1.5f, 1.0f, 1.5f };
|
|
b3ShapeProxy proxyAbove = { &above, 1, 0.5f };
|
|
bool hitAbove = b3OverlapHeightField( hf, b3Transform_identity, &proxyAbove );
|
|
ENSURE( hitAbove == false );
|
|
|
|
// Sphere centered on the surface. Radius pokes through.
|
|
b3Vec3 through = { 1.5f, 0.0f, 1.5f };
|
|
b3ShapeProxy proxyThrough = { &through, 1, 0.5f };
|
|
bool hitThrough = b3OverlapHeightField( hf, b3Transform_identity, &proxyThrough );
|
|
ENSURE( hitThrough == true );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
static int FileRoundtrip( void )
|
|
{
|
|
float heights[9] = { 0.0f, 0.5f, -0.3f, 0.1f, 0.0f, 0.0f, 0.0f, 0.2f, 0.0f };
|
|
uint8_t materials[4] = { 0, B3_HEIGHT_FIELD_HOLE, 1, 2 };
|
|
|
|
b3HeightFieldDef def = { 0 };
|
|
def.heights = heights;
|
|
def.materialIndices = materials;
|
|
def.scale = (b3Vec3){ 1.5f, 2.0f, 0.75f };
|
|
def.countX = 3;
|
|
def.countZ = 3;
|
|
def.globalMinimumHeight = -1.0f;
|
|
def.globalMaximumHeight = 1.0f;
|
|
def.clockwiseWinding = true;
|
|
|
|
const char* path = "test_height_field_roundtrip.dat";
|
|
b3DumpHeightData( &def, path );
|
|
|
|
b3HeightFieldData* loaded = b3LoadHeightField( path );
|
|
remove( path );
|
|
|
|
ENSURE( loaded != NULL );
|
|
ENSURE( loaded->rowCount == def.countZ );
|
|
ENSURE( loaded->columnCount == def.countX );
|
|
ENSURE( loaded->clockwise == def.clockwiseWinding );
|
|
|
|
ENSURE_SMALL( loaded->scale.x - def.scale.x, FLT_EPSILON );
|
|
ENSURE_SMALL( loaded->scale.y - def.scale.y, FLT_EPSILON );
|
|
ENSURE_SMALL( loaded->scale.z - def.scale.z, FLT_EPSILON );
|
|
ENSURE_SMALL( loaded->minHeight - def.globalMinimumHeight, FLT_EPSILON );
|
|
ENSURE_SMALL( loaded->maxHeight - def.globalMaximumHeight, FLT_EPSILON );
|
|
|
|
int cellCount = ( def.countX - 1 ) * ( def.countZ - 1 );
|
|
for ( int i = 0; i < cellCount; ++i )
|
|
{
|
|
ENSURE( b3GetHeightFieldMaterialIndices( loaded )[i] == materials[i] );
|
|
}
|
|
|
|
// Recovered heights round-trip within the quantization tolerance.
|
|
float quantum = ( def.globalMaximumHeight - def.globalMinimumHeight ) / (float)UINT16_MAX;
|
|
for ( int i = 0; i < def.countX * def.countZ; ++i )
|
|
{
|
|
float recovered = loaded->minHeight + loaded->heightScale * b3GetHeightFieldCompressedHeights( loaded )[i];
|
|
ENSURE_SMALL( recovered - heights[i], 2.0f * quantum );
|
|
}
|
|
|
|
b3DestroyHeightField( loaded );
|
|
return 0;
|
|
}
|
|
|
|
static int ShapeCastVerticalStraddle( void )
|
|
{
|
|
// Regression: a vertical shape cast whose swept volume straddles a cell
|
|
// boundary must test every cell it overlaps. The field is flat at y = 0 with
|
|
// only cell (0,0) solid; the surrounding cells are holes. Each sphere is
|
|
// dropped straight down with its center nudged just past a boundary of the
|
|
// solid cell, so that cell sits on the trailing (-x / -z) side of the sweep.
|
|
// A cull AABB pinned to the leading box corner skips the solid cell entirely
|
|
// and reports a miss.
|
|
float heights[9] = { 0 };
|
|
uint8_t materials[4] = { 0, B3_HEIGHT_FIELD_HOLE, B3_HEIGHT_FIELD_HOLE, B3_HEIGHT_FIELD_HOLE };
|
|
|
|
b3HeightFieldDef def = { 0 };
|
|
def.heights = heights;
|
|
def.materialIndices = materials;
|
|
def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f };
|
|
def.countX = 3;
|
|
def.countZ = 3;
|
|
def.globalMinimumHeight = -1.0f;
|
|
def.globalMaximumHeight = 1.0f;
|
|
def.clockwiseWinding = false;
|
|
|
|
b3HeightFieldData* hf = b3CreateHeightField( &def );
|
|
|
|
// Solid cell (0,0) spans x,z in [0,1]. Radius 0.3 with the center 0.05 past a
|
|
// boundary still reaches back into the solid cell.
|
|
const float radius = 0.3f;
|
|
|
|
// Straddle the x = 1 edge: solid cell is on the -x side. Contact lands on the
|
|
// cell edge, sqrt(0.05^2 + cy^2) = radius -> cy = 0.2958040,
|
|
// fraction = (10 - cy) / 20 = 0.4852098.
|
|
{
|
|
b3Vec3 center = { 1.05f, 10.0f, 0.5f };
|
|
b3ShapeCastInput input = { 0 };
|
|
input.proxy = (b3ShapeProxy){ ¢er, 1, radius };
|
|
input.translation = (b3Vec3){ 0.0f, -20.0f, 0.0f };
|
|
input.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3ShapeCastHeightField( hf, &input );
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.4852098f, 2e-3f );
|
|
}
|
|
|
|
// Straddle the z = 1 edge: solid cell is on the -z side (same geometry).
|
|
{
|
|
b3Vec3 center = { 0.5f, 10.0f, 1.05f };
|
|
b3ShapeCastInput input = { 0 };
|
|
input.proxy = (b3ShapeProxy){ ¢er, 1, radius };
|
|
input.translation = (b3Vec3){ 0.0f, -20.0f, 0.0f };
|
|
input.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3ShapeCastHeightField( hf, &input );
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.4852098f, 2e-3f );
|
|
}
|
|
|
|
// Straddle the (1,1) corner: solid cell is diagonally trailing. Contact lands
|
|
// on the corner vertex, sqrt(2*0.05^2 + cy^2) = radius -> cy = 0.2915476,
|
|
// fraction = (10 - cy) / 20 = 0.4854226.
|
|
{
|
|
b3Vec3 center = { 1.05f, 10.0f, 1.05f };
|
|
b3ShapeCastInput input = { 0 };
|
|
input.proxy = (b3ShapeProxy){ ¢er, 1, radius };
|
|
input.translation = (b3Vec3){ 0.0f, -20.0f, 0.0f };
|
|
input.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3ShapeCastHeightField( hf, &input );
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.4854226f, 2e-3f );
|
|
}
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
// Brute-force shape cast: cast the proxy against every (non-hole) triangle and
|
|
// keep the closest hit. This is the ground truth for b3ShapeCastHeightField.
|
|
static b3CastOutput BruteForceShapeCast( const b3HeightFieldData* hf, const b3ShapeCastInput* input )
|
|
{
|
|
b3CastOutput best = { 0 };
|
|
float bestFraction = input->maxFraction;
|
|
|
|
int triangleCount = b3GetHeightFieldTriangleCount( hf );
|
|
for ( int t = 0; t < triangleCount; ++t )
|
|
{
|
|
int cellIndex = t >> 1;
|
|
if ( b3GetHeightFieldMaterialIndices( hf )[cellIndex] == B3_HEIGHT_FIELD_HOLE )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
b3Triangle tri = b3GetHeightFieldTriangle( hf, t );
|
|
|
|
b3ShapeCastPairInput pair = { 0 };
|
|
pair.proxyA = (b3ShapeProxy){ tri.vertices, 3, 0.0f };
|
|
pair.proxyB = input->proxy;
|
|
pair.transform = b3Transform_identity;
|
|
pair.translationB = input->translation;
|
|
pair.maxFraction = bestFraction;
|
|
pair.canEncroach = input->canEncroach;
|
|
|
|
b3CastOutput out = b3ShapeCast( &pair );
|
|
if ( out.hit && out.fraction < bestFraction )
|
|
{
|
|
bestFraction = out.fraction;
|
|
best = out;
|
|
best.triangleIndex = t;
|
|
}
|
|
}
|
|
|
|
return best;
|
|
}
|
|
|
|
static int ShapeCastBruteForce( void )
|
|
{
|
|
// b3ShapeCastHeightField walks the grid and culls cells; the brute-force cast
|
|
// against every triangle is the ground truth. The grid walk must never miss a
|
|
// closer hit, regardless of cast direction, origin or radius.
|
|
b3Vec3 scale = { 2.0f, 1.5f, 2.0f };
|
|
b3HeightFieldData* hf = b3CreateWave( 10, 10, scale, 0.1f, 0.03333f, false );
|
|
|
|
// Documented repro from sample/sample_mesh.cpp "Height Field": a sphere cast
|
|
// that moves only in z (and y). Body at (-9,0,-9), world origin (5.5,4,2.913)
|
|
// -> local (14.5,4,11.913). The grid walk used to terminate one row early
|
|
// because it compared a clamped-sweep fraction against an input-space one.
|
|
{
|
|
b3Vec3 origin = { 14.5f, 4.0f, 11.913f };
|
|
b3ShapeCastInput input = { 0 };
|
|
input.proxy = (b3ShapeProxy){ &origin, 1, 0.2f };
|
|
input.translation = (b3Vec3){ 0.0f, -8.0f, 6.397f };
|
|
input.maxFraction = 1.0f;
|
|
|
|
b3CastOutput grid = b3ShapeCastHeightField( hf, &input );
|
|
b3CastOutput brute = BruteForceShapeCast( hf, &input );
|
|
ENSURE( brute.hit == true );
|
|
ENSURE( grid.hit == brute.hit );
|
|
ENSURE_SMALL( grid.fraction - brute.fraction, 2e-3f );
|
|
}
|
|
|
|
// Sweep origins across the field with assorted directions and radii.
|
|
const float radii[] = { 0.15f, 0.4f, 0.9f };
|
|
const b3Vec3 deltas[] = {
|
|
{ 0.0f, -8.0f, 0.0f }, // vertical
|
|
{ 0.0f, -8.0f, 6.4f }, // z only (+ y)
|
|
{ 5.1f, -8.0f, 0.0f }, // x only (+ y)
|
|
{ 0.0f, -8.0f, -6.4f }, // -z
|
|
{ -5.1f, -8.0f, 0.0f }, // -x
|
|
{ 6.0f, -8.0f, 5.0f }, // diagonal
|
|
{ -7.0f, -8.0f, 4.0f }, // diagonal, mixed sign
|
|
{ 9.0f, -3.0f, -9.0f }, // shallow diagonal
|
|
};
|
|
|
|
int failures = 0;
|
|
for ( int xi = 0; xi < 5; ++xi )
|
|
{
|
|
for ( int zi = 0; zi < 5; ++zi )
|
|
{
|
|
// 0.05 nudge keeps the swept box straddling cell boundaries.
|
|
b3Vec3 origin = { 1.0f + 4.0f * xi + 0.05f, 4.0f, 1.0f + 4.0f * zi + 0.05f };
|
|
|
|
for ( int di = 0; di < ARRAY_COUNT( deltas ); ++di )
|
|
{
|
|
for ( int ri = 0; ri < ARRAY_COUNT( radii ); ++ri )
|
|
{
|
|
b3ShapeCastInput input = { 0 };
|
|
input.proxy = (b3ShapeProxy){ &origin, 1, radii[ri] };
|
|
input.translation = deltas[di];
|
|
input.maxFraction = 1.0f;
|
|
|
|
b3CastOutput grid = b3ShapeCastHeightField( hf, &input );
|
|
b3CastOutput brute = BruteForceShapeCast( hf, &input );
|
|
|
|
float diff = grid.fraction - brute.fraction;
|
|
diff = diff < 0.0f ? -diff : diff;
|
|
|
|
if ( grid.hit != brute.hit || ( brute.hit && diff > 2e-3f ) )
|
|
{
|
|
printf( " mismatch: origin=(%.2f,%.2f,%.2f) delta=(%.2f,%.2f,%.2f) r=%.2f"
|
|
" grid(hit=%d,f=%.5f) brute(hit=%d,f=%.5f tri=%d)\n",
|
|
origin.x, origin.y, origin.z, deltas[di].x, deltas[di].y, deltas[di].z, radii[ri],
|
|
grid.hit, grid.fraction, brute.hit, brute.fraction, brute.triangleIndex );
|
|
failures += 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
ENSURE( failures == 0 );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
// Brute-force ray cast: cast the ray against every (non-hole) triangle and keep
|
|
// the closest hit. b3GetHeightFieldTriangle returns vertices in the same winding
|
|
// order that b3ShapeCastHeightField feeds to b3IntersectRayTriangle, so this is a
|
|
// pure traversal/culling check. The per-triangle math is identical.
|
|
static b3CastOutput BruteForceRayCast( const b3HeightFieldData* hf, const b3RayCastInput* input )
|
|
{
|
|
b3CastOutput best = { 0 };
|
|
float bestFraction = input->maxFraction;
|
|
|
|
b3V32 rayStart = b3LoadV( &input->origin.x );
|
|
b3V32 rayDelta = b3LoadV( &input->translation.x );
|
|
|
|
int triangleCount = b3GetHeightFieldTriangleCount( hf );
|
|
for ( int t = 0; t < triangleCount; ++t )
|
|
{
|
|
int cellIndex = t >> 1;
|
|
if ( b3GetHeightFieldMaterialIndices( hf )[cellIndex] == B3_HEIGHT_FIELD_HOLE )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
b3Triangle tri = b3GetHeightFieldTriangle( hf, t );
|
|
b3V32 v1 = b3LoadV( &tri.vertices[0].x );
|
|
b3V32 v2 = b3LoadV( &tri.vertices[1].x );
|
|
b3V32 v3 = b3LoadV( &tri.vertices[2].x );
|
|
|
|
// b3IntersectRayTriangle returns 1.0 on a miss.
|
|
float alpha = b3IntersectRayTriangle( rayStart, rayDelta, v1, v2, v3 );
|
|
if ( alpha < bestFraction )
|
|
{
|
|
bestFraction = alpha;
|
|
best.hit = true;
|
|
best.fraction = alpha;
|
|
best.triangleIndex = t;
|
|
}
|
|
}
|
|
|
|
return best;
|
|
}
|
|
|
|
static int RayCastBruteForce( void )
|
|
{
|
|
// b3RayCastHeightField routes through the same grid walk as the shape cast
|
|
// (radius-zero point proxy), so it is subject to the same early-termination
|
|
// bug. The brute-force cast against every triangle is the ground truth.
|
|
b3Vec3 scale = { 2.0f, 1.5f, 2.0f };
|
|
b3HeightFieldData* hf = b3CreateWave( 10, 10, scale, 0.1f, 0.03333f, false );
|
|
|
|
const b3Vec3 deltas[] = {
|
|
{ 0.0f, -8.0f, 0.0f }, // straight down
|
|
{ 0.0f, -8.0f, 12.0f }, // down + z
|
|
{ 12.0f, -8.0f, 0.0f }, // down + x
|
|
{ 0.0f, -8.0f, -12.0f }, // down - z
|
|
{ -12.0f, -8.0f, 0.0f }, // down - x
|
|
{ 14.0f, -8.0f, 11.0f }, // diagonal
|
|
{ -13.0f, -8.0f, 9.0f }, // diagonal, mixed sign
|
|
{ 16.0f, -4.0f, -15.0f }, // shallow diagonal
|
|
};
|
|
|
|
int failures = 0;
|
|
for ( int xi = 0; xi < 5; ++xi )
|
|
{
|
|
for ( int zi = 0; zi < 5; ++zi )
|
|
{
|
|
// 0.05 nudge keeps the ray off cell boundaries.
|
|
b3Vec3 origin = { 1.0f + 4.0f * xi + 0.05f, 4.0f, 1.0f + 4.0f * zi + 0.05f };
|
|
|
|
for ( int di = 0; di < ARRAY_COUNT( deltas ); ++di )
|
|
{
|
|
b3RayCastInput input = { 0 };
|
|
input.origin = origin;
|
|
input.translation = deltas[di];
|
|
input.maxFraction = 1.0f;
|
|
|
|
b3CastOutput grid = b3RayCastHeightField( hf, &input );
|
|
b3CastOutput brute = BruteForceRayCast( hf, &input );
|
|
|
|
float diff = grid.fraction - brute.fraction;
|
|
diff = diff < 0.0f ? -diff : diff;
|
|
|
|
if ( grid.hit != brute.hit || ( brute.hit && diff > 1e-4f ) )
|
|
{
|
|
printf( " mismatch: origin=(%.2f,%.2f,%.2f) delta=(%.2f,%.2f,%.2f)"
|
|
" grid(hit=%d,f=%.6f tri=%d) brute(hit=%d,f=%.6f tri=%d)\n",
|
|
origin.x, origin.y, origin.z, deltas[di].x, deltas[di].y, deltas[di].z, grid.hit,
|
|
grid.fraction, grid.triangleIndex, brute.hit, brute.fraction, brute.triangleIndex );
|
|
failures += 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
ENSURE( failures == 0 );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
// Flat 3x3 vertex field at y = 0, one cell material per entry. Caller destroys.
|
|
static b3HeightFieldData* MakeFlatField( uint8_t* materials, bool clockwise )
|
|
{
|
|
float heights[9] = { 0 };
|
|
|
|
b3HeightFieldDef def = { 0 };
|
|
def.heights = heights;
|
|
def.materialIndices = materials;
|
|
def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f };
|
|
def.countX = 3;
|
|
def.countZ = 3;
|
|
def.globalMinimumHeight = -1.0f;
|
|
def.globalMaximumHeight = 1.0f;
|
|
def.clockwiseWinding = clockwise;
|
|
|
|
return b3CreateHeightField( &def );
|
|
}
|
|
|
|
static int ShapeCastBackside( void )
|
|
{
|
|
uint8_t materials[4] = { 1, 0, 0, 0 };
|
|
b3HeightFieldData* hf = MakeFlatField( materials, false );
|
|
|
|
// Falling onto the front (upper) face reports the hit, the triangle, and its material.
|
|
// The surface sits at y = 0, so a sphere of radius 0.2 stops with its center at y = 0.2.
|
|
b3Vec3 above = { 0.3f, 5.0f, 0.25f };
|
|
b3ShapeCastInput down = { 0 };
|
|
down.proxy = (b3ShapeProxy){ &above, 1, 0.2f };
|
|
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
|
down.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3ShapeCastHeightField( hf, &down );
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.48f, 1e-2f );
|
|
ENSURE( out.normal.y > 0.99f );
|
|
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
|
ENSURE( out.materialIndex == 1 );
|
|
|
|
// Rising from behind the face is culled
|
|
b3Vec3 below = { 0.3f, -5.0f, 0.25f };
|
|
b3ShapeCastInput up = { 0 };
|
|
up.proxy = (b3ShapeProxy){ &below, 1, 0.2f };
|
|
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
|
up.maxFraction = 1.0f;
|
|
|
|
b3CastOutput missed = b3ShapeCastHeightField( hf, &up );
|
|
ENSURE( missed.hit == false );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
// A clockwise height field faces down, see HeightFieldWinding. Shape cast backside
|
|
// culling must respect the flag: a rising sphere hits from below, not from above.
|
|
static int ShapeCastClockwiseWinding( void )
|
|
{
|
|
uint8_t materials[4] = { 1, 0, 0, 0 };
|
|
b3HeightFieldData* hf = MakeFlatField( materials, true );
|
|
|
|
b3Vec3 below = { 0.3f, -5.0f, 0.25f };
|
|
b3ShapeCastInput up = { 0 };
|
|
up.proxy = (b3ShapeProxy){ &below, 1, 0.2f };
|
|
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
|
up.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3ShapeCastHeightField( hf, &up );
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.48f, 1e-2f );
|
|
ENSURE( out.normal.y < -0.99f );
|
|
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
|
ENSURE( out.materialIndex == 1 );
|
|
|
|
b3Vec3 above = { 0.3f, 5.0f, 0.25f };
|
|
b3ShapeCastInput down = { 0 };
|
|
down.proxy = (b3ShapeProxy){ &above, 1, 0.2f };
|
|
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
|
down.maxFraction = 1.0f;
|
|
|
|
b3CastOutput missed = b3ShapeCastHeightField( hf, &down );
|
|
ENSURE( missed.hit == false );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
// Ray casts run through the shape cast. The default winding faces up, so a ray from below is the
|
|
// back side and gets culled.
|
|
static int RayCastBackside( void )
|
|
{
|
|
uint8_t materials[4] = { 1, 0, 0, 0 };
|
|
b3HeightFieldData* hf = MakeFlatField( materials, false );
|
|
|
|
b3RayCastInput down = { 0 };
|
|
down.origin = (b3Vec3){ 0.3f, 5.0f, 0.25f };
|
|
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
|
down.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3RayCastHeightField( hf, &down );
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.5f, 1e-3f );
|
|
ENSURE( out.normal.y > 0.99f );
|
|
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
|
ENSURE( out.materialIndex == 1 );
|
|
|
|
b3RayCastInput up = { 0 };
|
|
up.origin = (b3Vec3){ 0.3f, -5.0f, 0.25f };
|
|
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
|
up.maxFraction = 1.0f;
|
|
|
|
b3CastOutput missed = b3RayCastHeightField( hf, &up );
|
|
ENSURE( missed.hit == false );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
// A clockwise field faces down. The winding swap already flips the triangle, so the ray normal
|
|
// must not be flipped a second time.
|
|
static int RayCastClockwiseWinding( void )
|
|
{
|
|
uint8_t materials[4] = { 1, 0, 0, 0 };
|
|
b3HeightFieldData* hf = MakeFlatField( materials, true );
|
|
|
|
b3RayCastInput up = { 0 };
|
|
up.origin = (b3Vec3){ 0.3f, -5.0f, 0.25f };
|
|
up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f };
|
|
up.maxFraction = 1.0f;
|
|
|
|
b3CastOutput out = b3RayCastHeightField( hf, &up );
|
|
ENSURE( out.hit == true );
|
|
ENSURE_SMALL( out.fraction - 0.5f, 1e-3f );
|
|
ENSURE( out.normal.y < -0.99f );
|
|
ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 );
|
|
ENSURE( out.materialIndex == 1 );
|
|
|
|
b3RayCastInput down = { 0 };
|
|
down.origin = (b3Vec3){ 0.3f, 5.0f, 0.25f };
|
|
down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f };
|
|
down.maxFraction = 1.0f;
|
|
|
|
b3CastOutput missed = b3RayCastHeightField( hf, &down );
|
|
ENSURE( missed.hit == false );
|
|
|
|
b3DestroyHeightField( hf );
|
|
return 0;
|
|
}
|
|
|
|
#define BUMPY_COUNT_X 9
|
|
#define BUMPY_COUNT_Z 7
|
|
|
|
// A bumpy field with holes, so cells have every mix of interior, border and hole neighbors
|
|
static b3HeightFieldData* MakeBumpyField( bool clockwise )
|
|
{
|
|
float heights[BUMPY_COUNT_X * BUMPY_COUNT_Z];
|
|
for ( int i = 0; i < BUMPY_COUNT_Z; ++i )
|
|
{
|
|
for ( int j = 0; j < BUMPY_COUNT_X; ++j )
|
|
{
|
|
heights[i * BUMPY_COUNT_X + j] = sinf( 0.9f * (float)i ) * cosf( 0.7f * (float)j ) + 0.1f * (float)( ( i * j ) % 3 );
|
|
}
|
|
}
|
|
|
|
uint8_t materials[( BUMPY_COUNT_X - 1 ) * ( BUMPY_COUNT_Z - 1 )] = { 0 };
|
|
materials[9] = B3_HEIGHT_FIELD_HOLE;
|
|
materials[20] = B3_HEIGHT_FIELD_HOLE;
|
|
materials[47] = B3_HEIGHT_FIELD_HOLE;
|
|
|
|
b3HeightFieldDef def = { 0 };
|
|
def.heights = heights;
|
|
def.materialIndices = materials;
|
|
def.scale = (b3Vec3){ 1.5f, 0.8f, 0.5f };
|
|
def.countX = BUMPY_COUNT_X;
|
|
def.countZ = BUMPY_COUNT_Z;
|
|
def.globalMinimumHeight = -2.0f;
|
|
def.globalMaximumHeight = 2.0f;
|
|
def.clockwiseWinding = clockwise;
|
|
return b3CreateHeightField( &def );
|
|
}
|
|
|
|
static bool IsHole( const b3HeightFieldData* hf, int triangleIndex )
|
|
{
|
|
return b3GetHeightFieldMaterial( hf, triangleIndex ) == B3_HEIGHT_FIELD_HOLE;
|
|
}
|
|
|
|
static b3Vec3 TriangleNormal( b3Triangle triangle )
|
|
{
|
|
b3Vec3 e1 = b3Sub( triangle.vertices[1], triangle.vertices[0] );
|
|
b3Vec3 e2 = b3Sub( triangle.vertices[2], triangle.vertices[0] );
|
|
return b3Normalize( b3Cross( e1, e2 ) );
|
|
}
|
|
|
|
// Neighbors found by brute force over shared grid points must match the grid walk
|
|
static int HeightFieldNeighborNormals( void )
|
|
{
|
|
for ( int pass = 0; pass < 2; ++pass )
|
|
{
|
|
b3HeightFieldData* hf = MakeBumpyField( pass == 1 );
|
|
int triangleCount = b3GetHeightFieldTriangleCount( hf );
|
|
int interiorCount = 0;
|
|
int boundaryCount = 0;
|
|
|
|
for ( int i = 0; i < triangleCount; ++i )
|
|
{
|
|
if ( IsHole( hf, i ) )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
b3Triangle triangle = b3GetHeightFieldTriangle( hf, i );
|
|
int indices[3] = { triangle.i1, triangle.i2, triangle.i3 };
|
|
|
|
for ( int k = 0; k < 3; ++k )
|
|
{
|
|
int a = indices[k];
|
|
int b = indices[( k + 1 ) % 3];
|
|
|
|
int neighbor = B3_NULL_INDEX;
|
|
for ( int j = 0; j < triangleCount && neighbor == B3_NULL_INDEX; ++j )
|
|
{
|
|
if ( j == i || IsHole( hf, j ) )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
b3Triangle other = b3GetHeightFieldTriangle( hf, j );
|
|
int otherIndices[3] = { other.i1, other.i2, other.i3 };
|
|
for ( int m = 0; m < 3; ++m )
|
|
{
|
|
if ( otherIndices[m] == b && otherIndices[( m + 1 ) % 3] == a )
|
|
{
|
|
neighbor = j;
|
|
}
|
|
}
|
|
}
|
|
|
|
b3Vec3 normal;
|
|
bool found = b3GetHeightFieldNeighborNormal( hf, i, k, &normal );
|
|
ENSURE( found == ( neighbor != B3_NULL_INDEX ) );
|
|
|
|
if ( found == false )
|
|
{
|
|
boundaryCount += 1;
|
|
continue;
|
|
}
|
|
|
|
interiorCount += 1;
|
|
b3Vec3 expected = TriangleNormal( b3GetHeightFieldTriangle( hf, neighbor ) );
|
|
ENSURE( b3Dot( normal, expected ) > 0.99999f );
|
|
}
|
|
}
|
|
|
|
ENSURE( interiorCount > 0 );
|
|
ENSURE( boundaryCount > 0 );
|
|
b3DestroyHeightField( hf );
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
// The shape level lookup dispatches to the mesh and height field versions
|
|
static int NeighborNormalDispatch( void )
|
|
{
|
|
b3HeightFieldData* hf = MakeBumpyField( false );
|
|
b3Shape heightShape = { 0 };
|
|
heightShape.type = b3_heightShape;
|
|
heightShape.heightField = hf;
|
|
|
|
b3Vec3 normal1, normal2;
|
|
ENSURE( b3GetNeighborNormal( &heightShape, 1, 1, &normal1 ) );
|
|
ENSURE( b3GetHeightFieldNeighborNormal( hf, 1, 1, &normal2 ) );
|
|
ENSURE( normal1.x == normal2.x && normal1.y == normal2.y && normal1.z == normal2.z );
|
|
b3DestroyHeightField( hf );
|
|
|
|
b3MeshData* data = b3CreateWaveMesh( 4, 4, 1.0f, 0.5f, 0.2f, 0.15f );
|
|
b3Shape meshShape = { 0 };
|
|
meshShape.type = b3_meshShape;
|
|
meshShape.mesh = (b3Mesh){ data, { 2.0f, 0.5f, -1.0f } };
|
|
|
|
bool found1 = b3GetNeighborNormal( &meshShape, 5, 1, &normal1 );
|
|
bool found2 = b3GetMeshNeighborNormal( &meshShape.mesh, 5, 1, &normal2 );
|
|
ENSURE( found1 && found2 );
|
|
ENSURE( normal1.x == normal2.x && normal1.y == normal2.y && normal1.z == normal2.z );
|
|
b3DestroyMesh( data );
|
|
|
|
return 0;
|
|
}
|
|
|
|
int HeightFieldTest( void )
|
|
{
|
|
RUN_SUBTEST( HeightFieldCreate );
|
|
RUN_SUBTEST( HeightFieldTriangleIndex );
|
|
RUN_SUBTEST( HeightFieldWinding );
|
|
RUN_SUBTEST( RayCastFlatField );
|
|
RUN_SUBTEST( OverlapAtSurface );
|
|
RUN_SUBTEST( FileRoundtrip );
|
|
RUN_SUBTEST( ShapeCastVerticalStraddle );
|
|
RUN_SUBTEST( ShapeCastBruteForce );
|
|
RUN_SUBTEST( RayCastBruteForce );
|
|
RUN_SUBTEST( ShapeCastBackside );
|
|
RUN_SUBTEST( ShapeCastClockwiseWinding );
|
|
RUN_SUBTEST( RayCastBackside );
|
|
RUN_SUBTEST( RayCastClockwiseWinding );
|
|
RUN_SUBTEST( HeightFieldNeighborNormals );
|
|
RUN_SUBTEST( NeighborNormalDispatch );
|
|
|
|
return 0;
|
|
}
|