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

@ -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 );