Height field fixes (#6)
Fix for height field flat edges Fix for height field shape cast Made B3_REST_OFFSET and official constant
This commit is contained in:
parent
aedb0e34aa
commit
758dc665dd
8 changed files with 471 additions and 341 deletions
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 ),
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 ) );
|
||||
|
||||
|
|
|
|||
|
|
@ -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] );
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue