diff --git a/benchmark/ghost_baseline.csv b/benchmark/ghost_baseline.csv index 738ce7b..99cb9d7 100644 --- a/benchmark/ghost_baseline.csv +++ b/benchmark/ghost_baseline.csv @@ -103,3 +103,13 @@ chamfer,0,0,box,0,2 chamfer,0,0,cylinder,0,2 chamfer,0,1,box,0,2 chamfer,0,1,cylinder,0,2 +rail,0,0,box,36,5 +rail,0,0,capsule,0,2 +rail,0,0,upright,0,5 +rail,0,0,sphere,0,4 +rail,0,0,cylinder,8,3 +rail,0,1,box,36,5 +rail,0,1,capsule,0,2 +rail,0,1,upright,0,5 +rail,0,1,sphere,0,4 +rail,0,1,cylinder,8,3 diff --git a/benchmark/ghost_baseline_hf.csv b/benchmark/ghost_baseline_hf.csv index 00672e0..15d89b1 100644 --- a/benchmark/ghost_baseline_hf.csv +++ b/benchmark/ghost_baseline_hf.csv @@ -103,3 +103,13 @@ chamfer,0,0,box,0,2 chamfer,0,0,cylinder,0,2 chamfer,0,1,box,0,2 chamfer,0,1,cylinder,0,2 +rail,0,0,box,0,5 +rail,0,0,capsule,0,4 +rail,0,0,upright,0,5 +rail,0,0,sphere,0,5 +rail,0,0,cylinder,0,4 +rail,0,1,box,0,5 +rail,0,1,capsule,0,4 +rail,0,1,upright,0,5 +rail,0,1,sphere,0,5 +rail,0,1,cylinder,0,4 diff --git a/benchmark/ghost_scan.c b/benchmark/ghost_scan.c index c5e0def..4394b3b 100644 --- a/benchmark/ghost_scan.c +++ b/benchmark/ghost_scan.c @@ -18,7 +18,7 @@ #include // Command line and result file keys -static const char* s_sceneKeys[e_ghostSceneCount] = { "flat", "ramp", "cliff", "crest", "bumps", "tjunction", "peel", "chamfer" }; +static const char* s_sceneKeys[e_ghostSceneCount] = { "flat", "ramp", "cliff", "crest", "bumps", "tjunction", "peel", "chamfer", "rail" }; static const char* s_bodyKeys[e_ghostBodyCount] = { "box", "capsule", "upright", "sphere", "cylinder" }; #define MAX_CONFIGS 256 diff --git a/include/box3d/types.h b/include/box3d/types.h index 7b81955..b4b1fe0 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -1628,7 +1628,6 @@ typedef struct b3SimplexCache /// Value use to compare length, area, volume of two simplexes. float metric; - // todo use an index of 0xFF as a sentinel and remove the count /// The number of stored simplex points uint16_t count; @@ -2813,6 +2812,12 @@ typedef struct b3ManifoldPoint /// Did this contact point exist in the previous step? bool persisted; + + // b3TriangleFeature + uint8_t triangleFeature; + + // b3MeshContactFlags + uint8_t meshFlags; } b3ManifoldPoint; /// A contact manifold describes the contact points between colliding shapes. @@ -2840,7 +2845,7 @@ typedef struct b3Manifold } b3Manifold; /// Cached separating axis feature. -typedef enum +typedef enum b3SeparatingFeature { b3_invalidAxis = 0, b3_backsideAxis, @@ -2856,7 +2861,7 @@ typedef enum } b3SeparatingFeature; /// Cached triangle feature. -typedef enum +typedef enum b3TriangleFeature { b3_featureNone = 0, b3_featureTriangleFace, @@ -2872,6 +2877,16 @@ typedef enum b3_featureVertex3 } b3TriangleFeature; +/// This gives debug information about how a contact point was generated. +typedef enum b3MeshContactFlags +{ + b3_meshContactHullFace = 0x01, + b3_meshContactSnapped = 0x02, + b3_meshContactUnverified = 0x04, + b3_meshContactDeep = 0x08, + b3_meshContactBlocked = 0x10, +} b3MeshContactFlags; + /// Separating axis test cache. Provides temporal acceleration of collision routines. typedef struct { @@ -2923,6 +2938,12 @@ typedef struct b3LocalManifoldPoint /// The triangle index when collide with a mesh or height-field. int triangleIndex; + + // b3TriangleFeature + uint8_t triangleFeature; + + // b3MeshContactFlags + uint8_t meshFlags; } b3LocalManifoldPoint; /// A local manifold with no dynamic information. Used by b3Collide functions. diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index b27d004..bb633a3 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -2,6 +2,7 @@ // SPDX-License-Identifier: MIT #include "gfx/draw.h" +#include "gfx/keycodes.h" #include "imgui.h" #include "mesh_loader.h" #include "meshoptimizer.h" @@ -2267,3 +2268,317 @@ public: }; static int sampleRollingDeck = RegisterSample( "Issues", "Rolling Deck", RollingDeck::Create ); + +// A car made of one dynamic box (the chassis) and four ray cast 'wheels'. +// There are no wheel bodies and no joints. + +// W/S to drive, A/D to steer and Space to brake. T toggles a camera that follows the car. + +// Each step every wheel casts a ray down the chassis' own down axis. Where a ray hits the ground +// the wheel applies two forces to the chassis at the contact point: +// - a spring and damper force along the contact normal, which holds the chassis up +// - a tyre force in the contact plane, which drives, brakes and stops the wheel sliding sideways +// The tyre force is limited by friction times the spring force, so a wheel that is lightly loaded +// grips less. Weight transfer, body roll and rollover come from where these forces are applied. +class RaycastVehicle : public Sample +{ +public: + static constexpr int m_wheelCount = 4; + static constexpr uint64_t m_vehicleBit = 0x2; + + struct Wheel + { + b3Vec3 localMount; + bool steered; + + // Distance from the mount down to the wheel's center + float suspensionLength; + bool grounded; + + // Rotation about the axle, for drawing only + float spinAngle; + float spinSpeed; + }; + + explicit RaycastVehicle( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 25.0f, 20.0f, 12.0f, { 0.0f, 1.0f, 0.0f } ); + } + + m_suspensionHertz = 1.5f; + m_suspensionDampingRatio = 0.45f; + m_friction = 1.0f; + m_wheelRadius = 0.35f; + m_suspensionTravel = 0.3f; + m_engineAcceleration = 5.0f; + m_brakeDeceleration = 8.0f; + m_maxSteeringAngle = 0.15f * B3_PI; + m_steeringAngle = 0.0f; + + AddGroundBox( 100.0f ); + + // A ramp to show the suspension working + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 15.0f, -0.5f, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.1f ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 6.0f, 0.5f, 4.0f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + // The chassis is the only body. Its shape has its own category so the wheel rays can skip + // it. + float halfLength = 2.2f; + float halfHeight = 0.7f; + float halfWidth = 0.9f; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 1.2f, 0.0f }; + m_chassisId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 130.0f; + shapeDef.filter.categoryBits = m_vehicleBit; + b3BoxHull box = b3MakeBoxHull( halfLength, halfHeight, halfWidth ); + b3CreateHullShape( m_chassisId, &shapeDef, &box.base ); + + // A car tips over when its lateral acceleration exceeds half the track width divided by + // the height of its center of mass. At the middle of this box the center of mass would + // be about as high as the half track, and the car would roll before its tyres slide. + // Lower it to near the floor, where the heavy parts of a car are. + b3MassData massData = b3Body_GetMassData( m_chassisId ); + massData.center = { 0.0f, -0.45f, 0.0f }; + b3Body_SetMassData( m_chassisId, massData ); + } + + // The mounts are the top of each wheel's travel, inside the chassis at the sides. +x is + // forward. + float axle = 0.75f * halfLength; + float mountHeight = -0.3f; + m_wheels[0] = { { axle, mountHeight, halfWidth }, true, m_suspensionTravel }; + m_wheels[1] = { { axle, mountHeight, -halfWidth }, true, m_suspensionTravel }; + m_wheels[2] = { { -axle, mountHeight, halfWidth }, false, m_suspensionTravel }; + m_wheels[3] = { { -axle, mountHeight, -halfWidth }, false, m_suspensionTravel }; + + // A cylinder along its y axis, centered, for drawing the wheels + float wheelWidth = 0.25f; + m_wheelHull = b3CreateCylinder( wheelWidth, m_wheelRadius, -0.5f * wheelWidth, 12 ); + } + + ~RaycastVehicle() override + { + b3DestroyHull( m_wheelHull ); + + if ( m_camera->m_thirdPerson ) + { + ToggleThirdPerson(); + } + } + + void Keyboard( int key, int action, int mods ) override + { + if ( key == KEY_T && action == ACTION_PRESS ) + { + ToggleThirdPerson(); + } + } + + bool DrawControls() override + { + ImGui::SliderFloat( "Suspension Hertz", &m_suspensionHertz, 0.5f, 5.0f, "%.1f" ); + ImGui::SliderFloat( "Suspension Damping", &m_suspensionDampingRatio, 0.0f, 2.0f, "%.2f" ); + ImGui::SliderFloat( "Tyre Friction", &m_friction, 0.0f, 2.0f, "%.2f" ); + + bool thirdPerson = m_camera->m_thirdPerson; + if ( ImGui::Checkbox( "Third Person (T)", &thirdPerson ) ) + { + ToggleThirdPerson(); + } + + return true; + } + + // Apply the forces of every wheel to the chassis for the coming step + void UpdateWheels( float timeStep, float throttle, float brake, float steer ) + { + b3WorldTransform transform = b3Body_GetTransform( m_chassisId ); + b3Vec3 forward = b3RotateVector( transform.q, b3Vec3_axisX ); + + // The acceleration the driver asks for along the chassis' nose. The brake opposes the + // chassis' motion and is capped at what stops it this step, so it never drives it + // backwards. + float speed = b3Dot( b3Body_GetLinearVelocity( m_chassisId ), forward ); + float acceleration = m_engineAcceleration * throttle; + float stopping = b3MinFloat( m_brakeDeceleration * brake, b3AbsFloat( speed ) / timeStep ); + acceleration -= speed > 0.0f ? stopping : -stopping; + + for ( int i = 0; i < m_wheelCount; ++i ) + { + UpdateWheel( m_wheels[i], transform, timeStep, acceleration, steer ); + } + } + + // Cast one wheel's ray and apply its suspension and tyre forces to the chassis + void UpdateWheel( Wheel& wheel, b3WorldTransform transform, float timeStep, float acceleration, + float steer ) + { + b3Vec3 up = b3RotateVector( transform.q, b3Vec3_axisY ); + b3Vec3 forward = b3RotateVector( transform.q, b3Vec3_axisX ); + + // The ray reaches from the mount down through the spring's travel and the wheel's radius + float reach = m_suspensionTravel + m_wheelRadius; + b3Vec3 translation = b3MulSV( -reach, up ); + + b3QueryFilter filter = b3DefaultQueryFilter(); + filter.categoryBits = m_vehicleBit; + filter.maskBits = ~m_vehicleBit; + + b3Pos mount = b3TransformWorldPoint( transform, wheel.localMount ); + b3RayResult hit = b3World_CastRayClosest( m_worldId, mount, translation, filter ); + wheel.grounded = hit.hit; + float fraction = hit.hit ? hit.fraction : 1.0f; + wheel.suspensionLength = fraction * reach - m_wheelRadius; + + // A wheel turns at the speed of the ground under it, and keeps that speed in the air + wheel.spinAngle = b3UnwindAngle( wheel.spinAngle + wheel.spinSpeed * timeStep ); + + if ( hit.hit == false ) + { + return; + } + + // Each wheel carries an equal share of the mass. The spring rate gives that share the + // requested natural frequency. + float share = b3Body_GetMass( m_chassisId ) / m_wheelCount; + float omega = 2.0f * B3_PI * m_suspensionHertz; + float stiffness = share * omega * omega; + float damping = 2.0f * m_suspensionDampingRatio * share * omega; + + // Suspension. A spring only pushes: a negative load would pull the chassis onto the + // ground and hold it there through a jump. + float compression = ( 1.0f - hit.fraction ) * reach; + float compressionSpeed = -b3Dot( b3Body_GetWorldPointVelocity( m_chassisId, mount ), up ); + float load = b3MaxFloat( stiffness * compression + damping * compressionSpeed, 0.0f ); + b3Body_ApplyForce( m_chassisId, b3MulSV( load, hit.normal ), hit.point, true ); + + // The wheel's rolling direction in the contact plane, and the direction across it + b3Vec3 heading = forward; + if ( wheel.steered ) + { + heading = b3RotateVector( b3MakeQuatFromAxisAngle( up, steer ), forward ); + } + b3Vec3 rolling = + b3Normalize( b3MulAdd( heading, -b3Dot( heading, hit.normal ), hit.normal ) ); + b3Vec3 lateral = b3Cross( hit.normal, rolling ); + + // The lateral force cancels the contact point's sideways sliding in one step. The rolling + // force gives this wheel's share of the mass the requested acceleration, resolved onto the + // wheel, so a steered wheel puts less of it into the nose direction. + b3Vec3 contactVelocity = b3Body_GetWorldPointVelocity( m_chassisId, hit.point ); + float lateralForce = -share * b3Dot( contactVelocity, lateral ) / timeStep; + float rollingForce = share * acceleration * b3Dot( forward, rolling ); + + // Rolling forward along +x turns the wheel backwards about its axle, the chassis' +z + wheel.spinSpeed = -b3Dot( contactVelocity, rolling ) / m_wheelRadius; + + // Friction circle: the tyre cannot give more than friction times the load on it + float maxForce = m_friction * load; + float magnitude = sqrtf( lateralForce * lateralForce + rollingForce * rollingForce ); + if ( magnitude > maxForce ) + { + lateralForce *= maxForce / magnitude; + rollingForce *= maxForce / magnitude; + } + + b3Vec3 tyreForce = + b3Add( b3MulSV( rollingForce, rolling ), b3MulSV( lateralForce, lateral ) ); + b3Body_ApplyForce( m_chassisId, tyreForce, hit.point, true ); + } + + void Step() override + { + float throttle = 0.0f; + throttle += IsKeyDown( KEY_W ) ? 1.0f : 0.0f; + throttle -= IsKeyDown( KEY_S ) ? 1.0f : 0.0f; + float brake = IsKeyDown( KEY_SPACE ) ? 1.0f : 0.0f; + + m_steeringAngle = 0.0f; + m_steeringAngle += IsKeyDown( KEY_A ) ? m_maxSteeringAngle : 0.0f; + m_steeringAngle -= IsKeyDown( KEY_D ) ? m_maxSteeringAngle : 0.0f; + + // Forces accumulate until the world steps, so apply them only on a step that runs + bool stepping = m_context->pause == false || m_context->singleStep > 0; + if ( stepping && m_context->hertz > 0.0f ) + { + UpdateWheels( 1.0f / m_context->hertz, throttle, brake, m_steeringAngle ); + } + + if ( m_camera->m_thirdPerson ) + { + m_camera->m_pivot = b3Body_GetPosition( m_chassisId ); + m_camera->UpdateTransform(); + } + + Sample::Step(); + } + + void Render() override + { + Sample::Render(); + + b3WorldTransform transform = b3Body_GetTransform( m_chassisId ); + b3Vec3 down = b3RotateVector( transform.q, b3Neg( b3Vec3_axisY ) ); + + // The wheel cylinder is along y. This turns it onto the axle, the chassis' z. + b3Quat cylinderToAxle = b3MakeQuatFromAxisAngle( b3Vec3_axisX, 0.5f * B3_PI ); + + for ( int i = 0; i < m_wheelCount; ++i ) + { + const Wheel& wheel = m_wheels[i]; + b3Pos mount = b3TransformWorldPoint( transform, wheel.localMount ); + b3Pos center = b3OffsetPos( mount, b3MulSV( wheel.suspensionLength, down ) ); + + float steer = wheel.steered ? m_steeringAngle : 0.0f; + b3Quat q = b3MulQuat( transform.q, b3MakeQuatFromAxisAngle( b3Vec3_axisY, steer ) ); + q = b3MulQuat( q, b3MakeQuatFromAxisAngle( b3Vec3_axisZ, wheel.spinAngle ) ); + q = b3MulQuat( q, cylinderToAxle ); + + b3HexColor color = wheel.grounded ? b3_colorGreen : b3_colorRed; + DrawLine( mount, center, MakeColor( color ) ); + DrawHull( { center, q }, m_wheelHull, MakeColor( b3_colorGray ) ); + } + + b3Vec3 forward = b3RotateVector( b3Body_GetRotation( m_chassisId ), b3Vec3_axisX ); + float speed = b3Dot( b3Body_GetLinearVelocity( m_chassisId ), forward ); + DrawTextLine( "speed = %.1f m/s", speed ); + } + + static Sample* Create( SampleContext* context ) + { + return new RaycastVehicle( context ); + } + + b3BodyId m_chassisId; + Wheel m_wheels[m_wheelCount]; + b3HullData* m_wheelHull; + + float m_suspensionHertz; + float m_suspensionDampingRatio; + float m_friction; + float m_wheelRadius; + float m_suspensionTravel; + float m_engineAcceleration; + float m_brakeDeceleration; + float m_maxSteeringAngle; + float m_steeringAngle; +}; + +static int sampleRaycastVehicle = + RegisterSample( "Vehicle", "Raycast Vehicle", RaycastVehicle::Create ); diff --git a/samples/sample_joint.cpp b/samples/sample_joint.cpp index 1139019..ecf8ede 100644 --- a/samples/sample_joint.cpp +++ b/samples/sample_joint.cpp @@ -2320,6 +2320,8 @@ public: m_upperSteeringDegrees = 45.0f; m_maxSteeringTorque = 5.0f; m_targetSteeringDegrees = 0.0f; + m_wheelType = WheelType::sphere; + m_cylinderSides = 16; b3BodyId groundId; { @@ -2371,15 +2373,10 @@ public: b3CreateParallelJoint( m_worldId, ¶llelJointDef ); } - shapeDef.density = 2.0f; - shapeDef.baseMaterial.friction = 3.0f; - bodyDef.type = b3_dynamicBody; bodyDef.allowFastRotation = true; bodyDef.rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ ); - // b3HullData* hull = b3CreateCylinder( 0.25f, 0.4f, 0.0f, 16 ); - b3WheelJointDef jointDef = b3DefaultWheelJointDef(); jointDef.base.bodyIdA = m_chassisId; jointDef.base.localFrameA.q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisX, b3Vec3_axisY ); @@ -2401,15 +2398,11 @@ public: jointDef.lowerSteeringLimit = B3_PI / 180.0f * m_lowerSteeringDegrees; jointDef.upperSteeringLimit = B3_PI / 180.0f * m_upperSteeringDegrees; - b3Sphere sphere = { b3Vec3_zero, 0.4f }; - { bodyDef.position = { 1.5f, 2.0f, 0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[0] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[0]; jointDef.base.localFrameA.p = { 1.5f, -0.5f, 0.8f }; jointDef.enableSteering = true; jointDef.enableSpinMotor = false; @@ -2418,11 +2411,9 @@ public: { bodyDef.position = { 1.5f, 2.0f, -0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[1] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[1]; jointDef.base.localFrameA.p = { 1.5f, -0.5f, -0.8f }; jointDef.enableSteering = true; jointDef.enableSpinMotor = false; @@ -2431,11 +2422,9 @@ public: { bodyDef.position = { -1.5f, 2.0f, 0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[2] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[2]; jointDef.base.localFrameA.p = { -1.5f, -0.5f, 0.8f }; jointDef.enableSteering = false; jointDef.enableSpinMotor = true; @@ -2444,18 +2433,21 @@ public: { bodyDef.position = { -1.5f, 2.0f, -0.8f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // b3CreateHullShape( bodyId, &shapeDef, hull ); + m_wheelIds[3] = b3CreateBody( m_worldId, &bodyDef ); - jointDef.base.bodyIdB = bodyId; + jointDef.base.bodyIdB = m_wheelIds[3]; jointDef.base.localFrameA.p = { -1.5f, -0.5f, -0.8f }; jointDef.enableSteering = false; jointDef.enableSpinMotor = true; m_rearRightId = b3CreateWheelJoint( m_worldId, &jointDef ); } - // b3DestroyHull( hull ); + for ( int i = 0; i < 4; ++i ) + { + m_wheelShapeIds[i] = b3_nullShapeId; + } + + CreateWheelShapes(); m_camera->m_thirdPerson = false; @@ -2474,6 +2466,59 @@ public: b3DestroyHeightField( m_heightField ); } + void CreateWheelShapes() + { + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 2.0f; + shapeDef.baseMaterial.friction = 3.0f; + + float radius = 0.4f; + float width = 0.25f; + + b3HullData* cylinder = nullptr; + if ( m_wheelType == WheelType::cylinder ) + { + float w = 1.5f * width; + cylinder = b3CreateCylinder( w, radius, -0.5f * w, m_cylinderSides ); + } + + for ( int i = 0; i < 4; ++i ) + { + if ( B3_IS_NON_NULL( m_wheelShapeIds[i] ) ) + { + b3DestroyShape( m_wheelShapeIds[i], false ); + } + + b3BodyId bodyId = m_wheelIds[i]; + + switch ( m_wheelType ) + { + case WheelType::sphere: + { + b3Sphere sphere = { b3Vec3_zero, radius }; + m_wheelShapeIds[i] = b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + break; + + case WheelType::capsule: + { + b3Capsule capsule = { { 0.0f, -0.5f * width, 0.0f }, { 0.0f, 0.5f * width, 0.0f }, radius }; + m_wheelShapeIds[i] = b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + break; + + case WheelType::cylinder: + m_wheelShapeIds[i] = b3CreateHullShape( bodyId, &shapeDef, cylinder ); + break; + } + } + + if ( cylinder != nullptr ) + { + b3DestroyHull( cylinder ); + } + } + void Keyboard( int key, int action, int mods ) override { if ( key == KEY_T && action == ACTION_PRESS ) @@ -2484,6 +2529,32 @@ public: bool DrawControls() override { + ImGui::Text( "Wheels" ); + if ( ImGui::RadioButton( "Sphere", m_wheelType == WheelType::sphere ) ) + { + m_wheelType = WheelType::sphere; + CreateWheelShapes(); + } + + if ( ImGui::RadioButton( "Capsule", m_wheelType == WheelType::capsule ) ) + { + m_wheelType = WheelType::capsule; + CreateWheelShapes(); + } + + if ( ImGui::RadioButton( "Cylinder", m_wheelType == WheelType::cylinder ) ) + { + m_wheelType = WheelType::cylinder; + CreateWheelShapes(); + } + + if ( ImGui::SliderInt( "Sides", &m_cylinderSides, 3, 32 ) && m_wheelType == WheelType::cylinder ) + { + CreateWheelShapes(); + } + + ImGui::Separator(); + ImGui::Text( "Suspension" ); if ( ImGui::SliderFloat( "Min##Suspension", &m_lowerTranslation, -10.0f, 10.0f, "%.1f" ) ) { @@ -2659,8 +2730,19 @@ public: return new Driving( context ); } + enum class WheelType + { + sphere, + capsule, + cylinder, + }; + b3HeightFieldData* m_heightField; b3BodyId m_chassisId; + b3BodyId m_wheelIds[4]; + b3ShapeId m_wheelShapeIds[4]; + WheelType m_wheelType; + int m_cylinderSides; b3JointId m_frontLeftId; b3JointId m_frontRightId; b3JointId m_rearLeftId; diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 7bc1c6d..f0e93d1 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -2666,6 +2666,45 @@ static void BuildGhostChamferExtended( std::vector& meshes ) BuildChamferBeam( meshes, 6.0f ); } +// A conveyor side rail running along x, from the Conveyor Circuit sample. The upper half of the inner +// wall facing -z and the 8 cm rail top meet at a convex edge, two cells along x so the vertex at x = 0 +// is interior. Quad diagonals alternate as in the conveyor mesh. +static void BuildGhostRail( std::vector& meshes ) +{ + GhostMesh mesh; + b3Vec2 section[3] = { { 0.0f, 0.175f }, { 0.0f, 0.35f }, { 0.08f, 0.35f } }; + for ( int row = 0; row < 3; ++row ) + { + for ( int column = 0; column < 3; ++column ) + { + mesh.vertices.push_back( { -0.25f + 0.25f * column, section[row].y, section[row].x } ); + } + } + + b3Vec3 facings[2] = { { 0.0f, 0.0f, -1.0f }, b3Vec3_axisY }; + for ( int row = 0; row < 2; ++row ) + { + for ( int column = 0; column < 2; ++column ) + { + int a = 3 * row + column; + int b = a + 1; + int c = a + 4; + int d = a + 3; + if ( ( row + column ) & 1 ) + { + AddGhostTriangle( mesh, a, d, b, facings[row] ); + AddGhostTriangle( mesh, d, c, b, facings[row] ); + } + else + { + AddGhostTriangle( mesh, a, d, c, facings[row] ); + AddGhostTriangle( mesh, c, b, a, facings[row] ); + } + } + } + meshes.push_back( mesh ); +} + static const float s_inch = 0.0254f; static const GhostScene s_hullGhostScenes[] = { @@ -2759,6 +2798,19 @@ static const GhostScene s_hullGhostScenes[] = { { 0.0f, 0.0f, 0.0f }, false, }, + { + "Rail edge", + "box pitched 60 degrees across the rail, its face cuts the convex rail edge 7 cm past the x = 0 " + "vertex, the real contact is edge on edge and perpendicular to the rail", + "the edge pair is shallower than the hull face and the hull face normal crosses the rail edge, so " + "SAT takes the edge pair. Without that the hull face is admitted at a rail edge vertex and leans 60 " + "degrees along the rail.", + BuildGhostRail, + { 0.4f, 0.2f, 0.4f }, + { 0.1f, 0.77f, -0.15f }, + { -15.0f, 0.0f, 60.0f }, + true, + }, }; class HullGhostManifold : public GhostManifold diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index 23f9c67..a818316 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -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 +#include #include #include #include +#include +#include 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 vertices; + std::vector indices; + std::vector 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> 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& 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 m_stations; + std::vector m_tangents; + std::vector m_laterals; + std::vector m_ups; + + std::vector m_triangleNormals; + std::vector m_edgeNeighbors; + std::vector> m_vertexRings; + std::vector> 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 ); diff --git a/shared/ghost_collisions.c b/shared/ghost_collisions.c index 115ea98..5c28b43 100644 --- a/shared/ghost_collisions.c +++ b/shared/ghost_collisions.c @@ -18,6 +18,11 @@ #define GHOST_CHAMFER_WIDTH 0.05f #define GHOST_CHAMFER_DROP ( GHOST_CHAMFER_WIDTH / 1.5f ) +// A conveyor side rail along x, the floor is on the -z side +#define GHOST_RAIL_HEIGHT 0.35f +#define GHOST_RAIL_THICKNESS 0.08f +#define GHOST_RAIL_SKIRT 0.4f + typedef struct PeelPreset { int columnCount; @@ -33,7 +38,7 @@ static const PeelPreset s_peelPresets[GHOST_PEEL_PRESET_COUNT] = { }; const char* g_ghostSceneNames[e_ghostSceneCount] = { - "Flat", "Concave", "Cliff", "Crest", "Bumps", "T-Junction", "Internal", "Chamfer", + "Flat", "Concave", "Cliff", "Crest", "Bumps", "T-Junction", "Internal", "Chamfer", "Rail", }; const char* g_ghostBodyNames[e_ghostBodyCount] = { "Box", "H Capsule", "V Capsule", "Sphere", "Cylinder" }; @@ -152,6 +157,92 @@ static void AddStrip( MeshBuilder* builder, const b3Vec2* profile, int count, fl free( points ); } +// The rail cross section is a profile in zy extruded along x. Each quad side is its edge direction along +// the section crossed with x, which is the outward normal for a section traversed with air on the left. +static void AddRail( MeshBuilder* builder, const b3Vec2* section, int count, float cellSize, bool mirror ) +{ + float sign = mirror ? -1.0f : 1.0f; + int xCount = b3MaxInt( 1, (int)ceilf( 2.0f * GHOST_HALF_LENGTH / cellSize - 0.01f ) ); + int stride = xCount + 1; + + int pointCount = 1; + for ( int k = 0; k + 1 < count; ++k ) + { + float dz = section[k + 1].x - section[k].x; + float dy = section[k + 1].y - section[k].y; + pointCount += b3MaxInt( 1, (int)ceilf( sqrtf( dz * dz + dy * dy ) / cellSize - 0.01f ) ); + } + + builder->vertices = realloc( builder->vertices, pointCount * stride * sizeof( b3Vec3 ) ); + int index = 0; + for ( int k = 0; k < count; ++k ) + { + int n = 1; + if ( k + 1 < count ) + { + float dz = section[k + 1].x - section[k].x; + float dy = section[k + 1].y - section[k].y; + n = b3MaxInt( 1, (int)ceilf( sqrtf( dz * dz + dy * dy ) / cellSize - 0.01f ) ); + } + + // The last point only closes the final segment + int last = k + 1 < count ? n : 1; + for ( int s = 0; s < last; ++s ) + { + float t = k + 1 < count ? (float)s / (float)n : 0.0f; + b3Vec2 a = section[k]; + b3Vec2 b = k + 1 < count ? section[k + 1] : a; + float z = a.x + t * ( b.x - a.x ); + float y = a.y + t * ( b.y - a.y ); + for ( int j = 0; j <= xCount; ++j ) + { + float x = -GHOST_HALF_LENGTH + 2.0f * GHOST_HALF_LENGTH * (float)j / (float)xCount; + builder->vertices[index++] = (b3Vec3){ sign * x, y, z }; + } + } + } + + builder->vertexCount = index; + + int triangleCount = 2 * ( pointCount - 1 ) * xCount; + builder->indices = realloc( builder->indices, 3 * triangleCount * sizeof( int32_t ) ); + builder->indexCount = 0; + for ( int i = 0; i + 1 < pointCount; ++i ) + { + for ( int j = 0; j < xCount; ++j ) + { + int a = i * stride + j; + int b = a + 1; + int c = a + stride + 1; + int d = a + stride; + + int corners[6] = { a, d, c, c, b, a }; + if ( ( i + j ) & 1 ) + { + int flipped[6] = { a, d, b, d, c, b }; + memcpy( corners, flipped, sizeof( corners ) ); + } + + for ( int k = 0; k < 6; k += 3 ) + { + builder->indices[builder->indexCount++] = corners[k]; + builder->indices[builder->indexCount++] = mirror ? corners[k + 2] : corners[k + 1]; + builder->indices[builder->indexCount++] = mirror ? corners[k + 1] : corners[k + 2]; + } + } + } +} + +static float EvaluateRail( float z, float cellZ ) +{ + if ( z < -0.5f * cellZ ) + { + return 0.0f; + } + + return z < 0.5f * cellZ ? GHOST_RAIL_HEIGHT : -GHOST_RAIL_SKIRT; +} + static void AddMesh( GhostCollisionsData* data, MeshBuilder* builder, bool frictionless ) { b3MeshDef meshDef = { 0 }; @@ -247,13 +338,16 @@ static void AddHeightField( b3WorldId worldId, GhostCollisionsData* data, float float cellX = ( x1 - x0 ) / (float)cellCountX; float cellZ = 2.0f * halfWidth / (float)cellCountZ; + // The mirror turns the body half way around y, which also reverses z + bool rail = data->settings.scene == e_ghostRail; float* heights = malloc( countX * countZ * sizeof( float ) ); for ( int row = 0; row < countZ; ++row ) { + float z = mirror ? halfWidth - cellZ * (float)row : -halfWidth + cellZ * (float)row; for ( int column = 0; column < countX; ++column ) { float x = mirror ? x1 - cellX * (float)column : x0 + cellX * (float)column; - heights[row * countX + column] = EvaluateProfile( data, x ); + heights[row * countX + column] = rail ? EvaluateRail( z, cellZ ) : EvaluateProfile( data, x ); } } @@ -374,6 +468,15 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) PushProfile( data, h, 0.0f ); break; + case e_ghostRail: + // Cross section in zy, from the floor up the inner wall, over the top and down the outer skirt + PushProfile( data, -GHOST_HALF_WIDTH, 0.0f ); + PushProfile( data, 0.0f, 0.0f ); + PushProfile( data, 0.0f, GHOST_RAIL_HEIGHT ); + PushProfile( data, GHOST_RAIL_THICKNESS, GHOST_RAIL_HEIGHT ); + PushProfile( data, GHOST_RAIL_THICKNESS, -GHOST_RAIL_SKIRT ); + break; + default: PushProfile( data, -h, 0.0f ); PushProfile( data, h, 0.0f ); @@ -421,6 +524,11 @@ void CreateGhostGround( b3WorldId worldId, GhostCollisionsData* data ) AddHeightField( worldId, data, -h, h, GetCellCount( 2.0f * h, cellSize ), groundHalfWidth, cellCountZ, false ); } } + else if ( settings->scene == e_ghostRail ) + { + AddRail( &builder, data->profile, data->profileCount, cellSize, mirror ); + AddMesh( data, &builder, false ); + } else if ( settings->scene == e_ghostTJunction ) { // Fine and coarse strips share only every other vertex along x = 0 @@ -478,6 +586,16 @@ void LaunchGhostBody( b3WorldId worldId, GhostCollisionsData* data ) bodyDef.position = (b3Pos){ s_peelPresets[settings->peelPreset].startZ, 80.0f, 0.0f }; bodyDef.rotation = yaw; } + else if ( settings->scene == e_ghostRail ) + { + // Dropped across the rail top, pitched so a box face sweeps into the rail edge at an angle. The real + // contact is edge on edge and perpendicular to the rail, a face normal there leans along the rail + // and catches the body. Found in the Conveyor Circuit sample. + b3Quat pitch = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 60.0f * B3_PI / 180.0f ); + bodyDef.position = (b3Pos){ GHOST_START_X, GHOST_RAIL_HEIGHT + 0.45f, -0.15f }; + bodyDef.rotation = b3MulQuat( pitch, yaw ); + bodyDef.linearVelocity = (b3Vec3){ settings->speed, 0.0f, 0.0f }; + } else { int segment = 0; @@ -714,7 +832,15 @@ int UpdateGhostCollisions( b3WorldId worldId, GhostCollisionsData* data, GhostRe } b3Vec3 normal = bodyIsA ? b3Neg( manifold->normal ) : manifold->normal; - if ( IsGhost( data, normal, xMin, xMax, zMin, zMax ) == false ) + if ( data->settings.scene == e_ghostRail ) + { + // The rail is the same all along x, so no valid normal leans along it + if ( b3AbsFloat( normal.x ) <= sinf( 5.0f * B3_PI / 180.0f ) ) + { + continue; + } + } + else if ( IsGhost( data, normal, xMin, xMax, zMin, zMax ) == false ) { continue; } diff --git a/shared/ghost_collisions.h b/shared/ghost_collisions.h index 572e262..6aaad1e 100644 --- a/shared/ghost_collisions.h +++ b/shared/ghost_collisions.h @@ -29,6 +29,7 @@ typedef enum GhostScene e_ghostTJunction, e_ghostInternal, e_ghostChamfer, + e_ghostRail, e_ghostSceneCount } GhostScene; diff --git a/src/contact.c b/src/contact.c index d200301..e5c9a60 100644 --- a/src/contact.c +++ b/src/contact.c @@ -581,6 +581,8 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* target->separation = source->separation; target->featureId = b3MakeFeatureId( source->pair ); target->triangleIndex = B3_NULL_INDEX; + target->triangleFeature = b3_featureNone; + target->meshFlags = 0; target->normalVelocity = 0.0f; } diff --git a/src/mesh_contact.c b/src/mesh_contact.c index a11457f..c1c6bd8 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -1075,10 +1075,11 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape bool blockSource = false; b3TriangleFeature nearFeature = manifold->feature; + b3NeighborResult result = b3_neighborAdmit; + uint8_t meshFlags = manifold->feature == b3_featureHullFace ? b3_meshContactHullFace : 0; if ( validateNeighbors && manifold->pointCount > 0 && manifold->feature != b3_featureTriangleFace ) { - b3NeighborResult result; if ( manifold->feature == b3_featureHullFace ) { result = b3TestHullFaceNeighbors( shapeA, triangleIndex, vertices, @@ -1115,6 +1116,15 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape result = shapeB->type == b3_sphereShape ? b3_neighborUnknown : b3_neighborSnap; } + if ( result == b3_neighborUnknown ) + { + meshFlags |= b3_meshContactUnverified; + } + else if ( result == b3_neighborSnap ) + { + meshFlags = b3_meshContactSnapped; + } + b3LocalManifold deepManifold = { 0 }; b3LocalManifoldPoint deepPoints[B3_MAX_CLIP_POINTS]; bool keepDeep = deep && result == b3_neighborSnap && shapeB->type == b3_hullShape; @@ -1152,6 +1162,11 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape // The kept manifold failed the neighbor test, so it is unverified like a boundary contact. blockSource = true; + meshFlags = b3_meshContactDeep | b3_meshContactUnverified; + if ( manifold->feature == b3_featureHullFace ) + { + meshFlags |= b3_meshContactHullFace; + } } } } @@ -1162,6 +1177,22 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape { B3_ASSERT( manifold->feature != b3_featureNone ); + b3TriangleFeature pointFeature = manifold->feature; + if ( pointFeature == b3_featureHullFace && nearFeature != b3_featureHullFace && nearFeature != b3_featureNone ) + { + pointFeature = nearFeature; + } + else if ( pointFeature == b3_featureHullFace && nearFeature == b3_featureNone && result == b3_neighborAdmit ) + { + pointFeature = b3_featureTriangleFace; + } + + for ( int i = 0; i < manifoldPointCount; ++i ) + { + manifold->points[i].triangleFeature = (uint8_t)pointFeature; + manifold->points[i].meshFlags = meshFlags; + } + manifold->triangleIndex = triangleIndex; manifold->triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); @@ -1265,6 +1296,11 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape manifold->normal = blockerNormal; manifold->triangleNormal = blockerNormal; manifold->feature = b3_featureTriangleFace; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + manifold->points[j].triangleFeature = b3_featureTriangleFace; + manifold->points[j].meshFlags |= b3_meshContactBlocked; + } } } } @@ -1426,6 +1462,8 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape cp->point = ap->point; cp->separation = ap->separation; cp->pair = ap->pair; + cp->triangleFeature = ap->triangleFeature; + cp->meshFlags = ap->meshFlags; cm->pointCount += 1; } } @@ -1563,6 +1601,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact target->separation = source->separation - restOffset; target->featureId = b3MakeFeatureId( source->pair ); target->triangleIndex = source->triangleIndex; + target->triangleFeature = source->triangleFeature; + target->meshFlags = source->meshFlags; // Preserve normal impulse if possible if ( matchedManifold != NULL ) @@ -1762,6 +1802,8 @@ int b3CollideMeshAndHull( b3Manifold* manifolds, int capacity, const b3Mesh* mes target->separation = source->separation - restOffset; target->featureId = b3MakeFeatureId( source->pair ); target->triangleIndex = source->triangleIndex; + target->triangleFeature = source->triangleFeature; + target->meshFlags = source->meshFlags; } } diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index 8a69d15..fcc1913 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -13,6 +13,10 @@ #include #include +#include + +// cosine of hull normal versus edge axis tolerance. +#define B3_CROSSING_TOLERANCE 0.1f typedef struct b3TriangleData { @@ -1311,14 +1315,29 @@ void b3CollideTriangleAndHull( b3LocalManifold* manifold, int capacity, b3Vec3 v // If a face axis has positive separation there may be no points. float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); + // The edge axis can win if the hull face axis is significantly aligned with the edge axis. + bool crossesEdge = false; + if ( manifold->pointCount >= 1 && manifold->feature == b3_featureHullFace && edgeQuery.separation > clipSeparation ) + { + b3Vec3 edgeAxis = b3Normalize( triangleEdges[edgeQuery.indexA] ); + + // Is the angle less than ~84 degrees? + crossesEdge = b3AbsFloat( b3Dot( faceQueryB.normal, edgeAxis ) ) > B3_CROSSING_TOLERANCE; + } + if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) || - ( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) ) + ( manifold->pointCount == 1 && edgeQuery.separation > clipSeparation + linearSlop ) || crossesEdge ) { B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 ); b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; manifold->pointCount = 0; b3CollideTriangleAndHullEdges( manifold, capacity, trianglePoint, triangleEdge, hullB, edgeQuery, cache ); + + if ( crossesEdge && manifold->pointCount == 0 ) + { + b3CollideHullFace( manifold, capacity, &triangle, hullB, faceQueryB, cache, enableSpeculative ); + } } }