diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index cdf085a..d9e69d8 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -1368,21 +1368,21 @@ B3_API float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId ); * @{ */ -/// Create a mover joint +/// Create a mover joint. /// @see b3MoverJointDef for details B3_API b3JointId b3CreateMoverJoint( b3WorldId worldId, const b3MoverJointDef* def ); -/// Set the desired relative linear velocity in meters per second +/// Set the desired relative linear velocity, usually in meters per second. B3_API void b3MoverJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ); -/// Get the desired relative linear velocity in meters per second +/// Get the desired relative linear velocity, usually in meters per second. B3_API b3Vec3 b3MoverJoint_GetLinearVelocity( b3JointId jointId ); -/// Set the joint maximum force, usually in newtons -B3_API void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, b3Vec3 maxForce ); +/// Set the joint maximum force, usually in newtons. +B3_API void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ); -/// Get the joint maximum force, usually in newtons -B3_API b3Vec3 b3MoverJoint_GetMaxVelocityForce( b3JointId jointId ); +/// Get the joint maximum force, usually in newtons. +B3_API float b3MoverJoint_GetMaxVelocityForce( b3JointId jointId ); /**@}*/ // mover_joint diff --git a/include/box3d/types.h b/include/box3d/types.h index cf24480..8be3369 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -769,8 +769,8 @@ typedef struct b3MotorJointDef /// @ingroup motor_joint B3_API b3MotorJointDef b3DefaultMotorJointDef( void ); -/// A mover joint is used to move a dynamic character mover through velocity commands. The x, y, and -/// z directions are handled separately. Does not affect rotation. +/// A mover joint is used to move a dynamic character mover through velocity commands. Force is only +/// applied in the xz plane of joint frame B. Does not affect rotation. /// @ingroup mover_joint typedef struct b3MoverJointDef { @@ -781,7 +781,7 @@ typedef struct b3MoverJointDef b3Vec3 linearVelocity; /// The maximum motor force in newtons. - b3Vec3 maxVelocityForce; + float maxForce; } b3MoverJointDef; /// Use this to initialize your joint definition diff --git a/samples/dynamic_mover.cpp b/samples/dynamic_mover.cpp index e42b406..8138c4f 100644 --- a/samples/dynamic_mover.cpp +++ b/samples/dynamic_mover.cpp @@ -155,14 +155,14 @@ void DynamicMover::Create( b3WorldId worldId, const DynamicMoverDef* def ) b3CreateCapsuleShape( m_moverId, &shapeDef, &m_capsule ); - CreateMoverJoint( m_groundId, { m_maxGroundForce, 0.0f, m_maxGroundForce } ); + CreateMoverJoint( m_groundId ); } -void DynamicMover::CreateMoverJoint( b3BodyId baseId, b3Vec3 maxForce ) +void DynamicMover::CreateMoverJoint( b3BodyId baseId ) { b3MoverJointDef moverDef = b3DefaultMoverJointDef(); moverDef.linearVelocity = m_velocity; - moverDef.maxVelocityForce = maxForce; + moverDef.maxForce = m_maxGroundForce; moverDef.base.bodyIdA = baseId; moverDef.base.bodyIdB = m_moverId; moverDef.base.collideConnected = true; @@ -355,9 +355,8 @@ void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 if ( B3_ID_EQUALS( b3Joint_GetBodyA( m_moverJointId ), baseId ) == false ) { - b3Vec3 maxForce = b3MoverJoint_GetMaxVelocityForce( m_moverJointId ); b3DestroyJoint( m_moverJointId, false ); - CreateMoverJoint( baseId, maxForce ); + CreateMoverJoint( baseId ); } m_velocity = b3Body_GetLinearVelocity( m_moverId ) - b3Body_GetLinearVelocity( baseId ); @@ -396,7 +395,7 @@ void DynamicMover::Update( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 maxForce = m_maxAirForce; } - b3MoverJoint_SetMaxVelocityForce( m_moverJointId, { maxForce, 0.0f, maxForce } ); + b3MoverJoint_SetMaxVelocityForce( m_moverJointId, maxForce ); float maxSpeed = speedScale * m_maxSpeed; b3Vec3 desiredVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right; @@ -499,8 +498,4 @@ void DynamicMover::Draw() const DrawLine( probe.origin, end, MakeColor( color ) ); DrawPoint( end, 6.0f, MakeColor( color ) ); } - - b3Pos position = b3Body_GetPosition( m_moverId ); - b3HexColor color = m_onGround ? b3_colorOrange : b3_colorAquamarine; - DrawLine( position, position + m_velocity, MakeColor( color ) ); } diff --git a/samples/dynamic_mover.h b/samples/dynamic_mover.h index 746acf8..fd73910 100644 --- a/samples/dynamic_mover.h +++ b/samples/dynamic_mover.h @@ -160,5 +160,5 @@ public: int m_selectedProbe; private: - void CreateMoverJoint( b3BodyId baseId, b3Vec3 maxForce ); + void CreateMoverJoint( b3BodyId baseId ); }; diff --git a/samples/host/camera.cpp b/samples/host/camera.cpp index c59a256..2b94117 100644 --- a/samples/host/camera.cpp +++ b/samples/host/camera.cpp @@ -12,7 +12,6 @@ static constexpr float ORBIT_SENS = 0.005f; // radians per pixel static constexpr float FLY_LOOK_SENS = 0.005f; // radians per pixel static constexpr float PAN_SENS = 0.005f; // meters per pixel per meter of radius static constexpr float RADIAL_ZOOM_SENS = 0.02f; // meters per pixel (alt+right-drag) -static constexpr float FLY_SPEED_STEP = 1.0f; // m/s per scroll tick static constexpr float ZOOM_STEP = 0.9f; // multiplier per scroll tick static constexpr float MIN_DIST = 0.1f; static constexpr float MIN_SPEED = 0.06f; // matches box3d's 0.001*60 @@ -502,7 +501,8 @@ void Camera::Update( float dt, int width, int height ) if ( m_speedScrollAccum != 0.0f ) { - m_speed += m_speedScrollAccum * FLY_SPEED_STEP; + m_speed *= powf( 1.1f, m_speedScrollAccum ); + //m_speed += m_speedScrollAccum; m_speed = b3ClampFloat( m_speed, MIN_SPEED, MAX_SPEED ); } } diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index 8cd9a75..e38dc5c 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -1193,7 +1193,7 @@ public: // Debris doesn't collide with the mover or other debris. boxShapeDef.filter.categoryBits = DebrisBit; - boxShapeDef.filter.maskBits = DynamicBit | StaticBit; + boxShapeDef.filter.maskBits = DebrisBit | ProjectileBit | DynamicBit | StaticBit; boxShapeDef.density = 100.0f; boxShapeDef.explosionScale = 0.05f; @@ -1298,6 +1298,7 @@ public: b3CreateRevoluteJoint( m_worldId, &jointDef ); } + // Jump puzzle for ( int i = 0; i < m_pillarCount; ++i ) { b3Pos top = GetPillarTop( i ); @@ -1338,9 +1339,6 @@ public: m_launcherHull = b3CreateCylinder( launcherLength, m_launcherRadius, -m_launcherBack, 5 ); m_launcherMesh = FindOrAddHull( m_launcherHull ); m_time = 0.0f; - m_lapTime = 0.0f; - m_bestLapTime = 0.0f; - m_lapCount = 0; } ~DynamicMoverSample() override @@ -1425,8 +1423,6 @@ public: m_gemIds[i] = b3CreateBody( m_worldId, &bodyDef ); b3CreateHullShape( m_gemIds[i], &shapeDef, &gem.base ); } - - m_gemsLeft = m_gemCount; } void UpdatePressurePlate() @@ -1476,23 +1472,10 @@ public: { b3DestroyBody( id ); id = b3_nullBodyId; - m_gemsLeft -= 1; break; } } } - - if ( m_gemsLeft == 0 ) - { - m_lapCount += 1; - if ( m_bestLapTime == 0.0f || m_lapTime < m_bestLapTime ) - { - m_bestLapTime = m_lapTime; - } - - m_lapTime = 0.0f; - SpawnGems(); - } } void SwapMoverShape() @@ -1515,15 +1498,13 @@ public: { float r = capsule.radius; float hy = 0.5f * b3Length( capsule.center2 - capsule.center1 ) + r; - b3BoxHull box = - b3MakeOffsetBoxHull( r, hy, r, b3Lerp( capsule.center1, capsule.center2, 0.5f ) ); + b3Vec3 c = b3Lerp( capsule.center1, capsule.center2, 0.5f ); + b3BoxHull box = b3MakeOffsetBoxHull( r, hy, r, c ); shapeDef.density = density; b3CreateHullShape( bodyId, &shapeDef, &box.base ); } else { - float r = capsule.radius; - float h = b3Length( capsule.center2 - capsule.center1 ); shapeDef.density = density; b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); } @@ -1536,19 +1517,17 @@ public: b3Vec3 direction; b3Vec3 right; b3Vec3 velocity; - b3Pos impact; - float flightTime; - bool hit; }; Aim ComputeAim() const { - Aim aim = {}; const b3Capsule& capsule = m_mover.m_capsule; b3Vec3 right = m_camera->GetRight(); right.y = 0.0f; right = b3Normalize( right ); + + Aim aim = {}; aim.right = right; b3Vec3 shoulderOffset = capsule.center2 + ( capsule.radius + m_launcherRadius ) * right; @@ -1557,28 +1536,6 @@ public: aim.muzzle = aim.shoulder + ( m_launcherFront + m_launcherRadius ) * aim.direction; aim.velocity = m_ballSpeed * aim.direction; - b3Vec3 gravity = b3World_GetGravity( m_worldId ); - b3QueryFilter filter = { ProjectileBit, AllBits & ~MoverBit, 0, "aim" }; - float maxTime = 3.0f; - float dt = 1.0f / 30.0f; - b3Pos p1 = aim.muzzle; - aim.flightTime = maxTime; - for ( float t = dt; t <= maxTime; t += dt ) - { - b3Pos p2 = aim.muzzle + t * aim.velocity + ( 0.5f * t * t ) * gravity; - b3RayResult result = - b3World_CastRayClosest( m_worldId, p1, b3SubPos( p2, p1 ), filter ); - if ( result.hit ) - { - aim.impact = result.point; - aim.flightTime = t - dt + result.fraction * dt; - aim.hit = true; - break; - } - - p1 = p2; - } - return aim; } @@ -1590,8 +1547,7 @@ public: b3DestroyBody( ballId ); } - // Back up by the travel of the coming step so the first drawn frame is at the tip. Capped - // at the shoulder so a fast ball never starts behind the mover. + // Back up the spawn point so we see it drawn at the muzzle. float backDistance = b3MinFloat( m_ballSpeed * leadTime, m_launcherFront + m_launcherRadius ); @@ -1599,6 +1555,8 @@ public: bodyDef.type = b3_dynamicBody; bodyDef.position = aim.muzzle - backDistance * aim.direction; bodyDef.linearVelocity = aim.velocity; + + // This is good use case for the bullet setting because this is a fast projectile. bodyDef.isBullet = true; bodyDef.name = "ball"; ballId = b3CreateBody( m_worldId, &bodyDef ); @@ -1616,10 +1574,9 @@ public: m_ballIndex = ( m_ballIndex + 1 ) % m_ballCapacity; } - void DrawAim( const Aim& aim ) const + void DrawWeapon( const Aim& aim ) const { - // The cylinder axis is local y. Rolling with the camera right keeps the flats steady as the - // view turns. + // The cylinder axis is local y. Roll with the camera to keep it steady. b3Matrix3 basis = { aim.right, aim.direction, b3Cross( aim.right, aim.direction ) }; b3WorldTransform launcherTransform = { aim.shoulder, b3MakeQuatFromMatrix( &basis ) }; b3Transform relative = b3ToRelativeTransform( launcherTransform, GetDrawOrigin() ); @@ -1752,7 +1709,7 @@ public: ImGui::Separator(); - ImGui::SliderFloat( "Ball Speed", &m_ballSpeed, 5.0f, 60.0f, "%.0f" ); + ImGui::SliderFloat( "Ball Speed", &m_ballSpeed, 10.0f, 200.0f, "%.0f" ); return true; } @@ -1765,7 +1722,6 @@ public: if ( pause == false && timeStep > 0.0f ) { m_time += timeStep; - m_lapTime += timeStep; for ( int i = 0; i < m_platformCount; ++i ) { @@ -1829,18 +1785,11 @@ public: if ( m_aiming ) { Aim aim = ComputeAim(); - DrawAim( aim ); + DrawWeapon( aim ); } DrawTextLine( "WASD = move, Space = jump, Shift = sprint, Ctrl = walk" ); DrawTextLine( "T = camera, RMB = aim, LMB = shoot" ); - DrawTextLine( "gems %d / %d, lap time %.1f s", m_gemCount - m_gemsLeft, m_gemCount, - m_lapTime ); - - if ( m_lapCount > 0 ) - { - DrawTextLine( "laps %d, best %.1f s", m_lapCount, m_bestLapTime ); - } float normalY = m_mover.m_castResult.hit ? m_mover.m_castResult.normal.y : 0.0f; DrawTextLine( "on ground %d, walkable %d, normal y %.2f", (int)m_mover.m_onGround, @@ -1881,11 +1830,7 @@ public: bool m_plateArmed; b3BodyId m_platformIds[m_platformCount]; b3BodyId m_gemIds[m_gemCount]; - int m_gemsLeft; - int m_lapCount; float m_time; - float m_lapTime; - float m_bestLapTime; bool m_jumpReleased; bool m_drawProbes; bool m_useBox; diff --git a/samples/sample_compound.cpp b/samples/sample_compound.cpp index e0e7694..0faeee0 100644 --- a/samples/sample_compound.cpp +++ b/samples/sample_compound.cpp @@ -13,6 +13,8 @@ #include +#include + class SimpleCompound : public Sample { public: @@ -484,6 +486,39 @@ static bool OverlapResultFcn( b3ShapeId shapeId, void* context ) return false; } +struct VillageBoxQuery +{ + b3AABB aabb; + int shapeCount; + int childCount; +}; + +static bool CountCompoundChildFcn( const b3CompoundData* compound, int childIndex, void* context ) +{ + (void)compound; + (void)childIndex; + int* count = (int*)context; + *count += 1; + return true; +} + +// The world query only reaches the compound as a whole, so descend into its children like a game would +static bool VillageBoxQueryFcn( b3ShapeId shapeId, void* context ) +{ + VillageBoxQuery* query = (VillageBoxQuery*)context; + query->shapeCount += 1; + + if ( b3Shape_GetType( shapeId ) == b3_compoundShape ) + { + b3WorldTransform bodyTransform = b3Body_GetTransform( b3Shape_GetBody( shapeId ) ); + b3Transform worldToLocal = b3InvMulWorldTransforms( bodyTransform, b3WorldTransform_identity ); + b3AABB localBox = b3AABB_Transform( worldToLocal, query->aabb ); + b3QueryCompound( b3Shape_GetCompound( shapeId ), localBox, CountCompoundChildFcn, &query->childCount ); + } + + return true; +} + class Village : public Sample { public: @@ -691,6 +726,80 @@ public: } } + // Fixed seed so timings compare across runs and builds + void CreateStressQueries() + { + uint32_t savedSeed = g_randomSeed; + g_randomSeed = 271828; + + float h = 0.45f * m_worldWidth; + + m_stressRays.resize( m_stressRayCount ); + for ( StressRay& ray : m_stressRays ) + { + ray.origin = { RandomFloatRange( -h, h ), 20.0f, RandomFloatRange( -h, h ) }; + ray.translation = { RandomFloatRange( -10.0f, 10.0f ), -40.0f, RandomFloatRange( -10.0f, 10.0f ) }; + } + + m_stressBoxes.resize( m_stressBoxCount ); + for ( b3AABB& box : m_stressBoxes ) + { + b3Vec3 center = { RandomFloatRange( -h, h ), RandomFloatRange( 0.0f, 8.0f ), RandomFloatRange( -h, h ) }; + b3Vec3 extents = RandomVec3( { 0.5f, 0.5f, 0.5f }, { 4.0f, 4.0f, 4.0f } ); + box = { center - extents, center + extents }; + } + + g_randomSeed = savedSeed; + } + + void RunStressQueries() + { + b3QueryFilter filter = b3DefaultQueryFilter(); + + uint64_t ticks = b3GetTicks(); + + int rayHitCount = 0; + for ( const StressRay& ray : m_stressRays ) + { + CastClosestContext context = {}; + (void)b3World_CastRay( m_worldId, ray.origin, ray.translation, filter, CastClosestCallback, &context ); + rayHitCount += context.hit ? 1 : 0; + } + + float rayTime = b3GetMillisecondsAndReset( &ticks ); + + VillageBoxQuery query = {}; + for ( b3AABB box : m_stressBoxes ) + { + query.aabb = box; + (void)b3World_OverlapAABB( m_worldId, box, filter, VillageBoxQueryFcn, &query ); + } + + float boxTime = b3GetMilliseconds( ticks ); + + int rayCount = (int)m_stressRays.size(); + int boxCount = (int)m_stressBoxes.size(); + + DrawTextLine( "rays = %d, hits = %d", rayCount, rayHitCount ); + DrawTextLine( "ray time = %.3f ms, ave = %.3f us", rayTime, 1000.0f * rayTime / rayCount ); + DrawTextLine( "aabb queries = %d, shapes = %d, children = %d", boxCount, query.shapeCount, query.childCount ); + DrawTextLine( "aabb time = %.3f ms, ave = %.3f us", boxTime, 1000.0f * boxTime / boxCount ); + } + + bool DrawControls() override + { + bool changed = ImGui::Checkbox( "Query Stress", &m_stressQueries ); + changed = ImGui::SliderInt( "Rays", &m_stressRayCount, 1, 100000 ) || changed; + changed = ImGui::SliderInt( "AABB Queries", &m_stressBoxCount, 1, 100000 ) || changed; + + if ( changed && m_stressQueries ) + { + CreateStressQueries(); + } + + return true; + } + void Render() override { Sample::Render(); @@ -742,6 +851,11 @@ public: DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson ); + if ( m_stressQueries ) + { + RunStressQueries(); + } + b3Vec3 translation = { 10.0f, -40.0f, -5.0f }; b3QueryFilter filter = b3DefaultQueryFilter(); @@ -828,10 +942,22 @@ public: return new Village( context ); } + struct StressRay + { + b3Pos origin; + b3Vec3 translation; + }; + b3CompoundData* m_compound; GeometricMover m_mover; float m_worldWidth; b3Pos m_rayOrigin; + + std::vector m_stressRays; + std::vector m_stressBoxes; + int m_stressRayCount = 1000; + int m_stressBoxCount = 1000; + bool m_stressQueries = false; }; static int sampleVillage = RegisterSample( "Compound", "Village", Village::Create ); diff --git a/src/joint.c b/src/joint.c index aa48179..6e3f54c 100644 --- a/src/joint.c +++ b/src/joint.c @@ -330,7 +330,8 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint else { // joint connected between sleeping and/or static bodies - B3_ASSERT( bodyA->setIndex >= b3_firstSleepingSet || bodyB->setIndex >= b3_firstSleepingSet ); + B3_ASSERT( bodyA->setIndex >= b3_firstSleepingSet || + bodyB->setIndex >= b3_firstSleepingSet ); B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); // joint should go into the sleeping set (not static set) @@ -530,7 +531,7 @@ b3JointId b3CreateMoverJoint( b3WorldId worldId, const b3MoverJointDef* def ) { B3_CHECK_JOINT_DEF( def ); B3_ASSERT( b3IsValidVec3( def->linearVelocity ) ); - B3_ASSERT( b3IsValidVec3( def->maxVelocityForce ) ); + B3_ASSERT( b3IsValidFloat( def->maxForce ) && def->maxForce >= 0.0f ); b3World* world = b3GetUnlockedWorldFromId( worldId ); if ( world == NULL ) @@ -543,7 +544,7 @@ b3JointId b3CreateMoverJoint( b3WorldId worldId, const b3MoverJointDef* def ) joint->moverJoint = (b3MoverJoint){ 0 }; joint->moverJoint.linearVelocity = def->linearVelocity; - joint->moverJoint.maxVelocityForce = def->maxVelocityForce; + joint->moverJoint.maxForce = def->maxForce; b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; B3_REC_CREATE( world, CreateMoverJoint, jointId, worldId, *def ); @@ -722,8 +723,10 @@ b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* float lowerAngle = b3MinFloat( def->lowerTwistAngle, def->upperTwistAngle ); float upperAngle = b3MaxFloat( def->lowerTwistAngle, def->upperTwistAngle ); - joint->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); - joint->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->sphericalJoint.lowerTwistAngle = + b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->sphericalJoint.upperTwistAngle = + b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); joint->sphericalJoint.maxMotorTorque = def->maxMotorTorque; joint->sphericalJoint.motorVelocity = def->motorVelocity; @@ -880,7 +883,8 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) if ( setIndex == b3_awakeSet ) { - b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex ); + b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, + joint->colorIndex, localIndex ); } else { @@ -1092,7 +1096,8 @@ bool b3Joint_IsAwake( b3JointId jointId ) return joint->setIndex == b3_awakeSet; } -void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ) +void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, + float* torque ) { float linearImpulse = 0.0f; float angularImpulse = 0.0f; @@ -1114,15 +1119,18 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo case b3_distanceJoint: { b3DistanceJoint* joint = &sim->distanceJoint; - linearImpulse = b3AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ); + linearImpulse = b3AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + + joint->motorImpulse ); } break; case b3_motorJoint: { b3MotorJoint* joint = &sim->motorJoint; - linearImpulse = b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); - angularImpulse = b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); + linearImpulse = + b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); + angularImpulse = + b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); } break; @@ -1169,7 +1177,8 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) ); b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse ); - impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); + impulse = + b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis ); angularImpulse = b3Length( impulse ); @@ -1189,8 +1198,10 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo // todo probably wrong b3WheelJoint* joint = &sim->wheelJoint; b3Vec2 perpImpulse = joint->linearImpulse; - float axialImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; - linearImpulse = sqrtf( perpImpulse.x * perpImpulse.x + perpImpulse.y * perpImpulse.y + axialImpulse * axialImpulse ); + float axialImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - + joint->upperSuspensionImpulse; + linearImpulse = sqrtf( perpImpulse.x * perpImpulse.x + perpImpulse.y * perpImpulse.y + + axialImpulse * axialImpulse ); angularImpulse = b3AbsFloat( joint->spinImpulse ); } break; @@ -1198,7 +1209,7 @@ void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, flo case b3_moverJoint: { b3MoverJoint* joint = &sim->moverJoint; - linearImpulse = b3Length( joint->linearVelocityImpulse ); + linearImpulse = b3Length2( joint->impulse ); } break; @@ -1398,7 +1409,8 @@ float b3Joint_GetLinearSeparation( b3JointId jointId ) } } - return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + return sqrtf( perpendicularSeparation * perpendicularSeparation + + limitSeparation * limitSeparation ); } case b3_revoluteJoint: @@ -1440,7 +1452,8 @@ float b3Joint_GetLinearSeparation( b3JointId jointId ) } } - return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + return sqrtf( perpendicularSeparation * perpendicularSeparation + + limitSeparation * limitSeparation ); } default: @@ -1864,11 +1877,11 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) break; case b3_moverJoint: - draw->DrawPointFcn( pB, 8.0f, b3_colorPlum, draw->context ); + b3DrawMoverJoint( draw, world, jointSim, transformA, transformB ); break; case b3_pogoJoint: - draw->DrawSegmentFcn( pA, pB, b3_colorLightGray, draw->context ); + b3DrawPogoJoint( draw, world, jointSim, transformA, transformB, scale ); break; default: @@ -1881,10 +1894,12 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) if ( draw->drawGraphColors ) { b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { - b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, - b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, - b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, - b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, + b3_colorCyan, b3_colorBlue, b3_colorViolet, b3_colorPink, + b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, + b3_colorPlum, b3_colorSnow, b3_colorTeal, b3_colorKhaki, + b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, }; int colorIndex = joint->colorIndex; @@ -1895,13 +1910,14 @@ void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) } } - if ( draw->drawJointExtras ) + if ( draw->drawJointExtras && joint->type != b3_moverJoint && joint->type != b3_pogoJoint ) { b3Vec3 force = b3GetJointConstraintForce( world, joint ); b3Vec3 torque = b3GetJointConstraintTorque( world, joint ); b3Pos p = b3LerpPosition( pA, pB, 0.5f ); - draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( 0.001f, force ) ), b3_colorAzure, draw->context ); + draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( draw->forceScale, force ) ), + b3_colorAzure, draw->context ); char buffer[64]; snprintf( buffer, 64, "f = %g, t = %g", b3Length( force ), b3Length( torque ) ); diff --git a/src/joint.h b/src/joint.h index e633f84..45da807 100644 --- a/src/joint.h +++ b/src/joint.h @@ -124,9 +124,11 @@ typedef struct b3MotorJoint typedef struct b3MoverJoint { b3Vec3 linearVelocity; - b3Vec3 maxVelocityForce; + float maxForce; - b3Vec3 linearVelocityImpulse; + b3Vec3 axisX; + b3Vec3 axisZ; + b3Vec2 impulse; int indexA; int indexB; @@ -448,6 +450,10 @@ void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ); void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB ); +void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, + b3WorldTransform transformB ); +void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, + b3WorldTransform transformB, float scale ); void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ); diff --git a/src/mover_joint.c b/src/mover_joint.c index 8407450..d0686c3 100644 --- a/src/mover_joint.c +++ b/src/mover_joint.c @@ -4,7 +4,6 @@ #include "body.h" #include "core.h" #include "joint.h" -#include "math_internal.h" #include "physics_world.h" #include "recording.h" #include "solver.h" @@ -13,6 +12,8 @@ // needed for dll export #include "box3d/box3d.h" +#include + void b3MoverJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity ) { B3_ASSERT( b3IsValidVec3( velocity ) ); @@ -28,24 +29,26 @@ b3Vec3 b3MoverJoint_GetLinearVelocity( b3JointId jointId ) return joint->moverJoint.linearVelocity; } -void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, b3Vec3 maxForce ) +void b3MoverJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce ) { - B3_ASSERT( b3IsValidVec3( maxForce ) ); + B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f ); b3World* world = b3GetWorld( jointId.world0 ); B3_REC( world, MoverJointSetMaxVelocityForce, jointId, maxForce ); b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint ); - joint->moverJoint.maxVelocityForce = maxForce; + joint->moverJoint.maxForce = maxForce; } -b3Vec3 b3MoverJoint_GetMaxVelocityForce( b3JointId jointId ) +float b3MoverJoint_GetMaxVelocityForce( b3JointId jointId ) { b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_moverJoint ); - return joint->moverJoint.maxVelocityForce; + return joint->moverJoint.maxForce; } b3Vec3 b3GetMoverJointForce( b3World* world, b3JointSim* base ) { - return b3MulSV( world->inv_h, base->moverJoint.linearVelocityImpulse ); + b3MoverJoint* joint = &base->moverJoint; + b3Vec3 P = b3Blend2( joint->impulse.x, joint->axisX, joint->impulse.y, joint->axisZ ); + return b3MulSV( world->inv_h, P ); } void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context ) @@ -83,12 +86,17 @@ void b3PrepareMoverJoint( b3JointSim* base, b3StepContext* context ) joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX; joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX; + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + joint->axisX = matrixB.cx; + joint->axisZ = matrixB.cz; + float k = mA + mB; joint->linearMass = k > 0.0f ? 1.0f / k : 0.0f; if ( context->enableWarmStarting == false ) { - joint->linearVelocityImpulse = b3Vec3_zero; + joint->impulse = (b3Vec2){ 0 }; } } @@ -109,16 +117,15 @@ void b3WarmStartMoverJoint( b3JointSim* base, b3StepContext* context ) b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB; + b3Vec3 P = b3Blend2( joint->impulse.x, joint->axisX, joint->impulse.y, joint->axisZ ); if ( stateA->flags & b3_dynamicFlag ) { - stateA->linearVelocity = - b3MulSub( stateA->linearVelocity, mA, joint->linearVelocityImpulse ); + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, P ); } if ( stateB->flags & b3_dynamicFlag ) { - stateB->linearVelocity = - b3MulAdd( stateB->linearVelocity, mB, joint->linearVelocityImpulse ); + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, P ); } } @@ -141,24 +148,37 @@ void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context ) b3Vec3 vA = stateA->linearVelocity; b3Vec3 vB = stateB->linearVelocity; - b3Vec3 maxForce = joint->maxVelocityForce; - if ( maxForce.x > 0.0f || maxForce.y > 0.0f || maxForce.z > 0.0f ) + float maxForce = joint->maxForce; + if ( maxForce > 0.0f ) { - b3Vec3 cdot = b3Sub( b3Sub( vB, vA ), joint->linearVelocity ); - b3Vec3 impulse = b3MulSV( -joint->linearMass, cdot ); + b3Vec3 vr = b3Sub( b3Sub( vB, vA ), joint->linearVelocity ); + b3Vec2 cdot = { b3Dot( vr, joint->axisX ), b3Dot( vr, joint->axisZ ) }; + b3Vec2 deltaImpulse = b3MulSV2( -joint->linearMass, cdot ); + b3Vec2 newImpulse = { + joint->impulse.x + deltaImpulse.x, + joint->impulse.y + deltaImpulse.y, + }; - b3Vec3 oldImpulse = joint->linearVelocityImpulse; - b3Vec3 maxImpulse = b3MulSV( context->h, maxForce ); - joint->linearVelocityImpulse = - b3Clamp( b3Add( oldImpulse, impulse ), b3Neg( maxImpulse ), maxImpulse ); - impulse = b3Sub( joint->linearVelocityImpulse, oldImpulse ); + float maxImpulse = context->h * maxForce; - vA = b3MulSub( vA, mA, impulse ); - vB = b3MulAdd( vB, mB, impulse ); + float lengthSquared = b3Dot2( newImpulse, newImpulse ); + if ( lengthSquared > maxImpulse * maxImpulse ) + { + float scale = maxImpulse / sqrtf( lengthSquared ); + newImpulse.x *= scale; + newImpulse.y *= scale; + } + + deltaImpulse = b3Sub2( newImpulse, joint->impulse ); + joint->impulse = newImpulse; + + b3Vec3 P = b3Blend2( deltaImpulse.x, joint->axisX, deltaImpulse.y, joint->axisZ ); + vA = b3MulSub( vA, mA, P ); + vB = b3MulAdd( vB, mB, P ); } else { - joint->linearVelocityImpulse = b3Vec3_zero; + joint->impulse = (b3Vec2){ 0.0f, 0.0f }; } if ( stateA->flags & b3_dynamicFlag ) @@ -171,3 +191,72 @@ void b3SolveMoverJoint( b3JointSim* base, b3StepContext* context ) stateB->linearVelocity = vB; } } + +void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, + b3WorldTransform transformA, b3WorldTransform transformB ) +{ + B3_UNUSED( draw, world, base, transformA, transformB ); + B3_ASSERT( base->type == b3_moverJoint ); + +#if 0 + b3MoverJoint* joint = &base->moverJoint; + + b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p ); + + b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); + b3BodyState* stateA = b3GetBodyState( world, bodyA ); + b3BodyState* stateB = b3GetBodyState( world, bodyB ); + b3Vec3 vA = stateA != NULL ? stateA->linearVelocity : b3Vec3_zero; + b3Vec3 vB = stateB != NULL ? stateB->linearVelocity : b3Vec3_zero; + b3Vec3 relativeVelocity = b3Sub( vB, vA ); + + b3Vec3 maxForce = joint->maxForce; + b3Vec3 force = b3MulSV( world->inv_h, joint->impulse ); + + // todo comment + float saturation = 0.999f; + bool saturated = ( maxForce.x > 0.0f && b3AbsFloat( force.x ) >= saturation * maxForce.x ) || + ( maxForce.y > 0.0f && b3AbsFloat( force.y ) >= saturation * maxForce.y ) || + ( maxForce.z > 0.0f && b3AbsFloat( force.z ) >= saturation * maxForce.z ); + + // todo comment + b3Vec3 target = { + maxForce.x > 0.0f ? joint->linearVelocity.x : relativeVelocity.x, + maxForce.y > 0.0f ? joint->linearVelocity.y : relativeVelocity.y, + maxForce.z > 0.0f ? joint->linearVelocity.z : relativeVelocity.z, + }; + + b3HexColor targetColor = saturated ? b3_colorRed : b3_colorLimeGreen; + b3Pos targetTip = b3OffsetPos( pB, target ); + draw->DrawSegmentFcn( pB, targetTip, targetColor, draw->context ); + draw->DrawPointFcn( targetTip, 6.0f, targetColor, draw->context ); + + b3Pos velocityTip = b3OffsetPos( pB, relativeVelocity ); + draw->DrawSegmentFcn( pB, velocityTip, b3_colorLightSkyBlue, draw->context ); + draw->DrawPointFcn( velocityTip, 4.0f, b3_colorLightSkyBlue, draw->context ); + + draw->DrawPointFcn( pB, 8.0f, b3_colorPlum, draw->context ); + + if ( draw->drawJointExtras ) + { + if ( bodyA->type != b3_staticBody ) + { + draw->DrawSegmentFcn( pB, transformA.p, b3_colorDimGray, draw->context ); + } + + b3WorldTransform boxTransform = { pB, b3Quat_identity }; + draw->DrawBoxFcn( b3MulSV( draw->forceScale, maxForce ), boxTransform, b3_colorGray, + draw->context ); + + b3HexColor forceColor = saturated ? b3_colorRed : b3_colorAzure; + draw->DrawSegmentFcn( pB, b3OffsetPos( pB, b3MulSV( draw->forceScale, force ) ), forceColor, + draw->context ); + + char buffer[96]; + snprintf( buffer, 96, "f = (%.1f, %.1f, %.1f)", force.x, force.y, force.z ); + draw->DrawStringFcn( pB, buffer, forceColor, draw->context ); + } + +#endif +} diff --git a/src/pogo_joint.c b/src/pogo_joint.c index 5306632..d2643ce 100644 --- a/src/pogo_joint.c +++ b/src/pogo_joint.c @@ -13,6 +13,8 @@ // needed for dll export #include "box3d/box3d.h" +#include + void b3PogoJoint_SetRestLength( b3JointId jointId, float length ) { B3_ASSERT( b3IsValidFloat( length ) && length >= 0.0f ); @@ -279,3 +281,86 @@ void b3SolvePogoJoint( b3JointSim* base, b3StepContext* context, bool useBias ) stateB->angularVelocity = wB; } } + +void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA, + b3WorldTransform transformB, float scale ) +{ + B3_ASSERT( base->type == b3_pogoJoint ); + + b3PogoJoint* joint = &base->pogoJoint; + + b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p ); + b3Vec3 axis = b3RotateVector( b3MulQuat( transformB.q, base->localFrameB.q ), b3Vec3_axisY ); + b3Vec3 perp1 = b3Perp( axis ); + b3Vec3 perp2 = b3Cross( axis, perp1 ); + + float length = b3Dot( b3SubPos( pB, pA ), axis ); + b3Pos foot = b3OffsetPos( pB, b3MulSV( -length, axis ) ); + + float force = world->inv_h * joint->impulse; + + // todo comment + float saturation = 0.999f; + + b3HexColor color; + if ( joint->hertz == 0.0f ) + { + color = b3_colorGray; + } + else if ( force > 0.0f && force >= saturation * joint->maxCompressionForce ) + { + color = b3_colorRed; + } + else if ( force < 0.0f && -force >= saturation * joint->maxTensionForce ) + { + color = b3_colorRed; + } + else if ( force > 0.0f ) + { + color = b3_colorLimeGreen; + } + else if ( force < 0.0f ) + { + color = b3_colorDeepSkyBlue; + } + else + { + color = b3_colorLightGray; + } + + int segmentCount = 12; + float width = 0.1f * scale; + b3Pos p1 = pB; + for ( int i = 1; i <= segmentCount; ++i ) + { + float side = i == segmentCount ? 0.0f : ( i & 1 ) ? width : -width; + float t = (float)i / (float)segmentCount; + b3Pos p2 = b3OffsetPos( pB, b3MulAdd( b3MulSV( -t * length, axis ), side, perp1 ) ); + draw->DrawSegmentFcn( p1, p2, color, draw->context ); + p1 = p2; + } + + b3Pos rest = b3OffsetPos( pB, b3MulSV( -joint->restLength, axis ) ); + float tick = 1.5f * width; + draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp1 ) ), b3OffsetPos( rest, b3MulSV( tick, perp1 ) ), + b3_colorWhite, draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( rest, b3MulSV( -tick, perp2 ) ), b3OffsetPos( rest, b3MulSV( tick, perp2 ) ), + b3_colorWhite, draw->context ); + + if ( b3Length( b3SubPos( pA, foot ) ) > B3_LINEAR_SLOP ) + { + draw->DrawSegmentFcn( foot, pA, b3_colorDimGray, draw->context ); + } + + draw->DrawPointFcn( pA, 6.0f, color, draw->context ); + draw->DrawSegmentFcn( pA, b3OffsetPos( pA, b3MulSV( 0.25f * scale, joint->normal ) ), b3_colorOrange, draw->context ); + + if ( draw->drawJointExtras ) + { + char buffer[96]; + snprintf( buffer, 96, "length = %.3f, rest = %.3f, f = %.1f, v = %.3f", length, joint->restLength, force, + joint->velocity ); + draw->DrawStringFcn( pB, buffer, color, draw->context ); + } +} diff --git a/src/recording.c b/src/recording.c index 8eb4f01..e3cdced 100644 --- a/src/recording.c +++ b/src/recording.c @@ -342,7 +342,7 @@ _Static_assert( sizeof( void* ) != 8 || sizeof( b3WeldJointDef ) == 128, "b3WeldJointDef changed: update b3RecW_WELDJOINTDEF and its reader together" ); _Static_assert( sizeof( void* ) != 8 || sizeof( b3WheelJointDef ) == 184, "b3WheelJointDef changed: update b3RecW_WHEELJOINTDEF and its reader together" ); -_Static_assert( sizeof( void* ) != 8 || sizeof( b3MoverJointDef ) == 136, +_Static_assert( sizeof( void* ) != 8 || sizeof( b3MoverJointDef ) == 128, "b3MoverJointDef changed: update b3RecW_MOVERJOINTDEF and its reader together" ); _Static_assert( sizeof( void* ) != 8 || sizeof( b3PogoJointDef ) == 152, "b3PogoJointDef changed: update b3RecW_POGOJOINTDEF and its reader together" ); @@ -556,7 +556,7 @@ void b3RecW_MOVERJOINTDEF( b3RecBuffer* buf, b3MoverJointDef v ) { b3RecW_JointBase( buf, &v.base ); b3RecW_VEC3( buf, v.linearVelocity ); - b3RecW_VEC3( buf, v.maxVelocityForce ); + b3RecW_F32( buf, v.maxForce ); } void b3RecW_POGOJOINTDEF( b3RecBuffer* buf, b3PogoJointDef v ) diff --git a/src/recording_ops.inl b/src/recording_ops.inl index 9905ebd..e66a593 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -215,7 +215,7 @@ B3_REC_OP( 0xE7, WheelJointSetTargetSteeringAngle, RET_NONE, ARG( JOINTID, joint // Mover joint B3_REC_OP( 0x81, CreateMoverJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOVERJOINTDEF, def ) ) B3_REC_OP( 0x82, MoverJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) ) -B3_REC_OP( 0x83, MoverJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, maxForce ) ) +B3_REC_OP( 0x83, MoverJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) ) // Pogo joint B3_REC_OP( 0x84, CreatePogoJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( POGOJOINTDEF, def ) ) diff --git a/src/replay.c b/src/replay.c index 20d0c60..16b5124 100644 --- a/src/replay.c +++ b/src/replay.c @@ -654,7 +654,7 @@ b3MoverJointDef b3RecR_MOVERJOINTDEF( b3RecReader* rdr ) b3MoverJointDef def = b3DefaultMoverJointDef(); b3RecR_JointBase( rdr, &def.base ); def.linearVelocity = b3RecR_VEC3( rdr ); - def.maxVelocityForce = b3RecR_VEC3( rdr ); + def.maxForce = b3RecR_F32( rdr ); return def; } diff --git a/src/simd_avx2.h b/src/simd_avx2.h index a751249..36c79d6 100644 --- a/src/simd_avx2.h +++ b/src/simd_avx2.h @@ -92,14 +92,6 @@ static inline b3FloatW8 b3MaxW8( b3FloatW8 a, b3FloatW8 b ) return _mm256_max_ps( a, b ); } -// Clamp a to [-b, b] -static inline b3FloatW8 b3SymClampW8( b3FloatW8 a, b3FloatW8 b ) -{ - b3FloatW8 nb = b3NegW8( b ); - b3FloatW8 c = b3MaxW8( nb, a ); - return b3MinW8( c, b ); -} - static inline b3FloatW8 b3AndW8( b3FloatW8 a, b3FloatW8 b ) { return _mm256_and_ps( a, b ); diff --git a/src/simd_neon.h b/src/simd_neon.h index c35f39b..ce9b560 100644 --- a/src/simd_neon.h +++ b/src/simd_neon.h @@ -89,14 +89,6 @@ static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) return vmaxq_f32( a, b ); } -// clamp a to [-b, b] -static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) -{ - b3FloatW4 nb = b3NegW4( b ); - b3FloatW4 c = b3MaxW4( nb, a ); - return b3MinW4( c, b ); -} - static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) { return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); diff --git a/src/simd_scalar.h b/src/simd_scalar.h index 56cb045..8ae1b85 100644 --- a/src/simd_scalar.h +++ b/src/simd_scalar.h @@ -104,21 +104,6 @@ static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) return r; } -// clamp a to [-b, b] -static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) -{ - b3FloatW4 r; - r.x = a.x <= b.x ? a.x : b.x; - r.y = a.y <= b.y ? a.y : b.y; - r.z = a.z <= b.z ? a.z : b.z; - r.w = a.w <= b.w ? a.w : b.w; - r.x = r.x <= -b.x ? -b.x : r.x; - r.y = r.y <= -b.y ? -b.y : r.y; - r.z = r.z <= -b.z ? -b.z : r.z; - r.w = r.w <= -b.w ? -b.w : r.w; - return r; -} - // Logical operations on the scalar path are 0/1 float values. Not bit-wise like SIMD. static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) diff --git a/src/simd_sse2.h b/src/simd_sse2.h index b09a17c..8041c47 100644 --- a/src/simd_sse2.h +++ b/src/simd_sse2.h @@ -92,14 +92,6 @@ static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) return _mm_max_ps( a, b ); } -// clamp a to [-b, b] -static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) -{ - b3FloatW4 nb = b3NegW4( b ); - b3FloatW4 c = b3MaxW4( nb, a ); - return b3MinW4( c, b ); -} - static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) { return _mm_and_ps( a, b ); diff --git a/src/simd_wide.h b/src/simd_wide.h index fe0bb9c..f7c8879 100644 --- a/src/simd_wide.h +++ b/src/simd_wide.h @@ -41,7 +41,6 @@ typedef B3_WIDE( b3Vec3 ) b3Vec3W; #define b3MulAddW B3_WIDE( b3MulAdd ) #define b3MinW B3_WIDE( b3Min ) #define b3MaxW B3_WIDE( b3Max ) -#define b3SymClampW B3_WIDE( b3SymClamp ) #define b3AndW B3_WIDE( b3And ) #define b3OrW B3_WIDE( b3Or ) #define b3AndNotW B3_WIDE( b3AndNot ) diff --git a/test/test_joint.c b/test/test_joint.c index 9c196ca..13ecd22 100644 --- a/test/test_joint.c +++ b/test/test_joint.c @@ -315,10 +315,10 @@ static int TestMoverJoint( void ) ENSURE( gotVelocity.x == 1.0f && gotVelocity.y == 2.0f && gotVelocity.z == -3.0f ); // The y axis has no force budget, so it must stay at rest while x and z reach the target - b3Vec3 maxForce = { 1000.0f, 0.0f, 1000.0f }; + float maxForce = 1000.0f; b3MoverJoint_SetMaxVelocityForce( jointId, maxForce ); - b3Vec3 gotForce = b3MoverJoint_GetMaxVelocityForce( jointId ); - ENSURE( gotForce.x == 1000.0f && gotForce.y == 0.0f && gotForce.z == 1000.0f ); + float gotForce = b3MoverJoint_GetMaxVelocityForce( jointId ); + ENSURE( gotForce == 1000.0f ); for ( int i = 0; i < 30; ++i ) { diff --git a/test/test_recording.c b/test/test_recording.c index 663b9a0..9c34b54 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -76,8 +76,9 @@ static int HullDedup( void ) { // Build a small convex hull b3Vec3 pts[8] = { - { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, - { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, + { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, }; b3HullData* hull = b3CreateHull( pts, 8, 8 ); ENSURE( hull != NULL ); @@ -127,7 +128,8 @@ static int HullDedup( void ) // entryCount is a little-endian u32 at the start of the registry block const uint8_t* rp = bytes + (int)regOff; - uint32_t entryCount = (uint32_t)rp[0] | ( (uint32_t)rp[1] << 8 ) | ( (uint32_t)rp[2] << 16 ) | ( (uint32_t)rp[3] << 24 ); + uint32_t entryCount = (uint32_t)rp[0] | ( (uint32_t)rp[1] << 8 ) | ( (uint32_t)rp[2] << 16 ) | + ( (uint32_t)rp[3] << 24 ); ENSURE( entryCount == 1 ); b3DestroyRecording( rec ); @@ -174,7 +176,8 @@ static int CrossWidthReplay( void ) for ( int k = 0; k < 3; ++k ) { seed = seed * 1664525u + 1013904223u; - v[k] = ( (float)( seed >> 8 ) / (float)( 1 << 24 ) - 0.5f ) * ( k == 1 ? 0.6f : 1.0f ); + v[k] = + ( (float)( seed >> 8 ) / (float)( 1 << 24 ) - 0.5f ) * ( k == 1 ? 0.6f : 1.0f ); } points[i] = (b3Vec3){ v[0], v[1], v[2] }; } @@ -204,8 +207,10 @@ static int CrossWidthReplay( void ) { b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; - bodyDef.position = (b3Pos){ 0.4f * (float)( i % 3 ) - 0.4f, 1.5f + 0.9f * (float)i, 0.3f * (float)( i % 2 ) }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.37f * (float)i ); + bodyDef.position = (b3Pos){ 0.4f * (float)( i % 3 ) - 0.4f, 1.5f + 0.9f * (float)i, + 0.3f * (float)( i % 2 ) }; + bodyDef.rotation = + b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.37f * (float)i ); b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); b3CreateHullShape( bodyId, &dynamicShape, hulls[i % hullCount] ); } @@ -452,8 +457,9 @@ static int ScrubBackward( void ) static int SeekWithHull( void ) { b3Vec3 pts[8] = { - { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, - { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, + { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, }; b3HullData* hull = b3CreateHull( pts, 8, 8 ); ENSURE( hull != NULL ); @@ -549,7 +555,8 @@ static void RecTestDestroyDebugShape( void* userShape, void* userContext ) counters->destroyed += 1; } -static void RecTestDrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, void* context ) +static void RecTestDrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, + void* context ) { (void)userShape; (void)transform; @@ -618,12 +625,14 @@ static int DebugShapeCallbacks( void ) b3Recording* loaded = b3LoadRecordingFromFile( path ); ENSURE( loaded != NULL ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); ENSURE( player != NULL ); // Wiring the callbacks rebuilds the world and rewinds to frame 0. DebugShapeCounters counters = { 0, 0 }; - b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, &counters ); + b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, + &counters ); ENSURE( b3RecPlayer_GetFrame( player ) == 0 ); // Replay to the end, then draw: createDebugShape fires once per shape (ground + 4 boxes = 5). @@ -635,8 +644,8 @@ static int DebugShapeCallbacks( void ) ENSURE( counters.created >= 5 ); ENSURE( !b3RecPlayer_HasDiverged( player ) ); - // A backward seek restores the empty seed in place, releasing the live debug shapes, then forward - // stepping recreates them through the callbacks, so more creates fire. + // A backward seek restores the empty seed in place, releasing the live debug shapes, then + // forward stepping recreates them through the callbacks, so more creates fire. int createdBefore = counters.created; b3RecPlayer_SeekFrame( player, 0 ); b3RecPlayer_SeekFrame( player, totalFrames ); @@ -705,7 +714,8 @@ static int PlayerAccessors( void ) b3World_StopRecording( worldId ); b3DestroyWorld( worldId ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); ENSURE( player != NULL ); // Info reflects the recorded tuning and a non-degenerate bounds. @@ -754,10 +764,11 @@ static int PlayerAccessors( void ) return 0; } -// A keyframe restore is a deterministic replay state, so shapes that persist must keep their renderer -// handle rather than being torn down and rebuilt every seek. Record a snapshot-seeded session (shapes -// exist at frame 0), replay with handle callbacks, scrub backward across keyframes repeatedly, and -// verify no new handles are built per restore and the create/destroy counts still balance at teardown. +// A keyframe restore is a deterministic replay state, so shapes that persist must keep their +// renderer handle rather than being torn down and rebuilt every seek. Record a snapshot-seeded +// session (shapes exist at frame 0), replay with handle callbacks, scrub backward across keyframes +// repeatedly, and verify no new handles are built per restore and the create/destroy counts still +// balance at teardown. static int KeyframeHandleReuse( void ) { b3WorldDef worldDef = b3DefaultWorldDef(); @@ -803,11 +814,13 @@ static int KeyframeHandleReuse( void ) b3World_StopRecording( worldId ); b3DestroyWorld( worldId ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); ENSURE( player != NULL ); DebugShapeCounters counters = { 0, 0 }; - b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, &counters ); + b3RecPlayer_SetDebugShapeCallbacks( player, RecTestCreateDebugShape, RecTestDestroyDebugShape, + &counters ); // Replay to the end and draw: one handle per shape. b3RecPlayer_SeekFrame( player, totalFrames ); @@ -815,8 +828,8 @@ static int KeyframeHandleReuse( void ) ENSURE( !b3RecPlayer_HasDiverged( player ) ); ENSURE( counters.created == shapeCount ); - // Scrub backward and forward across keyframes. Each restore keeps the persistent shapes' handles, - // so drawing builds no new ones. + // Scrub backward and forward across keyframes. Each restore keeps the persistent shapes' + // handles, so drawing builds no new ones. int createdAfterFirstDraw = counters.created; int seekTargets[] = { 40, 70, 20, 60, 8, 75 }; for ( int k = 0; k < (int)( sizeof( seekTargets ) / sizeof( seekTargets[0] ) ); ++k ) @@ -841,8 +854,9 @@ static bool QueryReplayOverlapFcn( b3ShapeId shapeId, void* context ) return true; } -static float QueryReplayCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, - int triangleIndex, int childIndex, void* context ) +static float QueryReplayCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, + uint64_t userMaterialId, int triangleIndex, int childIndex, + void* context ) { (void)shapeId; (void)point; @@ -855,8 +869,9 @@ static float QueryReplayCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, return fraction; } -static float CompoundHitCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, - int triangleIndex, int childIndex, void* context ) +static float CompoundHitCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, + uint64_t userMaterialId, int triangleIndex, int childIndex, + void* context ) { (void)point; (void)normal; @@ -871,7 +886,8 @@ static float CompoundHitCastFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, return 1.0f; } -static bool QueryReplayPlaneFcn( b3ShapeId shapeId, const b3PlaneResult* planes, int planeCount, void* context ) +static bool QueryReplayPlaneFcn( b3ShapeId shapeId, const b3PlaneResult* planes, int planeCount, + void* context ) { (void)shapeId; (void)planes; @@ -887,9 +903,9 @@ static bool QueryReplayMoverFilterFcn( b3ShapeId shapeId, void* context ) return true; } -// Issue all seven world queries each frame, then replay. Every query is re-issued against the replay -// world and compared to what was recorded, so a clean (non-diverged) replay proves the queries -// reproduce. Also opens a player and confirms the per-frame query store surfaces all seven. +// Issue all seven world queries each frame, then replay. Every query is re-issued against the +// replay world and compared to what was recorded, so a clean (non-diverged) replay proves the +// queries reproduce. Also opens a player and confirms the per-frame query store surfaces all seven. static int QueryReplay( void ) { b3Recording* rec = b3CreateRecording( 0 ); @@ -939,7 +955,8 @@ static int QueryReplay( void ) b3World_CastRay( worldId, origin, translation, filter, QueryReplayCastFcn, NULL ); b3World_CastRayClosest( worldId, origin, translation, filter ); b3World_CastShape( worldId, origin, &proxy, translation, filter, QueryReplayCastFcn, NULL ); - b3World_CastMover( worldId, origin, &mover, translation, filter, QueryReplayMoverFilterFcn, NULL ); + b3World_CastMover( worldId, origin, &mover, translation, filter, QueryReplayMoverFilterFcn, + NULL ); b3World_CollideMover( worldId, origin, &mover, filter, QueryReplayPlaneFcn, NULL ); b3World_Step( worldId, 1.0f / 60.0f, 4 ); @@ -952,7 +969,8 @@ static int QueryReplay( void ) ENSURE( b3ValidateReplay( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ) ); // Player path: seek to a mid frame and confirm the per-frame store holds all seven queries. - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); ENSURE( player != NULL ); b3RecPlayer_SeekFrame( player, 15 ); @@ -980,10 +998,10 @@ static int QueryReplay( void ) return 0; } -// Tagged queries: the caller (id, label) is hashed into a key that rides the QueryTag op, with the id -// and label interned in the trailing tag table. The same label under two entity ids is two distinct -// keys. All survive a file round-trip and resolve back through b3RecQueryInfo; untagged queries report -// key 0 / id 0 / name NULL. +// Tagged queries: the caller (id, label) is hashed into a key that rides the QueryTag op, with the +// id and label interned in the trailing tag table. The same label under two entity ids is two +// distinct keys. All survive a file round-trip and resolve back through b3RecQueryInfo; untagged +// queries report key 0 / id 0 / name NULL. static int TaggedQuery( void ) { b3Recording* rec = b3CreateRecording( 0 ); @@ -1042,7 +1060,8 @@ static int TaggedQuery( void ) b3Recording* loaded = b3LoadRecordingFromFile( path ); ENSURE( loaded != NULL ); - b3RecPlayer* player = b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); + b3RecPlayer* player = + b3CreatePlayer( b3Recording_GetData( loaded ), b3Recording_GetSize( loaded ), 1 ); ENSURE( player != NULL ); b3RecPlayer_SeekFrame( player, 5 ); @@ -1178,7 +1197,8 @@ static int AllOps( void ) b3ShapeDef sphereShapeDef = b3DefaultShapeDef(); sphereShapeDef.density = 1.0f; - // Over-length shape name so replay exercises the clamp in the shape def reader, like the body above. + // Over-length shape name so replay exercises the clamp in the shape def reader, like the body + // above. sphereShapeDef.name = "sphereNameThatExceedsTheLimit"; b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; b3ShapeId sphereShapeId = b3CreateSphereShape( bodyId, &sphereShapeDef, &sphere ); @@ -1199,8 +1219,9 @@ static int AllOps( void ) // Custom hull shape on a third dynamic body b3Vec3 hullPts[8] = { - { -0.5f, -0.5f, -0.5f }, { 0.5f, -0.5f, -0.5f }, { 0.5f, 0.5f, -0.5f }, { -0.5f, 0.5f, -0.5f }, - { -0.5f, -0.5f, 0.5f }, { 0.5f, -0.5f, 0.5f }, { 0.5f, 0.5f, 0.5f }, { -0.5f, 0.5f, 0.5f }, + { -0.5f, -0.5f, -0.5f }, { 0.5f, -0.5f, -0.5f }, { 0.5f, 0.5f, -0.5f }, + { -0.5f, 0.5f, -0.5f }, { -0.5f, -0.5f, 0.5f }, { 0.5f, -0.5f, 0.5f }, + { 0.5f, 0.5f, 0.5f }, { -0.5f, 0.5f, 0.5f }, }; b3HullData* customHull = b3CreateHull( hullPts, 8, 8 ); ENSURE( customHull != NULL ); @@ -1237,9 +1258,10 @@ static int AllOps( void ) ENSURE( b3Body_IsValid( xformBodyId ) ); b3ShapeDef xformShapeDef = b3DefaultShapeDef(); xformShapeDef.density = 1.0f; - b3Transform xformXf = { (b3Vec3){ 0.1f, 0.2f, -0.1f }, b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.4f ) }; - b3ShapeId xformShapeId = - b3CreateTransformedHullShape( xformBodyId, &xformShapeDef, customHull, xformXf, (b3Vec3){ 1.25f, 0.75f, 1.5f } ); + b3Transform xformXf = { (b3Vec3){ 0.1f, 0.2f, -0.1f }, + b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.4f ) }; + b3ShapeId xformShapeId = b3CreateTransformedHullShape( + xformBodyId, &xformShapeDef, customHull, xformXf, (b3Vec3){ 1.25f, 0.75f, 1.5f } ); ENSURE( b3Shape_IsValid( xformShapeId ) ); // Mesh, height field, and compound static shapes (3D-only) @@ -1252,7 +1274,8 @@ static int AllOps( void ) b3MeshData* swapMeshData = b3CreateGridMesh( 4, 4, 1.5f, 0, false ); ENSURE( swapMeshData != NULL ); b3ShapeDef meshShapeDef = b3DefaultShapeDef(); - b3ShapeId meshShapeId = b3CreateMeshShape( meshBodyId, &meshShapeDef, meshData, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + b3ShapeId meshShapeId = + b3CreateMeshShape( meshBodyId, &meshShapeDef, meshData, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); ENSURE( b3Shape_IsValid( meshShapeId ) ); b3BodyDef hfBodyDef = b3DefaultBodyDef(); @@ -1339,8 +1362,8 @@ static int AllOps( void ) // Body mutators: SetTransform, SetLinearVelocity/AngularVelocity (Vec3), SetName, // damping, gravity scale, sleep threshold, SetAwake, EnableSleep, SetBullet, SetMotionLocks, - // SetMassData, ApplyMassFromShapes, SetType, SetTargetTransform, Disable/Enable, EnableContactRecycling, - // EnableHitEvents, all force/impulse/torque variants + // SetMassData, ApplyMassFromShapes, SetType, SetTargetTransform, Disable/Enable, + // EnableContactRecycling, EnableHitEvents, all force/impulse/torque variants b3Body_SetTransform( bodyId, (b3Pos){ 1.0f, 6.0f, 0.0f }, b3Quat_identity ); b3Body_SetLinearVelocity( bodyId, (b3Vec3){ 0.5f, 0.0f, 0.0f } ); b3Body_SetAngularVelocity( bodyId, (b3Vec3){ 0.0f, 0.25f, 0.0f } ); @@ -1407,7 +1430,8 @@ static int AllOps( void ) b3Body_ApplyForce( bodyId, (b3Vec3){ 0.0f, 50.0f, 0.0f }, (b3Pos){ 1.0f, 6.0f, 0.0f }, true ); b3Body_ApplyForceToCenter( bodyId, (b3Vec3){ 5.0f, 0.0f, 0.0f }, true ); b3Body_ApplyTorque( bodyId, (b3Vec3){ 0.0f, 1.0f, 0.0f }, true ); - b3Body_ApplyLinearImpulse( bodyId, (b3Vec3){ 0.1f, 0.0f, 0.0f }, (b3Pos){ 1.0f, 6.0f, 0.0f }, true ); + b3Body_ApplyLinearImpulse( bodyId, (b3Vec3){ 0.1f, 0.0f, 0.0f }, (b3Pos){ 1.0f, 6.0f, 0.0f }, + true ); b3Body_ApplyLinearImpulseToCenter( bodyId, (b3Vec3){ 0.0f, 0.1f, 0.0f }, true ); b3Body_ApplyAngularImpulse( bodyId, (b3Vec3){ 0.0f, 0.05f, 0.0f }, true ); @@ -1443,7 +1467,8 @@ static int AllOps( void ) b3RevoluteJoint_SetSpringDampingRatio( revId, 0.5f ); b3RevoluteJoint_SetTargetAngle( revId, 0.25f ); b3Joint_SetLocalFrameA( revId, (b3Transform){ (b3Vec3){ 1.0f, 0.0f, 0.0f }, b3Quat_identity } ); - b3Joint_SetLocalFrameB( revId, (b3Transform){ (b3Vec3){ -1.0f, 0.0f, 0.0f }, b3Quat_identity } ); + b3Joint_SetLocalFrameB( revId, + (b3Transform){ (b3Vec3){ -1.0f, 0.0f, 0.0f }, b3Quat_identity } ); b3Joint_SetConstraintTuning( revId, 60.0f, 2.0f ); b3Joint_SetForceThreshold( revId, 100.0f ); b3Joint_SetTorqueThreshold( revId, 50.0f ); @@ -1587,7 +1612,7 @@ static int AllOps( void ) moverDef.base.bodyIdB = moverBodyId; moverDef.base.collideConnected = true; moverDef.linearVelocity = (b3Vec3){ 1.0f, 0.0f, 0.5f }; - moverDef.maxVelocityForce = (b3Vec3){ 0.5f, 0.0f, 0.5f }; + moverDef.maxForce = 0.5f; b3JointId moverJointId = b3CreateMoverJoint( worldId, &moverDef ); b3PogoJointDef pogoDef = b3DefaultPogoJointDef(); @@ -1656,7 +1681,8 @@ static int AllOps( void ) b3World_CastRayClosest( worldId, qorigin, qTranslation, qfilter ); b3World_CastShape( worldId, qorigin, &proxy, qTranslation, qfilter, QueryReplayCastFcn, NULL ); b3Capsule mover = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, 0.3f }; - b3World_CastMover( worldId, qorigin, &mover, qTranslation, qfilter, QueryReplayMoverFilterFcn, NULL ); + b3World_CastMover( worldId, qorigin, &mover, qTranslation, qfilter, QueryReplayMoverFilterFcn, + NULL ); b3World_CollideMover( worldId, qorigin, &mover, qfilter, QueryReplayPlaneFcn, NULL ); float timeStep = 1.0f / 60.0f; @@ -1669,7 +1695,7 @@ static int AllOps( void ) b3Body_ApplyLinearImpulseToCenter( capsuleBodyId, (b3Vec3){ 2.0f, 0.0f, 0.0f }, true ); b3Body_SetGravityScale( bodyId, 1.0f ); b3Body_SetSafetyFactor( fastId, 0.4f ); - b3MoverJoint_SetMaxVelocityForce( moverJointId, (b3Vec3){ 60.0f, 0.0f, 60.0f } ); + b3MoverJoint_SetMaxVelocityForce( moverJointId, 60.0f ); } // The dynamic mover rebuilds its pogo every step, warm started from the old one @@ -1765,8 +1791,9 @@ static int AllOps( void ) ENSURE( b3RecPlayer_GetFrame( player ) == seekTargets[k] ); bool hitCompound = false; - b3World_CastRay( b3RecPlayer_GetWorldId( player ), (b3Pos){ 30.0f, 10.0f, 0.0f }, (b3Vec3){ 0.0f, -20.0f, 0.0f }, - b3DefaultQueryFilter(), CompoundHitCastFcn, &hitCompound ); + b3World_CastRay( b3RecPlayer_GetWorldId( player ), (b3Pos){ 30.0f, 10.0f, 0.0f }, + (b3Vec3){ 0.0f, -20.0f, 0.0f }, b3DefaultQueryFilter(), + CompoundHitCastFcn, &hitCompound ); ENSURE( hitCompound ); } while ( b3RecPlayer_StepFrame( player ) ) @@ -1825,8 +1852,9 @@ static int AllOps( void ) static int TransformedHullRoundTrip( void ) { b3Vec3 pts[8] = { - { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f }, - { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, + { -1.0f, -1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, -1.0f }, { -1.0f, -1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, + { 1.0f, 1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, }; b3HullData* hull = b3CreateHull( pts, 8, 8 ); ENSURE( hull != NULL ); @@ -1842,8 +1870,10 @@ static int TransformedHullRoundTrip( void ) b3ShapeDef shapeDef = b3DefaultShapeDef(); shapeDef.density = 1.0f; - // Baked transform with a rotation and non-uniform scale, the path Unreal uses for instanced hulls. - b3Transform xf = { (b3Vec3){ 0.25f, 0.0f, -0.5f }, b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 0.0f, 1.0f }, 0.3f ) }; + // Baked transform with a rotation and non-uniform scale, the path Unreal uses for instanced + // hulls. + b3Transform xf = { (b3Vec3){ 0.25f, 0.0f, -0.5f }, + b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 0.0f, 1.0f }, 0.3f ) }; b3Vec3 scl = { 1.5f, 0.5f, 2.0f }; for ( int i = 0; i < 3; ++i ) { @@ -1855,8 +1885,9 @@ static int TransformedHullRoundTrip( void ) ENSURE( b3Shape_IsValid( sid ) ); } - // A plain hull after the transformed ones: if the transformed creates desynced the id pool, this - // shape's recorded id would not match what replay allocates and b3ValidateReplay would fail. + // A plain hull after the transformed ones: if the transformed creates desynced the id pool, + // this shape's recorded id would not match what replay allocates and b3ValidateReplay would + // fail. { b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; @@ -1866,8 +1897,8 @@ static int TransformedHullRoundTrip( void ) ENSURE( b3Shape_IsValid( sid ) ); } - // Step past the keyframe interval so replay captures a keyframe. That path re-serializes the live - // world and interns the baked hull, which must already be in the pre-seeded registry. + // Step past the keyframe interval so replay captures a keyframe. That path re-serializes the + // live world and interns the baked hull, which must already be in the pre-seeded registry. for ( int i = 0; i < 20; ++i ) { b3World_Step( worldId, 1.0f / 60.0f, 4 ); @@ -1985,7 +2016,8 @@ static int CorruptCompound( void ) int recSize = b3Recording_GetSize( rec ); ENSURE( b3ValidateReplay( recData, recSize, 1 ) ); - // The registry entry is a kind byte and a 4 byte length, then the blob, which leads with its version + // The registry entry is a kind byte and a 4 byte length, then the blob, which leads with its + // version uint64_t version = B3_COMPOUND_VERSION; int blobOffset = -1; for ( int i = 5; i + (int)sizeof( version ) <= recSize; ++i ) @@ -2028,9 +2060,10 @@ static int CorruptCompound( void ) } // Geometry that shares a content hash but differs in bytes must dedup exactly. Mirrors the keyframe -// flow that crashed: the seed appends every slot 1:1 (even byte-identical duplicates a hash collision -// left in an already-recorded file), then capture must resolve a live blob back to an existing slot -// without growing the registry. Forces the collision by handing the same hash to distinct blobs. +// flow that crashed: the seed appends every slot 1:1 (even byte-identical duplicates a hash +// collision left in an already-recorded file), then capture must resolve a live blob back to an +// existing slot without growing the registry. Forces the collision by handing the same hash to +// distinct blobs. static int GeometryHashCollision( void ) { enum @@ -2067,8 +2100,8 @@ static int GeometryHashCollision( void ) b3FreeRegistry( ® ); - // Seed-then-capture: appending byte-identical duplicate slots keeps id == slot index, and a later - // exact intern still resolves to one of them without appending a new entry. + // Seed-then-capture: appending byte-identical duplicate slots keeps id == slot index, and a + // later exact intern still resolves to one of them without appending a new entry. b3GeometryRegistry seeded = { 0 }; uint8_t slot0[n]; uint8_t slot1[n]; @@ -2092,11 +2125,12 @@ static int GeometryHashCollision( void ) return 0; } -// Staged stepping must reveal a mid-stream body at its creation transform. A body created and given an -// impulse in one recorded step is first placed by CreateBody, then displaced by the following Step. Atomic -// replay fuses the two, so the body is only ever seen already moved. Staged replay parks between them so -// the pre-integration pose is drawable. Verify the parked pose is the creation transform, that atomic -// replay does not show it, and that the extra park does not perturb the end state. +// Staged stepping must reveal a mid-stream body at its creation transform. A body created and given +// an impulse in one recorded step is first placed by CreateBody, then displaced by the following +// Step. Atomic replay fuses the two, so the body is only ever seen already moved. Staged replay +// parks between them so the pre-integration pose is drawable. Verify the parked pose is the +// creation transform, that atomic replay does not show it, and that the extra park does not perturb +// the end state. static int StagedStepCreationPose( void ) { b3Recording* rec = b3CreateRecording( 0 ); @@ -2118,8 +2152,8 @@ static int StagedStepCreationPose( void ) b3World_Step( worldId, timeStep, subStepCount ); } - // The creation transform under test. No ground, so the body is the only create and stays ballistic. - // Components are exact in float so the round-trip pose compares tight. + // The creation transform under test. No ground, so the body is the only create and stays + // ballistic. Components are exact in float so the round-trip pose compares tight. const b3Pos spawn = { 1.0, 20.0, -2.0 }; b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; @@ -2131,7 +2165,8 @@ static int StagedStepCreationPose( void ) shapeDef.density = 1.0f; b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - // Impulse along +x so the first integrated step moves the body a visible distance off the spawn. + // Impulse along +x so the first integrated step moves the body a visible distance off the + // spawn. b3Body_ApplyLinearImpulseToCenter( bodyId, (b3Vec3){ 5.0f, 0.0f, 0.0f }, true ); const int creationFrame = leadFrames + 1; @@ -2170,8 +2205,8 @@ static int StagedStepCreationPose( void ) uint64_t atomicHash = b3HashWorldState( b3GetWorldFromId( b3RecPlayer_GetWorldId( atomic ) ) ); b3DestroyPlayer( atomic ); - // Staged replay: step forward until the first pre-step park. It must sit at the creation frame's - // pre-integration state with the new body at exactly its spawn transform. + // Staged replay: step forward until the first pre-step park. It must sit at the creation + // frame's pre-integration state with the new body at exactly its spawn transform. b3RecPlayer* staged = b3CreatePlayer( data, sz, 1 ); ENSURE( staged != NULL ); while ( !b3RecPlayer_IsAtEnd( staged ) ) @@ -2183,7 +2218,8 @@ static int StagedStepCreationPose( void ) } } ENSURE( b3RecPlayer_IsAtPreStep( staged ) ); - ENSURE( b3RecPlayer_GetFrame( staged ) == creationFrame - 1 ); // parked before the step, frame not advanced + ENSURE( b3RecPlayer_GetFrame( staged ) == + creationFrame - 1 ); // parked before the step, frame not advanced b3BodyId stagedId = b3RecPlayer_GetBodyId( staged, 0 ); ENSURE( b3Body_IsValid( stagedId ) ); @@ -2197,7 +2233,8 @@ static int StagedStepCreationPose( void ) ENSURE( b3RecPlayer_GetFrame( staged ) == creationFrame ); ENSURE( !b3RecPlayer_IsAtPreStep( staged ) ); - // Run staged to the end: the same op stream, so it must land on the atomic end state bit for bit. + // Run staged to the end: the same op stream, so it must land on the atomic end state bit for + // bit. while ( !b3RecPlayer_IsAtEnd( staged ) ) { b3RecPlayer_SubStepFrame( staged ); @@ -2212,9 +2249,9 @@ static int StagedStepCreationPose( void ) return 0; } -// Shape names are debug only and do not feed the determinism hash, so b3ValidateReplay cannot catch a -// broken name round-trip. Replay through the player and read the names back to prove the def field and -// the ShapeSetName op survive serialization. +// Shape names are debug only and do not feed the determinism hash, so b3ValidateReplay cannot catch +// a broken name round-trip. Replay through the player and read the names back to prove the def +// field and the ShapeSetName op survive serialization. static int ShapeNameReplay( void ) { const char* names[3] = { @@ -2298,8 +2335,9 @@ static int ShapeNameReplay( void ) // A box sliding across a mesh floor, with the option to swap both geometries and retune a // per-triangle material part way through. Returns the final state hash so the caller can prove the // mutations move the simulation. Recording is optional so the same scene serves as the control. -static uint64_t RunGeometryMutatorScene( b3Recording* rec, bool mutate, const b3MeshData* meshA, const b3MeshData* meshB, - const b3HullData* swapHull, float swapFriction ) +static uint64_t RunGeometryMutatorScene( b3Recording* rec, bool mutate, const b3MeshData* meshA, + const b3MeshData* meshB, const b3HullData* swapHull, + float swapFriction ) { b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = 1; @@ -2321,7 +2359,8 @@ static uint64_t RunGeometryMutatorScene( b3Recording* rec, bool mutate, const b3 b3ShapeDef meshShapeDef = b3DefaultShapeDef(); meshShapeDef.materials = meshMaterials; meshShapeDef.materialCount = 2; - b3ShapeId meshShapeId = b3CreateMeshShape( meshBodyId, &meshShapeDef, meshA, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + b3ShapeId meshShapeId = + b3CreateMeshShape( meshBodyId, &meshShapeDef, meshA, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); b3BodyDef boxBodyDef = b3DefaultBodyDef(); boxBodyDef.type = b3_dynamicBody; @@ -2384,11 +2423,13 @@ static int GeometryMutatorReplay( void ) b3BoxHull swapHull = b3MakeBoxHull( 0.25f, 1.5f, 0.25f ); const float swapFriction = 0.95f; - uint64_t controlHash = RunGeometryMutatorScene( NULL, false, meshA, meshB, &swapHull.base, swapFriction ); + uint64_t controlHash = + RunGeometryMutatorScene( NULL, false, meshA, meshB, &swapHull.base, swapFriction ); b3Recording* rec = b3CreateRecording( 0 ); ENSURE( rec != NULL ); - uint64_t mutatedHash = RunGeometryMutatorScene( rec, true, meshA, meshB, &swapHull.base, swapFriction ); + uint64_t mutatedHash = + RunGeometryMutatorScene( rec, true, meshA, meshB, &swapHull.base, swapFriction ); // Without this the replay gate below could pass on a recording that never carried the ops. ENSURE( mutatedHash != controlHash );