add a small amount of debug data to manifolds

refine edge contact rule
This commit is contained in:
Erin Catto 2026-11-03 17:40:11 -08:00
commit 590ab5a7d9
13 changed files with 1668 additions and 33 deletions

View file

@ -5,15 +5,20 @@
#include "human.h"
#include "mesh_loader.h"
#include "sample.h"
#include "utils.h"
#include "gfx/draw.h"
#include "gfx/debug_adapter.h"
#include "box3d/box3d.h"
#include <algorithm>
#include <float.h>
#include <imgui.h>
#include <queue>
#include <stdio.h>
#include <string.h>
#include <vector>
enum class ShapeType
{
@ -1774,3 +1779,953 @@ public:
};
static int sampleGhostCollisions = RegisterSample( "Mesh", "Ghost Collisions", GhostCollisions::Create );
// A closed conveyor circuit swept from a channel profile and finely tessellated, so every body slides over
// thousands of internal edges. The loop runs over a washboard, up a helix, through a chicane and down a
// sharp ramp.
// A pushing manifold is counted as a ghost when its normal matches no nearby triangle and does not lie
// on the arc between two triangles that meet at a convex crease.
class ConveyorCircuit : public Sample
{
public:
enum BodyType
{
e_mixedBody,
e_boxBody,
e_capsuleBody,
e_sphereBody,
e_cylinderBody,
e_bodyTypeCount
};
static constexpr int m_maxBodies = 300;
static constexpr int m_markerCapacity = 256;
// Material 0 is the rails and skirts, the rest are belt directions
static constexpr int m_yawBinCount = 128;
static constexpr int m_materialCount = m_yawBinCount + 1;
static constexpr int m_segmentCount = 8;
static constexpr int m_profileCount = 8;
static constexpr float m_radius = 5.0f;
static constexpr float m_straightLength = 16.0f;
static constexpr float m_chicaneRadius = 3.0f;
static constexpr float m_chicaneAngle = 25.0f * B3_DEG_TO_RAD;
static constexpr float m_halfWidth = 1.0f;
static constexpr float m_railHeight = 0.35f;
static constexpr float m_railThickness = 0.08f;
static constexpr float m_skirtDepth = 0.4f;
static constexpr float m_baseHeight = 1.0f;
static constexpr float m_liftHeight = 1.5f;
static constexpr float m_bumpStart = 5.0f;
static constexpr float m_bumpEnd = 11.0f;
static constexpr float m_bumpLength = 1.0f;
static constexpr float m_bumpHeight = 0.06f;
static constexpr float m_rampLength = 6.0f;
static constexpr float m_spawnDistance = 1.0f;
struct Segment
{
float length;
float curvature;
};
enum GhostClass
{
e_faceGhost,
e_edgeGhost,
e_vertexGhost,
e_otherGhost,
e_ghostClassCount
};
struct Marker
{
b3Pos point;
b3Vec3 normal;
};
explicit ConveyorCircuit( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 35.0f, 40.0f, 38.0f, { 0.0f, 1.5f, 0.0f } );
}
AddGroundBox( 30.0f );
float chicaneArc = m_chicaneRadius * m_chicaneAngle;
float chicaneAdvance = 4.0f * m_chicaneRadius * sinf( m_chicaneAngle );
float lead = 2.0f;
// The chicane returns to the original heading and line, so the loop still closes
m_segments[0] = { m_straightLength, 0.0f };
m_segments[1] = { B3_PI * m_radius, 1.0f / m_radius };
m_segments[2] = { lead, 0.0f };
m_segments[3] = { chicaneArc, 1.0f / m_chicaneRadius };
m_segments[4] = { 2.0f * chicaneArc, -1.0f / m_chicaneRadius };
m_segments[5] = { chicaneArc, 1.0f / m_chicaneRadius };
m_segments[6] = { m_straightLength - lead - chicaneAdvance, 0.0f };
m_segments[7] = { B3_PI * m_radius, 1.0f / m_radius };
m_totalLength = 0.0f;
for ( int i = 0; i < m_segmentCount; ++i )
{
m_segmentStart[i] = m_totalLength;
m_totalLength += m_segments[i].length;
}
// Channel cross section in lateral and up coordinates, traversed with air on the left
m_profile[0] = { -m_halfWidth - m_railThickness, -m_skirtDepth };
m_profile[1] = { -m_halfWidth - m_railThickness, m_railHeight };
m_profile[2] = { -m_halfWidth, m_railHeight };
m_profile[3] = { -m_halfWidth, 0.0f };
m_profile[4] = { m_halfWidth, 0.0f };
m_profile[5] = { m_halfWidth, m_railHeight };
m_profile[6] = { m_halfWidth + m_railThickness, m_railHeight };
m_profile[7] = { m_halfWidth + m_railThickness, -m_skirtDepth };
m_cylinderHull = b3CreateCylinder( 0.4f, 0.25f, -0.2f, 16 );
m_cellSize = 0.25f;
m_beltSpeed = 2.5f;
m_targetCount = 80;
m_spawnInterval = 15;
m_bodyType = e_mixedBody;
m_toleranceDegrees = 10.0f;
m_contactRecycling = true;
m_drawFlow = true;
m_drawGhosts = true;
m_groundId = b3_nullBodyId;
m_bodyCount = 0;
m_spawnIndex = 0;
m_spawnTimer = 0;
ResetCounters();
CreateCircuit();
}
~ConveyorCircuit() override
{
DestroyCircuit();
b3DestroyHull( m_cylinderHull );
}
float EvaluateHeight( float s ) const
{
float helix0 = m_segmentStart[1];
float helix1 = m_segmentStart[2];
float ramp0 = m_segmentStart[6] + 1.0f;
float ramp1 = ramp0 + m_rampLength;
if ( s < helix0 )
{
if ( m_bumpStart <= s && s <= m_bumpEnd )
{
float phase = 2.0f * B3_PI * ( s - m_bumpStart ) / m_bumpLength;
return 0.5f * m_bumpHeight * ( 1.0f - cosf( phase ) );
}
return 0.0f;
}
if ( s < helix1 )
{
float u = ( s - helix0 ) / ( helix1 - helix0 );
return m_liftHeight * u * u * ( 3.0f - 2.0f * u );
}
if ( s < ramp0 )
{
return m_liftHeight;
}
// Linear with sharp creases at both ends
if ( s < ramp1 )
{
return m_liftHeight * ( ramp1 - s ) / m_rampLength;
}
return 0.0f;
}
b3Vec3 EvaluateCenter( float s ) const
{
float x = -0.5f * m_straightLength;
float z = -m_radius;
float yaw = 0.0f;
float remaining = s;
for ( int i = 0; i < m_segmentCount && remaining > 0.0f; ++i )
{
float ds = b3MinFloat( remaining, m_segments[i].length );
float k = m_segments[i].curvature;
if ( k == 0.0f )
{
x += ds * cosf( yaw );
z += ds * sinf( yaw );
}
else
{
float yaw1 = yaw + k * ds;
x += ( sinf( yaw1 ) - sinf( yaw ) ) / k;
z -= ( cosf( yaw1 ) - cosf( yaw ) ) / k;
yaw = yaw1;
}
remaining -= ds;
}
return { x, m_baseHeight + EvaluateHeight( s ), z };
}
void BuildStations()
{
int count = b3MaxInt( 32, (int)ceilf( m_totalLength / m_cellSize ) );
m_stations.resize( count );
m_tangents.resize( count );
m_laterals.resize( count );
m_ups.resize( count );
for ( int k = 0; k < count; ++k )
{
m_stations[k] = EvaluateCenter( m_totalLength * (float)k / (float)count );
}
// The chord across each station bisects a crease, so the rails stay square to the belt
for ( int k = 0; k < count; ++k )
{
b3Vec3 next = m_stations[( k + 1 ) % count];
b3Vec3 prev = m_stations[( k + count - 1 ) % count];
b3Vec3 t = b3Normalize( next - prev );
b3Vec3 l = b3Normalize( b3Cross( t, b3Vec3_axisY ) );
m_tangents[k] = t;
m_laterals[k] = l;
m_ups[k] = b3Cross( l, t );
}
}
b3SurfaceMaterial MakeMaterial( int index ) const
{
b3SurfaceMaterial material = b3DefaultSurfaceMaterial();
if ( index == 0 )
{
material.friction = 0.2f;
material.customColor = b3_colorGoldenRod;
return material;
}
// Horizontal is enough, the solver projects tangent velocity onto the contact plane
float yaw = -B3_PI + 2.0f * B3_PI * (float)( index - 1 ) / (float)m_yawBinCount;
material.friction = 0.8f;
material.tangentVelocity = { m_beltSpeed * cosf( yaw ), 0.0f, m_beltSpeed * sinf( yaw ) };
material.customColor = b3_colorSlateGray;
return material;
}
void CreateCircuit()
{
BuildStations();
// The last row repeats the first station and welds to it, closing the loop
int stationCount = (int)m_stations.size();
std::vector<b3Vec3> vertices;
std::vector<int32_t> indices;
std::vector<uint8_t> materialIndices;
for ( int j = 0; j + 1 < m_profileCount; ++j )
{
b3Vec2 pa = m_profile[j];
b3Vec2 delta = { m_profile[j + 1].x - pa.x, m_profile[j + 1].y - pa.y };
float length = sqrtf( delta.x * delta.x + delta.y * delta.y );
int columnCount = b3MaxInt( 1, (int)ceilf( length / m_cellSize - 0.01f ) );
bool floor = j == 3;
int base = (int)vertices.size();
int stride = columnCount + 1;
for ( int k = 0; k <= stationCount; ++k )
{
int station = k % stationCount;
for ( int i = 0; i <= columnCount; ++i )
{
float alpha = (float)i / (float)columnCount;
float u = pa.x + alpha * delta.x;
float v = pa.y + alpha * delta.y;
vertices.push_back( m_stations[station] + u * m_laterals[station] + v * m_ups[station] );
}
}
for ( int k = 0; k < stationCount; ++k )
{
int station = k;
b3Vec3 expected = -delta.y * m_laterals[station] + delta.x * m_ups[station];
int material = 0;
if ( floor )
{
b3Vec3 direction = m_stations[( k + 1 ) % stationCount] - m_stations[station];
float yaw = atan2f( direction.z, direction.x );
int bin = (int)floorf( ( yaw + B3_PI ) / ( 2.0f * B3_PI ) * m_yawBinCount + 0.5f );
material = 1 + bin % m_yawBinCount;
}
for ( int i = 0; i < columnCount; ++i )
{
int a = base + k * stride + i;
int b = a + 1;
int c = a + stride + 1;
int d = a + stride;
int corners[6] = { a, b, c, c, d, a };
if ( ( k + i ) & 1 )
{
int flipped[6] = { a, b, d, b, c, d };
memcpy( corners, flipped, sizeof( corners ) );
}
for ( int n = 0; n < 6; n += 3 )
{
b3Vec3 v1 = vertices[corners[n]];
b3Vec3 v2 = vertices[corners[n + 1]];
b3Vec3 v3 = vertices[corners[n + 2]];
bool reversed = b3Dot( b3Cross( v2 - v1, v3 - v1 ), expected ) < 0.0f;
indices.push_back( corners[n] );
indices.push_back( reversed ? corners[n + 2] : corners[n + 1] );
indices.push_back( reversed ? corners[n + 1] : corners[n + 2] );
materialIndices.push_back( (uint8_t)material );
}
}
}
}
b3MeshDef meshDef = {};
meshDef.vertices = vertices.data();
meshDef.indices = indices.data();
meshDef.materialIndices = materialIndices.data();
meshDef.vertexCount = (int)vertices.size();
meshDef.triangleCount = (int)( indices.size() / 3 );
meshDef.weldVertices = true;
meshDef.weldTolerance = 0.001f;
meshDef.identifyEdges = true;
m_mesh = b3CreateMesh( &meshDef, nullptr, 0 );
b3SurfaceMaterial materials[m_materialCount];
for ( int i = 0; i < m_materialCount; ++i )
{
materials[i] = MakeMaterial( i );
}
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.name = "circuit";
m_groundId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.materials = materials;
shapeDef.materialCount = m_materialCount;
m_shapeId = b3CreateMeshShape( m_groundId, &shapeDef, m_mesh, b3Vec3_one );
BuildAdjacency();
m_markerCount = 0;
m_markerIndex = 0;
}
// The body holding the mesh shapes must go before the mesh data
void DestroyCircuit()
{
if ( B3_IS_NON_NULL( m_groundId ) )
{
b3DestroyBody( m_groundId );
m_groundId = b3_nullBodyId;
}
if ( m_mesh != nullptr )
{
b3DestroyMesh( m_mesh );
m_mesh = nullptr;
}
}
void UpdateBeltSpeed()
{
for ( int i = 1; i < m_materialCount; ++i )
{
b3Shape_SetMeshMaterial( m_shapeId, MakeMaterial( i ), i );
}
}
b3Pos GetSpawnPosition() const
{
b3Vec3 center = EvaluateCenter( m_spawnDistance );
float lateral = RandomFloatRange( -0.6f, 0.6f );
return b3ToPos( { center.x, center.y + 1.2f, center.z + lateral } );
}
void SpawnBody()
{
int type = m_bodyType == e_mixedBody ? 1 + m_spawnIndex % ( e_bodyTypeCount - 1 ) : m_bodyType;
m_spawnIndex += 1;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = GetSpawnPosition();
bodyDef.rotation = RandomQuat();
// A jammed body would fall asleep on a moving belt
bodyDef.enableSleep = false;
bodyDef.enableContactRecycling = m_contactRecycling;
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.friction = 0.6f;
// Round bodies would roll back down the helix without it
shapeDef.baseMaterial.rollingResistance = 0.3f;
switch ( type )
{
case e_boxBody:
{
float halfLength = ( m_spawnIndex & 1 ) ? 0.4f : 0.25f;
b3BoxHull box = b3MakeBoxHull( halfLength, 0.15f, 0.25f );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
}
break;
case e_capsuleBody:
{
b3Capsule capsule = { { -0.3f, 0.0f, 0.0f }, { 0.3f, 0.0f, 0.0f }, 0.15f };
b3CreateCapsuleShape( bodyId, &shapeDef, &capsule );
}
break;
case e_sphereBody:
{
b3Sphere sphere = { b3Vec3_zero, 0.25f };
b3CreateSphereShape( bodyId, &shapeDef, &sphere );
}
break;
default:
b3CreateHullShape( bodyId, &shapeDef, m_cylinderHull );
break;
}
m_bodyIds[m_bodyCount++] = bodyId;
}
void DestroyBodies()
{
for ( int i = 0; i < m_bodyCount; ++i )
{
b3DestroyBody( m_bodyIds[i] );
}
m_bodyCount = 0;
m_spawnIndex = 0;
m_spawnTimer = 0;
}
void ResetCounters()
{
memset( m_classCounts, 0, sizeof( m_classCounts ) );
memset( m_flagCounts, 0, sizeof( m_flagCounts ) );
m_ghostCount = 0;
m_manifoldCount = 0;
m_stepGhostCount = 0;
m_recycleCount = 0;
m_markerCount = 0;
m_markerIndex = 0;
}
void BuildAdjacency()
{
int triangleCount = m_mesh->triangleCount;
int vertexCount = m_mesh->vertexCount;
const b3MeshTriangle* triangles = b3GetMeshTriangles( m_mesh );
const b3Vec3* vertices = b3GetMeshVertices( m_mesh );
m_triangleNormals.resize( triangleCount );
m_edgeNeighbors.assign( 3 * triangleCount, B3_NULL_INDEX );
m_vertexRings.assign( vertexCount, {} );
m_vertexTriangles.assign( vertexCount, {} );
// Sorting by vertex pair puts the two sides of a shared edge next to each other
std::vector<std::pair<uint64_t, int>> edges;
edges.reserve( 3 * triangleCount );
for ( int t = 0; t < triangleCount; ++t )
{
int v[3] = { triangles[t].index1, triangles[t].index2, triangles[t].index3 };
m_triangleNormals[t] = b3Normalize( b3Cross( vertices[v[1]] - vertices[v[0]], vertices[v[2]] - vertices[v[0]] ) );
for ( int e = 0; e < 3; ++e )
{
int a = v[e];
int b = v[e < 2 ? e + 1 : 0];
uint64_t key = ( (uint64_t)b3MinInt( a, b ) << 32 ) | (uint64_t)b3MaxInt( a, b );
edges.push_back( { key, 3 * t + e } );
m_vertexRings[a].push_back( b );
m_vertexRings[b].push_back( a );
m_vertexTriangles[a].push_back( t );
}
}
std::sort( edges.begin(), edges.end() );
for ( size_t i = 0; i + 1 < edges.size(); ++i )
{
// A non-manifold edge leaves every side without a neighbor
uint64_t key = edges[i].first;
bool pair = edges[i + 1].first == key;
bool single = ( i + 2 >= edges.size() || edges[i + 2].first != key ) && ( i == 0 || edges[i - 1].first != key );
if ( pair && single )
{
m_edgeNeighbors[edges[i].second] = edges[i + 1].second / 3;
m_edgeNeighbors[edges[i + 1].second] = edges[i].second / 3;
}
}
for ( std::vector<int>& ring : m_vertexRings )
{
std::sort( ring.begin(), ring.end() );
ring.erase( std::unique( ring.begin(), ring.end() ), ring.end() );
}
}
bool MatchesFace( b3Vec3 normal, int triangleIndex ) const
{
return b3Dot( normal, m_triangleNormals[triangleIndex] ) >= m_cosTolerance;
}
// Direction in the face plane pointing away from the edge into the face
static b3Vec3 GetInwardDirection( b3Vec3 edgeStart, b3Vec3 axis, b3Vec3 opposite )
{
b3Vec3 d = opposite - edgeStart;
return b3Normalize( d - b3Dot( d, axis ) * axis );
}
// The normal cone of a convex edge is the arc between its two face normals. Neither face may rise
// above the edge along the normal, so a concave edge only admits its face normals.
bool AdmitsEdge( b3Vec3 normal, int triangleIndex, int edgeIndex ) const
{
int neighbor = m_edgeNeighbors[3 * triangleIndex + edgeIndex];
if ( MatchesFace( normal, triangleIndex ) || ( neighbor != B3_NULL_INDEX && MatchesFace( normal, neighbor ) ) )
{
return true;
}
const b3MeshTriangle* triangles = b3GetMeshTriangles( m_mesh );
const b3Vec3* vertices = b3GetMeshVertices( m_mesh );
int v[3] = { triangles[triangleIndex].index1, triangles[triangleIndex].index2, triangles[triangleIndex].index3 };
int ia = v[edgeIndex];
int ib = v[edgeIndex < 2 ? edgeIndex + 1 : 0];
int ic = v[edgeIndex > 0 ? edgeIndex - 1 : 2];
b3Vec3 a = vertices[ia];
b3Vec3 axis = b3Normalize( vertices[ib] - a );
if ( b3AbsFloat( b3Dot( normal, axis ) ) > m_sinTolerance )
{
return false;
}
if ( b3Dot( normal, GetInwardDirection( a, axis, vertices[ic] ) ) > m_sinTolerance )
{
return false;
}
if ( neighbor != B3_NULL_INDEX )
{
int w[3] = { triangles[neighbor].index1, triangles[neighbor].index2, triangles[neighbor].index3 };
for ( int k = 0; k < 3; ++k )
{
if ( w[k] != ia && w[k] != ib && b3Dot( normal, GetInwardDirection( a, axis, vertices[w[k]] ) ) > m_sinTolerance )
{
return false;
}
}
}
return true;
}
// A vertex normal is valid when the vertex is a local support point in that direction
bool AdmitsVertex( b3Vec3 normal, int vertexIndex ) const
{
for ( int t : m_vertexTriangles[vertexIndex] )
{
if ( MatchesFace( normal, t ) )
{
return true;
}
}
const b3Vec3* vertices = b3GetMeshVertices( m_mesh );
b3Vec3 vertex = vertices[vertexIndex];
for ( int other : m_vertexRings[vertexIndex] )
{
if ( b3Dot( normal, b3Normalize( vertices[other] - vertex ) ) > m_sinTolerance )
{
return false;
}
}
return true;
}
bool AdmitsPoint( b3Vec3 normal, const b3ManifoldPoint& point ) const
{
const b3MeshTriangle& triangle = b3GetMeshTriangles( m_mesh )[point.triangleIndex];
int v[3] = { triangle.index1, triangle.index2, triangle.index3 };
// The normal was replaced by the face normal of a nearby triangle
if ( point.meshFlags & b3_meshContactBlocked )
{
for ( int k = 0; k < 3; ++k )
{
for ( int other : m_vertexTriangles[v[k]] )
{
if ( MatchesFace( normal, other ) )
{
return true;
}
}
}
return false;
}
switch ( point.triangleFeature )
{
case b3_featureTriangleFace:
return MatchesFace( normal, point.triangleIndex );
case b3_featureEdge1:
case b3_featureEdge2:
case b3_featureEdge3:
return AdmitsEdge( normal, point.triangleIndex, point.triangleFeature - b3_featureEdge1 );
case b3_featureVertex1:
case b3_featureVertex2:
case b3_featureVertex3:
return AdmitsVertex( normal, v[point.triangleFeature - b3_featureVertex1] );
default:
// No feature was resolved, so any feature of the triangle may own the normal
if ( MatchesFace( normal, point.triangleIndex ) )
{
return true;
}
for ( int k = 0; k < 3; ++k )
{
if ( AdmitsEdge( normal, point.triangleIndex, k ) || AdmitsVertex( normal, v[k] ) )
{
return true;
}
}
return false;
}
}
static GhostClass Classify( const b3ManifoldPoint& point )
{
switch ( point.triangleFeature )
{
case b3_featureTriangleFace:
return e_faceGhost;
case b3_featureEdge1:
case b3_featureEdge2:
case b3_featureEdge3:
return e_edgeGhost;
case b3_featureVertex1:
case b3_featureVertex2:
case b3_featureVertex3:
return e_vertexGhost;
default:
return e_otherGhost;
}
}
void CountGhosts( b3BodyId bodyId, b3Pos center )
{
// Clustering merges manifolds whose normals agree within its threshold and hands them one normal,
// so a point may carry a normal that far from its own feature
float angle = m_toleranceDegrees * B3_DEG_TO_RAD + acosf( 0.996f );
m_cosTolerance = cosf( angle );
m_sinTolerance = sinf( angle );
b3ContactData contacts[16];
int contactCount = b3Body_GetContactData( bodyId, contacts, 16 );
for ( int i = 0; i < contactCount; ++i )
{
bool bodyIsA = B3_ID_EQUALS( b3Shape_GetBody( contacts[i].shapeIdA ), bodyId );
b3ShapeId otherId = bodyIsA ? contacts[i].shapeIdB : contacts[i].shapeIdA;
if ( b3Shape_GetType( otherId ) != b3_meshShape )
{
continue;
}
for ( int j = 0; j < contacts[i].manifoldCount; ++j )
{
const b3Manifold* manifold = contacts[i].manifolds + j;
b3Vec3 normal = bodyIsA ? b3Neg( manifold->normal ) : manifold->normal;
// Speculative points that never pushed are harmless. Clustering hands every point the normal
// of one member, so the manifold is sound if any pushing point owns that normal.
int pushCount = 0;
int firstPush = 0;
bool admitted = false;
for ( int k = 0; k < manifold->pointCount && admitted == false; ++k )
{
const b3ManifoldPoint& point = manifold->points[k];
if ( point.totalNormalImpulse <= 0.0f )
{
continue;
}
firstPush = pushCount == 0 ? k : firstPush;
pushCount += 1;
admitted = AdmitsPoint( normal, point );
}
if ( pushCount == 0 )
{
continue;
}
m_manifoldCount += 1;
if ( admitted )
{
continue;
}
const b3ManifoldPoint& point = manifold->points[firstPush];
m_ghostCount += 1;
m_stepGhostCount += 1;
m_classCounts[Classify( point )] += 1;
for ( int bit = 0; bit < m_flagCount; ++bit )
{
m_flagCounts[bit] += ( point.meshFlags >> bit ) & 1;
}
b3Vec3 anchor = bodyIsA ? point.anchorA : point.anchorB;
m_markers[m_markerIndex] = { b3OffsetPos( center, anchor ), normal };
m_markerIndex = ( m_markerIndex + 1 ) % m_markerCapacity;
m_markerCount = b3MinInt( m_markerCount + 1, m_markerCapacity );
}
}
}
void Step() override
{
// Contacts are computed at the start of the step and anchors stay relative to that body position.
// A fast body moves past the margin within a step, so the end position would query the wrong triangles.
for ( int i = 0; i < m_bodyCount; ++i )
{
m_contactCenters[i] = b3Body_GetWorldCenter( m_bodyIds[i] );
}
Sample::Step();
if ( m_didStep )
{
m_stepGhostCount = 0;
for ( int i = 0; i < m_bodyCount; ++i )
{
b3BodyId bodyId = m_bodyIds[i];
b3Pos position = b3Body_GetPosition( bodyId );
// Off the belt and on the ground, so drop it back in at the start
if ( position.y < m_baseHeight - 0.5f )
{
b3Body_SetTransform( bodyId, GetSpawnPosition(), RandomQuat() );
b3Body_SetLinearVelocity( bodyId, b3Vec3_zero );
b3Body_SetAngularVelocity( bodyId, b3Vec3_zero );
m_recycleCount += 1;
continue;
}
CountGhosts( bodyId, m_contactCenters[i] );
}
m_spawnTimer += 1;
if ( m_bodyCount < m_targetCount && m_spawnTimer >= m_spawnInterval )
{
SpawnBody();
m_spawnTimer = 0;
}
}
float rate = m_manifoldCount > 0 ? 1.0e6f * (float)m_ghostCount / (float)m_manifoldCount : 0.0f;
DrawTextLine( "ghost manifolds: %d this step, %d of %d pushing (%.0f ppm)", m_stepGhostCount, m_ghostCount,
m_manifoldCount, rate );
DrawTextLine( "ghost feature: face %d, edge %d, vertex %d, unresolved %d", m_classCounts[e_faceGhost],
m_classCounts[e_edgeGhost], m_classCounts[e_vertexGhost], m_classCounts[e_otherGhost] );
DrawTextLine( "ghost origin: hull face %d, snapped %d, unverified %d, deep %d, blocked %d", m_flagCounts[0],
m_flagCounts[1], m_flagCounts[2], m_flagCounts[3], m_flagCounts[4] );
DrawTextLine( "bodies: %d, recycled: %d", m_bodyCount, m_recycleCount );
DrawTextLine( "triangles: %d, stations: %d", m_mesh->triangleCount, (int)m_stations.size() );
}
void Render() override
{
Sample::Render();
int stationCount = (int)m_stations.size();
if ( m_drawFlow )
{
int stride = b3MaxInt( 1, stationCount / 48 );
for ( int k = 0; k < stationCount; k += stride )
{
b3Vec3 p = m_stations[k] + 0.05f * m_ups[k];
b3Vec3 t = m_tangents[k];
DrawArrow( b3ToPos( p ), b3ToPos( p + 0.25f * m_beltSpeed * t ), MakeColor( b3_colorBlueViolet ) );
}
}
if ( m_drawGhosts )
{
for ( int i = 0; i < m_markerCount; ++i )
{
b3Pos p = m_markers[i].point;
DrawPoint( p, 8.0f, MakeColor( b3_colorRed ) );
DrawLine( p, p + 0.5f * m_markers[i].normal, MakeColor( b3_colorRed ) );
}
}
}
bool DrawControls() override
{
bool rebuild = false;
bool respawn = false;
ImGui::SliderFloat( "Cell Size", &m_cellSize, 0.1f, 0.5f, "%.2f m" );
rebuild |= ImGui::IsItemDeactivatedAfterEdit();
if ( ImGui::SliderFloat( "Belt Speed", &m_beltSpeed, 0.0f, 6.0f, "%.1f m/s" ) )
{
UpdateBeltSpeed();
}
ImGui::SliderInt( "Body Count", &m_targetCount, 1, m_maxBodies );
ImGui::SliderInt( "Spawn Interval", &m_spawnInterval, 1, 60 );
const char* bodyNames[e_bodyTypeCount] = { "Mixed", "Box", "Capsule", "Sphere", "Cylinder" };
int bodyType = m_bodyType;
if ( ImGui::Combo( "Bodies", &bodyType, bodyNames, e_bodyTypeCount ) )
{
m_bodyType = (BodyType)bodyType;
respawn = true;
}
if ( ImGui::Checkbox( "Contact Recycling", &m_contactRecycling ) )
{
for ( int i = 0; i < m_bodyCount; ++i )
{
b3Body_EnableContactRecycling( m_bodyIds[i], m_contactRecycling );
}
}
ImGui::SliderFloat( "Ghost Tolerance", &m_toleranceDegrees, 1.0f, 20.0f, "%.0f deg" );
ImGui::Checkbox( "Draw Flow", &m_drawFlow );
ImGui::Checkbox( "Draw Ghosts", &m_drawGhosts );
respawn |= ImGui::Button( "Respawn" );
if ( ImGui::Button( "Reset Counters" ) )
{
ResetCounters();
}
if ( rebuild )
{
DestroyCircuit();
CreateCircuit();
ResetCounters();
}
if ( respawn )
{
DestroyBodies();
ResetCounters();
}
// Shrinking the count drops the newest bodies
while ( m_bodyCount > m_targetCount )
{
m_bodyCount -= 1;
b3DestroyBody( m_bodyIds[m_bodyCount] );
}
return true;
}
static Sample* Create( SampleContext* context )
{
return new ConveyorCircuit( context );
}
Segment m_segments[m_segmentCount];
float m_segmentStart[m_segmentCount];
float m_totalLength;
b3Vec2 m_profile[m_profileCount];
std::vector<b3Vec3> m_stations;
std::vector<b3Vec3> m_tangents;
std::vector<b3Vec3> m_laterals;
std::vector<b3Vec3> m_ups;
std::vector<b3Vec3> m_triangleNormals;
std::vector<int> m_edgeNeighbors;
std::vector<std::vector<int>> m_vertexRings;
std::vector<std::vector<int>> m_vertexTriangles;
b3BodyId m_groundId;
b3MeshData* m_mesh = nullptr;
b3ShapeId m_shapeId = {};
b3HullData* m_cylinderHull;
b3BodyId m_bodyIds[m_maxBodies];
b3Pos m_contactCenters[m_maxBodies];
int m_bodyCount;
int m_spawnIndex;
int m_spawnTimer;
float m_cellSize;
float m_beltSpeed;
int m_targetCount;
int m_spawnInterval;
BodyType m_bodyType;
float m_toleranceDegrees;
float m_cosTolerance = 1.0f;
float m_sinTolerance = 0.0f;
bool m_contactRecycling;
bool m_drawFlow;
bool m_drawGhosts;
static constexpr int m_flagCount = 5;
int m_classCounts[e_ghostClassCount];
int m_flagCounts[m_flagCount];
int m_ghostCount;
int m_manifoldCount;
int m_stepGhostCount;
int m_recycleCount;
Marker m_markers[m_markerCapacity];
int m_markerCount;
int m_markerIndex;
};
static int sampleConveyorCircuit = RegisterSample( "Mesh", "Conveyor Circuit", ConveyorCircuit::Create );