diff --git a/include/box3d/constants.h b/include/box3d/constants.h index d96aa55..1df20c8 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -43,24 +43,30 @@ B3_API float b3GetLengthUnitsPerMeter( void ); #define B3_MIN_CAPSULE_LENGTH ( B3_LINEAR_SLOP ) -// The distance between shapes where they are considered overlapped. This is needed -// because GJK may return small positive values for overlapped shapes in degenerate -// configurations. +/// The distance between shapes where they are considered overlapped. This is needed +/// because GJK may return small positive values for overlapped shapes in degenerate +/// configurations. #define B3_OVERLAP_SLOP ( 0.1f * B3_LINEAR_SLOP ) -// Maximum number of simultaneous worlds that can be allocated +/// Maximum number of simultaneous worlds that can be allocated #ifndef B3_MAX_WORLDS #define B3_MAX_WORLDS 128 #endif -// The maximum rotation of a body per time step. This limit is very large and is used -// to prevent numerical problems. You shouldn't need to adjust this. -// @warning increasing this to 0.5f * b3_pi or greater will break continuous collision. +/// The maximum rotation of a body per time step. This limit is very large and is used +/// to prevent numerical problems. You shouldn't need to adjust this. +/// @warning increasing this to 0.5f * b3_pi or greater will break continuous collision. #define B3_MAX_ROTATION ( 0.25f * B3_PI ) -// @warning modifying this can have a significant impact on performance and stability +/// @warning modifying this can have a significant impact on performance and stability #define B3_SPECULATIVE_DISTANCE ( 4.0f * B3_LINEAR_SLOP ) +/// The rest offset is used for mesh contact to reduce ghost collisions and assist with CCD. +/// The rest offset adjusts the contact point separation value, making the solver push the shapes +/// apart by this distance. +/// Must be at least B3_LINEAR_SLOP and less than B3_SPECULATIVE_DISTANCE. +#define B3_MESH_REST_OFFSET ( 1.0f * B3_LINEAR_SLOP ) + /// The default contact recycling distance. #define B3_CONTACT_RECYCLE_DISTANCE ( 10.0f * B3_LINEAR_SLOP ) @@ -68,16 +74,16 @@ B3_API float b3GetLengthUnitsPerMeter( void ); /// is cos(angle/2)^2. This value corresponds to 10 degrees. #define B3_CONTACT_RECYCLE_ANGULAR_DISTANCE ( 0.99240388f ) -// This is used to fatten AABBs in the dynamic tree. This allows proxies -// to move by a small amount without triggering a tree adjustment. This is in meters. -// @warning modifying this can have a significant impact on performance +/// This is used to fatten AABBs in the dynamic tree. This allows proxies +/// to move by a small amount without triggering a tree adjustment. This is in meters. +/// @warning modifying this can have a significant impact on performance #define B3_MAX_AABB_MARGIN ( 0.05f * b3GetLengthUnitsPerMeter() ) -// Per-shape AABB margin is a fraction of the shape extent (capped by B3_MAX_AABB_MARGIN). -// Small shapes get small margins; large shapes are clamped to the cap. +/// Per-shape AABB margin is a fraction of the shape extent (capped by B3_MAX_AABB_MARGIN). +/// Small shapes get small margins; large shapes are clamped to the cap. #define B3_AABB_MARGIN_FRACTION 0.125f -// The time that a body must be still before it will go to sleep. In seconds. +/// The time that a body must be still before it will go to sleep. In seconds. #define B3_TIME_TO_SLEEP 0.5f /// Maximum length of the body name. Must be at least 1. Includes null termination. @@ -85,7 +91,7 @@ B3_API float b3GetLengthUnitsPerMeter( void ); #define B3_NAME_LENGTH 18 #endif -// These generous limits allow for easy hashing. See b3ShapePairKey. +/// These generous limits allow for easy hashing. See b3ShapePairKey. #define B3_SHAPE_POWER 22 #define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER ) #define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER ) diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index f88a42a..879e49a 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -408,20 +408,29 @@ public: if ( m_context->restart == false ) { m_camera->SetView( 45.0f, 30.0f, 12.0f, b3Vec3_zero ); - EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -10.0f, 0.0f, -10.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + m_heightField = b3CreateGrid( 40, 40, { 0.5f, 1.0f, 0.5f }, false ); + //m_heightField = b3CreateWave( 40, 40, {1.0f, 2.0f, 1.0f}, 0.02f, 0.04f, false ); + b3CreateHeightFieldShape( groundId, &shapeDef, m_heightField ); + + m_gridMesh = b3CreateGridMesh( 40, 40, 0.5f, 1, true ); + //b3CreateMeshShape( groundId, &shapeDef, m_gridMesh, b3Vec3_one ); } { b3BodyDef bodyDef = b3DefaultBodyDef(); b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + // m_boxMesh = b3CreateBoxMesh( { 0.0f, 1.0f, 0.0f }, { 1.0f, 1.0f, 1.0f }, true ); b3ShapeDef shapeDef = b3DefaultShapeDef(); - b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); - b3CreateHullShape( groundId, &shapeDef, &box.base ); - - //m_boxMesh = b3CreateBoxMesh( { 0.0f, 1.0f, 0.0f }, { 1.0f, 1.0f, 1.0f }, true ); - m_boxMesh = b3CreatePlatformMesh( { 0.0f, 1.5f, 0.0f }, 1.0f, 2.0f, 5.0f ); - // m_scale = { -0.6f, 1.0f, 2.0f }; + m_boxMesh = b3CreatePlatformMesh( { 0.0f, 0.5f, 0.0f }, 1.0f, 2.0f, 5.0f ); b3Vec3 scale = b3Vec3_one; b3CreateMeshShape( groundId, &shapeDef, m_boxMesh, scale ); } @@ -445,6 +454,8 @@ public: ~SBoxMover() override { b3DestroyMesh( m_boxMesh ); + b3DestroyHeightField( m_heightField ); + b3DestroyMesh( m_gridMesh ); } void Render() override @@ -460,6 +471,8 @@ public: } b3MeshData* m_boxMesh; + b3HeightField* m_heightField; + b3MeshData* m_gridMesh; }; static int sampleBoxMesh = SampleManager::Register( "Issues", "s&box mover", SBoxMover::Create ); diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index a7168a7..85155ec 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -792,8 +792,8 @@ public: m_rowCount = 400; m_columnCount = 400; #else - m_rowCount = 100; - m_columnCount = 100; + m_rowCount = 10; + m_columnCount = 10; #endif m_groundId = {}; @@ -807,9 +807,9 @@ public: // m_rayOrigin = { 30.0f, 2.0f, 0.0f }; // m_rayTranslation = { -60.0f, -4.0f, 0.0f }; - m_rayOrigin = { 0.0f, 2.0f, 15.0f }; - m_rayTranslation = { 0.0f, -4.0f, 0.0f }; - m_radius = 0.571f; + m_rayOrigin = { 5.5f, 4.0f, 1.01f }; + m_rayTranslation = { 0.0f, -8.0f, 0.0f }; + m_radius = 0.2f; m_heightField = nullptr; @@ -887,11 +887,11 @@ public: float fontSize = ImGui::GetFontSize(); float height = 18.0f * fontSize; ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); - ImGui::SetNextWindowSize( { 20.0f * fontSize, height } ); + ImGui::SetNextWindowSize( { 40.0f * fontSize, height } ); ImGui::Begin( "Height Field", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); - ImGui::PushItemWidth( 200.0f ); + ImGui::PushItemWidth( 500.0f ); if ( ImGui::SliderInt( "columns", &m_columnCount, 1, 500 ) ) { @@ -913,10 +913,10 @@ public: CreateScene(); } - ImGui::SliderFloat( "ray x", &m_rayOrigin.x, -2.0f * m_radius - 1.0f * m_scale.x * ( m_columnCount - 1 ), - 2.0f * m_radius + 1.0f * m_scale.x * ( m_columnCount - 1 ) ); - ImGui::SliderFloat( "ray z", &m_rayOrigin.z, -2.0f * m_radius - 1.0f * m_scale.z * ( m_rowCount - 1 ), - 2.0f * m_radius + 1.0f * m_scale.z * ( m_rowCount - 1 ) ); + ImGui::SliderFloat( "ray x", &m_rayOrigin.x, -2.0f * m_radius - 0.5f * m_scale.x * ( m_columnCount - 1 ), + 2.0f * m_radius + 0.5f * m_scale.x * ( m_columnCount - 1 ) ); + ImGui::SliderFloat( "ray z", &m_rayOrigin.z, -2.0f * m_radius - 0.5f * m_scale.z * ( m_rowCount - 1 ), + 2.0f * m_radius + 0.5f * m_scale.z * ( m_rowCount - 1 ) ); ImGui::SliderFloat( "delta x", &m_rayTranslation.x, -2.0f * m_scale.x * ( m_columnCount - 1 ), 2.0f * m_scale.x * ( m_columnCount - 1 ) ); ImGui::SliderFloat( "delta z", &m_rayTranslation.z, -2.0f * m_scale.z * ( m_rowCount - 1 ), diff --git a/samples/scene.cpp b/samples/scene.cpp index 76f0dd1..6587554 100644 --- a/samples/scene.cpp +++ b/samples/scene.cpp @@ -1223,180 +1223,6 @@ void ResizeScene( Scene* scene, const Camera* camera ) CheckOpenGL(); } -#if 0 -UniqueMesh CreateUniqueMesh( const b3Vec3* vertexPositions, const b3Vec3* vertexNormals, int triangleCount, - const int* triangleIndices, const Edge* edges, int edgeCount, const int* materials, Arena arena ) -{ - UniqueMesh shape = {}; - - // Vertex buffers - { - // Create vertex buffers - int vertexCount = 3 * triangleCount; - VertexData* vertexData = (VertexData*)arena.Allocate( vertexCount * sizeof( VertexData ) ); - - int index = 0; - for ( int triangleIndex = 0; triangleIndex < triangleCount; ++triangleIndex ) - { - int vertexIndex1 = triangleIndices[3 * triangleIndex + 0]; - int vertexIndex2 = triangleIndices[3 * triangleIndex + 1]; - int vertexIndex3 = triangleIndices[3 * triangleIndex + 2]; - - b3Vec3 vertexPosition1 = vertexPositions[vertexIndex1]; - b3Vec3 vertexPosition2 = vertexPositions[vertexIndex2]; - b3Vec3 vertexPosition3 = vertexPositions[vertexIndex3]; - - vertexData[index + 0].position = vertexPosition1; - vertexData[index + 1].position = vertexPosition2; - vertexData[index + 2].position = vertexPosition3; - - b3Vec3 vertexNormal1, vertexNormal2, vertexNormal3; - if ( vertexNormals ) - { - vertexNormal1 = vertexNormals[vertexIndex1]; - vertexNormal2 = vertexNormals[vertexIndex2]; - vertexNormal3 = vertexNormals[vertexIndex3]; - } - else - { - b3Vec3 edge1 = vertexPosition2 - vertexPosition1; - b3Vec3 edge2 = vertexPosition3 - vertexPosition1; - b3Vec3 normal = b3Cross( edge1, edge2 ); - normal = b3Normalize( normal ); - - vertexNormal1 = normal; - vertexNormal2 = normal; - vertexNormal3 = normal; - } - - vertexData[index + 0].normal = vertexNormal1; - vertexData[index + 1].normal = vertexNormal2; - vertexData[index + 2].normal = vertexNormal3; - - if ( materials != nullptr ) - { - vertexData[index + 0].material = materials[triangleIndex]; - vertexData[index + 1].material = materials[triangleIndex]; - vertexData[index + 2].material = materials[triangleIndex]; - } - else - { - vertexData[index + 0].material = -1; - vertexData[index + 1].material = -1; - vertexData[index + 2].material = -1; - } - - index += 3; - } - - assert( index == vertexCount ); - - // Fill vertex buffers - shape.triangleCount = triangleCount; - - glGenVertexArrays( 1, &shape.vertexArrayId ); - glGenBuffers( 1, &shape.vertexBufferId ); - - // Copy static vertex data - glBindVertexArray( shape.vertexArrayId ); - glBindBuffer( GL_ARRAY_BUFFER, shape.vertexBufferId ); - glBufferData( GL_ARRAY_BUFFER, vertexCount * sizeof( VertexData ), vertexData, GL_STATIC_DRAW ); - - // positions - uint32_t positionAttribute = 0; - glEnableVertexAttribArray( positionAttribute ); - glVertexAttribPointer( positionAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( VertexData ), - (void*)offsetof( VertexData, position ) ); - - // normals - uint32_t normalAttribute = 1; - glEnableVertexAttribArray( normalAttribute ); - glVertexAttribPointer( normalAttribute, 3, GL_FLOAT, GL_FALSE, sizeof( VertexData ), - (void*)offsetof( VertexData, normal ) ); - - // materials - uint32_t materialAttribute = 2; - glEnableVertexAttribArray( materialAttribute ); - glVertexAttribIPointer( materialAttribute, 1, GL_INT, sizeof( VertexData ), (void*)offsetof( VertexData, material ) ); - - glBindBuffer( GL_ARRAY_BUFFER, 0 ); - glBindVertexArray( 0 ); - - shape.shadowCaster = true; - } - - // Edge buffer - { - // Create edge buffer - EdgeData* edgeBuffer = (EdgeData*)arena.Allocate( 2 * edgeCount * sizeof( EdgeData ) ); - - for ( int edgeIndex = 0; edgeIndex < edgeCount; ++edgeIndex ) - { - int vertexIndex1 = edges[edgeIndex].index1; - int vertexIndex2 = edges[edgeIndex].index2; - int flags = edges[edgeIndex].flags; - - const b3Vec3* vb = vertexPositions; - b3Vec3 vertexPosition1 = vb[vertexIndex1]; - b3Vec3 vertexPosition2 = vb[vertexIndex2]; - - edgeBuffer[2 * edgeIndex + 0].position = vertexPosition1; - edgeBuffer[2 * edgeIndex + 0].flags = flags; - edgeBuffer[2 * edgeIndex + 1].position = vertexPosition2; - edgeBuffer[2 * edgeIndex + 1].flags = flags; - } - - // Fill edge buffer - shape.edgeCount = edgeCount; - - glGenVertexArrays( 1, &shape.edgeArrayId ); - glGenBuffers( 1, &shape.edgeBufferId ); - - glBindVertexArray( shape.edgeArrayId ); - glBindBuffer( GL_ARRAY_BUFFER, shape.edgeBufferId ); - glBufferData( GL_ARRAY_BUFFER, 2 * edgeCount * sizeof( EdgeData ), edgeBuffer, GL_STATIC_DRAW ); - - uint32_t positionIndex = 0; - glEnableVertexAttribArray( positionIndex ); - glVertexAttribPointer( positionIndex, 3, GL_FLOAT, GL_FALSE, sizeof( EdgeData ), (void*)offsetof( EdgeData, position ) ); - - uint32_t flagsIndex = 1; - glEnableVertexAttribArray( flagsIndex ); - glVertexAttribIPointer( flagsIndex, 1, GL_INT, sizeof( EdgeData ), (void*)offsetof( EdgeData, flags ) ); - - glBindBuffer( GL_ARRAY_BUFFER, 0 ); - glBindVertexArray( 0 ); - } - - CheckOpenGL(); - - return shape; -} - -void DestroyUniqueMesh( UniqueMesh* shape ) -{ - if ( shape->vertexBufferId != 0 ) - { - glDeleteBuffers( 1, &shape->vertexBufferId ); - } - - if ( shape->vertexArrayId != 0 ) - { - glDeleteVertexArrays( 1, &shape->vertexArrayId ); - } - - if ( shape->edgeBufferId != 0 ) - { - glDeleteBuffers( 1, &shape->edgeBufferId ); - } - - if ( shape->edgeArrayId != 0 ) - { - glDeleteVertexArrays( 1, &shape->edgeArrayId ); - } -} -#endif - void DrawDebugCapsule( Scene* scene, DebugCapsule* capsule, b3Transform transform, b3HexColor color, float alpha ) { float radius = capsule->radius; diff --git a/src/height_field.c b/src/height_field.c index cb79d46..c5a3c68 100644 --- a/src/height_field.c +++ b/src/height_field.c @@ -251,6 +251,11 @@ b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) flags1 |= b3_concaveEdge2; flags2 |= b3_concaveEdge2; } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge2; + flags2 |= b3_inverseConcaveEdge2; + } } B3_UNUSED( center1 ); @@ -297,6 +302,10 @@ b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) { flags1 |= b3_concaveEdge3; } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge3; + } } int bottomCellIndex = ( row + 1 ) * ( columnCount - 1 ) + column; @@ -339,6 +348,10 @@ b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) { flags2 |= b3_concaveEdge3; } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_inverseConcaveEdge3; + } } int leftCellIndex = row * ( columnCount - 1 ) + column - 1; @@ -381,6 +394,10 @@ b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) { flags1 |= b3_concaveEdge1; } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge1; + } } int rightCellIndex = row * ( columnCount - 1 ) + column + 1; @@ -423,6 +440,10 @@ b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) { flags2 |= b3_concaveEdge1; } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_inverseConcaveEdge1; + } } B3_ASSERT( 0 <= flags1 && flags1 <= UINT8_MAX ); @@ -451,6 +472,41 @@ b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) _Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" ); _Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" ); +// Decode the four corner vertices of a height field cell into local space. +// Output order matches the index naming used throughout this file: +// corners[0] = (column, row), corners[1] = (column + 1, row), +// corners[2] = (column, row + 1), corners[3] = (column + 1, row + 1). +static inline void b3GetHeightFieldCellCorners( const b3HeightField* hf, int row, int column, b3Vec3 corners[4] ) +{ + B3_ASSERT( 0 <= row && row < hf->rowCount - 1 && 0 <= column && column < hf->columnCount - 1 ); + + int columnCount = hf->columnCount; + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + float minHeight = hf->minHeight; + float heightScale = hf->heightScale; + const uint16_t* heights = hf->compressedHeights; + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 scale = hf->scale; + corners[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + corners[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + corners[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + corners[3] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); +} + b3Triangle b3GetHeightFieldTriangle( const b3HeightField* heightField, int triangleIndex ) { B3_ASSERT( 0 <= triangleIndex ); @@ -475,35 +531,23 @@ b3Triangle b3GetHeightFieldTriangle( const b3HeightField* heightField, int trian B3_ASSERT( heightField->materialIndices[cellIndex] != B3_HEIGHT_FIELD_HOLE ); B3_UNUSED( cellIndex ); - float minHeight = heightField->minHeight; - float heightScale = heightField->heightScale; - const uint16_t* heights = heightField->compressedHeights; + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); - float height11 = minHeight + heightScale * heights[index11]; - float height12 = minHeight + heightScale * heights[index12]; - float height21 = minHeight + heightScale * heights[index21]; - float height22 = minHeight + heightScale * heights[index22]; - - float x1 = (float)( column ); - float x2 = (float)( column + 1 ); - float z1 = (float)( row ); - float z2 = (float)( row + 1 ); - - b3Vec3 scale = heightField->scale; if ( ( triangleIndex & 1 ) == 0 ) { - triangle.vertices[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); - triangle.vertices[1] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); - triangle.vertices[2] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + triangle.vertices[0] = corners[0]; + triangle.vertices[1] = corners[2]; + triangle.vertices[2] = corners[1]; triangle.i1 = index11; triangle.i2 = index21; triangle.i3 = index12; } else { - triangle.vertices[0] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); - triangle.vertices[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); - triangle.vertices[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + triangle.vertices[0] = corners[3]; + triangle.vertices[1] = corners[1]; + triangle.vertices[2] = corners[2]; triangle.i1 = index22; triangle.i2 = index12; triangle.i3 = index21; @@ -584,6 +628,12 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S b3Vec3 clampedDelta = b3MulSV( maxFraction - minFraction, shapeDelta ); b3Vec3 clampedEnd = b3Add( clampedStart, clampedDelta ); + // Preserve the un-shifted center sweep. clampedStart/clampedEnd get pushed out to the + // leading box corner below to drive the grid DDA, but the swept-volume AABB used to + // cull cells must stay centered on the actual shape path. + b3Vec3 centerStart = clampedStart; + b3Vec3 centerEnd = clampedEnd; + // The grid traversal starts from the leading shape bounds corner float signX, signZ; if ( shapeTranslation.x >= 0.0f ) @@ -691,15 +741,19 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S float bestFraction = input->maxFraction; + // nextFractionX / nextFractionZ advance in units of the clamped sweep + // [minFraction, maxFraction], but bestFraction is a fraction of the full input + // translation. Precompute the affine map from clamped space to input space so + // the loop termination test compares like with like — otherwise it can exit + // early and miss a closer hit in a later cell. + float gridFractionScale = input->maxFraction * ( maxFraction - minFraction ); + float gridFractionOffset = input->maxFraction * minFraction; + int rowCount = heightField->rowCount; int columnCount = heightField->columnCount; int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); B3_UNUSED( cellCount ); - float minHeight = heightField->minHeight; - float heightScale = heightField->heightScale; - const uint16_t* heights = heightField->compressedHeights; - b3ShapeCastPairInput pairInput = { 0 }; pairInput.transformA = b3Transform_identity; pairInput.proxyB = input->proxy; @@ -708,8 +762,8 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S pairInput.canEncroach = input->canEncroach; b3AABB castBounds; - castBounds.lowerBound = b3Sub( b3Min( clampedStart, clampedEnd ), shapeExtents ); - castBounds.upperBound = b3Add( b3Max( clampedStart, clampedEnd ), shapeExtents ); + castBounds.lowerBound = b3Sub( b3Min( centerStart, centerEnd ), shapeExtents ); + castBounds.upperBound = b3Add( b3Max( centerStart, centerEnd ), shapeExtents ); b3V32 rayOrigin = b3LoadV( &shapeStart.x ); b3V32 rayTranslation = b3LoadV( &shapeTranslation.x ); @@ -763,27 +817,12 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S continue; } - // index = row * columnCount + column - int index11 = row * columnCount + column; - - int index12 = index11 + 1; - int index21 = ( row + 1 ) * columnCount + column; - int index22 = index21 + 1; - - float height11 = minHeight + heightScale * heights[index11]; - float height12 = minHeight + heightScale * heights[index12]; - float height21 = minHeight + heightScale * heights[index21]; - float height22 = minHeight + heightScale * heights[index22]; - - float x1 = (float)( column ); - float x2 = (float)( column + 1 ); - float z1 = (float)( row ); - float z2 = (float)( row + 1 ); - - b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); - b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); - b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); - b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; // I know the min/max x and z values, but not the min/max heights. b3AABB bounds; @@ -917,8 +956,12 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S } } - // These fractions always increase to guarantee the loop eventually exits - if ( nextFractionX > bestFraction && nextFractionZ > bestFraction ) + // These fractions always increase to guarantee the loop eventually exits. + // Map them from clamped-sweep space into input-translation space before + // comparing against bestFraction. + float inputFractionX = nextFractionX == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionX * gridFractionScale; + float inputFractionZ = nextFractionZ == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionZ * gridFractionScale; + if ( inputFractionX > bestFraction && inputFractionZ > bestFraction ) { break; } @@ -1010,11 +1053,6 @@ bool b3OverlapHeightField( const b3HeightField* shape, b3Transform shapeTransfor b3SimplexCache cache = { 0 }; - float minHeight = shape->minHeight; - float heightScale = shape->heightScale; - - const uint16_t* heights = shape->compressedHeights; - // Outer loop on rows and inner loop on columns so that triangle indices // increase monotonically. for ( int row = minRow; row <= maxRow; ++row ) @@ -1039,26 +1077,12 @@ bool b3OverlapHeightField( const b3HeightField* shape, b3Transform shapeTransfor continue; } - // index = row * columnCount + column - int index11 = row * shape->columnCount + column; - int index12 = index11 + 1; - int index21 = ( row + 1 ) * shape->columnCount + column; - int index22 = index21 + 1; - - float height11 = minHeight + heightScale * heights[index11]; - float height12 = minHeight + heightScale * heights[index12]; - float height21 = minHeight + heightScale * heights[index21]; - float height22 = minHeight + heightScale * heights[index22]; - - float x1 = (float)( column ); - float x2 = (float)( column + 1 ); - float z1 = (float)( row ); - float z2 = (float)( row + 1 ); - - b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); - b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); - b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); - b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( shape, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; b3V32 v11 = b3LoadV( &point11.x ); b3V32 v12 = b3LoadV( &point12.x ); @@ -1117,11 +1141,6 @@ void b3QueryHeightField( const b3HeightField* heightField, b3AABB bounds, b3Mesh int minCol = (int)floorf( bounds.lowerBound.x / scale.x ); int maxCol = (int)floorf( bounds.upperBound.x / scale.x ); - float minHeight = heightField->minHeight; - float heightScale = heightField->heightScale; - - const uint16_t* heights = heightField->compressedHeights; - // Outer loop on rows and inner loop on columns so that triangle indices // increase monotonically. for ( int row = minRow; row <= maxRow; ++row ) @@ -1146,26 +1165,12 @@ void b3QueryHeightField( const b3HeightField* heightField, b3AABB bounds, b3Mesh continue; } - // index = row * columnCount + column - int index11 = row * heightField->columnCount + column; - int index12 = index11 + 1; - int index21 = ( row + 1 ) * heightField->columnCount + column; - int index22 = index21 + 1; - - float height11 = minHeight + heightScale * heights[index11]; - float height12 = minHeight + heightScale * heights[index12]; - float height21 = minHeight + heightScale * heights[index21]; - float height22 = minHeight + heightScale * heights[index22]; - - float x1 = (float)( column ); - float x2 = (float)( column + 1 ); - float z1 = (float)( row ); - float z2 = (float)( row + 1 ); - - b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); - b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); - b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); - b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; // I know the min/max x and z values, but not the min/max heights. // This could be done with no branching in SIMD. @@ -1223,9 +1228,6 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H int minCol = (int)floorf( localMinX / scale.x ); int maxCol = (int)floorf( localMaxX / scale.x ); - float minHeight = shape->minHeight; - float heightScale = shape->heightScale; - const uint16_t* heights = shape->compressedHeights; int planeCount = 0; // Outer loop on rows and inner loop on columns so that triangle indices @@ -1252,26 +1254,12 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H continue; } - // index = row * columnCount + column - int index11 = row * shape->columnCount + column; - int index12 = index11 + 1; - int index21 = ( row + 1 ) * shape->columnCount + column; - int index22 = index21 + 1; - - float height11 = minHeight + heightScale * heights[index11]; - float height12 = minHeight + heightScale * heights[index12]; - float height21 = minHeight + heightScale * heights[index21]; - float height22 = minHeight + heightScale * heights[index22]; - - float x1 = (float)( column ); - float x2 = (float)( column + 1 ); - float z1 = (float)( row ); - float z2 = (float)( row + 1 ); - - b3Vec3 point11 = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); - b3Vec3 point12 = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); - b3Vec3 point21 = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); - b3Vec3 point22 = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( shape, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; b3V32 v11 = b3LoadV( &point11.x ); b3V32 v12 = b3LoadV( &point12.x ); @@ -1427,8 +1415,7 @@ b3HeightField* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float } else { - int materialCount = 3; - materialIndices[k] = (uint8_t)( ( i + j ) % materialCount ); + materialIndices[k] = 0; } } } diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 543e402..01ec2ff 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -512,6 +512,13 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q ); float linearSlop = B3_LINEAR_SLOP; + // This should push apart shapes after a time of impact event. + // In the past I've called this `polygon skin`, but PhysX and Unreal + // call it `rest offset` which seems appropriate in this case. + // It leads to a small visual gap but seems to improve the quality of mesh + // collision, especially for hull versus mesh. + float restOffset = B3_MESH_REST_OFFSET; + // Make room for clip points int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * triangleCount; @@ -791,13 +798,6 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact return false; } - // todo this should push apart shapes after a time of impact event - // todo consider doing this based on speed - // todo this must be constant or varied smoothly or there will be jitter - // In the past I've called this `polygon skin`, but PhysX and Unreal call it `rest offset` which - // seems appropriate in this case. - float restOffset = linearSlop; - b3Cluster* clusters = b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) ); int* clusterMemberships = b3Bump( &arena, acceptedManifoldCount * sizeof( int ) ); diff --git a/src/physics_world.c b/src/physics_world.c index fcec31b..a79bdf3 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -154,6 +154,9 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) { B3_CHECK_DEF( def ); + B3_ASSERT( B3_LINEAR_SLOP <= B3_MESH_REST_OFFSET ); + B3_ASSERT( B3_MESH_REST_OFFSET < B3_SPECULATIVE_DISTANCE ); + int worldId = B3_NULL_INDEX; for ( int i = 0; i < B3_MAX_WORLDS; ++i ) { @@ -581,7 +584,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* // This is inspired by persistent contact manifolds used in some physics engines, such as PhysX. // However, this allows larger relative motion and has fewer tuning parameters (just one). if ( ( isFast == false || isMeshContact == false ) && recycleDistance > 0.0f && - (contact->flags & b3_relativeTransformValid) && (contact->flags & b3_contactRecycleFlag) ) + ( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) ) { float angleA = b3DotQuat( transformA.q, contact->cachedTransformA.q ); float angleB = b3DotQuat( transformB.q, contact->cachedTransformB.q ); @@ -982,7 +985,6 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) world->profile = (b3Profile){ 0 }; - world->activeTaskCount = 0; world->taskCount = 0; @@ -1089,7 +1091,6 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) // Make sure all tasks that were started were also finished B3_ASSERT( world->activeTaskCount == 0 ); - // Swap end event array buffers world->endEventArrayIndex = 1 - world->endEventArrayIndex; b3Array_Clear( world->sensorEndEvents[world->endEventArrayIndex] ); diff --git a/test/test_height_field.c b/test/test_height_field.c index a664e68..295d986 100644 --- a/test/test_height_field.c +++ b/test/test_height_field.c @@ -3,8 +3,9 @@ #include "test_macros.h" -// b3GetHeightFieldTriangle is internal +// b3GetHeightFieldTriangle and b3IntersectRayTriangle are internal #include "shape.h" +#include "simd.h" #include "box3d/collision.h" #include "box3d/math_functions.h" @@ -233,6 +234,299 @@ static int FileRoundtrip( void ) 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; + + b3HeightField* 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 b3HeightField* 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 ( hf->materialIndices[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.transformA = b3Transform_identity; + pair.transformB = 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 }; + b3HeightField* 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 b3HeightField* 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 ( hf->materialIndices[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 }; + b3HeightField* 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; +} + int HeightFieldTest( void ) { RUN_SUBTEST( HeightFieldCreate ); @@ -241,6 +535,9 @@ int HeightFieldTest( void ) RUN_SUBTEST( RayCastFlatField ); RUN_SUBTEST( OverlapAtSurface ); RUN_SUBTEST( FileRoundtrip ); + RUN_SUBTEST( ShapeCastVerticalStraddle ); + RUN_SUBTEST( ShapeCastBruteForce ); + RUN_SUBTEST( RayCastBruteForce ); return 0; }