// 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 #include #include 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; }